[gazebo] 01/04: Imported Upstream version 6.0.0
Jose Luis Rivero
jrivero-guest at moszumanska.debian.org
Thu Jul 30 17:03:45 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 d2625f08a34072a5eddb24a387effe52f770f402
Author: Jose Luis Rivero <jrivero at osrfoundation.org>
Date: Thu Jul 30 16:32:05 2015 +0000
Imported Upstream version 6.0.0
---
.hg_archival.txt | 8 +-
.hgignore | 1 +
CMakeLists.txt | 104 +-
Changelog.md | 496 +-
Migration.md | 129 +
cmake/DefaultCFlags.cmake | 43 +-
cmake/FindFreeimage.cmake | 14 +-
cmake/FindGraphviz.cmake | 101 +
cmake/FindSSE.cmake | 2 +-
cmake/GPL-2 | 339 +
cmake/GazeboTestUtils.cmake | 46 +-
cmake/GazeboUtils.cmake | 45 +-
cmake/SearchForStuff.cmake | 315 +-
cmake/gazebo-config.cmake.in | 11 +-
cmake/gazebo_config.h.in | 2 +
cmake/pkgconfig/gazebo.in | 4 +-
cmake/setup.sh.in | 6 +-
configure.bat | 91 +
deps/ann/include/ann/ANN.h | 2 +-
deps/ann/include/ann/ANNx.h | 2 +-
deps/ann/include/ann/LGPL | 165 +
deps/libccd/CMakeLists.txt | 12 +-
deps/libccd/include/ccd/ccd.h | 31 +-
deps/opende/CMakeLists.txt | 33 +-
deps/opende/GIMPACT/CMakeLists.txt | 9 +-
deps/opende/GIMPACT/include/GIMPACT/gimpact.h | 28 +-
deps/opende/OPCODE/CMakeLists.txt | 9 +-
deps/opende/OPCODE/Ice/IceHPoint.h | 296 +-
deps/opende/OPCODE/Ice/IcePoint.h | 1030 +-
deps/opende/OPCODE/Ice/IceTypes.h | 268 +-
deps/opende/OPCODE/Opcode.h | 44 +-
deps/opende/include/ode/h5dump.h | 45 +
deps/opende/include/ode/misc.h | 16 +-
deps/opende/include/ode/objects.h | 144 +-
deps/opende/include/ode/odecpp.h | 8 +
deps/opende/ou/CMakeLists.txt | 10 +-
deps/opende/ou/include/ou/atomic.h | 84 +-
deps/opende/ou/include/ou/atomicflags.h | 4 +-
deps/opende/ou/include/ou/customization.h | 6 +-
deps/opende/ou/include/ou/enumarrays.h | 18 +-
deps/opende/ou/include/ou/ou_dll.h | 29 +
deps/opende/ou/include/ou/simpleflags.h | 4 +-
deps/opende/ou/include/ou/threadlocalstorage.h | 6 +-
deps/opende/ou/src/threadlocalstorage.cpp | 1692 +-
deps/opende/src/collision_trimesh_trimesh_new.cpp | 6 +
deps/opende/src/heightfield.cpp | 6 +
deps/opende/src/io.cpp | 457 +
deps/opende/src/io.h | 104 +
deps/opende/src/ioh5.cpp | 433 +
deps/opende/src/ioh5.h | 414 +
deps/opende/src/joints/hinge.cpp | 20 -
deps/opende/src/joints/hinge2.cpp | 2 -
deps/opende/src/joints/joint.cpp | 4 -
deps/opende/src/joints/joint.h | 9 -
deps/opende/src/joints/piston.cpp | 2 -
deps/opende/src/joints/screw.cpp | 42 +-
deps/opende/src/joints/slider.cpp | 21 -
deps/opende/src/joints/universal.cpp | 2 -
deps/opende/src/lcp.cpp | 22 +-
deps/opende/src/objects.h | 89 +-
deps/opende/src/ode.cpp | 615 +-
deps/opende/src/odeou.cpp | 34 +-
deps/opende/src/quickstep.cpp | 2245 +-
deps/opende/src/quickstep_cg_lcp.cpp | 142 +
deps/opende/src/quickstep_cg_lcp.h | 42 +
deps/opende/src/quickstep_pgs_lcp.cpp | 1563 +
deps/opende/src/quickstep_pgs_lcp.h | 81 +
deps/opende/src/quickstep_update_bodies.cpp | 377 +
deps/opende/src/quickstep_update_bodies.h | 45 +
deps/opende/src/quickstep_util.cpp | 524 +
deps/opende/src/quickstep_util.h | 270 +
deps/opende/src/step.cpp | 130 +-
deps/opende/src/step_dart_pgs_wrapper.cpp | 46 +
deps/opende/src/step_dart_pgs_wrapper.h | 24 +
deps/opende/src/symm.c | 2 +-
deps/opende/src/util.cpp | 309 +-
deps/win/tinyxml/VERSION_2.6.2 | 2 +
deps/win/tinyxml/tinystr.cpp | 111 +
deps/win/tinyxml/tinystr.h | 305 +
deps/win/tinyxml/tinyxml.cpp | 1863 +
deps/win/tinyxml/tinyxml.h | 1805 +
deps/win/tinyxml/tinyxmlerror.cpp | 52 +
deps/win/tinyxml/tinyxmlparser.cpp | 1640 +
doc/gazebo.in | 9 +-
doc/header.html | 3 +-
doc/mainpage.html | 19 +-
examples/ignition/CMakeLists.txt | 17 +
examples/ignition/angle.cc | 45 +
examples/ignition/box.cc | 46 +
examples/ignition/vector3.cc | 50 +
examples/plugins/CMakeLists.txt | 2 +
examples/plugins/animate_joints/CMakeLists.txt | 1 +
examples/plugins/animate_pose/CMakeLists.txt | 6 +-
examples/plugins/animate_pose/animate_pose.cc | 11 +-
examples/plugins/camera/CMakeLists.txt | 6 +-
examples/plugins/camera/camera_dump.cc | 2 +-
examples/plugins/custom_messages/CMakeLists.txt | 7 +-
examples/plugins/factory/CMakeLists.txt | 5 +-
examples/plugins/factory/factory.cc | 7 +-
.../gui_overlay_plugin_spawn/CMakeLists.txt | 18 +-
.../plugins/gui_overlay_plugin_time/CMakeLists.txt | 17 +-
examples/plugins/hello_world/CMakeLists.txt | 6 +-
examples/plugins/mainwindow_example/CMakeLists.txt | 17 +-
examples/plugins/model_move/CMakeLists.txt | 27 +
examples/plugins/model_move/README.md | 80 +
examples/plugins/model_move/model_move.cc | 178 +
examples/plugins/model_move/model_move.hh | 78 +
examples/plugins/model_move/model_move.world | 37 +
.../model_move/model_move_with_movement.world | 44 +
examples/plugins/model_move/path_publisher.cc | 61 +
examples/plugins/model_push/CMakeLists.txt | 6 +-
examples/plugins/model_visuals/CMakeLists.txt | 6 +-
examples/plugins/parameters/CMakeLists.txt | 1 +
examples/plugins/presentation.cc | 2 +-
examples/plugins/projector/CMakeLists.txt | 1 +
examples/plugins/system_gui_plugin/CMakeLists.txt | 2 +-
examples/plugins/world_edit/CMakeLists.txt | 7 +-
examples/plugins/world_edit/world_edit.cc | 4 +-
examples/stand_alone/actuator/CMakeLists.txt | 28 +
.../stand_alone/actuator/actuator_example.world | 226 +
examples/stand_alone/actuator/data/plot.py | 69 +
examples/stand_alone/actuator/main.cc | 214 +
examples/stand_alone/animated_box/CMakeLists.txt | 10 +-
examples/stand_alone/animated_box/animated_box.cc | 25 +-
.../animated_box/independent_listener.cc | 6 +-
examples/stand_alone/arrange/CMakeLists.txt | 6 +-
.../stand_alone/arrange/arrange_plugin_test.cc | 6 +-
.../stand_alone/clone_simulation/CMakeLists.txt | 9 +-
examples/stand_alone/custom_main/CMakeLists.txt | 6 +-
.../custom_main_pkgconfig/CMakeLists.txt | 19 +
.../stand_alone/custom_main_pkgconfig/README.md | 6 +
.../custom_main_pkgconfig/custom_main.cc | 41 +
examples/stand_alone/listener/CMakeLists.txt | 7 +-
examples/stand_alone/listener/listener.cc | 6 +-
examples/stand_alone/publisher/CMakeLists.txt | 7 +-
examples/stand_alone/publisher/publisher.cc | 7 +-
examples/stand_alone/test_fixture/CMakeLists.txt | 23 +
.../test_fixture/gtest/cmake/internal_utils.cmake | 227 +
.../test_fixture/gtest/gtest-1.7.0.diff | 44 +
.../gtest/include/gtest/gtest-death-test.h | 294 +
.../gtest/include/gtest/gtest-message.h | 250 +
.../gtest/include/gtest/gtest-param-test.h | 1421 +
.../gtest/include/gtest/gtest-param-test.h.pump | 487 +
.../gtest/include/gtest/gtest-printers.h | 856 +
.../test_fixture/gtest/include/gtest/gtest-spi.h | 232 +
.../gtest/include/gtest/gtest-test-part.h | 179 +
.../gtest/include/gtest/gtest-typed-test.h | 260 +
.../test_fixture/gtest/include/gtest/gtest.h | 2291 ++
.../gtest/include/gtest/gtest_pred_impl.h | 358 +
.../test_fixture/gtest/include/gtest/gtest_prod.h | 58 +
.../gtest/internal/gtest-death-test-internal.h | 319 +
.../gtest/include/gtest/internal/gtest-filepath.h | 206 +
.../gtest/include/gtest/internal/gtest-internal.h | 1158 +
.../include/gtest/internal/gtest-linked_ptr.h | 233 +
.../gtest/internal/gtest-param-util-generated.h | 5143 +++
.../internal/gtest-param-util-generated.h.pump | 301 +
.../include/gtest/internal/gtest-param-util.h | 619 +
.../gtest/include/gtest/internal/gtest-port.h | 1948 ++
.../gtest/include/gtest/internal/gtest-string.h | 167 +
.../gtest/include/gtest/internal/gtest-tuple.h | 1012 +
.../include/gtest/internal/gtest-tuple.h.pump | 339 +
.../gtest/include/gtest/internal/gtest-type-util.h | 3331 ++
.../include/gtest/internal/gtest-type-util.h.pump | 297 +
.../test_fixture/gtest/src/gtest-all.cc | 48 +
.../test_fixture/gtest/src/gtest-death-test.cc | 1344 +
.../test_fixture/gtest/src/gtest-filepath.cc | 382 +
.../test_fixture/gtest/src/gtest-internal-inl.h | 1218 +
.../test_fixture/gtest/src/gtest-port.cc | 805 +
.../test_fixture/gtest/src/gtest-printers.cc | 363 +
.../test_fixture/gtest/src/gtest-test-part.cc | 110 +
.../test_fixture/gtest/src/gtest-typed-test.cc | 110 +
.../stand_alone/test_fixture/gtest/src/gtest.cc | 5016 +++
.../test_fixture/gtest/src/gtest_main.cc | 38 +
examples/stand_alone/test_fixture/test.cc | 38 +
examples/stand_alone/transporter/CMakeLists.txt | 17 +
examples/stand_alone/transporter/README.md | 19 +
examples/stand_alone/transporter/transporter.cc | 46 +
gazebo/CMakeLists.txt | 97 +-
gazebo/Master.cc | 2 +-
gazebo/Server.cc | 45 +-
gazebo/common/Animation.cc | 33 +-
gazebo/common/Animation.hh | 15 +-
gazebo/common/Animation_TEST.cc | 24 +-
gazebo/common/AudioDecoder.cc | 4 +-
gazebo/common/AudioDecoder.hh | 2 +-
gazebo/common/BVHLoader.cc | 74 +-
gazebo/common/BVHLoader.hh | 9 +-
gazebo/common/Base64.hh | 4 +-
gazebo/common/CMakeLists.txt | 90 +-
gazebo/common/ColladaExporter.cc | 23 +-
gazebo/common/ColladaExporter.hh | 3 +-
gazebo/common/ColladaExporter_TEST.cc | 32 +-
gazebo/common/ColladaLoader.cc | 265 +-
gazebo/common/ColladaLoader.hh | 49 +-
gazebo/common/ColladaLoaderPrivate.hh | 16 +-
gazebo/common/ColladaLoader_TEST.cc | 12 +-
gazebo/common/Color.cc | 82 +-
gazebo/common/Color.hh | 23 +-
gazebo/common/Color_TEST.cc | 37 +-
gazebo/common/CommonIface.cc | 21 +
gazebo/common/CommonIface.hh | 17 +-
gazebo/common/CommonTypes.hh | 7 +-
gazebo/common/Console.cc | 33 +-
gazebo/common/Console.hh | 6 +-
gazebo/common/Dem.cc | 50 +-
gazebo/common/Dem.hh | 41 +-
gazebo/common/DemPrivate.hh | 4 +-
gazebo/common/Dem_TEST.cc | 25 +-
gazebo/common/Event.hh | 382 +-
gazebo/common/Event_TEST.cc | 37 +
gazebo/common/Events.hh | 2 +-
gazebo/common/Exception.hh | 6 +-
gazebo/common/GTSMeshUtils.cc | 296 +-
gazebo/common/GTSMeshUtils.hh | 47 +-
gazebo/common/GTSMeshUtils_TEST.cc | 80 +
gazebo/common/HeightmapData.hh | 22 +-
gazebo/common/Image.cc | 4 +
gazebo/common/Image.hh | 2 +-
gazebo/common/ImageHeightmap.cc | 14 +-
gazebo/common/ImageHeightmap.hh | 14 +-
gazebo/common/ImageHeightmap_TEST.cc | 20 +-
gazebo/common/KeyEvent.hh | 2 +-
gazebo/common/KeyFrame.cc | 52 +-
gazebo/common/KeyFrame.hh | 46 +-
gazebo/common/Material.hh | 2 +-
gazebo/common/Mesh.cc | 363 +-
gazebo/common/Mesh.hh | 264 +-
gazebo/common/MeshCSG.cc | 20 +-
gazebo/common/MeshCSG.hh | 23 +-
gazebo/common/MeshExporter.hh | 2 +-
gazebo/common/MeshLoader.hh | 2 +-
gazebo/common/MeshManager.cc | 797 +-
gazebo/common/MeshManager.hh | 180 +-
gazebo/common/MeshManager_TEST.cc | 253 +
gazebo/common/Mesh_TEST.cc | 81 +-
gazebo/common/ModelDatabase.cc | 7 +-
gazebo/common/ModelDatabase.hh | 2 +-
gazebo/common/ModelDatabasePrivate.hh | 2 +-
gazebo/common/MouseEvent.cc | 203 +
gazebo/common/MouseEvent.hh | 205 +-
gazebo/common/MouseEventPrivate.hh | 77 +
gazebo/common/MovingWindowFilter_TEST.cc | 19 +-
gazebo/common/PID.cc | 11 +-
gazebo/common/PID.hh | 2 +-
gazebo/common/Plugin.hh | 45 +-
gazebo/common/STLLoader.cc | 58 +-
gazebo/common/STLLoader.hh | 6 +-
gazebo/common/SVGLoader.cc | 1003 +
gazebo/common/SVGLoader.hh | 188 +
gazebo/common/SVGLoaderPrivate.hh | 43 +
gazebo/common/SVGLoader_TEST.cc | 218 +
gazebo/common/SingletonT.hh | 3 +-
gazebo/common/Skeleton.cc | 188 +-
gazebo/common/Skeleton.hh | 182 +-
gazebo/common/SkeletonAnimation.cc | 200 +-
gazebo/common/SkeletonAnimation.hh | 169 +-
gazebo/common/SphericalCoordinates.cc | 138 +-
gazebo/common/SphericalCoordinates.hh | 131 +-
gazebo/common/SphericalCoordinatesPrivate.hh | 15 +-
gazebo/common/SphericalCoordinates_TEST.cc | 83 +-
gazebo/common/SystemPaths.cc | 21 +-
gazebo/common/SystemPaths.hh | 4 +-
gazebo/common/SystemPaths_TEST.cc | 6 +-
gazebo/common/Time.cc | 256 +-
gazebo/common/Time.hh | 60 +-
gazebo/common/Time_TEST.cc | 102 +-
gazebo/common/Timer.cc | 36 +-
gazebo/common/Timer.hh | 17 +-
gazebo/common/Timer_TEST.cc | 108 +
gazebo/common/UpdateInfo.hh | 2 +-
gazebo/common/Video.cc | 4 +-
gazebo/common/Video.hh | 2 +-
gazebo/common/ffmpeg_inc.cc | 44 +
gazebo/common/ffmpeg_inc.h | 24 +-
gazebo/common/win_dirent.h | 783 +
gazebo/gazebo.1.ronn | 2 +
gazebo/gazebo.cc | 101 +-
gazebo/gazebo.hh | 7 +-
gazebo/gazebo_client.cc | 103 +
gazebo/gazebo_client.hh | 64 +
gazebo/gazebo_main.cc | 6 +-
gazebo/gazebo_shared.cc | 101 +
gazebo/gazebo_shared.hh | 49 +
gazebo/gui/Actions.cc | 120 +-
gazebo/gui/Actions.hh | 161 +-
gazebo/gui/ApplyWrenchDialog.cc | 1285 +
gazebo/gui/ApplyWrenchDialog.hh | 224 +
gazebo/gui/ApplyWrenchDialogPrivate.hh | 140 +
gazebo/gui/ApplyWrenchDialog_TEST.cc | 656 +
gazebo/gui/ApplyWrenchDialog_TEST.hh | 37 +
gazebo/gui/BoxMaker.cc | 85 +-
gazebo/gui/CMakeLists.txt | 78 +-
gazebo/gui/ConfigWidget.cc | 1475 +-
gazebo/gui/ConfigWidget.hh | 254 +-
gazebo/gui/ConfigWidget_TEST.cc | 430 +-
gazebo/gui/ConfigWidget_TEST.hh | 3 +
gazebo/gui/CylinderMaker.cc | 73 +-
gazebo/gui/DataLogger.cc | 386 +-
gazebo/gui/DataLogger.hh | 39 +-
gazebo/gui/DataLogger_TEST.cc | 80 +-
gazebo/gui/EntityMaker.cc | 7 +
gazebo/gui/GLWidget.cc | 506 +-
gazebo/gui/GLWidget.hh | 65 +-
gazebo/gui/GLWidget_TEST.cc | 30 +-
gazebo/gui/GLWidget_TEST2.cc | 6 -
gazebo/gui/GuiEvents.cc | 4 +
gazebo/gui/GuiEvents.hh | 44 +
gazebo/gui/GuiIface.cc | 41 +-
gazebo/gui/IncrementalPlot.cc | 6 +
gazebo/gui/InsertModelWidget.cc | 24 +-
gazebo/gui/JointControlWidget.cc | 7 +
gazebo/gui/LaserVisualization_TEST.cc | 99 +
gazebo/gui/LaserVisualization_TEST.hh | 32 +
gazebo/gui/LayersWidget.cc | 89 +
gazebo/gui/LayersWidget.hh | 58 +
gazebo/gui/LayersWidgetPrivate.hh | 39 +
gazebo/gui/LayersWidget_TEST.cc | 152 +
gazebo/gui/LayersWidget_TEST.hh | 32 +
gazebo/gui/LightMaker.cc | 33 +-
gazebo/gui/LightMaker.hh | 4 +-
gazebo/gui/LogPlayWidget.cc | 466 +
gazebo/gui/LogPlayWidget.hh | 160 +
gazebo/gui/LogPlayWidgetPrivate.hh | 85 +
gazebo/gui/MainWindow.cc | 626 +-
gazebo/gui/MainWindow.hh | 93 +-
gazebo/gui/MainWindow_TEST.cc | 644 +-
gazebo/gui/MainWindow_TEST.hh | 21 +
gazebo/gui/MeshMaker.cc | 47 +-
gazebo/gui/ModelAlign.cc | 56 +-
gazebo/gui/ModelAlign.hh | 14 +-
gazebo/gui/ModelAlignPrivate.hh | 9 -
gazebo/gui/ModelAlign_TEST.cc | 134 +-
gazebo/gui/ModelAlign_TEST.hh | 3 +
gazebo/gui/ModelBuilderWidget.cc | 7 +
gazebo/gui/ModelEditWidget.cc | 6 +
gazebo/gui/ModelListWidget.cc | 184 +-
gazebo/gui/ModelListWidget.hh | 16 +
gazebo/gui/ModelListWidget_TEST.cc | 4 +
gazebo/gui/ModelMaker.cc | 36 +-
gazebo/gui/ModelManipulator.cc | 261 +-
gazebo/gui/ModelManipulator.hh | 15 +
gazebo/gui/ModelManipulatorPrivate.hh | 7 +-
gazebo/gui/ModelRightMenu.cc | 159 +-
gazebo/gui/ModelRightMenu.hh | 39 +-
gazebo/gui/ModelSnap.cc | 80 +-
gazebo/gui/ModelSnap.hh | 4 +
gazebo/gui/ModelSnapPrivate.hh | 3 -
gazebo/gui/OculusWindow.cc | 1 +
gazebo/gui/Projection_TEST.cc | 108 +
gazebo/gui/Projection_TEST.hh | 31 +
gazebo/gui/RenderWidget.cc | 307 +-
gazebo/gui/RenderWidget.hh | 47 +-
gazebo/gui/RenderWidget_TEST.cc | 55 +
gazebo/gui/RenderWidget_TEST.hh | 32 +
gazebo/gui/SaveDialog.cc | 505 +-
gazebo/gui/SaveDialog.hh | 101 +-
gazebo/gui/SaveDialogPrivate.hh | 61 +
gazebo/gui/SaveDialog_TEST.cc | 86 +
gazebo/gui/SaveDialog_TEST.hh | 56 +
gazebo/gui/SpaceNav.cc | 29 +-
gazebo/gui/SpaceNavPrivate.hh | 5 +
gazebo/gui/SphereMaker.cc | 57 +-
gazebo/gui/SplashScreen.cc | 95 +
gazebo/gui/SplashScreen.hh | 56 +
gazebo/gui/SplashScreenPrivate.hh | 37 +
gazebo/gui/SplashScreen_TEST.cc | 59 +
gazebo/gui/SplashScreen_TEST.hh | 32 +
gazebo/gui/TimePanel.cc | 388 +-
gazebo/gui/TimePanel.hh | 110 +-
gazebo/gui/TimePanelPrivate.hh | 65 +
gazebo/gui/TimePanel_TEST.cc | 58 +-
gazebo/gui/TimePanel_TEST.hh | 6 +-
gazebo/gui/TimeWidget.cc | 308 +
gazebo/gui/TimeWidget.hh | 125 +
gazebo/gui/TimeWidgetPrivate.hh | 82 +
gazebo/gui/TimeWidget_TEST.cc | 186 +
gazebo/gui/TimeWidget_TEST.hh | 35 +
gazebo/gui/ToolsWidget.cc | 6 +
gazebo/gui/TopicSelector.cc | 6 +
gazebo/gui/ViewAngleWidget.cc | 262 +
gazebo/gui/ViewAngleWidget.hh | 115 +
gazebo/gui/ViewAngleWidgetPrivate.hh | 62 +
gazebo/gui/ViewAngleWidget_TEST.cc | 306 +
gazebo/gui/ViewAngleWidget_TEST.hh | 35 +
gazebo/gui/building/BaseInspectorDialog.cc | 134 +
gazebo/gui/building/BaseInspectorDialog.hh | 84 +
gazebo/gui/building/BuildingEditor.cc | 53 +-
gazebo/gui/building/BuildingEditor.hh | 25 +-
gazebo/gui/building/BuildingEditorEvents.cc | 9 +-
gazebo/gui/building/BuildingEditorEvents.hh | 109 +-
gazebo/gui/building/BuildingEditorPalette.cc | 207 +-
gazebo/gui/building/BuildingEditorPalette.hh | 69 +-
.../gui/building/BuildingEditorPalettePrivate.hh | 16 +
gazebo/gui/building/BuildingEditorWidget.hh | 2 +-
gazebo/gui/building/BuildingItem.hh | 2 +-
gazebo/gui/building/BuildingMaker.cc | 510 +-
gazebo/gui/building/BuildingMaker.hh | 50 +-
gazebo/gui/building/BuildingModelManip.cc | 2 +
gazebo/gui/building/BuildingModelManip.hh | 2 +-
gazebo/gui/building/CMakeLists.txt | 17 +-
gazebo/gui/building/DoorItem.hh | 3 +-
gazebo/gui/building/EditorItem.hh | 2 +-
gazebo/gui/building/EditorView.cc | 13 +-
gazebo/gui/building/EditorView.hh | 4 +-
gazebo/gui/building/FinishBuildingDialog.cc | 260 -
gazebo/gui/building/FinishBuildingDialog.hh | 127 -
gazebo/gui/building/FloorItem.hh | 3 +-
gazebo/gui/building/GrabberHandle.hh | 2 +-
gazebo/gui/building/GridLines.hh | 2 +-
gazebo/gui/building/ImportImageDialog.cc | 13 +-
gazebo/gui/building/ImportImageDialog.hh | 2 +-
gazebo/gui/building/ImportImageView.hh | 2 +-
gazebo/gui/building/LevelInspectorDialog.cc | 116 +-
gazebo/gui/building/LevelInspectorDialog.hh | 32 +-
gazebo/gui/building/LevelWidget.hh | 2 +-
gazebo/gui/building/MeasureItem.hh | 2 +-
gazebo/gui/building/RectItem.hh | 3 +-
gazebo/gui/building/RotateHandle.hh | 2 +-
gazebo/gui/building/ScaleWidget.hh | 2 +-
gazebo/gui/building/SegmentItem.hh | 2 +-
gazebo/gui/building/StairsInspectorDialog.cc | 169 +-
gazebo/gui/building/StairsInspectorDialog.hh | 32 +-
gazebo/gui/building/StairsItem.hh | 3 +-
gazebo/gui/building/WallInspectorDialog.cc | 191 +-
gazebo/gui/building/WallInspectorDialog.hh | 32 +-
gazebo/gui/building/WallSegmentItem.hh | 2 +-
gazebo/gui/building/WindowDoorInspectorDialog.cc | 48 +-
gazebo/gui/building/WindowDoorInspectorDialog.hh | 4 +-
gazebo/gui/building/WindowItem.hh | 3 +-
gazebo/gui/images/com.png | Bin 0 -> 3095 bytes
gazebo/gui/images/down_spin_arrow.png | Bin 0 -> 165 bytes
gazebo/gui/images/down_spin_arrow_disabled.png | Bin 0 -> 166 bytes
gazebo/gui/images/log_pause.png | Bin 0 -> 1634 bytes
gazebo/gui/images/log_play.png | Bin 0 -> 1864 bytes
gazebo/gui/images/log_record.png | Bin 0 -> 1903 bytes
gazebo/gui/images/log_record.svg | 114 +
gazebo/gui/images/log_step_forward.png | Bin 0 -> 610 bytes
gazebo/gui/images/splash.svg | 116 +
gazebo/gui/images/trashcan.png | Bin 0 -> 1256 bytes
gazebo/gui/images/up_spin_arrow.png | Bin 0 -> 158 bytes
gazebo/gui/images/up_spin_arrow_disabled.png | Bin 0 -> 159 bytes
gazebo/gui/images/view_angle_back.png | Bin 0 -> 1467 bytes
gazebo/gui/images/view_angle_back.svg | 99 +
gazebo/gui/images/view_angle_bottom.png | Bin 0 -> 1473 bytes
gazebo/gui/images/view_angle_bottom.svg | 99 +
gazebo/gui/images/view_angle_front.png | Bin 0 -> 1261 bytes
gazebo/gui/images/view_angle_front.svg | 99 +
gazebo/gui/images/view_angle_home.png | Bin 0 -> 1534 bytes
gazebo/gui/images/view_angle_home.svg | 87 +
gazebo/gui/images/view_angle_left.png | Bin 0 -> 1428 bytes
gazebo/gui/images/view_angle_left.svg | 99 +
gazebo/gui/images/view_angle_right.png | Bin 0 -> 1662 bytes
gazebo/gui/images/view_angle_right.svg | 99 +
gazebo/gui/images/view_angle_top.png | Bin 0 -> 1891 bytes
gazebo/gui/images/view_angle_top.svg | 99 +
gazebo/gui/main.cc | 2 +
gazebo/gui/model/CMakeLists.txt | 81 +-
gazebo/gui/model/CollisionConfig.cc | 312 +
gazebo/gui/model/CollisionConfig.hh | 139 +
gazebo/gui/model/ExtrudeDialog.cc | 311 +
gazebo/gui/model/ExtrudeDialog.hh | 95 +
gazebo/gui/model/ExtrudeDialogPrivate.hh | 52 +
gazebo/gui/model/ExtrudeDialog_TEST.cc | 78 +
gazebo/gui/model/ExtrudeDialog_TEST.hh | 35 +
gazebo/gui/model/GraphScene.cc | 110 +
gazebo/gui/model/GraphScene.hh | 82 +
gazebo/gui/model/GraphView.cc | 128 +
gazebo/gui/model/GraphView.hh | 83 +
gazebo/gui/model/ImportDialog.cc | 53 +-
gazebo/gui/model/ImportDialog.hh | 14 +-
gazebo/gui/model/JointInspector.cc | 446 +-
gazebo/gui/model/JointInspector.hh | 135 +-
gazebo/gui/model/JointMaker.cc | 922 +-
gazebo/gui/model/JointMaker.hh | 159 +-
gazebo/gui/model/JointMaker_TEST.cc | 334 +-
gazebo/gui/model/JointMaker_TEST.hh | 9 +
gazebo/gui/model/LinkConfig.cc | 89 +
gazebo/gui/model/LinkConfig.hh | 67 +
gazebo/gui/model/LinkInspector.cc | 144 +
gazebo/gui/model/LinkInspector.hh | 107 +
gazebo/gui/model/ModelCreator.cc | 1751 +-
gazebo/gui/model/ModelCreator.hh | 306 +-
gazebo/gui/model/ModelCreator_TEST.cc | 205 +
gazebo/gui/model/ModelCreator_TEST.hh | 35 +
gazebo/gui/model/ModelData.cc | 871 +
gazebo/gui/model/ModelData.hh | 150 +-
gazebo/gui/model/ModelEditor.cc | 300 +-
gazebo/gui/model/ModelEditor.hh | 62 +-
gazebo/gui/model/ModelEditorEvents.cc | 24 +
gazebo/gui/model/ModelEditorEvents.hh | 346 +-
gazebo/gui/model/ModelEditorPalette.cc | 768 +-
gazebo/gui/model/ModelEditorPalette.hh | 175 +-
gazebo/gui/model/ModelEditorPalette_TEST.cc | 85 +
gazebo/gui/model/ModelEditorPalette_TEST.hh | 32 +
gazebo/gui/model/ModelEditorPrivate.hh | 103 +
gazebo/gui/model/ModelEditor_TEST.cc | 70 +
gazebo/gui/model/ModelEditor_TEST.hh | 32 +
gazebo/gui/model/SchematicViewWidget.cc | 208 +
gazebo/gui/model/SchematicViewWidget.hh | 120 +
gazebo/gui/model/SchematicViewWidget_TEST.cc | 82 +
gazebo/gui/model/SchematicViewWidget_TEST.hh | 32 +
gazebo/gui/model/VisualConfig.cc | 333 +
gazebo/gui/model/VisualConfig.hh | 147 +
gazebo/gui/model/qgv/CMakeLists.txt | 48 +
gazebo/gui/model/qgv/GraphViz.pri | 17 +
gazebo/gui/model/qgv/LICENSE.txt | 502 +
gazebo/gui/model/qgv/QGVCore.pro | 47 +
gazebo/gui/model/qgv/QGVEdge.cpp | 207 +
gazebo/gui/model/qgv/QGVEdge.h | 89 +
gazebo/gui/model/qgv/QGVNode.cpp | 137 +
gazebo/gui/model/qgv/QGVNode.h | 75 +
gazebo/gui/model/qgv/QGVScene.cpp | 342 +
gazebo/gui/model/qgv/QGVScene.h | 112 +
gazebo/gui/model/qgv/QGVSubGraph.cpp | 146 +
gazebo/gui/model/qgv/QGVSubGraph.h | 74 +
gazebo/gui/model/qgv/private/QGVCore.cpp | 135 +
gazebo/gui/model/qgv/private/QGVCore.h | 106 +
gazebo/gui/model/qgv/private/QGVEdgePrivate.cpp | 34 +
gazebo/gui/model/qgv/private/QGVEdgePrivate.h | 36 +
gazebo/gui/model/qgv/private/QGVGraphPrivate.cpp | 34 +
gazebo/gui/model/qgv/private/QGVGraphPrivate.h | 36 +
gazebo/gui/model/qgv/private/QGVGvcPrivate.cpp | 34 +
gazebo/gui/model/qgv/private/QGVGvcPrivate.h | 49 +
gazebo/gui/model/qgv/private/QGVNodePrivate.cpp | 16 +
gazebo/gui/model/qgv/private/QGVNodePrivate.h | 36 +
gazebo/gui/model/qgv/qgv.h | 36 +
gazebo/gui/resources.qrc | 19 +
gazebo/gui/style.qss | 201 +-
gazebo/gui/terrain/CMakeLists.txt | 2 +
gazebo/gui/terrain/TerrainEditor.hh | 2 +-
gazebo/gui/terrain/TerrainEditorPalette.cc | 26 +-
gazebo/gui/terrain/TerrainEditorPalette.hh | 2 +-
gazebo/gui/viewers/CMakeLists.txt | 12 +-
gazebo/gui/viewers/ImageView.cc | 7 +
gazebo/gui/viewers/ImageView.hh | 2 +-
gazebo/gui/viewers/ImagesView.cc | 6 +
gazebo/gui/viewers/ImagesView.hh | 2 +-
gazebo/gui/viewers/LaserView.cc | 6 +
gazebo/gui/viewers/LaserView.hh | 2 +-
gazebo/gui/viewers/TextView.cc | 7 +
gazebo/gui/viewers/TextView.hh | 2 +-
gazebo/gui/viewers/TopicView.cc | 7 +
gazebo/gui/viewers/TopicView.hh | 4 +-
gazebo/gui/viewers/ViewFactory.hh | 6 +-
gazebo/gzserver.1.ronn | 2 +
gazebo/math/Angle.cc | 24 +
gazebo/math/Angle.hh | 25 +-
gazebo/math/Box.cc | 28 +
gazebo/math/Box.hh | 27 +-
gazebo/math/Box_TEST.cc | 30 +
gazebo/math/CMakeLists.txt | 3 +-
gazebo/math/Filter.hh | 12 +-
gazebo/math/Helpers.hh | 13 +-
gazebo/math/Kmeans.hh | 2 +-
gazebo/math/Matrix3.cc | 21 +
gazebo/math/Matrix3.hh | 6 +-
gazebo/math/Matrix3_TEST.cc | 19 +
gazebo/math/Matrix4.cc | 30 +
gazebo/math/Matrix4.hh | 21 +-
gazebo/math/Plane.cc | 28 +-
gazebo/math/Plane.hh | 21 +-
gazebo/math/Pose.cc | 19 +
gazebo/math/Pose.hh | 25 +-
gazebo/math/Quaternion.cc | 25 +
gazebo/math/Quaternion.hh | 89 +-
gazebo/math/Quaternion_TEST.cc | 87 +
gazebo/math/Rand.cc | 12 +-
gazebo/math/Rand.hh | 2 +-
gazebo/math/Rand_TEST.cc | 2 +
gazebo/math/RotationSpline.hh | 2 +-
gazebo/math/SignalStats.cc | 20 +-
gazebo/math/SignalStats.hh | 10 +-
gazebo/math/Spline.hh | 2 +-
gazebo/math/Vector2d.cc | 21 +
gazebo/math/Vector2d.hh | 24 +-
gazebo/math/Vector2i.cc | 21 +-
gazebo/math/Vector2i.hh | 29 +-
gazebo/math/Vector3.cc | 21 +
gazebo/math/Vector3.hh | 27 +-
gazebo/math/Vector3Stats.hh | 2 +-
gazebo/math/Vector4.hh | 2 +-
gazebo/msgs/CMakeLists.txt | 43 +-
gazebo/msgs/altimeter.proto | 23 +
gazebo/msgs/cessna.proto | 50 +
gazebo/msgs/generator/GazeboGenerator.cc | 1 +
gazebo/msgs/geometry.proto | 18 +-
gazebo/msgs/gui_camera.proto | 3 +
gazebo/msgs/joint.proto | 3 +-
gazebo/msgs/log_playback_control.proto | 29 +
gazebo/msgs/log_playback_stats.proto | 17 +
gazebo/msgs/magnetometer.proto | 18 +
gazebo/msgs/model.proto | 1 +
gazebo/msgs/msgs.cc | 1118 +-
gazebo/msgs/msgs.hh | 355 +-
gazebo/msgs/msgs_TEST.cc | 1229 +-
gazebo/msgs/physics.proto | 5 +
gazebo/msgs/rest_error.proto | 12 +
gazebo/msgs/rest_login.proto | 12 +
gazebo/msgs/rest_logout.proto | 11 +
gazebo/msgs/rest_post.proto | 12 +
gazebo/msgs/scene.proto | 5 +-
gazebo/msgs/sim_event.proto | 9 +-
gazebo/msgs/surface.proto | 1 +
gazebo/msgs/visual.proto | 39 +-
gazebo/msgs/world_stats.proto | 15 +-
gazebo/msgs/wrench.proto | 7 +-
gazebo/physics/Actor.cc | 181 +-
gazebo/physics/Actor.hh | 17 +-
gazebo/physics/BallJoint.hh | 2 +-
gazebo/physics/Base.cc | 10 +-
gazebo/physics/Base.hh | 10 +-
gazebo/physics/BoxShape.cc | 17 +-
gazebo/physics/BoxShape.hh | 5 +-
gazebo/physics/BoxShape_TEST.cc | 49 +-
gazebo/physics/CMakeLists.txt | 14 +
gazebo/physics/Collision.cc | 23 +-
gazebo/physics/Collision.hh | 2 +-
gazebo/physics/CollisionState.cc | 9 +-
gazebo/physics/CollisionState.hh | 2 +-
gazebo/physics/Contact.cc | 30 +-
gazebo/physics/Contact.hh | 5 +-
gazebo/physics/ContactManager.cc | 6 +
gazebo/physics/ContactManager.hh | 4 +-
gazebo/physics/ContactManager_TEST.cc | 4 +-
gazebo/physics/CylinderShape.cc | 6 +
gazebo/physics/CylinderShape.hh | 11 +-
gazebo/physics/CylinderShape_TEST.cc | 49 +-
gazebo/physics/Entity.cc | 21 +-
gazebo/physics/Entity.hh | 2 +-
gazebo/physics/FixedJoint.hh | 64 +
gazebo/physics/GearboxJoint.hh | 8 +-
gazebo/physics/Gripper.cc | 6 +
gazebo/physics/Gripper.hh | 2 +-
gazebo/physics/HeightmapShape.cc | 27 +-
gazebo/physics/HeightmapShape.hh | 5 +-
gazebo/physics/Hinge2Joint.hh | 2 +-
gazebo/physics/HingeJoint.hh | 2 +-
gazebo/physics/Inertial.cc | 2 +-
gazebo/physics/Inertial.hh | 8 +-
gazebo/physics/Inertial_TEST.cc | 2 +-
gazebo/physics/Joint.cc | 39 +-
gazebo/physics/Joint.hh | 2 +-
gazebo/physics/JointController.cc | 6 +
gazebo/physics/JointController.hh | 2 +-
gazebo/physics/JointState.cc | 6 +-
gazebo/physics/JointState.hh | 11 +-
gazebo/physics/JointWrench.hh | 2 +-
gazebo/physics/Link.cc | 239 +-
gazebo/physics/Link.hh | 69 +-
gazebo/physics/LinkState.cc | 18 +-
gazebo/physics/LinkState.hh | 15 +-
gazebo/physics/MapShape.cc | 7 +
gazebo/physics/MapShape.hh | 4 +-
gazebo/physics/MeshShape.cc | 39 +-
gazebo/physics/MeshShape.hh | 2 +-
gazebo/physics/Model.cc | 37 +-
gazebo/physics/Model.hh | 16 +-
gazebo/physics/ModelState.cc | 31 +-
gazebo/physics/ModelState.hh | 13 +-
gazebo/physics/MultiRayShape.cc | 13 +
gazebo/physics/MultiRayShape.hh | 5 +-
gazebo/physics/PhysicsEngine.cc | 144 +-
gazebo/physics/PhysicsEngine.hh | 58 +-
gazebo/physics/PhysicsEngine_TEST.cc | 118 +-
gazebo/physics/PhysicsFactory.cc | 11 +-
gazebo/physics/PhysicsFactory.hh | 6 +-
gazebo/physics/PhysicsIface.cc | 50 +-
gazebo/physics/PhysicsIface.hh | 34 +-
gazebo/physics/PhysicsTypes.hh | 10 +
gazebo/physics/PlaneShape.cc | 18 +-
gazebo/physics/PlaneShape.hh | 5 +-
gazebo/physics/PolylineShape.cc | 148 +-
gazebo/physics/PolylineShape.hh | 80 +-
gazebo/physics/Population.cc | 6 +
gazebo/physics/Population.hh | 4 +-
gazebo/physics/PresetManager.cc | 634 +
gazebo/physics/PresetManager.hh | 221 +
gazebo/physics/PresetManagerPrivate.hh | 71 +
gazebo/physics/PresetManager_TEST.cc | 291 +
gazebo/physics/RayShape.cc | 12 +
gazebo/physics/RayShape.hh | 5 +-
gazebo/physics/Road.cc | 8 +-
gazebo/physics/Road.hh | 2 +-
gazebo/physics/Road_TEST.cc | 2 +-
gazebo/physics/ScrewJoint.hh | 2 +-
gazebo/physics/Shape.cc | 23 +
gazebo/physics/Shape.hh | 13 +-
gazebo/physics/SliderJoint.hh | 2 +-
gazebo/physics/SphereShape.cc | 6 +
gazebo/physics/SphereShape.hh | 11 +-
gazebo/physics/SphereShape_TEST.cc | 49 +-
gazebo/physics/State.cc | 20 +-
gazebo/physics/State.hh | 30 +-
gazebo/physics/SurfaceParams.cc | 20 +-
gazebo/physics/SurfaceParams.hh | 18 +-
gazebo/physics/UniversalJoint.hh | 2 +-
gazebo/physics/World.cc | 393 +-
gazebo/physics/World.hh | 28 +-
gazebo/physics/WorldPrivate.hh | 13 +
gazebo/physics/WorldState.cc | 28 +-
gazebo/physics/WorldState.hh | 10 +-
gazebo/physics/bullet/BulletBallJoint.hh | 2 +-
gazebo/physics/bullet/BulletBoxShape.hh | 2 +-
gazebo/physics/bullet/BulletCollision.hh | 2 +-
gazebo/physics/bullet/BulletCylinderShape.hh | 2 +-
gazebo/physics/bullet/BulletFixedJoint.cc | 235 +
gazebo/physics/bullet/BulletFixedJoint.hh | 92 +
gazebo/physics/bullet/BulletHeightmapShape.hh | 2 +-
gazebo/physics/bullet/BulletHinge2Joint.cc | 22 +-
gazebo/physics/bullet/BulletHinge2Joint.hh | 2 +-
gazebo/physics/bullet/BulletHingeJoint.cc | 36 +-
gazebo/physics/bullet/BulletHingeJoint.hh | 2 +-
gazebo/physics/bullet/BulletJoint.cc | 45 +-
gazebo/physics/bullet/BulletJoint.hh | 5 +-
gazebo/physics/bullet/BulletLink.cc | 18 +-
gazebo/physics/bullet/BulletLink.hh | 6 +-
gazebo/physics/bullet/BulletMesh.hh | 2 +-
gazebo/physics/bullet/BulletMeshShape.hh | 2 +-
gazebo/physics/bullet/BulletMotionState.hh | 2 +-
gazebo/physics/bullet/BulletMultiRayShape.hh | 2 +-
gazebo/physics/bullet/BulletPhysics.cc | 264 +-
gazebo/physics/bullet/BulletPhysics.hh | 6 +-
gazebo/physics/bullet/BulletPhysics_TEST.cc | 2 +-
gazebo/physics/bullet/BulletPlaneShape.hh | 2 +-
gazebo/physics/bullet/BulletPolylineShape.hh | 2 +-
gazebo/physics/bullet/BulletRaySensor.hh | 2 +-
gazebo/physics/bullet/BulletRayShape.hh | 2 +-
gazebo/physics/bullet/BulletScrewJoint.hh | 2 +-
gazebo/physics/bullet/BulletSliderJoint.cc | 46 +-
gazebo/physics/bullet/BulletSliderJoint.hh | 2 +-
gazebo/physics/bullet/BulletSphereShape.hh | 2 +-
gazebo/physics/bullet/BulletSurfaceParams.cc | 20 +-
gazebo/physics/bullet/BulletSurfaceParams.hh | 7 +-
gazebo/physics/bullet/BulletTypes.hh | 2 +-
gazebo/physics/bullet/BulletUniversalJoint.hh | 2 +-
gazebo/physics/bullet/CMakeLists.txt | 2 +
gazebo/physics/dart/CMakeLists.txt | 10 +-
gazebo/physics/dart/DARTBallJoint.cc | 12 +-
gazebo/physics/dart/DARTBallJoint.hh | 5 +-
gazebo/physics/dart/DARTBoxShape.cc | 96 +
gazebo/physics/dart/DARTBoxShape.hh | 76 +-
gazebo/physics/dart/DARTBoxShapePrivate.hh | 40 +
gazebo/physics/dart/DARTCollision.cc | 40 +-
gazebo/physics/dart/DARTCollision.hh | 27 +-
gazebo/physics/dart/DARTCollisionPrivate.hh | 57 +
gazebo/physics/dart/DARTCylinderShape.cc | 96 +
gazebo/physics/dart/DARTCylinderShape.hh | 69 +-
gazebo/physics/dart/DARTCylinderShapePrivate.hh | 40 +
gazebo/physics/dart/DARTFixedJoint.cc | 126 +
gazebo/physics/dart/DARTFixedJoint.hh | 78 +
gazebo/physics/dart/DARTHeightmapShape.cc | 6 +-
gazebo/physics/dart/DARTHeightmapShape.hh | 16 +-
gazebo/physics/dart/DARTHeightmapShapePrivate.hh | 40 +
gazebo/physics/dart/DARTHinge2Joint.cc | 82 +-
gazebo/physics/dart/DARTHinge2Joint.hh | 5 +-
gazebo/physics/dart/DARTHingeJoint.cc | 49 +-
gazebo/physics/dart/DARTHingeJoint.hh | 5 +-
gazebo/physics/dart/DARTJoint.cc | 196 +-
gazebo/physics/dart/DARTJoint.hh | 30 +-
gazebo/physics/dart/DARTJointPrivate.hh | 69 +
gazebo/physics/dart/DARTLink.cc | 221 +-
gazebo/physics/dart/DARTLink.hh | 29 +-
gazebo/physics/dart/DARTLinkPrivate.hh | 74 +
gazebo/physics/dart/DARTMesh.cc | 11 +-
gazebo/physics/dart/DARTMesh.hh | 9 +-
gazebo/physics/dart/DARTMeshPrivate.hh | 40 +
gazebo/physics/dart/DARTMeshShape.cc | 13 +-
gazebo/physics/dart/DARTMeshShape.hh | 10 +-
gazebo/physics/dart/DARTMeshShapePrivate.hh | 53 +
gazebo/physics/dart/DARTModel.cc | 39 +-
gazebo/physics/dart/DARTModel.hh | 16 +-
gazebo/physics/dart/DARTModelPrivate.hh | 57 +
gazebo/physics/dart/DARTMultiRayShape.cc | 9 +-
gazebo/physics/dart/DARTMultiRayShape.hh | 10 +-
gazebo/physics/dart/DARTMultiRayShapePrivate.hh | 40 +
gazebo/physics/dart/DARTPhysics.cc | 133 +-
gazebo/physics/dart/DARTPhysics.hh | 14 +-
gazebo/physics/dart/DARTPhysicsPrivate.hh | 51 +
gazebo/physics/dart/DARTPlaneShape.cc | 66 +
gazebo/physics/dart/DARTPlaneShape.hh | 37 +-
gazebo/physics/dart/DARTPlaneShapePrivate.hh | 40 +
gazebo/physics/dart/DARTPolylineShape.cc | 10 +-
gazebo/physics/dart/DARTPolylineShape.hh | 10 +-
gazebo/physics/dart/DARTPolylineShapePrivate.hh | 54 +
gazebo/physics/dart/DARTRayShape.cc | 16 +-
gazebo/physics/dart/DARTRayShape.hh | 10 +-
gazebo/physics/dart/DARTRayShapePrivate.hh | 40 +
gazebo/physics/dart/DARTScrewJoint.cc | 90 +-
gazebo/physics/dart/DARTScrewJoint.hh | 5 +-
gazebo/physics/dart/DARTSliderJoint.cc | 49 +-
gazebo/physics/dart/DARTSliderJoint.hh | 5 +-
gazebo/physics/dart/DARTSphereShape.cc | 86 +
gazebo/physics/dart/DARTSphereShape.hh | 56 +-
gazebo/physics/dart/DARTSphereShapePrivate.hh | 40 +
gazebo/physics/dart/DARTSurfaceParams.cc | 100 +
gazebo/physics/dart/DARTSurfaceParams.hh | 67 +
gazebo/physics/dart/DARTSurfaceParamsPrivate.hh | 49 +
gazebo/physics/dart/DARTTypes.hh | 4 +-
gazebo/physics/dart/DARTUniversalJoint.cc | 79 +-
gazebo/physics/dart/DARTUniversalJoint.hh | 6 +-
gazebo/physics/dart/dart_inc.h | 45 +-
gazebo/physics/ode/CMakeLists.txt | 9 +-
gazebo/physics/ode/ODEBallJoint.hh | 2 +-
gazebo/physics/ode/ODEBoxShape.hh | 2 +-
gazebo/physics/ode/ODECollision.cc | 6 +
gazebo/physics/ode/ODECollision.hh | 2 +-
gazebo/physics/ode/ODECylinderShape.hh | 2 +-
gazebo/physics/ode/ODEFixedJoint.cc | 138 +
gazebo/physics/ode/ODEFixedJoint.hh | 83 +
gazebo/physics/ode/ODEGearboxJoint.hh | 2 +-
gazebo/physics/ode/ODEHeightmapShape.hh | 2 +-
gazebo/physics/ode/ODEHinge2Joint.hh | 2 +-
gazebo/physics/ode/ODEHingeJoint.hh | 2 +-
gazebo/physics/ode/ODEJoint.cc | 54 +-
gazebo/physics/ode/ODEJoint.hh | 5 +-
gazebo/physics/ode/ODEJoint_TEST.cc | 2 +-
gazebo/physics/ode/ODELink.cc | 43 +-
gazebo/physics/ode/ODELink.hh | 6 +-
gazebo/physics/ode/ODEMesh.hh | 2 +-
gazebo/physics/ode/ODEMeshShape.hh | 2 +-
gazebo/physics/ode/ODEMultiRayShape.hh | 2 +-
gazebo/physics/ode/ODEPhysics.cc | 782 +-
gazebo/physics/ode/ODEPhysics.hh | 125 +-
gazebo/physics/ode/ODEPhysicsPrivate.hh | 99 +
gazebo/physics/ode/ODEPhysics_TEST.cc | 133 +-
gazebo/physics/ode/ODEPlaneShape.hh | 2 +-
gazebo/physics/ode/ODEPolylineShape.hh | 2 +-
gazebo/physics/ode/ODERayShape.hh | 2 +-
gazebo/physics/ode/ODEScrewJoint.hh | 2 +-
gazebo/physics/ode/ODESliderJoint.hh | 2 +-
gazebo/physics/ode/ODESphereShape.hh | 2 +-
gazebo/physics/ode/ODESurfaceParams.cc | 31 +-
gazebo/physics/ode/ODESurfaceParams.hh | 19 +-
gazebo/physics/ode/ODEUniversalJoint.hh | 3 +-
gazebo/physics/simbody/CMakeLists.txt | 5 +-
gazebo/physics/simbody/SimbodyBallJoint.hh | 2 +-
gazebo/physics/simbody/SimbodyBoxShape.hh | 2 +-
gazebo/physics/simbody/SimbodyCollision.cc | 1 -
gazebo/physics/simbody/SimbodyCollision.hh | 2 +-
gazebo/physics/simbody/SimbodyCylinderShape.hh | 2 +-
gazebo/physics/simbody/SimbodyFixedJoint.cc | 102 +
gazebo/physics/simbody/SimbodyFixedJoint.hh | 76 +
gazebo/physics/simbody/SimbodyHeightmapShape.hh | 2 +-
gazebo/physics/simbody/SimbodyHinge2Joint.hh | 3 +-
gazebo/physics/simbody/SimbodyHingeJoint.hh | 2 +-
gazebo/physics/simbody/SimbodyJoint.cc | 6 +-
gazebo/physics/simbody/SimbodyJoint.hh | 2 +-
gazebo/physics/simbody/SimbodyLink.cc | 8 +
gazebo/physics/simbody/SimbodyLink.hh | 6 +-
gazebo/physics/simbody/SimbodyMesh.hh | 2 +-
gazebo/physics/simbody/SimbodyMeshShape.hh | 2 +-
gazebo/physics/simbody/SimbodyModel.hh | 2 +-
gazebo/physics/simbody/SimbodyMultiRayShape.hh | 2 +-
gazebo/physics/simbody/SimbodyPhysics.cc | 412 +-
gazebo/physics/simbody/SimbodyPhysics.hh | 6 +-
gazebo/physics/simbody/SimbodyPlaneShape.hh | 2 +-
gazebo/physics/simbody/SimbodyPolylineShape.hh | 2 +-
gazebo/physics/simbody/SimbodyRayShape.hh | 2 +-
gazebo/physics/simbody/SimbodyScrewJoint.cc | 18 +-
gazebo/physics/simbody/SimbodyScrewJoint.hh | 2 +-
gazebo/physics/simbody/SimbodySliderJoint.hh | 3 +-
gazebo/physics/simbody/SimbodySphereShape.hh | 2 +-
gazebo/physics/simbody/SimbodyUniversalJoint.hh | 2 +-
gazebo/rendering/ApplyWrenchVisual.cc | 519 +
gazebo/rendering/ApplyWrenchVisual.hh | 124 +
gazebo/rendering/ApplyWrenchVisualPrivate.hh | 72 +
gazebo/rendering/ArrowVisual.cc | 16 +-
gazebo/rendering/ArrowVisual.hh | 2 +-
gazebo/rendering/AxisVisual.cc | 19 +-
gazebo/rendering/AxisVisual.hh | 14 +-
gazebo/rendering/CMakeLists.txt | 52 +-
gazebo/rendering/COMVisual.cc | 165 +-
gazebo/rendering/COMVisual.hh | 17 +-
gazebo/rendering/COMVisualPrivate.hh | 15 +-
gazebo/rendering/COMVisual_TEST.cc | 89 +
gazebo/rendering/Camera.cc | 140 +-
gazebo/rendering/Camera.hh | 24 +-
gazebo/rendering/CameraPrivate.hh | 2 +-
gazebo/rendering/CameraVisual.cc | 57 +-
gazebo/rendering/CameraVisual.hh | 5 +-
gazebo/rendering/ContactVisual.cc | 14 +-
gazebo/rendering/ContactVisual.hh | 2 +-
gazebo/rendering/Conversions.hh | 2 +-
gazebo/rendering/DepthCamera.cc | 15 +-
gazebo/rendering/DepthCamera.hh | 2 +-
gazebo/rendering/Distortion.hh | 2 +-
gazebo/rendering/DynamicLines.hh | 2 +-
gazebo/rendering/DynamicRenderable.hh | 2 +-
gazebo/rendering/FPSViewController.cc | 18 +-
gazebo/rendering/FPSViewController.hh | 2 +-
gazebo/rendering/GpuLaser.cc | 25 +-
gazebo/rendering/GpuLaser.hh | 15 +-
gazebo/rendering/GpuLaser_TEST.cc | 3 +-
gazebo/rendering/Grid.cc | 1 -
gazebo/rendering/Grid.hh | 2 +-
gazebo/rendering/Heightmap.cc | 195 +-
gazebo/rendering/Heightmap.hh | 57 +-
gazebo/rendering/Heightmap_TEST.cc | 3 +-
gazebo/rendering/InertiaVisual.cc | 149 +
gazebo/rendering/InertiaVisual.hh | 63 +
gazebo/rendering/InertiaVisualPrivate.hh | 45 +
gazebo/rendering/JointVisual.cc | 225 +-
gazebo/rendering/JointVisual.hh | 30 +-
gazebo/rendering/JointVisualPrivate.hh | 13 +-
gazebo/rendering/JointVisual_TEST.cc | 91 +-
gazebo/rendering/LaserVisual.cc | 97 +-
gazebo/rendering/LaserVisual.hh | 2 +-
gazebo/rendering/LaserVisualPrivate.hh | 8 +-
gazebo/rendering/Light.cc | 15 +-
gazebo/rendering/Light.hh | 3 +-
gazebo/rendering/LinkFrameVisual.cc | 91 +
gazebo/rendering/LinkFrameVisual.hh | 58 +
gazebo/rendering/LinkFrameVisualPrivate.hh | 43 +
gazebo/rendering/LinkFrameVisual_TEST.cc | 89 +
gazebo/rendering/Material.cc | 35 +
gazebo/rendering/Material.hh | 15 +-
gazebo/rendering/MovableText.cc | 5 +
gazebo/rendering/MovableText.hh | 2 +-
gazebo/rendering/OculusCamera.cc | 569 +-
gazebo/rendering/OculusCamera.hh | 84 +-
gazebo/rendering/OculusCameraPrivate.hh | 94 +
gazebo/rendering/OrbitViewController.cc | 55 +-
gazebo/rendering/OrbitViewController.hh | 44 +-
gazebo/rendering/OriginVisual.cc | 76 +
gazebo/rendering/OriginVisual.hh | 51 +
gazebo/rendering/OriginVisualPrivate.hh | 53 +
gazebo/rendering/OriginVisual_TEST.cc | 65 +
gazebo/rendering/OrthoViewController.cc | 314 +
gazebo/rendering/OrthoViewController.hh | 91 +
gazebo/rendering/OrthoViewControllerPrivate.hh | 33 +
gazebo/rendering/Projector.cc | 4 +-
gazebo/rendering/Projector.hh | 2 +-
gazebo/rendering/RFIDTagVisual.cc | 8 +
gazebo/rendering/RFIDTagVisual.hh | 2 +-
gazebo/rendering/RFIDVisual.cc | 8 +
gazebo/rendering/RFIDVisual.hh | 2 +-
gazebo/rendering/RTShaderSystem.cc | 31 +-
gazebo/rendering/RTShaderSystem.hh | 7 +-
gazebo/rendering/RayQuery.cc | 9 +-
gazebo/rendering/RayQuery.hh | 2 +-
gazebo/rendering/RenderEngine.cc | 44 +-
gazebo/rendering/RenderEngine.hh | 3 +-
gazebo/rendering/RenderEvents.cc | 2 +
gazebo/rendering/RenderEvents.hh | 36 +-
gazebo/rendering/RenderTypes.hh | 20 +
gazebo/rendering/RenderingIface.hh | 12 +-
gazebo/rendering/Road2d.cc | 32 +-
gazebo/rendering/Road2d.hh | 4 +-
gazebo/rendering/Scene.cc | 504 +-
gazebo/rendering/Scene.hh | 70 +-
gazebo/rendering/ScenePrivate.hh | 24 +-
gazebo/rendering/Scene_TEST.cc | 322 +
gazebo/rendering/SelectionObj.cc | 207 +-
gazebo/rendering/SelectionObj.hh | 25 +-
gazebo/rendering/SelectionObj_TEST.cc | 176 +
gazebo/rendering/SonarVisual.cc | 14 +-
gazebo/rendering/SonarVisual.hh | 2 +-
gazebo/rendering/SonarVisual_TEST.cc | 3 +-
gazebo/rendering/TransmitterVisual.cc | 8 +
gazebo/rendering/TransmitterVisual.hh | 2 +-
gazebo/rendering/TransmitterVisual_TEST.cc | 3 +-
gazebo/rendering/UserCamera.cc | 268 +-
gazebo/rendering/UserCamera.hh | 45 +-
gazebo/rendering/UserCameraPrivate.hh | 27 +-
gazebo/rendering/VideoVisual.cc | 1 +
gazebo/rendering/VideoVisual.hh | 2 +-
gazebo/rendering/ViewController.cc | 17 +-
gazebo/rendering/ViewController.hh | 13 +-
gazebo/rendering/Visual.cc | 1170 +-
gazebo/rendering/Visual.hh | 122 +-
gazebo/rendering/VisualPrivate.hh | 57 +
gazebo/rendering/Visual_TEST.cc | 643 +-
gazebo/rendering/WindowManager.cc | 18 +-
gazebo/rendering/WindowManager.hh | 8 +-
gazebo/rendering/WireBox.cc | 1 -
gazebo/rendering/WireBox.hh | 2 +-
gazebo/rendering/WrenchVisual.cc | 17 +-
gazebo/rendering/WrenchVisual.hh | 2 +-
gazebo/rendering/deferred_shading/AmbientLight.hh | 2 +-
gazebo/rendering/deferred_shading/CMakeLists.txt | 2 +
gazebo/rendering/deferred_shading/DeferredLight.hh | 3 +-
.../rendering/deferred_shading/DeferredLightCP.hh | 4 +-
.../deferred_shading/GBufferMaterialGenerator.cc | 4 +
.../deferred_shading/GBufferMaterialGenerator.hh | 3 +-
.../deferred_shading/GBufferSchemeHandler.hh | 2 +-
gazebo/rendering/deferred_shading/GeomUtils.cc | 1 +
gazebo/rendering/deferred_shading/GeomUtils.hh | 2 +-
.../deferred_shading/LightMaterialGenerator.hh | 4 +-
.../deferred_shading/ListenerFactoryLogic.hh | 3 +-
.../deferred_shading/MaterialGenerator.hh | 4 +-
gazebo/rendering/deferred_shading/MergeCP.hh | 4 +-
.../deferred_shading/MergeMaterialGenerator.cc | 1 +
.../deferred_shading/MergeMaterialGenerator.hh | 3 +-
.../deferred_shading/MergeSchemeHandler.hh | 2 +-
.../deferred_shading/NullSchemeHandler.hh | 2 +-
gazebo/rendering/deferred_shading/SSAOLogic.hh | 2 +-
.../deferred_shading/TechniqueDefinitions.hh | 8 +-
gazebo/rendering/ogre_gazebo.h | 1 +
gazebo/rendering/selection_buffer/CMakeLists.txt | 2 +
.../rendering/selection_buffer/MaterialSwitcher.cc | 4 +-
.../rendering/selection_buffer/MaterialSwitcher.hh | 2 +-
.../rendering/selection_buffer/SelectionBuffer.cc | 11 +-
.../rendering/selection_buffer/SelectionBuffer.hh | 2 +-
.../selection_buffer/SelectionRenderListener.hh | 2 +-
gazebo/rendering/skyx/CMakeLists.txt | 2 +
gazebo/rendering/skyx/include/Prerequisites.h | 24 +-
gazebo/sensors/AltimeterSensor.cc | 214 +
gazebo/sensors/AltimeterSensor.hh | 93 +
gazebo/sensors/AltimeterSensorPrivate.hh | 49 +
gazebo/sensors/AltimeterSensor_TEST.cc | 310 +
gazebo/sensors/CMakeLists.txt | 12 +-
gazebo/sensors/CameraSensor.cc | 20 +-
gazebo/sensors/CameraSensor.hh | 9 +-
gazebo/sensors/ContactSensor.cc | 12 +-
gazebo/sensors/ContactSensor.hh | 10 +-
gazebo/sensors/DepthCameraSensor.cc | 9 +-
gazebo/sensors/DepthCameraSensor.hh | 9 +-
gazebo/sensors/ForceTorqueSensor.cc | 56 +-
gazebo/sensors/ForceTorqueSensor.hh | 25 +-
gazebo/sensors/ForceTorqueSensor_TEST.cc | 8 +-
gazebo/sensors/GaussianNoiseModel.cc | 50 +-
gazebo/sensors/GaussianNoiseModel.hh | 6 +
gazebo/sensors/GpsSensor.cc | 109 +-
gazebo/sensors/GpsSensor.hh | 33 +-
gazebo/sensors/GpsSensor_TEST.cc | 6 +-
gazebo/sensors/GpuRaySensor.cc | 88 +-
gazebo/sensors/GpuRaySensor.hh | 38 +-
gazebo/sensors/GpuRaySensor_TEST.cc | 18 +-
gazebo/sensors/ImuSensor.cc | 356 +-
gazebo/sensors/ImuSensor.hh | 87 +-
gazebo/sensors/ImuSensor_TEST.cc | 40 +-
gazebo/sensors/MagnetometerSensor.cc | 176 +
gazebo/sensors/MagnetometerSensor.hh | 79 +
gazebo/sensors/MagnetometerSensorPrivate.hh | 49 +
gazebo/sensors/MagnetometerSensor_TEST.cc | 150 +
gazebo/sensors/MultiCameraSensor.cc | 29 +-
gazebo/sensors/MultiCameraSensor.hh | 5 +-
gazebo/sensors/Noise.cc | 14 +
gazebo/sensors/Noise.hh | 4 +
gazebo/sensors/Noise_TEST.cc | 5 +-
gazebo/sensors/RFIDSensor.cc | 37 +-
gazebo/sensors/RFIDSensor.hh | 28 +-
gazebo/sensors/RFIDTag.cc | 23 +-
gazebo/sensors/RFIDTag.hh | 22 +-
gazebo/sensors/RaySensor.cc | 81 +-
gazebo/sensors/RaySensor.hh | 41 +-
gazebo/sensors/RaySensor_TEST.cc | 20 +-
gazebo/sensors/Sensor.cc | 92 +-
gazebo/sensors/Sensor.hh | 34 +-
gazebo/sensors/SensorFactory.cc | 10 +
gazebo/sensors/SensorManager.cc | 11 +-
gazebo/sensors/SensorManager.hh | 4 +-
gazebo/sensors/SensorManager_TEST.cc | 2 +-
gazebo/sensors/SensorTypes.hh | 107 +-
gazebo/sensors/Sensor_TEST.cc | 86 +-
gazebo/sensors/SensorsIface.cc | 6 +
gazebo/sensors/SonarSensor.cc | 30 +-
gazebo/sensors/SonarSensor.hh | 10 +-
gazebo/sensors/SonarSensor_TEST.cc | 4 +-
gazebo/sensors/WirelessReceiver.cc | 14 +-
gazebo/sensors/WirelessReceiver_TEST.cc | 2 +-
gazebo/sensors/WirelessTransceiver.cc | 9 +-
gazebo/sensors/WirelessTransceiver.hh | 9 +-
gazebo/sensors/WirelessTransmitter.cc | 44 +-
gazebo/sensors/WirelessTransmitter.hh | 18 +-
gazebo/sensors/WirelessTransmitter_TEST.cc | 27 +-
gazebo/test/CMakeLists.txt | 10 +
gazebo/test/ServerFixture.cc | 1528 +
gazebo/test/ServerFixture.hh | 664 +
gazebo/test/ServerFixtureRecord.cc | 58 +
gazebo/test/helper_physics_generator.hh | 57 +
gazebo/transport/CMakeLists.txt | 11 +
gazebo/transport/CallbackHelper.hh | 6 +-
gazebo/transport/Connection.cc | 113 +-
gazebo/transport/Connection.hh | 6 +-
gazebo/transport/ConnectionManager.cc | 6 +
gazebo/transport/ConnectionManager.hh | 2 +-
gazebo/transport/Connection_TEST.cc | 1 -
gazebo/transport/IOManager.hh | 2 +-
gazebo/transport/Node.cc | 8 +-
gazebo/transport/Node.hh | 5 +-
gazebo/transport/Publication.hh | 2 +-
gazebo/transport/PublicationTransport.cc | 6 +
gazebo/transport/PublicationTransport.hh | 2 +-
gazebo/transport/Publisher.cc | 10 +-
gazebo/transport/Publisher.hh | 2 +-
gazebo/transport/SubscribeOptions.hh | 2 +-
gazebo/transport/Subscriber.cc | 6 +
gazebo/transport/Subscriber.hh | 2 +-
gazebo/transport/SubscriptionTransport.cc | 7 +
gazebo/transport/SubscriptionTransport.hh | 2 +-
gazebo/transport/TopicManager.cc | 6 +
gazebo/transport/TopicManager.hh | 2 +-
gazebo/transport/TransportIface.cc | 6 +
gazebo/transport/TransportIface.hh | 39 +-
gazebo/util/CMakeLists.txt | 17 +-
gazebo/util/Diagnostics.cc | 30 +-
gazebo/util/Diagnostics.hh | 8 +-
gazebo/util/LogPlay.cc | 195 +-
gazebo/util/LogPlay.hh | 74 +-
gazebo/util/LogPlay_TEST.cc | 186 +
gazebo/util/LogRecord.cc | 36 +-
gazebo/util/LogRecord.hh | 2 +-
gazebo/util/LogRecord_TEST.cc | 8 +-
gazebo/util/OpenAL.cc | 62 +-
gazebo/util/OpenAL.hh | 48 +-
gazebo/util/OpenAL_TEST.cc | 63 +-
gazebo/util/system.hh | 467 +-
ign-math-migration.md | 38 +
interfaces/player/ActarrayInterface.cc | 2 +-
interfaces/player/BumperInterface.cc | 3 +-
interfaces/player/CMakeLists.txt | 1 +
interfaces/player/FiducialInterface.cc | 3 +-
interfaces/player/GazeboDriver.cc | 21 +-
interfaces/player/GazeboDriver.hh | 6 +-
interfaces/player/Graphics3dInterface.cc | 2 +-
interfaces/player/GripperInterface.cc | 3 +-
interfaces/player/IRInterface.cc | 3 +-
interfaces/player/LaserInterface.cc | 4 +-
interfaces/player/OpaqueInterface.cc | 3 +-
interfaces/player/PTZInterface.cc | 3 +-
interfaces/player/Position2dInterface.cc | 10 +-
interfaces/player/Position3dInterface.cc | 3 +-
interfaces/player/SimulationInterface.cc | 10 +-
media/fonts/CMakeLists.txt | 3 -
media/fonts/maiden.ttf | Bin 26112 -> 0 bytes
media/fonts/sample.fontdef | 121 -
media/fonts/solo5.ttf | Bin 11236 -> 0 bytes
media/gui/fonts/BITSTREAM-LICENSE | 40 +
media/gui/fonts/CMakeLists.txt | 1 -
media/gui/fonts/baemuk.txt | 12 -
media/materials/programs/CMakeLists.txt | 66 +-
.../programs/OculusDistortionFragment.glsl | 14 +
.../materials/programs/OculusDistortionVertex.glsl | 45 +
media/materials/programs/warp.vert | 9 -
media/materials/programs/warpWithChromeAb.frag | 40 -
media/materials/scripts/CMakeLists.txt | 46 +-
media/materials/scripts/gazebo.material | 142 +
media/materials/scripts/glsl120.program | 20 -
media/materials/scripts/oculus.compositor | 43 -
media/materials/scripts/oculus.material | 68 +
media/materials/scripts/warp.material | 21 -
media/materials/textures/CMakeLists.txt | 2 +
media/materials/textures/com.png | Bin 0 -> 444 bytes
media/materials/textures/runway.png | Bin 0 -> 15154 bytes
models/ground_plane/model.sdf | 3 +
plugins/ActuatorPlugin.cc | 177 +
plugins/ActuatorPlugin.hh | 106 +
plugins/ArrangePlugin.cc | 2 +-
plugins/BreakableJointPlugin.cc | 9 +-
plugins/BuoyancyPlugin.cc | 178 +
plugins/BuoyancyPlugin.hh | 96 +
plugins/CMakeLists.txt | 35 +-
plugins/CameraPlugin.cc | 9 +-
plugins/CessnaGUIPlugin.cc | 338 +
plugins/CessnaGUIPlugin.hh | 123 +
plugins/CessnaPlugin.cc | 247 +
plugins/CessnaPlugin.hh | 146 +
plugins/DepthCameraPlugin.cc | 3 +-
plugins/DiffDrivePlugin.cc | 12 +-
plugins/DiffDrivePlugin.hh | 1 -
plugins/ElevatorPlugin.cc | 494 +
plugins/ElevatorPlugin.hh | 82 +
plugins/ElevatorPluginPrivate.hh | 307 +
plugins/ForceTorquePlugin.cc | 5 +
plugins/GpuRayPlugin.cc | 3 +-
plugins/HydraDemoPlugin.hh | 2 +-
plugins/HydraPlugin.cc | 29 +-
plugins/HydraPlugin.hh | 2 +-
plugins/ImuSensorPlugin.hh | 4 +-
plugins/LiftDragPlugin.cc | 156 +-
plugins/LiftDragPlugin.hh | 38 +-
plugins/ModelPropShop.cc | 10 +-
plugins/MudPlugin.cc | 3 +-
plugins/RaySensorNoisePlugin.cc | 2 +-
plugins/RubblePlugin.cc | 8 +-
plugins/RubblePlugin.hh | 6 +-
plugins/SkidSteerDrivePlugin.cc | 4 +-
plugins/TimerGUIPlugin.cc | 294 +-
plugins/TimerGUIPlugin.hh | 60 +-
plugins/TransporterPlugin.cc | 186 +
plugins/TransporterPlugin.hh | 109 +
plugins/TransporterPluginPrivate.hh | 78 +
plugins/VehiclePlugin.cc | 4 +
plugins/events/CMakeLists.txt | 12 +-
plugins/events/EventSource.cc | 11 +-
plugins/events/EventSource.hh | 20 +-
plugins/events/ExistenceEventSource.cc | 2 +-
plugins/events/ExistenceEventSource.hh | 6 +-
plugins/events/InRegionEventSource.cc | 96 +-
plugins/events/InRegionEventSource.hh | 63 +-
plugins/events/OccupiedEventSource.cc | 127 +
plugins/events/OccupiedEventSource.hh | 96 +
plugins/events/Region.cc | 58 +
plugins/events/Region.hh | 66 +
plugins/events/SimEventsPlugin.cc | 34 +-
plugins/events/SimEventsPlugin.hh | 5 +-
plugins/events/SimStateEventSource.cc | 23 +-
plugins/events/SimStateEventSource.hh | 12 +-
plugins/rest_web/CMakeLists.txt | 61 +
plugins/rest_web/RestApi.cc | 380 +
plugins/rest_web/RestApi.hh | 106 +
plugins/rest_web/RestException.hh | 33 +
plugins/rest_web/RestUiLoginDialog.cc | 124 +
plugins/rest_web/RestUiLoginDialog.hh | 108 +
plugins/rest_web/RestUiLogoutDialog.cc | 75 +
plugins/rest_web/RestUiLogoutDialog.hh | 47 +
plugins/rest_web/RestUiPlugin.cc | 126 +
plugins/rest_web/RestUiPlugin.hh | 77 +
plugins/rest_web/RestUiWidget.cc | 121 +
plugins/rest_web/RestUiWidget.hh | 105 +
plugins/rest_web/RestWebPlugin.cc | 344 +
plugins/rest_web/RestWebPlugin.hh | 117 +
test/CMakeLists.txt | 14 -
test/ServerFixture.cc | 1361 -
test/ServerFixture.hh | 571 -
test/cmake/plugin/CMakeLists.txt | 2 +
test/data/svg/arc_circle.svg | 39 +
test/data/svg/arc_test.svg | 120 +
test/data/svg/capsule.svg | 124 +
test/data/svg/chassis.svg | 396 +
test/data/svg/loader.svg | 75 +
test/examples/CMakeLists.txt | 4 +-
test/examples/example_plugins.cc | 99 -
test/examples/examples_build.cc | 130 +
test/integration/CMakeLists.txt | 23 +-
test/integration/aero_plugin.cc | 4 +-
test/integration/bandwidth.cc | 4 +-
test/integration/camera.cc | 2 +-
test/integration/camera_sensor.cc | 6 +-
test/integration/concave_mesh.cc | 60 +-
test/integration/contact_sensor.cc | 165 +-
test/integration/dem.cc | 10 +-
test/integration/factory.cc | 76 +-
test/integration/file_handling.cc | 12 +-
test/integration/gpu_laser.cc | 16 +-
test/integration/gripper.cc | 3 +-
test/integration/gz_camera.cc | 2 +-
test/integration/gz_joint.cc | 2 +-
test/integration/gz_log.cc | 2 +-
test/integration/gz_model.cc | 2 +-
test/integration/gz_physics.cc | 35 +-
test/integration/gz_world.cc | 2 +-
test/integration/heightmap.cc | 55 +-
test/integration/helper_physics_generator.hh | 49 -
test/integration/imu.cc | 159 +-
test/integration/insert_model.cc | 10 +-
test/integration/joint_controller.cc | 2 +-
test/integration/joint_force_torque.cc | 4 +-
test/integration/joint_gearbox.cc | 2 +-
test/integration/joint_get_force_torque.cc | 372 +
test/integration/joint_revolute.cc | 67 +-
test/integration/joint_screw.cc | 14 +-
test/integration/joint_set_position_test.cc | 4 +-
test/integration/joint_spawn.cc | 93 +-
test/integration/joint_test.cc | 2 +-
test/integration/joint_test.hh | 121 +-
test/integration/joint_universal.cc | 4 +-
test/integration/laser.cc | 38 +-
test/integration/model.cc | 3 +-
test/integration/model_database.cc | 13 +-
test/integration/multicamera_sensor.cc | 142 +-
test/integration/noise.cc | 14 +-
test/integration/nondefault_world.cc | 4 +-
test/integration/ogre_log.cc | 3 +-
test/integration/physics.cc | 8 +-
.../physics_basic_controller_response.cc | 6 +-
test/integration/physics_collision.cc | 99 +-
test/integration/physics_friction.cc | 360 +-
test/integration/physics_inertia_ratio.cc | 10 +-
test/integration/physics_link.cc | 602 +-
test/integration/physics_msgs.cc | 263 +-
test/integration/physics_presets.cc | 296 +
test/integration/physics_thread_safe.cc | 4 +-
test/integration/pioneer2dx.cc | 8 +-
test/integration/plugin.cc | 2 +-
test/integration/polyline.cc | 87 +-
test/integration/pr2.cc | 10 +-
test/integration/projector.cc | 2 +-
test/integration/rendering_sensor.cc | 2 +-
test/integration/rest_web.cc | 146 +
test/integration/road.cc | 4 +-
test/integration/scans_cmp.h | 514 +-
test/integration/sdf.cc | 2 +-
test/integration/sensor.cc | 3 +-
test/integration/server_fixture.cc | 4 +-
test/integration/sim_events.cc | 86 +-
test/integration/speed.cc | 53 +-
test/integration/speed_pr2.cc | 4 +-
test/integration/speed_thread_pr2.cc | 116 +
test/integration/state_log.cc | 2 +-
test/integration/stress_spawn_models.cc | 2 +-
test/integration/surface_properties.cc | 95 +-
test/integration/swarm.cc | 68 +
test/integration/transceiver.cc | 18 +-
test/integration/transport.cc | 2 +-
test/integration/transport_stress_nodes.cc | 2 +-
test/integration/transporter.cc | 99 +
test/integration/world.cc | 7 +-
test/integration/world_clone.cc | 3 +-
test/integration/world_entity_below_point.cc | 4 +-
test/integration/world_population.cc | 4 +-
test/integration/world_reset.cc | 4 +-
test/integration/worlds_installed.cc | 1 +
test/logs/invalidHeader.log | 23 +
test/logs/state.log | 23 +
test/logs/state2.log | 20 +
test/performance/factory_stress.cc | 2 +-
test/performance/image_convert_stress.cc | 2 +-
test/performance/sensor_stress.cc | 2 +-
test/performance/set_world_pose.cc | 2 +-
test/performance/transport_stress.cc | 2 +-
test/pkgconfig/CMakeLists.txt | 2 +-
test/pkgconfig/config-pkgconfig.cc | 23 +-
test/pkgconfig/plugin/CMakeLists.txt | 13 +-
test/plugins/CMakeLists.txt | 4 +
test/plugins/ForceTorqueModelRemovalTestPlugin.cc | 12 +-
test/plugins/Issue1208Plugin.hh | 6 +
test/plugins/rest-web/gzclient.bash | 3 +
test/plugins/rest-web/gzserver.bash | 4 +
test/plugins/rest-web/key-cert.pem | 15 +
test/plugins/rest-web/key.pem | 15 +
test/plugins/rest-web/package.json | 18 +
test/plugins/rest-web/readme.txt | 51 +
test/plugins/rest-web/server.js | 119 +
test/regression/1082_pid_limits.cc | 2 +-
test/regression/1112_force_torque_model_removal.cc | 2 +-
test/regression/1124_collision_pose.cc | 2 +-
test/regression/1146_collision_bounding_box.cc | 2 +-
test/regression/1208_world_plugin_init.cc | 4 +-
test/regression/1375_world_reset.cc | 4 +-
test/regression/1569_hydra_crash.cc | 43 +
test/regression/346_save_lights.cc | 6 +-
test/regression/351_world_step.cc | 38 +
test/regression/494_joint_axis_frame.cc | 4 +-
test/regression/602_unsubscribe_segfault.cc | 105 +
test/regression/624_collision_world_pose.cc | 4 +-
test/regression/846_typo_in_camera.cc | 3 +-
test/regression/876_random_number_generator.cc | 2 +-
test/regression/940_force_torque_sensor_frame.cc | 68 +-
test/regression/978_joint_anchor.cc | 4 +-
test/regression/CMakeLists.txt | 3 +
test/worlds/collide_bitmask.world | 190 +
test/worlds/collide_without_contact.world | 1 +
test/worlds/contact.world | 202 +-
test/worlds/contact_sensors_multiple.world | 67 +
test/worlds/force_torque_frame_test.world | 2 +-
test/worlds/hydra_test.world | 21 +
test/worlds/laser_lines_test.world | 42 +
test/worlds/model_self_collide.world | 378 +
test/worlds/ortho_box.world | 35 +
test/worlds/pr2_no_sensors_test.world | 2840 ++
test/worlds/presets.world | 75 +
test/worlds/presets_nodefault.world | 75 +
test/worlds/ray_test.world | 3 +
test/worlds/rest_web.world | 133 +
test/worlds/revolute_joint_test.world | 1 -
test/worlds/test_altimeter_linear_angular.world | 85 +
test/worlds/test_altimeter_rotation.world | 46 +
test/worlds/test_layers.world | 41 +
test/worlds/transporter_test.world | 86 +
test/worlds/usercamera_test.world | 40 +
test/worlds/world_step.world | 325 +
tools/CMakeLists.txt | 36 +-
tools/bitbucket_pullrequests | 40 +-
tools/check_test_ran.py | 2 +-
tools/code_check.sh | 10 +-
tools/cpplint.py | 2 +-
tools/gz.1.roff | 4 +
tools/gz.cc | 38 +-
tools/gz_TEST.cc | 36 +-
tools/gz_log.cc | 19 +-
tools/gz_log_TEST.cc | 21 +-
tools/gz_topic.cc | 12 +-
tools/junit_to_csv.rb | 21 +
tools/sdf2pov.cc | 16 +-
win_addpath.bat | 37 +
worlds/CMakeLists.txt | 9 +-
worlds/arat.world | 184 -
worlds/blank.world | 1 +
worlds/cessna_demo.world | 217 +
worlds/elevator.world | 334 +
worlds/flocking.world | 34094 +++++++++++++++++++
worlds/flocking.world.erb | 60 +
worlds/imu_demo.world | 156 +
worlds/magnetometer.world | 49 +
worlds/ortho.world | 19 +
worlds/osrf_elevator.world | 284 +
worlds/polyline.world | 62 +-
worlds/shapes_bitmask.world | 87 +
worlds/shapes_layers.world | 88 +
worlds/timer_gui.world | 28 +-
worlds/transporter.world | 50 +
worlds/underwater.world | 179 +
1394 files changed, 155675 insertions(+), 20970 deletions(-)
diff --git a/.hg_archival.txt b/.hg_archival.txt
index d1ce13d..576ba0d 100644
--- a/.hg_archival.txt
+++ b/.hg_archival.txt
@@ -1,5 +1,5 @@
repo: e803bb5fe03c1370cbb9f434912cd73fd1e7942d
-node: 430e66533f8810646b90d001fc9c83220ae503e9
-branch: gazebo5
-latesttag: gazebo5_5.0.0
-latesttagdistance: 6
+node: 8b634ebbd50868f4009824b89cabde57f3015ce9
+branch: gazebo6
+latesttag: gazebo6_6.0.0
+latesttagdistance: 1
diff --git a/.hgignore b/.hgignore
index cdc740f..40d78aa 100644
--- a/.hgignore
+++ b/.hgignore
@@ -1,5 +1,6 @@
syntax: glob
+test/plugins/rest-web/node_modules
build
build_*
Ogre.log
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 63ea6f7..9104a60 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,12 +9,12 @@ project (Gazebo)
string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
-set (GAZEBO_MAJOR_VERSION 5)
+set (GAZEBO_MAJOR_VERSION 6)
set (GAZEBO_MINOR_VERSION 0)
# The patch version may have been bumped for prerelease purposes; be sure to
# check gazebo-release/ubuntu/debian/changelog at default to determine what the
# next patch version should be for a regular release.
-set (GAZEBO_PATCH_VERSION 1)
+set (GAZEBO_PATCH_VERSION 0)
set (GAZEBO_VERSION ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION})
set (GAZEBO_VERSION_FULL ${GAZEBO_MAJOR_VERSION}.${GAZEBO_MINOR_VERSION}.${GAZEBO_PATCH_VERSION})
@@ -155,6 +155,10 @@ endif (NOT CMAKE_BUILD_TYPE)
# uppercase old supported and deprecated modes
string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE_UPPERCASE)
+if (NOT DEFINED HDF5_INSTRUMENT)
+ set (HDF5_INSTRUMENT FALSE)
+endif()
+
set (BUILD_TYPE_PROFILE FALSE)
set (BUILD_TYPE_RELEASE FALSE)
set (BUILD_TYPE_RELWITHDEBINFO FALSE)
@@ -181,8 +185,12 @@ endif()
#####################################
# Handle CFlags
+# USE_UPSTREAM_CFLAGS (default TRUE)
+if(NOT DEFINED USE_UPSTREAM_CFLAGS)
+ set (USE_UPSTREAM_CFLAGS True)
+endif()
+
unset (CMAKE_C_FLAGS_ALL CACHE)
-unset (CMAKE_CXX_FLAGS CACHE)
# USE_HOST_CFLAGS (default TRUE)
# Will check building host machine for proper cflags
@@ -191,9 +199,11 @@ if(NOT DEFINED USE_HOST_CFLAGS OR USE_HOST_CFLAGS)
include (${gazebo_cmake_dir}/HostCFlags.cmake)
endif()
-# USE_UPSTREAM_CFLAGS (default TRUE)
# Will use predefined gazebo developers cflags
-if(NOT DEFINED USE_UPSTREAM_CFLAGS OR USE_UPSTREAM_CFLAGS)
+# this needs to be called after HostCFlags
+if(USE_UPSTREAM_CFLAGS)
+ # use gazebo own set of flags
+ unset (CMAKE_CXX_FLAGS CACHE)
message(STATUS "Enable upstream CFlags")
include(${gazebo_cmake_dir}/DefaultCFlags.cmake)
endif()
@@ -209,7 +219,18 @@ if (UNIX AND NOT APPLE)
filter_valid_compiler_flags(-fvisibility=hidden -fvisibility-inlines-hidden)
endif()
-set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${VALID_CXX_FLAGS}")
+if (MSVC)
+ # Unable to be filtered flags (failing due to limitations in filter_valid_compiler_warnings)
+ # Handling exceptions rightly
+ set(UNFILTERED_FLAGS "/EHsc")
+endif()
+
+# Visual Studio enables c++11 support by default
+if (NOT MSVC)
+ set(UNFILTERED_FLAGS "-std=c++11")
+endif()
+
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${VALID_CXX_FLAGS} ${UNFILTERED_FLAGS}")
#################################################
# OS Specific initialization
@@ -225,6 +246,9 @@ endif()
# Main includes for compilation
include_directories(${PROJECT_SOURCE_DIR} ${PROJECT_BINARY_DIR})
+include_directories(SYSTEM
+ ${IGNITION-MATH_INCLUDE_DIRS}
+)
#################################################
# Configure 3rd Party Packages after OS Specific initialization
@@ -317,12 +341,6 @@ else (build_errors)
set (Boost_PKGCONFIG_LIBS "${Boost_PKGCONFIG_LIBS} ${bname}")
endforeach(b)
- # set additional ldflags for osx
- if (APPLE)
- # append stdc++
- set (APPLE_PKGCONFIG_LIBS "${APPLE_PKGCONFIG_LIBS} -lstdc++")
- ENDIF (APPLE)
-
foreach (pkgconfig ${pkgconfig_files})
configure_file(${CMAKE_SOURCE_DIR}/cmake/pkgconfig/${pkgconfig}.in ${CMAKE_CURRENT_BINARY_DIR}/cmake/pkgconfig/${pkgconfig}.pc @ONLY)
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/cmake/pkgconfig/${pkgconfig}.pc DESTINATION ${LIB_INSTALL_DIR}/pkgconfig COMPONENT pkgconfig)
@@ -331,43 +349,67 @@ else (build_errors)
########################################
# Make the cmake config files
set(PKG_NAME ${PROJECT_NAME_UPPER})
+
+ # Order is important, if A depends on B, please add B after A.
+ # The list should have at the very end the libraries
+ # without internal interdependencies
set(PKG_LIBRARIES
gazebo
- gazebo_common
- gazebo_gimpact
- gazebo_gui
+ gazebo_client
gazebo_gui_building
gazebo_gui_viewers
- gazebo_math
- gazebo_msgs
- gazebo_ode
- gazebo_opcode
- gazebo_opende_ou
- gazebo_physics
- gazebo_physics_ode
+ gazebo_gui_model
+ gazebo_gui
+ gazebo_sensors
gazebo_rendering
gazebo_selection_buffer
- gazebo_sensors
- gazebo_skyx
- gazebo_transport
- gazebo_util)
+ )
- if (NOT CCD_FOUND)
- set(PKG_LIBRARIES ${PKG_LIBRARIES} gazebo_ccd)
+ # "Second level" deps. Depends on a variety of first level deps
+ if (HAVE_DART)
+ set(PKG_LIBRARIES ${PKG_LIBRARIES} gazebo_physics_dart)
+ endif()
+ if (HAVE_BULLET)
+ set(PKG_LIBRARIES ${PKG_LIBRARIES} gazebo_physics_bullet)
+ endif()
+ if (HAVE_SIMBODY)
+ set(PKG_LIBRARIES ${PKG_LIBRARIES} gazebo_physics_simbody)
endif()
if (INCLUDE_PLAYER)
set(PKG_LIBRARIES ${PKG_LIBRARIES} gazebo_player)
endif()
- if (HAVE_BULLET)
- set(PKG_LIBRARIES ${PKG_LIBRARIES} gazebo_physics_bullet)
- endif()
+ set(PKG_LIBRARIES ${PKG_LIBRARIES}
+ gazebo_physics_ode
+ gazebo_physics
+ gazebo_ode
+ )
+ # "First level" deps, mostly depends on math and/or common
if (${OGRE_VERSION} GREATER 1.7.4)
set(PKG_LIBRARIES ${PKG_LIBRARIES} gazebo_rendering_deferred)
endif()
+ set(PKG_LIBRARIES ${PKG_LIBRARIES}
+ gazebo_transport
+ gazebo_msgs
+ gazebo_util
+ gazebo_common)
+
+ # No other internal dependencies:
+ set(PKG_LIBRARIES ${PKG_LIBRARIES}
+ gazebo_skyx
+ gazebo_gimpact
+ gazebo_opcode
+ gazebo_opende_ou
+ gazebo_math
+ )
+
+ if (NOT CCD_FOUND)
+ set(PKG_LIBRARIES ${PKG_LIBRARIES} gazebo_ccd)
+ endif()
+
set(PKG_DEPENDS Boost Protobuf SDFormat OGRE)
set(cmake_conf_file "cmake/gazebo-config.cmake")
diff --git a/Changelog.md b/Changelog.md
index 82e0657..cd3aaa2 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -1,6 +1,421 @@
+## Gazebo 6.0
+
+1. Added magnetometer sensor. A contribution from Andrew Symington.
+ * [Pull request #1788](https://bitbucket.org/osrf/gazebo/pull-request/1788)
+
+1. Added altimeter sensor. A contribution from Andrew Symington.
+ * [Pull request #1792](https://bitbucket.org/osrf/gazebo/pull-request/1792)
+
+1. Implement more control options for log playback:
+ 1. Rewind: The simulation starts from the beginning.
+ 1. Forward: The simulation jumps to the end of the log file.
+ 1. Seek: The simulation jumps to a specific point specified by its simulation
+ time.
+ * [Pull request #1737](https://bitbucket.org/osrf/gazebo/pull-request/1737)
+
+1. Added Gazebo splash screen
+ * [Pull request #1745](https://bitbucket.org/osrf/gazebo/pull-request/1745)
+
+1. Added a transporter plugin which allows models to move from one location
+ to another based on their location and the location of transporter pads.
+ * [Pull request #1738](https://bitbucket.org/osrf/gazebo/pull-request/1738)
+
+1. Implement forward/backwards multi-step for log playback. Now, the semantics
+of a multi-step while playing back a log session are different from a multi-step
+during a live simulation. While playback, a multi-step simulates all the
+intermediate steps as before, but the client only perceives a single step.
+E.g: You have a log file containing a 1 hour simulation session. You want to
+jump to the minute 00H::30M::00S to check a specific aspect of the simulation.
+You should not see continuous updates until minute 00H:30M:00S. Instead, you
+should visualize a single jump to the specific instant of the simulation that
+you are interested.
+ * [Pull request #1623](https://bitbucket.org/osrf/gazebo/pull-request/1623)
+
+1. Added browse button to log record dialog.
+ * [Pull request #1719](https://bitbucket.org/osrf/gazebo/pull-request/1719)
+
+1. Improved SVG support: arcs in paths, and contours made of multiple paths.
+ * [Pull request #1608](https://bitbucket.org/osrf/gazebo/pull-request/1608)
+
+1. Added simulation iterations to the world state.
+ * [Pull request #1722](https://bitbucket.org/osrf/gazebo/pull-request/1722)
+
+1. Added multiple LiftDrag plugins to the cessna_demo.world to allow the Cessna
+C-172 model to fly.
+ * [Pull request #1715](https://bitbucket.org/osrf/gazebo/pull-request/1715)
+
+1. Added a plugin to control a Cessna C-172 via messages (CessnaPlugin), and a
+GUI plugin to test this functionality with the keyboard (CessnaGUIPlugin). Added
+world with the Cessna model and the two previous plugins loaded
+(cessna_demo.world).
+ * [Pull request #1712](https://bitbucket.org/osrf/gazebo/pull-request/1712)
+
+1. Added world with OSRF building and an elevator
+ * [Pull request #1697](https://bitbucket.org/osrf/gazebo/pull-request/1697)
+
+1. Fixed collide bitmask by changing default value from 0x1 to 0xffff.
+ * [Pull request #1696](https://bitbucket.org/osrf/gazebo/pull-request/1696)
+
+1. Added a plugin to control an elevator (ElevatorPlugin), and an OccupiedEvent plugin that sends a message when a model is within a specified region.
+ * [Pull request #1694](https://bitbucket.org/osrf/gazebo/pull-request/1694)
+ * [Pull request #1775](https://bitbucket.org/osrf/gazebo/pull-request/1775)
+
+1. Added Layers tab and meta information for visuals.
+ * [Pull request #1674](https://bitbucket.org/osrf/gazebo/pull-request/1674)
+
+1. Added countdown behavior for common::Timer and exposed the feature in TimerGUIPlugin.
+ * [Pull request #1690](https://bitbucket.org/osrf/gazebo/pull-request/1690)
+
+1. Added BuoyancyPlugin for simulating the buoyancy of an object in a column of fluid.
+ * [Pull request #1622](https://bitbucket.org/osrf/gazebo/pull-request/1622)
+
+1. Added ComputeVolume function for simple shape subclasses of Shape.hh.
+ * [Pull request #1605](https://bitbucket.org/osrf/gazebo/pull-request/1605)
+
+1. Add option to parallelize the ODE quickstep constraint solver,
+which solves an LCP twice with different parameters in order
+to corrected for position projection errors.
+ * [Pull request #1561](https://bitbucket.org/osrf/gazebo/pull-request/1561)
+
+1. Get/Set user camera pose in GUI.
+ * [Pull request #1649](https://bitbucket.org/osrf/gazebo/pull-request/1649)
+ * [Issue #1595](https://bitbucket.org/osrf/gazebo/issue/1595)
+
+1. Added ViewAngleWidget, removed hard-coded reset view and removed MainWindow::Reset(). Also added GLWidget::GetSelectedVisuals().
+ * [Pull request #1768](https://bitbucket.org/osrf/gazebo/pull-request/1768)
+ * [Issue #1507](https://bitbucket.org/osrf/gazebo/issue/1507)
+
+1. Windows support. This consists mostly of numerous small changes to support
+compilation on Windows.
+ * [Pull request #1616](https://bitbucket.org/osrf/gazebo/pull-request/1616)
+ * [Pull request #1618](https://bitbucket.org/osrf/gazebo/pull-request/1618)
+ * [Pull request #1620](https://bitbucket.org/osrf/gazebo/pull-request/1620)
+ * [Pull request #1625](https://bitbucket.org/osrf/gazebo/pull-request/1625)
+ * [Pull request #1626](https://bitbucket.org/osrf/gazebo/pull-request/1626)
+ * [Pull request #1627](https://bitbucket.org/osrf/gazebo/pull-request/1627)
+ * [Pull request #1628](https://bitbucket.org/osrf/gazebo/pull-request/1628)
+ * [Pull request #1629](https://bitbucket.org/osrf/gazebo/pull-request/1629)
+ * [Pull request #1630](https://bitbucket.org/osrf/gazebo/pull-request/1630)
+ * [Pull request #1631](https://bitbucket.org/osrf/gazebo/pull-request/1631)
+ * [Pull request #1632](https://bitbucket.org/osrf/gazebo/pull-request/1632)
+ * [Pull request #1633](https://bitbucket.org/osrf/gazebo/pull-request/1633)
+ * [Pull request #1635](https://bitbucket.org/osrf/gazebo/pull-request/1635)
+ * [Pull request #1637](https://bitbucket.org/osrf/gazebo/pull-request/1637)
+ * [Pull request #1639](https://bitbucket.org/osrf/gazebo/pull-request/1639)
+ * [Pull request #1647](https://bitbucket.org/osrf/gazebo/pull-request/1647)
+ * [Pull request #1650](https://bitbucket.org/osrf/gazebo/pull-request/1650)
+ * [Pull request #1651](https://bitbucket.org/osrf/gazebo/pull-request/1651)
+ * [Pull request #1653](https://bitbucket.org/osrf/gazebo/pull-request/1653)
+ * [Pull request #1654](https://bitbucket.org/osrf/gazebo/pull-request/1654)
+ * [Pull request #1657](https://bitbucket.org/osrf/gazebo/pull-request/1657)
+ * [Pull request #1658](https://bitbucket.org/osrf/gazebo/pull-request/1658)
+ * [Pull request #1659](https://bitbucket.org/osrf/gazebo/pull-request/1659)
+ * [Pull request #1660](https://bitbucket.org/osrf/gazebo/pull-request/1660)
+ * [Pull request #1661](https://bitbucket.org/osrf/gazebo/pull-request/1661)
+ * [Pull request #1669](https://bitbucket.org/osrf/gazebo/pull-request/1669)
+ * [Pull request #1670](https://bitbucket.org/osrf/gazebo/pull-request/1670)
+ * [Pull request #1672](https://bitbucket.org/osrf/gazebo/pull-request/1672)
+ * [Pull request #1682](https://bitbucket.org/osrf/gazebo/pull-request/1682)
+ * [Pull request #1683](https://bitbucket.org/osrf/gazebo/pull-request/1683)
+
+1. Install `libgazebo_server_fixture`. This will facilitate tests external to the main gazebo repository. See `examples/stand_alone/test_fixture`.
+ * [Pull request #1606](https://bitbucket.org/osrf/gazebo/pull-request/1606)
+
+1. Laser visualization renders light blue for rays that do not hit obstacles, and dark blue for other rays.
+ * [Pull request #1607](https://bitbucket.org/osrf/gazebo/pull-request/1607)
+ * [Issue #1576](https://bitbucket.org/osrf/gazebo/issue/1576)
+
+1. Add VisualType enum to Visual and clean up visuals when entity is deleted.
+ * [Pull request #1614](https://bitbucket.org/osrf/gazebo/pull-request/1614)
+
+1. Alert user of connection problems when using the REST service plugin
+ * [Pull request #1655](https://bitbucket.org/osrf/gazebo/pull-request/1655)
+ * [Issue #1574](https://bitbucket.org/osrf/gazebo/issue/1574)
+
+1. ignition-math is now a dependency.
+ + [http://ignitionrobotics.org/libraries/math](http://ignitionrobotics.org/libraries/math)
+ + [Gazebo::math migration](https://bitbucket.org/osrf/gazebo/src/583edbeb90759d43d994cc57c0797119dd6d2794/ign-math-migration.md)
+
+1. Detect uuid library during compilation.
+ * [Pull request #1655](https://bitbucket.org/osrf/gazebo/pull-request/1655)
+ * [Issue #1572](https://bitbucket.org/osrf/gazebo/issue/1572)
+
+1. New accessors in LogPlay class.
+ * [Pull request #1577](https://bitbucket.org/osrf/gazebo/pull-request/1577)
+
+1. Added a plugin to send messages to an existing website.
+ Added gui::MainWindow::AddMenu and msgs/rest_error, msgs/rest_login, msgs rest/post
+ * [Pull request #1524](https://bitbucket.org/osrf/gazebo/pull-request/1524)
+
+1. Fix deprecation warnings when using SDFormat 3.0.2, 3.0.3 prereleases
+ * [Pull request #1568](https://bitbucket.org/osrf/gazebo/pull-request/1568)
+
+1. Use GAZEBO_CFLAGS or GAZEBO_CXX_FLAGS in CMakeLists.txt for example plugins
+ * [Pull request #1573](https://bitbucket.org/osrf/gazebo/pull-request/1573)
+
+1. Added Link::OnWrenchMsg subscriber with test
+ * [Pull request #1582](https://bitbucket.org/osrf/gazebo/pull-request/1582)
+
+1. Show/hide GUI overlays using the menu bar.
+ * [Pull request #1555](https://bitbucket.org/osrf/gazebo/pull-request/1555)
+
+1. Added world origin indicator rendering::OriginVisual.
+ * [Pull request #1700](https://bitbucket.org/osrf/gazebo/pull-request/1700)
+
+1. Show/hide toolbars using the menu bars and shortcut.
+ Added MainWindow::CloneAction.
+ Added Window menu to Model Editor.
+ * [Pull request #1584](https://bitbucket.org/osrf/gazebo/pull-request/1584)
+
+1. Added event to show/hide toolbars.
+ * [Pull request #1707](https://bitbucket.org/osrf/gazebo/pull-request/1707)
+
+1. Added optional start/stop/reset buttons to timer GUI plugin.
+ * [Pull request #1576](https://bitbucket.org/osrf/gazebo/pull-request/1576)
+
+1. Timer GUI Plugin: Treat negative positions as positions from the ends
+ * [Pull request #1703](https://bitbucket.org/osrf/gazebo/pull-request/1703)
+
+1. Added Visual::GetDepth() and Visual::GetNthAncestor()
+ * [Pull request #1613](https://bitbucket.org/osrf/gazebo/pull-request/1613)
+
+1. Added a context menu for links
+ * [Pull request #1589](https://bitbucket.org/osrf/gazebo/pull-request/1589)
+
+1. Separate TimePanel's display into TimeWidget and LogPlayWidget.
+ * [Pull request #1564](https://bitbucket.org/osrf/gazebo/pull-request/1564)
+
+1. Display confirmation message after log is saved
+ * [Pull request #1646](https://bitbucket.org/osrf/gazebo/pull-request/1646)
+
+1. Added LogPlayView to display timeline and LogPlaybackStatistics message type.
+ * [Pull request #1724](https://bitbucket.org/osrf/gazebo/pull-request/1724)
+
+1. Added Time::FormattedString and removed all other FormatTime functions.
+ * [Pull request #1710](https://bitbucket.org/osrf/gazebo/pull-request/1710)
+
+1. Added support for Oculus DK2
+ * [Pull request #1526](https://bitbucket.org/osrf/gazebo/pull-request/1526)
+
+1. Use collide_bitmask from SDF to perform collision filtering
+ * [Pull request #1470](https://bitbucket.org/osrf/gazebo/pull-request/1470)
+
+1. Pass Coulomb surface friction parameters to DART.
+ * [Pull request #1420](https://bitbucket.org/osrf/gazebo/pull-request/1420)
+
+1. Added ModelAlign::SetHighlighted
+ * [Pull request #1598](https://bitbucket.org/osrf/gazebo/pull-request/1598)
+
+1. Added various Get functions to Visual. Also added a ConvertGeometryType function to msgs.
+ * [Pull request #1402](https://bitbucket.org/osrf/gazebo/pull-request/1402)
+
+1. Get and Set visibility of SelectionObj's handles, with unit test.
+ * [Pull request #1417](https://bitbucket.org/osrf/gazebo/pull-request/1417)
+
+1. Set material of SelectionObj's handles.
+ * [Pull request #1472](https://bitbucket.org/osrf/gazebo/pull-request/1472)
+
+1. Add SelectionObj::Fini with tests and make Visual::Fini virtual
+ * [Pull request #1685](https://bitbucket.org/osrf/gazebo/pull-request/1685)
+
+1. Allow link selection with the mouse if parent model already selected.
+ * [Pull request #1409](https://bitbucket.org/osrf/gazebo/pull-request/1409)
+
+1. Added ModelRightMenu::EntityTypes.
+ * [Pull request #1414](https://bitbucket.org/osrf/gazebo/pull-request/1414)
+
+1. Scale joint visuals according to link size.
+ * [Pull request #1591](https://bitbucket.org/osrf/gazebo/pull-request/1591)
+ * [Issue #1563](https://bitbucket.org/osrf/gazebo/issue/1563)
+
+1. Added Gazebo/CoM material.
+ * [Pull request #1439](https://bitbucket.org/osrf/gazebo/pull-request/1439)
+
+1. Added arc parameter to MeshManager::CreateTube
+ * [Pull request #1436](https://bitbucket.org/osrf/gazebo/pull-request/1436)
+
+1. Added View Inertia and InertiaVisual, changed COMVisual to sphere proportional to mass.
+ * [Pull request #1445](https://bitbucket.org/osrf/gazebo/pull-request/1445)
+
+1. Added View Link Frame and LinkFrameVisual. Visual::SetTransparency goes into texture_unit.
+ * [Pull request #1762](https://bitbucket.org/osrf/gazebo/pull-request/1762)
+ * [Issue #853](https://bitbucket.org/osrf/gazebo/issue/853)
+
+1. Changed the position of Save and Cancel buttons on editor dialogs
+ * [Pull request #1442](https://bitbucket.org/osrf/gazebo/pull-request/1442)
+ * [Issue #1377](https://bitbucket.org/osrf/gazebo/issue/1377)
+
+1. Fixed Visual material updates
+ * [Pull request #1454](https://bitbucket.org/osrf/gazebo/pull-request/1454)
+ * [Issue #1455](https://bitbucket.org/osrf/gazebo/issue/1455)
+
+1. Added Matrix3::Inverse() and tests
+ * [Pull request #1481](https://bitbucket.org/osrf/gazebo/pull-request/1481)
+
+1. Implemented AddLinkForce for ODE.
+ * [Pull request #1456](https://bitbucket.org/osrf/gazebo/pull-request/1456)
+
+1. Updated ConfigWidget class to parse enum values.
+ * [Pull request #1518](https://bitbucket.org/osrf/gazebo/pull-request/1518)
+
+1. Added PresetManager to physics libraries and corresponding integration test.
+ * [Pull request #1471](https://bitbucket.org/osrf/gazebo/pull-request/1471)
+
+1. Sync name and location on SaveDialog.
+ * [Pull request #1563](https://bitbucket.org/osrf/gazebo/pull-request/1563)
+
+1. Added Apply Force/Torque dialog
+ * [Pull request #1600](https://bitbucket.org/osrf/gazebo/pull-request/1600)
+
+1. Added Apply Force/Torque visuals
+ * [Pull request #1619](https://bitbucket.org/osrf/gazebo/pull-request/1619)
+
+1. Added Apply Force/Torque OnMouseRelease and ActivateWindow
+ * [Pull request #1699](https://bitbucket.org/osrf/gazebo/pull-request/1699)
+
+1. Added Apply Force/Torque mouse interactions, modes, activation
+ * [Pull request #1731](https://bitbucket.org/osrf/gazebo/pull-request/1731)
+
+1. Added inertia pose getter for COMVisual and COMVisual_TEST
+ * [Pull request #1581](https://bitbucket.org/osrf/gazebo/pull-request/1581)
+
+1. Model editor updates
+ 1. Joint preview using JointVisuals.
+ * [Pull request #1369](https://bitbucket.org/osrf/gazebo/pull-request/1369)
+
+ 1. Added inspector for configuring link, visual, and collision properties.
+ * [Pull request #1408](https://bitbucket.org/osrf/gazebo/pull-request/1408)
+
+ 1. Saving, exiting, generalizing SaveDialog.
+ * [Pull request #1401](https://bitbucket.org/osrf/gazebo/pull-request/1401)
+
+ 1. Inspectors redesign
+ * [Pull request #1586](https://bitbucket.org/osrf/gazebo/pull-request/1586)
+
+ 1. Edit existing model.
+ * [Pull request #1425](https://bitbucket.org/osrf/gazebo/pull-request/1425)
+
+ 1. Add joint inspector to link's context menu.
+ * [Pull request #1449](https://bitbucket.org/osrf/gazebo/pull-request/1449)
+ * [Issue #1443](https://bitbucket.org/osrf/gazebo/issue/1443)
+
+ 1. Added button to select mesh file on inspector.
+ * [Pull request #1460](https://bitbucket.org/osrf/gazebo/pull-request/1460)
+ * [Issue #1450](https://bitbucket.org/osrf/gazebo/issue/1450)
+
+ 1. Renamed Part to Link.
+ * [Pull request #1478](https://bitbucket.org/osrf/gazebo/pull-request/1478)
+
+ 1. Fix snapping inside editor.
+ * [Pull request #1489](https://bitbucket.org/osrf/gazebo/pull-request/1489)
+ * [Issue #1457](https://bitbucket.org/osrf/gazebo/issue/1457)
+
+ 1. Moved DataLogger from Window menu to the toolbar and moved screenshot button to the right.
+ * [Pull request #1665](https://bitbucket.org/osrf/gazebo/pull-request/1665)
+
+ 1. Keep loaded model's name.
+ * [Pull request #1516](https://bitbucket.org/osrf/gazebo/pull-request/1516)
+ * [Issue #1504](https://bitbucket.org/osrf/gazebo/issue/1504)
+
+ 1. Added ExtrudeDialog.
+ * [Pull request #1483](https://bitbucket.org/osrf/gazebo/pull-request/1483)
+
+ 1. Hide time panel inside editor and keep main window's paused state.
+ * [Pull request #1500](https://bitbucket.org/osrf/gazebo/pull-request/1500)
+
+ 1. Fixed pose issues and added ModelCreator_TEST.
+ * [Pull request #1509](https://bitbucket.org/osrf/gazebo/pull-request/1509)
+ * [Issue #1497](https://bitbucket.org/osrf/gazebo/issue/1497)
+ * [Issue #1509](https://bitbucket.org/osrf/gazebo/issue/1509)
+
+ 1. Added list of links and joints.
+ * [Pull request #1515](https://bitbucket.org/osrf/gazebo/pull-request/1515)
+ * [Issue #1418](https://bitbucket.org/osrf/gazebo/issue/1418)
+
+ 1. Expose API to support adding items to the palette.
+ * [Pull request #1565](https://bitbucket.org/osrf/gazebo/pull-request/1565)
+
+ 1. Added menu for toggling joint visualization
+ * [Pull request #1551](https://bitbucket.org/osrf/gazebo/pull-request/1551)
+ * [Issue #1483](https://bitbucket.org/osrf/gazebo/issue/1483)
+
+ 1. Add schematic view to model editor
+ * [Pull request #1562](https://bitbucket.org/osrf/gazebo/pull-request/1562)
+
+1. Building editor updates
+ 1. Make palette tips tooltip clickable to open.
+ * [Pull request #1519](https://bitbucket.org/osrf/gazebo/pull-request/1519)
+ * [Issue #1370](https://bitbucket.org/osrf/gazebo/issue/1370)
+
+ 1. Add measurement unit to building inspectors.
+ * [Pull request #1741](https://bitbucket.org/osrf/gazebo/pull-request/1741)
+ * [Issue #1363](https://bitbucket.org/osrf/gazebo/issue/1363)
+
+ 1. Add `BaseInspectorDialog` as a base class for inspectors.
+ * [Pull request #1749](https://bitbucket.org/osrf/gazebo/pull-request/1749)
+
## Gazebo 5.0
-### Gazebo 5.0.0
+### Gazebo 5.x.x
+
+1. Added lines to laser sensor visualization
+ * [Pull request #1742](https://bitbucket.org/osrf/gazebo/pull-request/1742)
+ * [Issue #935](https://bitbucket.org/osrf/gazebo/issue/935)
+
+1. Fix BulletSliderJoint friction for bullet 2.83
+ * [Pull request #1686](https://bitbucket.org/osrf/gazebo/pull-request/1686)
+
+1. Fix heightmap model texture loading.
+ * [Pull request #1592](https://bitbucket.org/osrf/gazebo/pull-request/1592)
+
+1. Disable failing pr2 test for dart
+ * [Pull request #1540](https://bitbucket.org/osrf/gazebo/pull-request/1540)
+ * [Issue #1435](https://bitbucket.org/osrf/gazebo/issue/1435)
+
+### Gazebo 5.1.0 (2015-03-20)
+1. Backport pull request #1527 (FindOGRE.cmake for non-Debian systems)
+ * [Pull request #1532](https://bitbucket.org/osrf/gazebo/pull-request/1532)
+
+1. Respect system cflags when not using USE_UPSTREAM_CFLAGS
+ * [Pull request #1531](https://bitbucket.org/osrf/gazebo/pull-request/1531)
+
+1. Allow light manipulation
+ * [Pull request #1529](https://bitbucket.org/osrf/gazebo/pull-request/1529)
+
+1. Allow sdformat 2.3.1+ or 3+ and fix tests
+ * [Pull request #1484](https://bitbucket.org/osrf/gazebo/pull-request/1484)
+
+1. Add Link::GetWorldAngularMomentum function and test.
+ * [Pull request #1482](https://bitbucket.org/osrf/gazebo/pull-request/1482)
+
+1. Preserve previous GAZEBO_MODEL_PATH values when sourcing setup.sh
+ * [Pull request #1430](https://bitbucket.org/osrf/gazebo/pull-request/1430)
+
+1. Implement Coulomb joint friction for DART
+ * [Pull request #1427](https://bitbucket.org/osrf/gazebo/pull-request/1427)
+ * [Issue #1281](https://bitbucket.org/osrf/gazebo/issue/1281)
+
+1. Fix simple shape normals.
+ * [Pull request #1477](https://bitbucket.org/osrf/gazebo/pull-request/1477)
+ * [Issue #1369](https://bitbucket.org/osrf/gazebo/issue/1369)
+
+1. Use Msg-to-SDF conversion functions in tests, add ServerFixture::SpawnModel(msgs::Model).
+ * [Pull request #1466](https://bitbucket.org/osrf/gazebo/pull-request/1466)
+
+1. Added Model Msg-to-SDF conversion functions and test.
+ * [Pull request #1429](https://bitbucket.org/osrf/gazebo/pull-request/1429)
+
+1. Added Joint Msg-to-SDF conversion functions and test.
+ * [Pull request #1419](https://bitbucket.org/osrf/gazebo/pull-request/1419)
+
+1. Added Visual, Material Msg-to-SDF conversion functions and ShaderType to string conversion functions.
+ * [Pull request #1415](https://bitbucket.org/osrf/gazebo/pull-request/1415)
+
+1. Implement Coulomb joint friction for BulletSliderJoint
+ * [Pull request #1452](https://bitbucket.org/osrf/gazebo/pull-request/1452)
+ * [Issue #1348](https://bitbucket.org/osrf/gazebo/issue/1348)
+
+### Gazebo 5.0.0 (2015-01-27)
1. Support for using [digital elevation maps](http://gazebosim.org/tutorials?tut=dem) has been added to debian packages.
1. C++11 support (C++11 compatible compiler is now required)
@@ -50,7 +465,7 @@
* [Issue #381](https://bitbucket.org/osrf/gazebo/issue/381)
1. Implement Coulomb joint friction for BulletHingeJoint
- * [Pull request #1221](https://bitbucket.org/osrf/gazebo/pull-request/1317)
+ * [Pull request #1317](https://bitbucket.org/osrf/gazebo/pull-request/1317)
* [Issue #1348](https://bitbucket.org/osrf/gazebo/issue/1348)
1. Implemented camera lens distortion.
@@ -96,6 +511,10 @@
1. Fixed crash on "permission denied" bug, added insert_model integration test.
* [Pull request #1329](https://bitbucket.org/osrf/gazebo/pull-request/1329/)
+1. Enable simbody joint tests, implement `SimbodyJoint::GetParam`, create
+ `Joint::GetParam`, fix bug in `BulletHingeJoint::SetParam`.
+ * [Pull request #1404](https://bitbucket.org/osrf/gazebo/pull-request/1404/)
+
1. Building editor updates
1. Fixed inspector resizing.
* [Pull request #1230](https://bitbucket.org/osrf/gazebo/pull-request/1230)
@@ -202,7 +621,78 @@
## Gazebo 4.0
-### Gazebo 4.x.x (yyyy-mm-dd)
+### Gazebo 4.x.x (2015-xx-xx)
+
+1. Fix build for Bullet 2.83, enable angle wrapping for BulletHingeJoint
+ * [Pull request #1664](https://bitbucket.org/osrf/gazebo/pull-request/1664)
+
+### Gazebo 4.1.3 (2015-05-07)
+
+1. Fix saving visual geom SDF values
+ * [Pull request #1597](https://bitbucket.org/osrf/gazebo/pull-request/1597)
+1. Fix heightmap model texture loading.
+ * [Pull request #1595](https://bitbucket.org/osrf/gazebo/pull-request/1595)
+1. Fix visual collision scale on separate client
+ * [Pull request #1585](https://bitbucket.org/osrf/gazebo/pull-request/1585)
+1. Fix several clang compiler warnings
+ * [Pull request #1594](https://bitbucket.org/osrf/gazebo/pull-request/1594)
+1. Fix blank save / browse dialogs
+ * [Pull request #1544](https://bitbucket.org/osrf/gazebo/pull-request/1544)
+
+### Gazebo 4.1.2 (2015-03-20)
+
+1. Fix quaternion documentation: target Gazebo_4.1
+ * [Pull request #1525](https://bitbucket.org/osrf/gazebo/pull-request/1525)
+1. Speed up World::Step in loops
+ * [Pull request #1492](https://bitbucket.org/osrf/gazebo/pull-request/1492)
+1. Reduce selection buffer updates -> 4.1
+ * [Pull request #1494](https://bitbucket.org/osrf/gazebo/pull-request/1494)
+1. Fix QT rendering, and rendering update rate
+ * [Pull request #1487](https://bitbucket.org/osrf/gazebo/pull-request/1487)
+1. Fix loading of SimbodyPhysics parameters
+ * [Pull request #1474](https://bitbucket.org/osrf/gazebo/pull-request/1474)
+1. Fix heightmap on OSX -> 4.1
+ * [Pull request #1455](https://bitbucket.org/osrf/gazebo/pull-request/1455)
+1. Remove extra pose tag in a world file that should not be there
+ * [Pull request #1458](https://bitbucket.org/osrf/gazebo/pull-request/1458)
+1. Better fix for #236 for IMU that doesn't require ABI changes
+ * [Pull request #1448](https://bitbucket.org/osrf/gazebo/pull-request/1448)
+1. Fix regression of #236 for ImuSensor in 4.1
+ * [Pull request #1446](https://bitbucket.org/osrf/gazebo/pull-request/1446)
+1. Preserve previous GAZEBO_MODEL_PATH values when sourcing setup.sh
+ * [Pull request #1430](https://bitbucket.org/osrf/gazebo/pull-request/1430)
+1. issue #857: fix segfault for simbody screw joint when setting limits due to uninitialized limitForce.
+ * [Pull request #1423](https://bitbucket.org/osrf/gazebo/pull-request/1423)
+1. Allow multiple contact sensors per link (#960)
+ * [Pull request #1413](https://bitbucket.org/osrf/gazebo/pull-request/1413)
+1. Fix for issue #351, ODE World Step
+ * [Pull request #1406](https://bitbucket.org/osrf/gazebo/pull-request/1406)
+1. Disable failing InelasticCollision/0 test (#1394)
+ * [Pull request #1405](https://bitbucket.org/osrf/gazebo/pull-request/1405)
+1. Prevent out of bounds array access in SkidSteerDrivePlugin (found by cppcheck 1.68)
+ * [Pull request #1379](https://bitbucket.org/osrf/gazebo/pull-request/1379)
+
+### Gazebo 4.1.1 (2015-01-15)
+
+1. Fix BulletPlaneShape bounding box (#1265)
+ * [Pull request #1367](https://bitbucket.org/osrf/gazebo/pull-request/1367)
+1. Fix dart linking errors on osx
+ * [Pull request #1372](https://bitbucket.org/osrf/gazebo/pull-request/1372)
+1. Update to player interfaces
+ * [Pull request #1324](https://bitbucket.org/osrf/gazebo/pull-request/1324)
+1. Handle GpuLaser name collisions (#1403)
+ * [Pull request #1360](https://bitbucket.org/osrf/gazebo/pull-request/1360)
+1. Add checks for handling array's with counts of zero, and read specular values
+ * [Pull request #1339](https://bitbucket.org/osrf/gazebo/pull-request/1339)
+1. Fix model list widget test
+ * [Pull request #1327](https://bitbucket.org/osrf/gazebo/pull-request/1327)
+1. Fix ogre includes
+ * [Pull request #1323](https://bitbucket.org/osrf/gazebo/pull-request/1323)
+
+### Gazebo 4.1.0 (2014-11-20)
+
+1. Modified GUI rendering to improve the rendering update rate.
+ * [Pull request #1487](https://bitbucket.org/osrf/gazebo/pull-request/1487)
### Gazebo 4.1.0 (2014-11-20)
diff --git a/Migration.md b/Migration.md
index fc3993a..90d81f8 100644
--- a/Migration.md
+++ b/Migration.md
@@ -1,3 +1,132 @@
+## Gazebo 5.X to 6.X
+
+### Modifications
+
+1. **gazebo/common/MouseEvent.hh**
+ * Replaced all member variables with functions that use Ignition Math.
+ * [Pull request #1777](https://bitbucket.org/osrf/gazebo/pull-request/1777)
+
+1. **gazebo/msgs/world_stats.proto**
+ + ***Removed:*** optional bool log_playback = 8;
+ + ***Replacement:*** optional LogPlaybackStatistics log_playback_stats = 8;
+
+1. **gazebo/physics/JointState.hh**
+ + ***Removed:*** public: JointState(JointPtr _joint, const common::Time
+ &_realTime, const common::Time &_simTime)
+ + ***Replacement:*** public: JointState(JointPtr _joint, const common::Time
+ &_realTime, const common::Time &_simTime, const uint64_t _iterations)
+
+1. **gazebo/physics/LinkState.hh**
+ + ***Removed:*** public: LinkState(const LinkPtr _link, const common::Time
+ &_realTime, const common::Time &_simTime)
+ + ***Replacement:*** public: LinkState(const LinkPtr _link,
+ const common::Time &_realTime, const common::Time &_simTime, const uint64_t
+ _iterations)
+ + ***Removed:*** public: void Load(const LinkPtr _link, const common::Time
+ &_realTime, const common::Time &_simTime)
+ + ***Replacement:*** public: void Load(const LinkPtr _link, const
+ common::Time &_realTime, const common::Time &_simTime, const uint64_t
+ _iterations)
+
+1. **gazebo/physics/ModelState.hh**
+ + ***Removed:*** public: ModelState(const ModelPtr _model, const
+ common::Time &_realTime, const common::Time &_simTime)
+ + ***Replacement:*** public: ModelState(const ModelPtr _model, const
+ common::Time &_realTime, const common::Time &_simTime, const uint64_t
+ _iterations)
+ + ***Removed:*** public: void Load(const ModelPtr _model, const common::Time
+ &_realTime, const common::Time &_simTime)
+ + ***Replacement:*** public: void Load(const ModelPtr _model, const
+ common::Time &_realTime, const common::Time &_simTime, const uint64_t
+ _iterations)
+
+1. **gazebo/physics/State.hh**
+ + ***Removed:*** public: State(const std::string &_name, const
+ common::Time &_realTime, const common::Time &_simTime)
+ + ***Replacement:*** public: State(const std::string &_name,
+ const common::Time &_realTime, const common::Time &_simTime, const uint64_t
+ _iterations)
+
+1. **gazebo/physics/ModelState.hh**
+ + ***Removed:*** public: void Load(const ModelPtr _model, const common::Time
+ &_realTime, const common::Time &_simTime)
+ + ***Replacement:*** public: void Load(const ModelPtr _model, const
+ common::Time &_realTime, const common::Time &_simTime, const uint64_t
+ _iterations)
+
+1. ignition-math is now a dependency. Many classes and functions are modified to use ignition-math, please see the pull request listing below for individual changes.
+ + [http://ignitionrobotics.org/libraries/math](http://ignitionrobotics.org/libraries/math)
+ + [Gazebo migration](https://bitbucket.org/osrf/gazebo/src/583edbeb90759d43d994cc57c0797119dd6d2794/ign-math-migration.md)
+ * [Pull request #1756](https://bitbucket.org/osrf/gazebo/pull-request/1756)
+ * [Pull request #1766](https://bitbucket.org/osrf/gazebo/pull-request/1766)
+ * [Pull request #1774](https://bitbucket.org/osrf/gazebo/pull-request/1774)
+ * [Pull request #1771](https://bitbucket.org/osrf/gazebo/pull-request/1771)
+ * [Pull request #1776](https://bitbucket.org/osrf/gazebo/pull-request/1776)
+ * [Pull request #1777](https://bitbucket.org/osrf/gazebo/pull-request/1777)
+ * [Pull request #1772](https://bitbucket.org/osrf/gazebo/pull-request/1772)
+ * [Pull request #1773](https://bitbucket.org/osrf/gazebo/pull-request/1773)
+ * [Pull request #1778](https://bitbucket.org/osrf/gazebo/pull-request/1778)
+
+1. Gazebo client's should now use `gazebo/gazebo_client.hh` and `libgazebo_client.so` instead of `gazebo/gazebo.hh` and `libgazebo.so`. This separates running a Gazebo server from a Gazebo client.
+
+1. **gazebo/rendering/GpuLaser.hh**
+ + ***Removed:*** protected: double near
+ + ***Replacement:*** protected: double nearClip
+
+1. **gazebo/rendering/GpuLaser.hh**
+ + ***Removed:*** protected: double far
+ + ***Replacement:*** protected: double farClip
+
+1. **gazebo/rendering/Visual.hh**
+ + ***Removed:*** public: void Fini();
+ + ***Replacement:*** public: virtual void Fini();
+
+1. **gazebo/common/MeshManager.hh**
+ + ***Removed:*** void CreateExtrudedPolyline(const std::string &_name, const std::vector<math::Vector2d> &_vertices, const double &_height, const math::Vector2d &_uvCoords)
+ + ***Replacement:*** void CreateExtrudedPolyline(const std::string &_name, const const std::vector<std::vector<math::Vector2d> > &_vertices, const double &_height, const math::Vector2d &_uvCoords)
+
+1. **gazebo/common/GTSMeshUtils.hh**
+ + ***Removed:*** public: static bool CreateExtrudedPolyline(const std::vector<math::Vector2d> &_vertices, const double &_height, SubMesh *_submesh)
+ + ***Replacement:*** public: static bool DelaunayTriangulation(const std::vector<std::vector<math::Vector2d> > &_path, SubMesh *_submesh)
+
+1. **gazebo/physics/PolylineShape.hh**
+ + ***Removed:*** public: std::vector<math::Vector2d> GetVertices() const
+ + ***Replacement:*** public: std::vector<std::vector<math::Vector2d> > GetVertices() const
+
+1. **gazebo/physics/SurfaceParams.hh**
+ + ***Removed:*** public: FrictionPyramid frictionPyramid
+ + ***Replacement:*** public: FrictionPyramidPtr GetFrictionPyramid() const
+
+### Deletions
+
+1. **gazebo/gui/RenderWidget.hh**
+ + The ShowEditor(bool _show)
+
+### Additions
+
+1. **gazebo/msgs/log_playback_control.proto**
+ + New message to control the playback from a log file.
+
+1. **gazebo/util/LogPlay.hh**
+ + public: bool Rewind()
+
+1. **gazebo/physics/LinkState.hh**
+ + public: virtual void SetIterations(const uint64_t _iterations)
+
+1. **gazebo/physics/ModelState.hh**
+ + public: virtual void SetIterations(const uint64_t _iterations)
+
+1. **gazebo/physics/State.hh**
+ + public: uint64_t GetIterations() const
+ + public: virtual void SetIterations(const uint64_t _iterations)
+
+1. **gazebo/physics/WorldState.hh**
+ + public: virtual void SetIterations(const uint64_t _iterations)
+
+1. **gazebo/util/LogPlay.hh**
+ + public: uint64_t GetInitialIterations() const
+ + public: bool HasIterations() const
+
## Gazebo 4.X to 5.X
### C++11 compiler required
diff --git a/cmake/DefaultCFlags.cmake b/cmake/DefaultCFlags.cmake
index 21ab60e..03e2ae4 100644
--- a/cmake/DefaultCFlags.cmake
+++ b/cmake/DefaultCFlags.cmake
@@ -6,39 +6,38 @@ set (CMAKE_LINK_FLAGS_PROFILE " -pg" CACHE INTERNAL "Link flags for profile" FOR
set (CMAKE_LINK_FLAGS_COVERAGE " --coverage" CACHE INTERNAL "Link flags for static code coverage" FORCE)
set (CMAKE_C_FLAGS_RELEASE "")
-if (NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
- # -s doesn't work with clang, see alternative in link below:
+if (NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang" AND NOT MSVC)
+ # -s doesn't work with clang or Visual Studio, see alternative in link below:
# http://stackoverflow.com/questions/6085491/gcc-vs-clang-symbol-strippingu
set (CMAKE_C_FLAGS_RELEASE "-s")
endif()
-set (CMAKE_C_FLAGS_RELEASE " ${CMAKE_C_FLAGS_RELEASE} -O3 -DNDEBUG ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for release" FORCE)
-set (CMAKE_CXX_FLAGS_RELEASE ${CMAKE_C_FLAGS_RELEASE})
-set (CMAKE_C_FLAGS_RELWITHDEBINFO " -g -O2 ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for release with debug support" FORCE)
-set (CMAKE_CXX_FLAGS_RELWITHDEBINFO ${CMAKE_C_FLAGS_RELWITHDEBINFO})
+if (NOT MSVC)
+ set (CMAKE_C_FLAGS_RELEASE " ${CMAKE_C_FLAGS_RELEASE} -O3 -DNDEBUG ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for release" FORCE)
+ set (CMAKE_CXX_FLAGS_RELEASE ${CMAKE_C_FLAGS_RELEASE})
+
+ set (CMAKE_C_FLAGS_RELWITHDEBINFO " -g -O2 ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for release with debug support" FORCE)
+ set (CMAKE_CXX_FLAGS_RELWITHDEBINFO ${CMAKE_C_FLAGS_RELWITHDEBINFO})
-set (CMAKE_C_FLAGS_DEBUG " -ggdb3 ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for debug" FORCE)
-set (CMAKE_CXX_FLAGS_DEBUG ${CMAKE_C_FLAGS_DEBUG})
+ set (CMAKE_C_FLAGS_DEBUG " -ggdb3 ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for debug" FORCE)
+ set (CMAKE_CXX_FLAGS_DEBUG ${CMAKE_C_FLAGS_DEBUG})
-set (CMAKE_C_FLAGS_PROFILE " -fno-omit-frame-pointer -g -pg ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for profile" FORCE)
-set (CMAKE_CXX_FLAGS_PROFILE ${CMAKE_C_FLAGS_PROFILE})
+ set (CMAKE_C_FLAGS_PROFILE " -fno-omit-frame-pointer -g -pg ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for profile" FORCE)
+ set (CMAKE_CXX_FLAGS_PROFILE ${CMAKE_C_FLAGS_PROFILE})
-set (CMAKE_C_FLAGS_COVERAGE " -g -O0 -Wformat=2 --coverage -fno-inline ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for static code coverage" FORCE)
-set (CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_C_FLAGS_COVERAGE}")
-if (NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
- # -fno-default-inline -fno-implicit-inline-templates are unimplemented, cause errors in clang
- # -fno-elide-constructors can cause seg-faults in clang 3.4 and earlier
- # http://llvm.org/bugs/show_bug.cgi?id=12208
- set (CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_COVERAGE} -fno-default-inline -fno-implicit-inline-templates -fno-elide-constructors")
+ set (CMAKE_C_FLAGS_COVERAGE " -g -O0 -Wformat=2 --coverage -fno-inline ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for static code coverage" FORCE)
+ set (CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_C_FLAGS_COVERAGE}")
+ if (NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+ # -fno-default-inline -fno-implicit-inline-templates are unimplemented, cause errors in clang
+ # -fno-elide-constructors can cause seg-faults in clang 3.4 and earlier
+ # http://llvm.org/bugs/show_bug.cgi?id=12208
+ set (CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_COVERAGE} -fno-default-inline -fno-implicit-inline-templates -fno-elide-constructors")
+ endif()
endif()
#####################################
# Set all the global build flags
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
-# Visual Studio enables c++11 support by default
-if (NOT MSVC)
- set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}} -std=c++11")
-endif()
set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
set (CMAKE_SHARED_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
set (CMAKE_MODULE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}")
@@ -62,7 +61,7 @@ elseif ("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang")
endif ()
elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC")
if (NOT MSVC12)
- message(FATAL_ERROR "${PROJECT_NAME} requires VS 2013 os greater.")
+ message(FATAL_ERROR "${PROJECT_NAME} requires VS 2013 or greater.")
endif()
else ()
message(FATAL_ERROR "Your C++ compiler does not support C++11.")
diff --git a/cmake/FindFreeimage.cmake b/cmake/FindFreeimage.cmake
index 875daa0..a989ec7 100644
--- a/cmake/FindFreeimage.cmake
+++ b/cmake/FindFreeimage.cmake
@@ -4,7 +4,7 @@ include (${gazebo_cmake_dir}/GazeboUtils.cmake)
########################################
# Find packages
if (PKG_CONFIG_FOUND)
- pkg_check_modules(freeimage freeimage>=${FREEIMAGE_VERSION})
+ pkg_check_modules(freeimage freeimage>=${MIN_FREEIMAGE_VERSION})
if (NOT freeimage_FOUND)
message (STATUS " freeimage.pc not found, trying freeimage_include_dir and freeimage_library_dir flags.")
endif (NOT freeimage_FOUND)
@@ -27,13 +27,21 @@ if (NOT freeimage_FOUND)
COMPILE_OUTPUT_VARIABLE FREEIMAGE_COMPILE_OUTPUT)
if (NOT FREEIMAGE_RUNS)
message (STATUS "${FREEIMAGE_COMPILE_OUTPUT}")
- BUILD_ERROR("Invalid FreeImage Version. Requires ${FREEIMAGE_VERSION}")
+ BUILD_ERROR("Invalid FreeImage Version. Requires ${MIN_FREEIMAGE_VERSION}")
else (NOT FREEIMAGE_RUNS)
message (STATUS " Looking for FreeImage.h - found")
endif (NOT FREEIMAGE_RUNS)
endif (NOT freeimage_INCLUDE_DIRS)
- find_library(freeimage_LIBRARIES freeimage)
+ if (WIN32)
+ if ("${CMAKE_BUILD_TYPE}" STREQUAL "Debug")
+ find_library(freeimage_LIBRARIES FreeImaged)
+ else()
+ find_library(freeimage_LIBRARIES FreeImage)
+ endif()
+ else(WIN32)
+ find_library(freeimage_LIBRARIES freeimage)
+ endif(WIN32)
if (NOT freeimage_LIBRARIES)
message (STATUS " Looking for libfreeimage - not found")
BUILD_ERROR("Missing: Unable to find libfreeimage")
diff --git a/cmake/FindGraphviz.cmake b/cmake/FindGraphviz.cmake
new file mode 100644
index 0000000..4d6d6fc
--- /dev/null
+++ b/cmake/FindGraphviz.cmake
@@ -0,0 +1,101 @@
+# - Try to find Graphviz cgraph library
+# Once done this will define
+#
+# GRAPHVIZ_FOUND - system has Graphviz installed
+# GRAPHVIZ_INCLUDE_DIR
+# GRAPHVIZ_GVC_LIBRARY
+# GRAPHVIZ_CGRAPH_LIBRARY
+# GRAPHVIZ_CDT_LIBRARY
+#
+#
+
+if ( GRAPHVIZ_CGRAPH_LIBRARY )
+ # in cache already
+ SET(Graphviz_FIND_QUIETLY TRUE)
+endif ( GRAPHVIZ_CGRAPH_LIBRARY )
+
+# use pkg-config to get the directories and then use these values
+# in the FIND_PATH() and FIND_LIBRARY() calls
+if( NOT WIN32 )
+ find_package(PkgConfig)
+
+ pkg_check_modules(GRAPHVIZ_GVC_PKG libgvc)
+ pkg_check_modules(GRAPHVIZ_CGRAPH_PKG libcgraph)
+ pkg_check_modules(GRAPHVIZ_CDT_PKG libcdt)
+endif( NOT WIN32 )
+
+FIND_LIBRARY(GRAPHVIZ_GVC_LIBRARY NAMES gvc libgvc
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ HINTS
+ ${GRAPHVIZ_GVC_PKG_LIBRARY_DIRS} # Generated by pkg-config
+)
+IF ( NOT(GRAPHVIZ_GVC_LIBRARY) )
+# MESSAGE(STATUS "Could not find libgvc." )
+ SET(GRAPHVIZ_GVC_FOUND FALSE)
+ELSE ()
+ SET(GRAPHVIZ_GVC_FOUND TRUE)
+ENDIF ()
+
+FIND_LIBRARY(GRAPHVIZ_CGRAPH_LIBRARY NAMES cgraph libcgraph
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ HINTS
+ ${GRAPHVIZ_CGRAPH_PKG_LIBRARY_DIRS} # Generated by pkg-config
+)
+IF ( NOT(GRAPHVIZ_CGRAPH_LIBRARY) )
+# MESSAGE(STATUS "Could not find libcgraph." )
+ SET(GRAPHVIZ_CGRAPH_FOUND FALSE)
+ELSE ()
+ SET(GRAPHVIZ_CGRAPH_FOUND TRUE)
+ENDIF ()
+
+FIND_LIBRARY(GRAPHVIZ_CDT_LIBRARY NAMES cdt libcdt
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ HINTS
+ ${GRAPHVIZ_CDT_PKG_LIBRARY_DIRS} # Generated by pkg-config
+)
+IF ( NOT(GRAPHVIZ_CDT_LIBRARY) )
+# MESSAGE(STATUS "Could not find libcdt." )
+ SET(GRAPHVIZ_CDT_FOUND FALSE)
+ELSE ()
+ SET(GRAPHVIZ_CDT_FOUND TRUE)
+ENDIF ()
+
+FIND_PATH(GRAPHVIZ_INCLUDE_DIR NAMES cgraph.h
+ PATHS
+ /usr/include/graphviz
+ /usr/local/include
+ HINTS
+ ${GRAPHVIZ_GVC_PKG_INCLUDE_DIRS} # Generated by pkg-config
+)
+IF ( NOT(GRAPHVIZ_INCLUDE_DIR) )
+ MESSAGE(STATUS "Could not find graphviz headers." )
+ENDIF ()
+
+include(FindPackageHandleStandardArgs)
+FIND_PACKAGE_HANDLE_STANDARD_ARGS(Graphviz_Gvc DEFAULT_MSG GRAPHVIZ_GVC_LIBRARY )
+FIND_PACKAGE_HANDLE_STANDARD_ARGS(Graphviz_Cgraph DEFAULT_MSG GRAPHVIZ_CGRAPH_LIBRARY )
+FIND_PACKAGE_HANDLE_STANDARD_ARGS(Graphviz_Cdt DEFAULT_MSG GRAPHVIZ_CDT_LIBRARY )
+FIND_PACKAGE_HANDLE_STANDARD_ARGS("Graphviz Headers" DEFAULT_MSG GRAPHVIZ_INCLUDE_DIR )
+
+
+# show the POPPLER_(XPDF/QT4)_INCLUDE_DIR and POPPLER_LIBRARIES variables only in the advanced view
+MARK_AS_ADVANCED(GRAPHVIZ_INCLUDE_DIR GRAPHVIZ_GVC_LIBRARY GRAPHVIZ_CGRAPH_LIBRARY GRAPHVIZ_CDT_LIBRARY)
+
+if (GRAPHVIZ_INCLUDE_DIR AND
+ GRAPHVIZ_CDT_LIBRARY AND
+ GRAPHVIZ_CGRAPH_LIBRARY AND
+ GRAPHVIZ_GVC_LIBRARY)
+ set(GRAPHVIZ_FOUND TRUE)
+ set(GRAPHVIZ_LIBRARIES
+ ${GRAPHVIZ_CDT_LIBRARY}
+ ${GRAPHVIZ_CGRAPH_LIBRARY}
+ ${GRAPHVIZ_GVC_LIBRARY})
+else ()
+ set(GRAPHVIZ_FOUND FALSE)
+endif ()
diff --git a/cmake/FindSSE.cmake b/cmake/FindSSE.cmake
index 26ad717..9cd131e 100644
--- a/cmake/FindSSE.cmake
+++ b/cmake/FindSSE.cmake
@@ -86,7 +86,7 @@ ELSEIF(CMAKE_SYSTEM_NAME MATCHES "Darwin")
ENDIF (SSE41_TRUE)
ELSEIF(CMAKE_SYSTEM_NAME MATCHES "Windows")
# TODO
- set(SSE2_FOUND true CACHE BOOL "SSE2 available on host")
+ set(SSE2_FOUND false CACHE BOOL "SSE2 available on host")
set(SSE3_FOUND false CACHE BOOL "SSE3 available on host")
set(SSSE3_FOUND false CACHE BOOL "SSSE3 available on host")
set(SSE4_1_FOUND false CACHE BOOL "SSE4.1 available on host")
diff --git a/cmake/GPL-2 b/cmake/GPL-2
new file mode 100644
index 0000000..d159169
--- /dev/null
+++ b/cmake/GPL-2
@@ -0,0 +1,339 @@
+ GNU GENERAL PUBLIC LICENSE
+ Version 2, June 1991
+
+ Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+ Preamble
+
+ The licenses for most software are designed to take away your
+freedom to share and change it. By contrast, the GNU General Public
+License is intended to guarantee your freedom to share and change free
+software--to make sure the software is free for all its users. This
+General Public License applies to most of the Free Software
+Foundation's software and to any other program whose authors commit to
+using it. (Some other Free Software Foundation software is covered by
+the GNU Lesser General Public License instead.) You can apply it to
+your programs, too.
+
+ When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+this service if you wish), that you receive source code or can get it
+if you want it, that you can change the software or use pieces of it
+in new free programs; and that you know you can do these things.
+
+ To protect your rights, we need to make restrictions that forbid
+anyone to deny you these rights or to ask you to surrender the rights.
+These restrictions translate to certain responsibilities for you if you
+distribute copies of the software, or if you modify it.
+
+ For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must give the recipients all the rights that
+you have. You must make sure that they, too, receive or can get the
+source code. And you must show them these terms so they know their
+rights.
+
+ We protect your rights with two steps: (1) copyright the software, and
+(2) offer you this license which gives you legal permission to copy,
+distribute and/or modify the software.
+
+ Also, for each author's protection and ours, we want to make certain
+that everyone understands that there is no warranty for this free
+software. If the software is modified by someone else and passed on, we
+want its recipients to know that what they have is not the original, so
+that any problems introduced by others will not reflect on the original
+authors' reputations.
+
+ Finally, any free program is threatened constantly by software
+patents. We wish to avoid the danger that redistributors of a free
+program will individually obtain patent licenses, in effect making the
+program proprietary. To prevent this, we have made it clear that any
+patent must be licensed for everyone's free use or not licensed at all.
+
+ The precise terms and conditions for copying, distribution and
+modification follow.
+
+ GNU GENERAL PUBLIC LICENSE
+ TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+ 0. This License applies to any program or other work which contains
+a notice placed by the copyright holder saying it may be distributed
+under the terms of this General Public License. The "Program", below,
+refers to any such program or work, and a "work based on the Program"
+means either the Program or any derivative work under copyright law:
+that is to say, a work containing the Program or a portion of it,
+either verbatim or with modifications and/or translated into another
+language. (Hereinafter, translation is included without limitation in
+the term "modification".) Each licensee is addressed as "you".
+
+Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope. The act of
+running the Program is not restricted, and the output from the Program
+is covered only if its contents constitute a work based on the
+Program (independent of having been made by running the Program).
+Whether that is true depends on what the Program does.
+
+ 1. You may copy and distribute verbatim copies of the Program's
+source code as you receive it, in any medium, provided that you
+conspicuously and appropriately publish on each copy an appropriate
+copyright notice and disclaimer of warranty; keep intact all the
+notices that refer to this License and to the absence of any warranty;
+and give any other recipients of the Program a copy of this License
+along with the Program.
+
+You may charge a fee for the physical act of transferring a copy, and
+you may at your option offer warranty protection in exchange for a fee.
+
+ 2. You may modify your copy or copies of the Program or any portion
+of it, thus forming a work based on the Program, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+ a) You must cause the modified files to carry prominent notices
+ stating that you changed the files and the date of any change.
+
+ b) You must cause any work that you distribute or publish, that in
+ whole or in part contains or is derived from the Program or any
+ part thereof, to be licensed as a whole at no charge to all third
+ parties under the terms of this License.
+
+ c) If the modified program normally reads commands interactively
+ when run, you must cause it, when started running for such
+ interactive use in the most ordinary way, to print or display an
+ announcement including an appropriate copyright notice and a
+ notice that there is no warranty (or else, saying that you provide
+ a warranty) and that users may redistribute the program under
+ these conditions, and telling the user how to view a copy of this
+ License. (Exception: if the Program itself is interactive but
+ does not normally print such an announcement, your work based on
+ the Program is not required to print an announcement.)
+
+These requirements apply to the modified work as a whole. If
+identifiable sections of that work are not derived from the Program,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works. But when you
+distribute the same sections as part of a whole which is a work based
+on the Program, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Program.
+
+In addition, mere aggregation of another work not based on the Program
+with the Program (or with a work based on the Program) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+ 3. You may copy and distribute the Program (or a work based on it,
+under Section 2) in object code or executable form under the terms of
+Sections 1 and 2 above provided that you also do one of the following:
+
+ a) Accompany it with the complete corresponding machine-readable
+ source code, which must be distributed under the terms of Sections
+ 1 and 2 above on a medium customarily used for software interchange; or,
+
+ b) Accompany it with a written offer, valid for at least three
+ years, to give any third party, for a charge no more than your
+ cost of physically performing source distribution, a complete
+ machine-readable copy of the corresponding source code, to be
+ distributed under the terms of Sections 1 and 2 above on a medium
+ customarily used for software interchange; or,
+
+ c) Accompany it with the information you received as to the offer
+ to distribute corresponding source code. (This alternative is
+ allowed only for noncommercial distribution and only if you
+ received the program in object code or executable form with such
+ an offer, in accord with Subsection b above.)
+
+The source code for a work means the preferred form of the work for
+making modifications to it. For an executable work, complete source
+code means all the source code for all modules it contains, plus any
+associated interface definition files, plus the scripts used to
+control compilation and installation of the executable. However, as a
+special exception, the source code distributed need not include
+anything that is normally distributed (in either source or binary
+form) with the major components (compiler, kernel, and so on) of the
+operating system on which the executable runs, unless that component
+itself accompanies the executable.
+
+If distribution of executable or object code is made by offering
+access to copy from a designated place, then offering equivalent
+access to copy the source code from the same place counts as
+distribution of the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+ 4. You may not copy, modify, sublicense, or distribute the Program
+except as expressly provided under this License. Any attempt
+otherwise to copy, modify, sublicense or distribute the Program is
+void, and will automatically terminate your rights under this License.
+However, parties who have received copies, or rights, from you under
+this License will not have their licenses terminated so long as such
+parties remain in full compliance.
+
+ 5. You are not required to accept this License, since you have not
+signed it. However, nothing else grants you permission to modify or
+distribute the Program or its derivative works. These actions are
+prohibited by law if you do not accept this License. Therefore, by
+modifying or distributing the Program (or any work based on the
+Program), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Program or works based on it.
+
+ 6. Each time you redistribute the Program (or any work based on the
+Program), the recipient automatically receives a license from the
+original licensor to copy, distribute or modify the Program subject to
+these terms and conditions. You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties to
+this License.
+
+ 7. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Program at all. For example, if a patent
+license would not permit royalty-free redistribution of the Program by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Program.
+
+If any portion of this section is held invalid or unenforceable under
+any particular circumstance, the balance of the section is intended to
+apply and the section as a whole is intended to apply in other
+circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system, which is
+implemented by public license practices. Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+ 8. If the distribution and/or use of the Program is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Program under this License
+may add an explicit geographical distribution limitation excluding
+those countries, so that distribution is permitted only in or among
+countries not thus excluded. In such case, this License incorporates
+the limitation as if written in the body of this License.
+
+ 9. The Free Software Foundation may publish revised and/or new versions
+of the General Public License from time to time. Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+Each version is given a distinguishing version number. If the Program
+specifies a version number of this License which applies to it and "any
+later version", you have the option of following the terms and conditions
+either of that version or of any later version published by the Free
+Software Foundation. If the Program does not specify a version number of
+this License, you may choose any version ever published by the Free Software
+Foundation.
+
+ 10. If you wish to incorporate parts of the Program into other free
+programs whose distribution conditions are different, write to the author
+to ask for permission. For software which is copyrighted by the Free
+Software Foundation, write to the Free Software Foundation; we sometimes
+make exceptions for this. Our decision will be guided by the two goals
+of preserving the free status of all derivatives of our free software and
+of promoting the sharing and reuse of software generally.
+
+ NO WARRANTY
+
+ 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
+FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
+OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
+PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
+OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
+TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
+PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
+REPAIR OR CORRECTION.
+
+ 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
+REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
+INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
+OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
+TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
+YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
+PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGES.
+
+ END OF TERMS AND CONDITIONS
+
+ How to Apply These Terms to Your New Programs
+
+ If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+ To do so, attach the following notices to the program. It is safest
+to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+ <one line to give the program's name and a brief idea of what it does.>
+ Copyright (C) <year> <name of author>
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with this program; if not, write to the Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+
+Also add information on how to contact you by electronic and paper mail.
+
+If the program is interactive, make it output a short notice like this
+when it starts in an interactive mode:
+
+ Gnomovision version 69, Copyright (C) year name of author
+ Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+ This is free software, and you are welcome to redistribute it
+ under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License. Of course, the commands you use may
+be called something other than `show w' and `show c'; they could even be
+mouse-clicks or menu items--whatever suits your program.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the program, if
+necessary. Here is a sample; alter the names:
+
+ Yoyodyne, Inc., hereby disclaims all copyright interest in the program
+ `Gnomovision' (which makes passes at compilers) written by James Hacker.
+
+ <signature of Ty Coon>, 1 April 1989
+ Ty Coon, President of Vice
+
+This General Public License does not permit incorporating your program into
+proprietary programs. If your program is a subroutine library, you may
+consider it more useful to permit linking proprietary applications with the
+library. If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.
diff --git a/cmake/GazeboTestUtils.cmake b/cmake/GazeboTestUtils.cmake
index b5d91ca..7a44e2a 100644
--- a/cmake/GazeboTestUtils.cmake
+++ b/cmake/GazeboTestUtils.cmake
@@ -10,10 +10,14 @@ macro (gz_build_tests)
if(USE_LOW_MEMORY_TESTS)
add_definitions(-DUSE_LOW_MEMORY_TESTS=1)
endif(USE_LOW_MEMORY_TESTS)
- add_executable(${BINARY_NAME} ${GTEST_SOURCE_file} ${GZ_BUILD_TESTS_EXTRA_EXE_SRCS})
+ add_executable(${BINARY_NAME} ${GTEST_SOURCE_file}
+ ${GZ_BUILD_TESTS_EXTRA_EXE_SRCS})
+
+ link_directories(${PROJECT_BINARY_DIR}/test)
add_dependencies(${BINARY_NAME}
- gtest gtest_main
+ gtest
+ gtest_main
gazebo_common
gazebo_math
gazebo_physics
@@ -21,29 +25,34 @@ macro (gz_build_tests)
gazebo_rendering
gazebo_msgs
gazebo_transport
- server_fixture
+ gazebo_test_fixture
)
target_link_libraries(${BINARY_NAME}
- libgtest.a
- libgtest_main.a
- ${CMAKE_BINARY_DIR}/test/libserver_fixture.a
- gazebo_common
- gazebo_math
+ # This two libraries are need to workaround on bug
+ # https://bitbucket.org/osrf/gazebo/issue/1516
gazebo_physics
gazebo_sensors
- gazebo_rendering
- gazebo_msgs
- gazebo_transport
+ # libgazebo will bring all most of gazebo libraries as dependencies
libgazebo
- pthread
+ gazebo_test_fixture
+ gtest
+ gtest_main
)
add_test(${BINARY_NAME} ${CMAKE_CURRENT_BINARY_DIR}/${BINARY_NAME}
--gtest_output=xml:${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml)
- set_tests_properties(${BINARY_NAME} PROPERTIES TIMEOUT 240)
+ 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.
@@ -84,16 +93,13 @@ if (VALID_DISPLAY)
)
target_link_libraries(${BINARY_NAME}
- gazebo_gui
- gazebo_common
- gazebo_math
+ # This two libraries are need to workaround on bug
+ # https://bitbucket.org/osrf/gazebo/issue/1516
gazebo_physics
gazebo_sensors
- gazebo_rendering
- gazebo_msgs
- gazebo_transport
+ # gazebo_gui and libgazebo will bring all most of gazebo libraries as dependencies
libgazebo
- pthread
+ gazebo_gui
${QT_QTTEST_LIBRARY}
${QT_LIBRARIES}
)
diff --git a/cmake/GazeboUtils.cmake b/cmake/GazeboUtils.cmake
index e5e0603..495f06c 100644
--- a/cmake/GazeboUtils.cmake
+++ b/cmake/GazeboUtils.cmake
@@ -95,7 +95,8 @@ endmacro (BUILD_WARNING)
#################################################
macro (gz_add_library _name)
- add_library(${_name} SHARED ${ARGN})
+ # Not defining STATIC or SHARED will use BUILD_SHARED_LIBS variable
+ add_library(${_name} ${ARGN})
target_link_libraries (${_name} ${general_libraries})
endmacro ()
@@ -125,10 +126,44 @@ endmacro ()
#################################################
macro (gz_setup_unix)
+ # Using dynamic linking in UNIX by default
+ set(BUILD_SHARED_LIBS TRUE)
endmacro()
#################################################
macro (gz_setup_windows)
+ # Using static linking in Windows by default
+ set(BUILD_SHARED_LIBS FALSE)
+ add_definitions(-DBUILDING_STATIC_LIBS -DWIN32_LEAN_AND_MEAN)
+
+ # Need for M_PI constant
+ add_definitions(-D_USE_MATH_DEFINES)
+
+ # Don't pull in the Windows min/max macros
+ add_definitions(-DNOMINMAX)
+
+ #use static libraries for FREEIMAGE
+ add_definitions(-DFREEIMAGE_LIB)
+
+ # Use dynamic linking for boost
+ add_definitions(-DBOOST_ALL_DYN_LINK)
+
+ # And force linking to MSVC dynamic runtime
+ if ("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "DEBUG")
+ add_definitions("/MDd")
+ else()
+ add_definitions("/MD")
+ endif()
+
+ # And we want exceptions
+ add_definitions("/EHsc")
+
+ if (MSVC AND CMAKE_SIZEOF_VOID_P EQUAL 8)
+ # Not need if proper cmake gnerator (-G "...Win64") is passed to cmake
+ # Enable as a second measure to workaround over bug
+ # http://www.cmake.org/Bug/print_bug_page.php?bug_id=11240
+ set(CMAKE_SHARED_LINKER_FLAGS "/machine:x64")
+ endif()
endmacro()
#################################################
@@ -151,6 +186,14 @@ macro (gz_setup_apple)
add_definitions(-DMAC_OS_X_VERSION=0)
endif ()
+ # libstdc++ used on 10.8 and earlier
+ # libc++ after that
+ if (${CMAKE_SYSTEM_VERSION} LESS 13)
+ set (APPLE_PKGCONFIG_LIBS "${APPLE_PKGCONFIG_LIBS} -lstdc++")
+ else()
+ set (APPLE_PKGCONFIG_LIBS "${APPLE_PKGCONFIG_LIBS} -lc++")
+ endif()
+
set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,-undefined -Wl,dynamic_lookup")
endmacro()
diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake
index bc03409..0bfe805 100644
--- a/cmake/SearchForStuff.cmake
+++ b/cmake/SearchForStuff.cmake
@@ -42,17 +42,29 @@ if (NOT PROTOBUF_PROTOC_LIBRARY)
BUILD_ERROR ("Missing: Google Protobuf Compiler Library (libprotoc-dev)")
endif()
+if ("${CMAKE_BUILD_TYPE}" STREQUAL "Debug")
+ set (GZ_PROTOBUF_LIBRARY ${PROTOBUF_LIBRARY_DEBUG})
+ set (GZ_PROTOBUF_PROTOC_LIBRARY ${PROTOBUF_PROTOC_LIBRARY_DEBUG})
+else()
+ set (GZ_PROTOBUF_LIBRARY ${PROTOBUF_LIBRARY})
+ set (GZ_PROTOBUF_PROTOC_LIBRARY ${PROTOBUF_PROTOC_LIBRARY})
+endif()
+
########################################
include (FindOpenGL)
if (NOT OPENGL_FOUND)
BUILD_ERROR ("Missing: OpenGL")
else ()
- APPEND_TO_CACHED_LIST(gazeboserver_include_dirs
- ${gazeboserver_include_dirs_desc}
- ${OPENGL_INCLUDE_DIR})
- APPEND_TO_CACHED_LIST(gazeboserver_link_libs
- ${gazeboserver_link_libs_desc}
- ${OPENGL_LIBRARIES})
+ if (OPENGL_INCLUDE_DIR)
+ APPEND_TO_CACHED_LIST(gazeboserver_include_dirs
+ ${gazeboserver_include_dirs_desc}
+ ${OPENGL_INCLUDE_DIR})
+ endif()
+ if (OPENGL_LIBRARIES)
+ APPEND_TO_CACHED_LIST(gazeboserver_link_libs
+ ${gazeboserver_link_libs_desc}
+ ${OPENGL_LIBRARIES})
+ endif()
endif ()
########################################
@@ -65,7 +77,23 @@ else ()
endif ()
########################################
+include (FindHDF5)
+find_package(HDF5)
+
+if (NOT HDF5_FOUND)
+ BUILD_WARNING("HDF5 not found")
+else ()
+ message(STATUS "HDF5 Found")
+endif ()
+########################################
# Find packages
+
+# In Visual Studio we use configure.bat to trick all path cmake
+# variables so let's consider that as a replacement for pkgconfig
+if (MSVC)
+ set (PKG_CONFIG_FOUND TRUE)
+endif()
+
if (PKG_CONFIG_FOUND)
pkg_check_modules(CURL libcurl)
if (NOT CURL_FOUND)
@@ -105,7 +133,7 @@ if (PKG_CONFIG_FOUND)
set(SimTK_INSTALL_DIR ${SimTK_INSTALL_PREFIX})
#list(APPEND CMAKE_MODULE_PATH ${SimTK_INSTALL_PREFIX}/share/cmake)
find_package(Simbody)
- if (SIMBODY_FOUND)
+ if (Simbody_FOUND)
set (HAVE_SIMBODY TRUE)
else()
BUILD_WARNING ("Simbody not found, for simbody physics engine option, please install libsimbody-dev.")
@@ -114,7 +142,7 @@ if (PKG_CONFIG_FOUND)
#################################################
# Find DART
- find_package(DARTCore 4.1 QUIET)
+ find_package(DARTCore 4.3.3 QUIET)
if (DARTCore_FOUND)
message (STATUS "Looking for DARTCore - found")
set (HAVE_DART TRUE)
@@ -126,56 +154,85 @@ if (PKG_CONFIG_FOUND)
#################################################
# Find tinyxml. Only debian distributions package tinyxml with a pkg-config
- # Use pkg_check_modules and fallback to manual detection (needed, at least, for MacOS)
- pkg_check_modules(tinyxml tinyxml)
- if (NOT tinyxml_FOUND)
- find_path (tinyxml_INCLUDE_DIRS tinyxml.h ${tinyxml_INCLUDE_DIRS} ENV CPATH)
- find_library(tinyxml_LIBRARIES NAMES tinyxml)
- set (tinyxml_FAIL False)
- if (NOT tinyxml_INCLUDE_DIRS)
- message (STATUS "Looking for tinyxml headers - not found")
- set (tinyxml_FAIL True)
- endif()
- if (NOT tinyxml_LIBRARIES)
- message (STATUS "Looking for tinyxml library - not found")
- set (tinyxml_FAIL True)
- endif()
+ # Use pkg_check_modules and fallback to manual detection
+ # (needed, at least, for MacOS)
+
+ # Use system installation on UNIX and Apple, and internal copy on Windows
+ if (UNIX OR APPLE)
+ message (STATUS "Using system tinyxml.")
+ set (USE_EXTERNAL_TINYXML True)
+ elseif(WIN32)
+ message (STATUS "Using internal tinyxml.")
+ set (USE_EXTERNAL_TINYXML False)
+ add_definitions(-DTIXML_USE_STL)
+ else()
+ message (STATUS "Unknown platform, unable to configure tinyxml.")
+ BUILD_ERROR("Unknown platform")
endif()
- if (tinyxml_FAIL)
- message (STATUS "Looking for tinyxml.h - not found")
- BUILD_ERROR("Missing: tinyxml")
+ if (USE_EXTERNAL_TINYXML)
+ pkg_check_modules(tinyxml tinyxml)
+ if (NOT tinyxml_FOUND)
+ find_path (tinyxml_INCLUDE_DIRS tinyxml.h ${tinyxml_INCLUDE_DIRS} ENV CPATH)
+ find_library(tinyxml_LIBRARIES NAMES tinyxml)
+ set (tinyxml_FAIL False)
+ if (NOT tinyxml_INCLUDE_DIRS)
+ message (STATUS "Looking for tinyxml headers - not found")
+ set (tinyxml_FAIL True)
+ endif()
+ if (NOT tinyxml_LIBRARIES)
+ message (STATUS "Looking for tinyxml library - not found")
+ set (tinyxml_FAIL True)
+ endif()
+ endif()
+
+ if (tinyxml_FAIL)
+ message (STATUS "Looking for tinyxml.h - not found")
+ BUILD_ERROR("Missing: tinyxml")
+ endif()
+ else()
+ # Needed in WIN32 since in UNIX the flag is added in the code installed
+ message (STATUS "Skipping search for tinyxml")
+ set (tinyxml_INCLUDE_DIRS "")
+ set (tinyxml_LIBRARIES "")
+ set (tinyxml_LIBRARY_DIRS "")
endif()
- #################################################
- # Find libtar.
- find_path (libtar_INCLUDE_DIRS libtar.h)
- find_library(libtar_LIBRARIES tar)
- set (LIBTAR_FOUND True)
-
- if (NOT libtar_INCLUDE_DIRS)
- message (STATUS "Looking for libtar.h - not found")
- set (LIBTAR_FOUND False)
- else ()
- message (STATUS "Looking for libtar.h - found")
- include_directories(${libtar_INCLUDE_DIRS})
- endif ()
- if (NOT libtar_LIBRARIES)
- message (STATUS "Looking for libtar.so - not found")
- set (LIBTAR_FOUND False)
- else ()
- message (STATUS "Looking for libtar.so - found")
- endif ()
+ if (NOT WIN32)
+ #################################################
+ # Find libtar.
+ find_path (libtar_INCLUDE_DIRS libtar.h)
+ find_library(libtar_LIBRARIES tar)
+ set (LIBTAR_FOUND True)
+
+ if (NOT libtar_INCLUDE_DIRS)
+ message (STATUS "Looking for libtar.h - not found")
+ set (LIBTAR_FOUND False)
+ else ()
+ message (STATUS "Looking for libtar.h - found")
+ include_directories(${libtar_INCLUDE_DIRS})
+ endif ()
+ if (NOT libtar_LIBRARIES)
+ message (STATUS "Looking for libtar.so - not found")
+ set (LIBTAR_FOUND False)
+ else ()
+ message (STATUS "Looking for libtar.so - found")
+ endif ()
- if (NOT LIBTAR_FOUND)
- BUILD_ERROR("Missing: libtar")
+ if (NOT LIBTAR_FOUND)
+ BUILD_ERROR("Missing: libtar")
+ endif()
+ else()
+ set(libtar_LIBRARIES "")
endif()
#################################################
# Find TBB
pkg_check_modules(TBB tbb)
+ set (TBB_PKG_CONFIG "tbb")
if (NOT TBB_FOUND)
message(STATUS "TBB not found, attempting to detect manually")
+ set (TBB_PKG_CONFIG "")
find_library(tbb_library tbb ENV LD_LIBRARY_PATH)
if (tbb_library)
@@ -188,9 +245,23 @@ if (PKG_CONFIG_FOUND)
#################################################
# Find OGRE
- execute_process(COMMAND pkg-config --modversion OGRE
- OUTPUT_VARIABLE OGRE_VERSION)
- string(REPLACE "\n" "" OGRE_VERSION ${OGRE_VERSION})
+ # On Windows, we assume that all the OGRE* defines are passed in manually
+ # to CMake.
+ if (NOT WIN32)
+ execute_process(COMMAND pkg-config --modversion OGRE
+ OUTPUT_VARIABLE OGRE_VERSION)
+ string(REPLACE "\n" "" OGRE_VERSION ${OGRE_VERSION})
+
+ string (REGEX REPLACE "^([0-9]+).*" "\\1"
+ OGRE_MAJOR_VERSION "${OGRE_VERSION}")
+ string (REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1"
+ OGRE_MINOR_VERSION "${OGRE_VERSION}")
+ string (REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1"
+ OGRE_PATCH_VERSION ${OGRE_VERSION})
+
+ set(OGRE_VERSION
+ ${OGRE_MAJOR_VERSION}.${OGRE_MINOR_VERSION}.${OGRE_PATCH_VERSION})
+ endif()
pkg_check_modules(OGRE-RTShaderSystem
OGRE-RTShaderSystem>=${MIN_OGRE_VERSION})
@@ -244,14 +315,18 @@ if (PKG_CONFIG_FOUND)
# Also find OGRE's plugin directory, which is provided in its .pc file as the
# `plugindir` variable. We have to call pkg-config manually to get it.
- execute_process(COMMAND pkg-config --variable=plugindir OGRE
- OUTPUT_VARIABLE _pkgconfig_invoke_result
- RESULT_VARIABLE _pkgconfig_failed)
- if(_pkgconfig_failed)
- BUILD_WARNING ("Failed to find OGRE's plugin directory. The build will succeed, but gazebo will likely fail to run.")
- else()
- # This variable will be substituted into cmake/setup.sh.in
- set (OGRE_PLUGINDIR ${_pkgconfig_invoke_result})
+ # On Windows, we assume that all the OGRE* defines are passed in manually
+ # to CMake.
+ if (NOT WIN32)
+ execute_process(COMMAND pkg-config --variable=plugindir OGRE
+ OUTPUT_VARIABLE _pkgconfig_invoke_result
+ RESULT_VARIABLE _pkgconfig_failed)
+ if(_pkgconfig_failed)
+ BUILD_WARNING ("Failed to find OGRE's plugin directory. The build will succeed, but gazebo will likely fail to run.")
+ else()
+ # This variable will be substituted into cmake/setup.sh.in
+ set (OGRE_PLUGINDIR ${_pkgconfig_invoke_result})
+ endif()
endif()
########################################
@@ -260,6 +335,7 @@ if (PKG_CONFIG_FOUND)
if (NOT CCD_FOUND)
message(STATUS "Using internal copy of libccd")
set(CCD_INCLUDE_DIRS "${CMAKE_SOURCE_DIR}/deps/libccd/include")
+ set(CCD_LIBRARY_DIRS "${CMAKE_BINARY_DIR}/deps/libccd")
set(CCD_LIBRARIES gazebo_ccd)
endif()
@@ -363,6 +439,36 @@ if (PKG_CONFIG_FOUND)
BUILD_WARNING ("Bullet > 2.82 not found, for bullet physics engine option, please install libbullet2.82-dev.")
endif()
+ if (BULLET_VERSION VERSION_GREATER 2.82)
+ add_definitions( -DLIBBULLET_VERSION_GT_282 )
+ endif()
+
+ ########################################
+ # Find libusb
+ pkg_check_modules(libusb-1.0 libusb-1.0)
+ if (NOT libusb-1.0_FOUND)
+ BUILD_WARNING ("libusb-1.0 not found. USB peripherals support will be disabled.")
+ set (HAVE_USB OFF CACHE BOOL "HAVE USB" FORCE)
+ else()
+ message (STATUS "Looking for libusb-1.0 - found. USB peripherals support enabled.")
+ set (HAVE_USB ON CACHE BOOL "HAVE USB" FORCE)
+ include_directories(${libusb-1.0_INCLUDE_DIRS})
+ link_directories(${libusb-1.0_LIBRARY_DIRS})
+ endif ()
+
+ #################################################
+ # Find Oculus SDK.
+ pkg_check_modules(OculusVR OculusVR)
+
+ if (HAVE_USB AND OculusVR_FOUND)
+ message (STATUS "Oculus Rift support enabled.")
+ set (HAVE_OCULUS ON CACHE BOOL "HAVE OCULUS" FORCE)
+ include_directories(SYSTEM ${OculusVR_INCLUDE_DIRS})
+ link_directories(${OculusVR_LIBRARY_DIRS})
+ else ()
+ BUILD_WARNING ("Oculus Rift support will be disabled.")
+ set (HAVE_OCULUS OFF CACHE BOOL "HAVE OCULUS" FORCE)
+ endif()
else (PKG_CONFIG_FOUND)
set (BUILD_GAZEBO OFF CACHE INTERNAL "Build Gazebo" FORCE)
BUILD_ERROR ("Error: pkg-config not found")
@@ -370,10 +476,12 @@ endif ()
########################################
# Find SDFormat
-find_package(SDFormat 2.1.0)
+set (SDFormat_MIN_VERSION 3.0.0)
+find_package(SDFormat ${SDFormat_MIN_VERSION})
+
if (NOT SDFormat_FOUND)
message (STATUS "Looking for SDFormat - not found")
- BUILD_ERROR ("Missing: SDF version >=2.1.0. Required for reading and writing SDF files.")
+ BUILD_ERROR ("Missing: SDF version >=${SDFormat_MIN_VERSION}. Required for reading and writing SDF files.")
else()
message (STATUS "Looking for SDFormat - found")
endif()
@@ -410,6 +518,7 @@ find_library(libdl_library dl /usr/lib /usr/local/lib)
if (NOT libdl_library)
message (STATUS "Looking for libdl - not found")
BUILD_ERROR ("Missing libdl: Required for plugins.")
+ set(libdl_library "")
else (NOT libdl_library)
message (STATUS "Looking for libdl - found")
endif ()
@@ -427,33 +536,6 @@ else ()
endif ()
########################################
-# Find libusb
-pkg_check_modules(libusb-1.0 libusb-1.0)
-if (NOT libusb-1.0_FOUND)
- BUILD_WARNING ("libusb-1.0 not found. USB peripherals support will be disabled.")
- set (HAVE_USB OFF CACHE BOOL "HAVE USB" FORCE)
-else()
- message (STATUS "Looking for libusb-1.0 - found. USB peripherals support enabled.")
- set (HAVE_USB ON CACHE BOOL "HAVE USB" FORCE)
- include_directories(${libusb-1.0_INCLUDE_DIRS})
- link_directories(${libusb-1.0_LIBRARY_DIRS})
-endif ()
-
-#################################################
-# Find Oculus SDK.
-pkg_check_modules(OculusVR OculusVR)
-
-if (HAVE_USB AND OculusVR_FOUND)
- message (STATUS "Oculus Rift support enabled.")
- set (HAVE_OCULUS ON CACHE BOOL "HAVE OCULUS" FORCE)
- include_directories(SYSTEM ${OculusVR_INCLUDE_DIRS})
- link_directories(${OculusVR_LIBRARY_DIRS})
-else ()
- BUILD_WARNING ("Oculus Rift support will be disabled.")
- set (HAVE_OCULUS OFF CACHE BOOL "HAVE OCULUS" FORCE)
-endif()
-
-########################################
# Include man pages stuff
include (${gazebo_cmake_dir}/Ronn2Man.cmake)
include (${gazebo_cmake_dir}/Man.cmake)
@@ -472,6 +554,67 @@ else()
endif()
########################################
+# Find xsltproc, which is used by tools/check_test_ran.py
+find_program(XSLTPROC xsltproc)
+if (NOT EXISTS ${XSLTPROC})
+ BUILD_WARNING("xsltproc not found. The check_test_ran.py script will cause tests to fail.")
+endif()
+
+########################################
+# Find uuid-dev Library
+#pkg_check_modules(uuid uuid)
+#if (uuid_FOUND)
+# message (STATUS "Looking for uuid - found")
+# set (HAVE_UUID TRUE)
+#else ()
+# set (HAVE_UUID FALSE)
+# BUILD_WARNING ("uuid-dev library not found - Gazebo will not have uuid support.")
+#endif ()
+
+########################################
+# Find uuid
+# - In UNIX we use uuid library.
+# - In Windows the native RPC call, no dependency needed.
+if (UNIX)
+ pkg_check_modules(uuid uuid)
+ if (uuid_FOUND)
+ message (STATUS "Looking for uuid - found")
+ set (HAVE_UUID TRUE)
+ else ()
+ set (HAVE_UUID FALSE)
+ BUILD_WARNING ("uuid-dev library not found - Gazebo will not have uuid support.")
+ endif ()
+else()
+ message (STATUS "Using Windows RPC UuidCreate function")
+ set (HAVE_UUID TRUE)
+endif()
+
+########################################
+# Find graphviz
+include (${gazebo_cmake_dir}/FindGraphviz.cmake)
+if (NOT GRAPHVIZ_FOUND)
+ message (STATUS "Looking for libgraphviz-dev - not found")
+ BUILD_WARNING ("Graphviz not found, Model editor's schematic view will be disabled.")
+ set (HAVE_GRAPHVIZ OFF CACHE BOOL "HAVE GRAPHVIZ" FORCE)
+else ()
+ message (STATUS "Looking for libgraphviz-dev - found")
+ set (HAVE_GRAPHVIZ ON CACHE BOOL "HAVE GRAPHVIZ" FORCE)
+endif ()
+
+########################################
+# Find ignition math in unix platforms
+# In Windows we expect a call from configure.bat script with the paths
+if (NOT WIN32)
+ find_package(ignition-math2 QUIET)
+ if (NOT ignition-math2_FOUND)
+ message(STATUS "Looking for ignition-math2-config.cmake - not found")
+ BUILD_ERROR ("Missing: Ignition math2 library.")
+ else()
+ message(STATUS "Looking for ignition-math2-config.cmake - found")
+ endif()
+endif()
+
+########################################
# Find QWT (QT graphing library)
#find_path(QWT_INCLUDE_DIR NAMES qwt.h PATHS
# /usr/include
diff --git a/cmake/gazebo-config.cmake.in b/cmake/gazebo-config.cmake.in
index 25f3bea..48c08f4 100644
--- a/cmake/gazebo-config.cmake.in
+++ b/cmake/gazebo-config.cmake.in
@@ -2,6 +2,9 @@ if (@PKG_NAME at _CONFIG_INCLUDED)
return()
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)
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@)
@@ -39,7 +42,7 @@ execute_process(COMMAND pkg-config --variable=prefix OGRE
# Add the OGRE cmake path to CMAKE_MODULE_PATH
set(CMAKE_MODULE_PATH
- "${OGRE_INSTALL_PREFIX}/share/OGRE/cmake/modules;${OGRE_INSTALL_PREFIX}/CMake;${CMAKE_MODULE_PATH}")
+ "${OGRE_INSTALL_PREFIX}/share/OGRE/cmake/modules;${OGRE_INSTALL_PREFIX}/lib/OGRE/cmake;${OGRE_INSTALL_PREFIX}/CMake;${CMAKE_MODULE_PATH}")
foreach(dep @PKG_DEPENDS@)
if(NOT ${dep}_FOUND)
@@ -63,9 +66,13 @@ foreach(dep @PKG_DEPENDS@)
list(APPEND GAZEBO_LIBRARIES ${OGRE_Terrain_LIBRARIES}
${OGRE_Paging_LIBRARIES})
endif()
-
endforeach()
+find_package(ignition-math2 REQUIRED)
+list(APPEND @PKG_NAME at _INCLUDE_DIRS ${IGNITION-MATH_INCLUDE_DIRS})
+list(APPEND @PKG_NAME at _LIBRARY_DIRS ${IGNITION-MATH_LIBRARY_DIRS})
+list(APPEND @PKG_NAME at _LIBRARIES ${IGNITION-MATH_LIBRARIES})
+
list(APPEND @PKG_NAME at _LDFLAGS -Wl,-rpath, at CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@/gazebo- at GAZEBO_VERSION@/plugins)
list(APPEND @PKG_NAME at _LDFLAGS -L at CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@)
list(APPEND @PKG_NAME at _LDFLAGS -L at CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@/gazebo- at GAZEBO_VERSION@/plugins)
diff --git a/cmake/gazebo_config.h.in b/cmake/gazebo_config.h.in
index 46425ca..33b11df 100644
--- a/cmake/gazebo_config.h.in
+++ b/cmake/gazebo_config.h.in
@@ -30,6 +30,8 @@
#cmakedefine HAVE_USB 1
#cmakedefine HAVE_OCULUS 1
#cmakedefine HAVE_SPNAV 1
+#cmakedefine HDF5_INSTRUMENT 1
+#cmakedefine HAVE_GRAPHVIZ 1
#ifdef BUILD_TYPE_PROFILE
#include <google/heap-checker.h>
diff --git a/cmake/pkgconfig/gazebo.in b/cmake/pkgconfig/gazebo.in
index 85f0aa6..fa693c6 100644
--- a/cmake/pkgconfig/gazebo.in
+++ b/cmake/pkgconfig/gazebo.in
@@ -5,6 +5,6 @@ includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@
Name: gazebo
Description: Gazebo Libraries
Version: @GAZEBO_VERSION_FULL@
-Requires.private: sdformat protobuf
+Requires: sdformat protobuf @TBB_PKG_CONFIG@ ignition-math2
Libs: -Wl,-rpath,${prefix}/@CMAKE_INSTALL_LIBDIR@/gazebo- at GAZEBO_VERSION@/plugins -L${libdir} -L${prefix}/@CMAKE_INSTALL_LIBDIR@/gazebo- at GAZEBO_VERSION@/plugins -lgazebo_transport -lgazebo_physics -lgazebo_sensors -lgazebo_rendering -lgazebo_gui -lgazebo_msgs -lgazebo_math -lgazebo_common -lgazebo @Boost_PKGCONFIG_LIBS@ @APPLE_PKGCONFIG_LIBS@
-CFlags: -I${includedir}/gazebo- at GAZEBO_VERSION@ @Boost_PKGCONFIG_CFLAGS@
+CFlags: -I${includedir}/gazebo- at GAZEBO_VERSION@ @Boost_PKGCONFIG_CFLAGS@ -std=c++11
diff --git a/cmake/setup.sh.in b/cmake/setup.sh.in
index e8d0765..ec472f6 100644
--- a/cmake/setup.sh.in
+++ b/cmake/setup.sh.in
@@ -1,7 +1,7 @@
export GAZEBO_MASTER_URI=http://@GAZEBO_DEFAULT_MASTER_HOST@:@GAZEBO_DEFAULT_MASTER_PORT@
export GAZEBO_MODEL_DATABASE_URI=@GAZEBO_MODEL_DATABASE_URI@
-export GAZEBO_RESOURCE_PATH=@GAZEBO_RESOURCE_PATH@
-export GAZEBO_PLUGIN_PATH=@GAZEBO_PLUGIN_PATH@
-export GAZEBO_MODEL_PATH=@GAZEBO_MODEL_PATH@
+export GAZEBO_RESOURCE_PATH=@GAZEBO_RESOURCE_PATH@:${GAZEBO_RESOURCE_PATH}
+export GAZEBO_PLUGIN_PATH=@GAZEBO_PLUGIN_PATH@:${GAZEBO_PLUGIN_PATH}
+export GAZEBO_MODEL_PATH=@GAZEBO_MODEL_PATH@:${GAZEBO_MODEL_PATH}
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:@GAZEBO_PLUGIN_PATH@
export OGRE_RESOURCE_PATH=@OGRE_RESOURCE_PATH@
diff --git a/configure.bat b/configure.bat
new file mode 100644
index 0000000..52d0a0e
--- /dev/null
+++ b/configure.bat
@@ -0,0 +1,91 @@
+:: This file is a helper for gazebo configuration (cmake) on Windows
+::
+:: It is designed to work with a workspace detailed in the INSTALL_WIN32.md
+:: file. Please follow the instructions there. The workspace layout should be:
+::
+:: gz-ws/
+:: sdformat/
+:: ... # all dependencies detailed in the INSTALL file
+:: #dep1/
+:: #dep2/
+:: ...
+:: gazebo/
+::
+:: Usage: cd gz-ws/gazebo/build && ../configure <Release|Debug>
+ at set build_type=Release
+ at if not "%1"=="" set build_type=%1
+ at echo Configuring for build type %build_type%
+
+ at set BOOST_PATH=%cd%\..\..\boost_1_56_0
+ at set BOOST_LIBRARY_DIR=%BOOST_PATH%\lib64-msvc-12.0
+
+ at set PROTOBUF_PATH=%cd%\..\..\protobuf-2.6.0-win64-vc12
+
+ at set CURL_PATH=%cd%\..\..\libcurl-vc12-x64-release-debug-static-ipv6-sspi-winssl\%build_type%
+ at set CURL_INCLUDE_DIR=%CURL_PATH%\include
+ at set CURL_LIBRARY_DIR=%CURL_PATH%\lib
+set CURL_LIBRARY_NAME=libcurl_a
+ at if "%build_type%"=="Debug" set CURL_LIBRARY_NAME=libcurl_a_debug
+
+ at set FREEIMAGE_PATH=%cd%\..\..\FreeImage-vc12-x64-release-debug
+ at set FREEIMAGE_LIBRARY_DIR=%FREEIMAGE_PATH%\x64\%build_type%\DLL
+ at set FREEIMAGE_INCLUDE_DIR=%FREEIMAGE_PATH%\Source
+
+ at set SDFORMAT_PATH=%cd%\..\..\sdformat\build\install\%build_type%
+ at set IGNITION-MATH_PATH=%cd%\..\..\ign-math\build\install\%build_type%
+
+ at set TBB_PATH=%cd%\..\..\tbb43_20141023oss
+ at set TBB_LIBRARY_DIR=%TBB_PATH%\lib\intel64\vc12
+ at set TBB_INCLUDEDIR=%TBB_PATH%\include
+
+ at set OGRE_VERSION=1.8.1
+ at set OGRE_PATH=%cd%\..\..\ogre_src_v1-8-1-vc12-x64-release-debug\build\install\%build_type%
+ at set OGRE_INCLUDE_DIR=%OGRE_PATH%\include;%OGRE_PATH%\include\OGRE;%OGRE_PATH%\include\OGRE\RTShaderSystem;%OGRE_PATH%\include\OGRE\Terrain;%OGRE_PATH%\include\OGRE\Paging
+ at set OGRE_LIBRARY_DIR=%OGRE_PATH%\lib\%build_type%
+ at set OGRE_PLUGIN_DIR=%OGRE_LIBRARY_DIR%\opt
+set OGRE_LIB_SUFFIX=.lib
+ at if "%build_type%"=="Debug" set OGRE_LIB_SUFFIX=_d.lib
+ at set OGRE_LIBS=%OGRE_LIBRARY_DIR%\OgreMain%OGRE_LIB_SUFFIX%;%OGRE_LIBRARY_DIR%\OgreOverlay%OGRE_LIB_SUFFIX%;%OGRE_LIBRARY_DIR%\OgreRTShaderSystem%OGRE_LIB_SUFFIX%;%OGRE_LIBRARY_DIR%\OgreTerrain%OGRE_LIB_SUFFIX%;%OGRE_LIBRARY_DIR%\OgrePaging%OGRE_LIB_SUFFIX%
+ at set OGRE_LIBS=%OGRE_LIBRARY_DIR%\OgreMain%OGRE_LIB_SUFFIX%;%OGRE_LIBRARY_DIR%\OgreRTShaderSystem%OGRE_LIB_SUFFIX%;%OGRE_LIBRARY_DIR%\OgreTerrain%OGRE_LIB_SUFFIX%;%OGRE_LIBRARY_DIR%\OgrePaging%OGRE_LIB_SUFFIX%
+
+ at set DLFCN_WIN32_PATH=%cd%\..\..\dlfcn-win32-vc12-x64-release-debug\build\install\%build_type%
+ at set DLFCN_WIN32_LIBRARY_DIR=%DLFCN_WIN32_PATH%\lib
+ at set DLFCN_WIN32_INCLUDE_DIR=%DLFCN_WIN32_PATH%\include
+
+ at set QT4_PATH=C:\Qt\4.8.6\x64\msvc2013
+ at set QT4_BIN_DIR=%QT4_PATH%\bin
+
+ at set IGN_MATH_PATH=%cd%\..\..\ign-math\build\install\%build_type%
+
+ at set INCLUDE=%FREEIMAGE_INCLUDE_DIR%;%TBB_INCLUDEDIR%;%DLFCN_WIN32_INCLUDE_DIR%;%INCLUDE%
+ at set LIB=%FREEIMAGE_LIBRARY_DIR%;%BOOST_LIBRARY_DIR%;%TBB_LIBRARY_DIR%;%DLFCN_WIN32_LIBRARY_DIR%;%LIB%
+ at set PATH=%QT4_BIN_DIR%;%PATH%
+
+cmake -G "NMake Makefiles"^
+ -DCMAKE_PREFIX_PATH="%SDFORMAT_PATH%;%IGNITION-MATH_PATH%"^
+ -DUSE_EXTERNAL_TINYXML:BOOL=False^
+ -DFREEIMAGE_RUNS=1^
+ -DPROTOBUF_SRC_ROOT_FOLDER="%PROTOBUF_PATH%"^
+ -DBOOST_ROOT:STRING="%BOOST_PATH%"^
+ -DBOOST_LIBRARYDIR:STRING="%BOOST_LIBRARY_DIR%"^
+ -DOGRE_FOUND=1^
+ -DOGRE-RTShaderSystem_FOUND=1^
+ -DOGRE-Terrain_FOUND=1^
+ -DOGRE_VERSION=%OGRE_VERSION%^
+ -DOGRE_PLUGINDIR="%OGRE_PLUGIN_DIR%"^
+ -DOGRE_INCLUDE_DIRS="%OGRE_INCLUDE_DIR%"^
+ -DOGRE_LIBRARIES="%OGRE_LIBS%"^
+ -DCURL_FOUND=1^
+ -DCURL_INCLUDEDIR="%CURL_INCLUDE_DIR%"^
+ -DCURL_LIBDIR="%CURL_LIBRARY_DIR%"^
+ -DCURL_LIBRARIES="%CURL_LIBRARY_NAME%"^
+ -DTBB_FOUND=1^
+ -DTBB_INCLUDEDIR="%TBB_INCLUDEDIR%"^
+ -DTBB_LIBRARY_DIR="%TBB_LIBRARY_DIR%"^
+ -DIGNITION-MATH_INCLUDE_DIRS:STRING="%IGN_MATH_PATH%\include\ignition\math2"^
+ -DIGNITION-MATH_LIBRARY_DIRS:STRING="%IGN_MATH_PATH%\lib"^
+ -DIGNITION-MATH_LIBRARIES="ignition-math2"^
+ -DCMAKE_INSTALL_PREFIX="install\%build_type%"^
+ -DCMAKE_BUILD_TYPE="%build_type%"^
+ -DENABLE_TESTS_COMPILATION:BOOL=False^
+ ..
diff --git a/deps/ann/include/ann/ANN.h b/deps/ann/include/ann/ANN.h
index b51b57b..8a85b09 100644
--- a/deps/ann/include/ann/ANN.h
+++ b/deps/ann/include/ann/ANN.h
@@ -5,7 +5,7 @@
// neighbor searching.
// Last modified: 01/27/10 (Version 1.1.2)
//----------------------------------------------------------------------
-// Copyright (c) 1997-2010 University of Maryland and Sunil Arya and
+// Copyright (c) 1997-2015 University of Maryland and Sunil Arya and
// David Mount. All Rights Reserved.
//
// This software and related documentation is part of the Approximate
diff --git a/deps/ann/include/ann/ANNx.h b/deps/ann/include/ann/ANNx.h
index d9b2848..f47864f 100644
--- a/deps/ann/include/ann/ANNx.h
+++ b/deps/ann/include/ann/ANNx.h
@@ -11,7 +11,7 @@
//
// Typical users of ANN should not need to access this file.
//----------------------------------------------------------------------
-// Copyright (c) 1997-2010 University of Maryland and Sunil Arya and
+// Copyright (c) 1997-2015 University of Maryland and Sunil Arya and
// David Mount. All Rights Reserved.
//
// This software and related documentation is part of the Approximate
diff --git a/deps/ann/include/ann/LGPL b/deps/ann/include/ann/LGPL
new file mode 100644
index 0000000..65c5ca8
--- /dev/null
+++ b/deps/ann/include/ann/LGPL
@@ -0,0 +1,165 @@
+ GNU LESSER GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+
+ This version of the GNU Lesser General Public License incorporates
+the terms and conditions of version 3 of the GNU General Public
+License, supplemented by the additional permissions listed below.
+
+ 0. Additional Definitions.
+
+ As used herein, "this License" refers to version 3 of the GNU Lesser
+General Public License, and the "GNU GPL" refers to version 3 of the GNU
+General Public License.
+
+ "The Library" refers to a covered work governed by this License,
+other than an Application or a Combined Work as defined below.
+
+ An "Application" is any work that makes use of an interface provided
+by the Library, but which is not otherwise based on the Library.
+Defining a subclass of a class defined by the Library is deemed a mode
+of using an interface provided by the Library.
+
+ A "Combined Work" is a work produced by combining or linking an
+Application with the Library. The particular version of the Library
+with which the Combined Work was made is also called the "Linked
+Version".
+
+ The "Minimal Corresponding Source" for a Combined Work means the
+Corresponding Source for the Combined Work, excluding any source code
+for portions of the Combined Work that, considered in isolation, are
+based on the Application, and not on the Linked Version.
+
+ The "Corresponding Application Code" for a Combined Work means the
+object code and/or source code for the Application, including any data
+and utility programs needed for reproducing the Combined Work from the
+Application, but excluding the System Libraries of the Combined Work.
+
+ 1. Exception to Section 3 of the GNU GPL.
+
+ You may convey a covered work under sections 3 and 4 of this License
+without being bound by section 3 of the GNU GPL.
+
+ 2. Conveying Modified Versions.
+
+ If you modify a copy of the Library, and, in your modifications, a
+facility refers to a function or data to be supplied by an Application
+that uses the facility (other than as an argument passed when the
+facility is invoked), then you may convey a copy of the modified
+version:
+
+ a) under this License, provided that you make a good faith effort to
+ ensure that, in the event an Application does not supply the
+ function or data, the facility still operates, and performs
+ whatever part of its purpose remains meaningful, or
+
+ b) under the GNU GPL, with none of the additional permissions of
+ this License applicable to that copy.
+
+ 3. Object Code Incorporating Material from Library Header Files.
+
+ The object code form of an Application may incorporate material from
+a header file that is part of the Library. You may convey such object
+code under terms of your choice, provided that, if the incorporated
+material is not limited to numerical parameters, data structure
+layouts and accessors, or small macros, inline functions and templates
+(ten or fewer lines in length), you do both of the following:
+
+ a) Give prominent notice with each copy of the object code that the
+ Library is used in it and that the Library and its use are
+ covered by this License.
+
+ b) Accompany the object code with a copy of the GNU GPL and this license
+ document.
+
+ 4. Combined Works.
+
+ You may convey a Combined Work under terms of your choice that,
+taken together, effectively do not restrict modification of the
+portions of the Library contained in the Combined Work and reverse
+engineering for debugging such modifications, if you also do each of
+the following:
+
+ a) Give prominent notice with each copy of the Combined Work that
+ the Library is used in it and that the Library and its use are
+ covered by this License.
+
+ b) Accompany the Combined Work with a copy of the GNU GPL and this license
+ document.
+
+ c) For a Combined Work that displays copyright notices during
+ execution, include the copyright notice for the Library among
+ these notices, as well as a reference directing the user to the
+ copies of the GNU GPL and this license document.
+
+ d) Do one of the following:
+
+ 0) Convey the Minimal Corresponding Source under the terms of this
+ License, and the Corresponding Application Code in a form
+ suitable for, and under terms that permit, the user to
+ recombine or relink the Application with a modified version of
+ the Linked Version to produce a modified Combined Work, in the
+ manner specified by section 6 of the GNU GPL for conveying
+ Corresponding Source.
+
+ 1) Use a suitable shared library mechanism for linking with the
+ Library. A suitable mechanism is one that (a) uses at run time
+ a copy of the Library already present on the user's computer
+ system, and (b) will operate properly with a modified version
+ of the Library that is interface-compatible with the Linked
+ Version.
+
+ e) Provide Installation Information, but only if you would otherwise
+ be required to provide such information under section 6 of the
+ GNU GPL, and only to the extent that such information is
+ necessary to install and execute a modified version of the
+ Combined Work produced by recombining or relinking the
+ Application with a modified version of the Linked Version. (If
+ you use option 4d0, the Installation Information must accompany
+ the Minimal Corresponding Source and Corresponding Application
+ Code. If you use option 4d1, you must provide the Installation
+ Information in the manner specified by section 6 of the GNU GPL
+ for conveying Corresponding Source.)
+
+ 5. Combined Libraries.
+
+ You may place library facilities that are a work based on the
+Library side by side in a single library together with other library
+facilities that are not Applications and are not covered by this
+License, and convey such a combined library under terms of your
+choice, if you do both of the following:
+
+ a) Accompany the combined library with a copy of the same work based
+ on the Library, uncombined with any other library facilities,
+ conveyed under the terms of this License.
+
+ b) Give prominent notice with the combined library that part of it
+ is a work based on the Library, and explaining where to find the
+ accompanying uncombined form of the same work.
+
+ 6. Revised Versions of the GNU Lesser General Public License.
+
+ The Free Software Foundation may publish revised and/or new versions
+of the GNU Lesser General Public License from time to time. Such new
+versions will be similar in spirit to the present version, but may
+differ in detail to address new problems or concerns.
+
+ Each version is given a distinguishing version number. If the
+Library as you received it specifies that a certain numbered version
+of the GNU Lesser General Public License "or any later version"
+applies to it, you have the option of following the terms and
+conditions either of that published version or of any later version
+published by the Free Software Foundation. If the Library as you
+received it does not specify a version number of the GNU Lesser
+General Public License, you may choose any version of the GNU Lesser
+General Public License ever published by the Free Software Foundation.
+
+ If the Library as you received it specifies that a proxy can decide
+whether future versions of the GNU Lesser General Public License shall
+apply, that proxy's public statement of acceptance of any version is
+permanent authorization for you to choose that version for the
+Library.
diff --git a/deps/libccd/CMakeLists.txt b/deps/libccd/CMakeLists.txt
index a4bf303..9a17e4e 100644
--- a/deps/libccd/CMakeLists.txt
+++ b/deps/libccd/CMakeLists.txt
@@ -12,7 +12,15 @@ set (sources
include_directories(SYSTEM ${CMAKE_SOURCE_DIR}/deps/libccd/include )
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_CCD")
-gz_add_library(gazebo_ccd ${sources})
-target_link_libraries(gazebo_ccd m)
+if (WIN32)
+ add_library(gazebo_ccd SHARED ${sources})
+else()
+ gz_add_library(gazebo_ccd ${sources})
+endif()
+
+if (NOT WIN32)
+ target_link_libraries(gazebo_ccd m)
+endif()
gz_install_library(gazebo_ccd)
diff --git a/deps/libccd/include/ccd/ccd.h b/deps/libccd/include/ccd/ccd.h
index 8072f74..8bfcbc1 100644
--- a/deps/libccd/include/ccd/ccd.h
+++ b/deps/libccd/include/ccd/ccd.h
@@ -18,6 +18,31 @@
#ifndef __CCD_H__
#define __CCD_H__
+#if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_CCD
+ #ifdef __GNUC__
+ #define CCD_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define CCD_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define CCD_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define CCD_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define CCD_HIDDEN
+#else
+ #if __GNUC__ >= 4
+ #define CCD_VISIBLE __attribute__ ((visibility ("default")))
+ #define CCD_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define CCD_VISIBLE
+ #define CCD_HIDDEN
+ #endif
+#endif
+
#include <ccd/vec3.h>
#ifdef __cplusplus
@@ -71,7 +96,7 @@ typedef struct _ccd_t ccd_t;
/**
* Default first direction.
*/
-void ccdFirstDirDefault(const void *o1, const void *o2, ccd_vec3_t *dir);
+CCD_VISIBLE void ccdFirstDirDefault(const void *o1, const void *o2, ccd_vec3_t *dir);
#define CCD_INIT(ccd) \
do { \
@@ -122,7 +147,7 @@ int ccdGJKPenetration(const void *obj1, const void *obj2, const ccd_t *ccd,
/**
* Returns true if two given objects intersect - MPR algorithm is used.
*/
-int ccdMPRIntersect(const void *obj1, const void *obj2, const ccd_t *ccd);
+CCD_VISIBLE int ccdMPRIntersect(const void *obj1, const void *obj2, const ccd_t *ccd);
/**
* Computes penetration of obj2 into obj1.
@@ -136,7 +161,7 @@ int ccdMPRIntersect(const void *obj1, const void *obj2, const ccd_t *ccd);
*
* Returns 0 if obj1 and obj2 intersect, otherwise -1 is returned.
*/
-int ccdMPRPenetration(const void *obj1, const void *obj2, const ccd_t *ccd,
+CCD_VISIBLE int ccdMPRPenetration(const void *obj1, const void *obj2, const ccd_t *ccd,
ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos);
#ifdef __cplusplus
diff --git a/deps/opende/CMakeLists.txt b/deps/opende/CMakeLists.txt
index fadd963..ce15870 100644
--- a/deps/opende/CMakeLists.txt
+++ b/deps/opende/CMakeLists.txt
@@ -10,13 +10,20 @@ include_directories(SYSTEM
${CMAKE_SOURCE_DIR}/deps/opende/OPCODE
${CMAKE_SOURCE_DIR}/deps/opende/GIMPACT/include
${CMAKE_SOURCE_DIR}/deps/threadpool
+ ${Boost_INCLUDE_DIRS}
${CCD_INCLUDE_DIRS}
)
link_directories(
${CCD_LIBRARY_DIRS}
+ ${Boost_LIBRARY_DIR}
)
+if (HAVE_DART)
+ include_directories(${DARTCore_INCLUDE_DIRS})
+ link_directories(${DARTCore_LIBRARY_DIRS})
+endif()
+
add_subdirectory(OPCODE)
add_subdirectory(GIMPACT)
add_subdirectory(ou)
@@ -162,6 +169,8 @@ src/cylinder.cpp
src/error.cpp
src/export-dif.cpp
src/heightfield.cpp
+src/io.cpp
+src/ioh5.cpp
src/lcp.cpp
src/mass.cpp
src/mat.cpp
@@ -177,11 +186,16 @@ src/odetls.cpp
src/optimize.cpp
src/plane.cpp
src/quickstep.cpp
+src/quickstep_cg_lcp.cpp
+src/quickstep_pgs_lcp.cpp
+src/quickstep_update_bodies.cpp
+src/quickstep_util.cpp
src/ray.cpp
src/robuststep.cpp
src/rotation.cpp
src/sphere.cpp
src/step.cpp
+src/step_dart_pgs_wrapper.cpp
src/symm.c
src/timer.cpp
src/util.cpp
@@ -221,6 +235,7 @@ include/ode/compatibility.h
include/ode/contact.h
include/ode/error.h
include/ode/export-dif.h
+include/ode/h5dump.h
include/ode/mass.h
include/ode/matrix.h
include/ode/memory.h
@@ -240,12 +255,21 @@ include/ode/timer.h
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DNDEBUG -DdNODEBUG -DdDOUBLE -DHAVE_CONFIG_H -DPIC")
if (SSE2_FOUND OR SSE3_FOUND OR SSSE3_FOUND OR SSE4_1_FOUND OR SSE4_2_FOUND)
- set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSSE")
+ set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DODE_SSE")
+endif()
+
+if (WIN32)
+ set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DWIN32 -DODE_DLL")
endif()
set (CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} ${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE}} -MF -MT -fno-strict-aliasing -DPIC ")
-gz_add_library(gazebo_ode ${sources})
+if (WIN32)
+ add_library(gazebo_ode SHARED ${sources})
+else()
+ gz_add_library(gazebo_ode ${sources})
+endif()
+
target_link_libraries(gazebo_ode
gazebo_opcode
gazebo_gimpact
@@ -253,6 +277,11 @@ target_link_libraries(gazebo_ode
${CCD_LIBRARIES}
${Boost_LIBRARIES})
+if (HDF5_FOUND AND HDF5_INSTRUMENT)
+ message(STATUS "HDF5 Found and Instrument enabled")
+ target_link_libraries(gazebo_ode ${HDF5_LIBRARIES})
+endif()
+
add_dependencies(gazebo_ode gazebo_opcode gazebo_gimpact)
add_dependencies(gazebo_ode gazebo_opende_ou)
diff --git a/deps/opende/GIMPACT/CMakeLists.txt b/deps/opende/GIMPACT/CMakeLists.txt
index 5c67034..5b57418 100644
--- a/deps/opende/GIMPACT/CMakeLists.txt
+++ b/deps/opende/GIMPACT/CMakeLists.txt
@@ -37,7 +37,14 @@ set ( headers
include/GIMPACT/gim_math.h
)
-gz_add_library(gazebo_gimpact ${sources})
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GIMPACT")
+
+if (WIN32)
+ add_library(gazebo_gimpact SHARED ${sources})
+else()
+ gz_add_library(gazebo_gimpact ${sources})
+endif()
+
gz_install_library(gazebo_gimpact)
gz_install_includes("GIMPACT" ${headers})
diff --git a/deps/opende/GIMPACT/include/GIMPACT/gimpact.h b/deps/opende/GIMPACT/include/GIMPACT/gimpact.h
index bcd22a1..c762afd 100644
--- a/deps/opende/GIMPACT/include/GIMPACT/gimpact.h
+++ b/deps/opende/GIMPACT/include/GIMPACT/gimpact.h
@@ -31,6 +31,30 @@ email: projectileman at yahoo.com
-----------------------------------------------------------------------------
*/
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef BUILDING_DLL_GIMPACT
+ #ifdef __GNUC__
+ #define GIMPACT_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GIMPACT_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GIMPACT_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GIMPACT_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GIMPACT_HIDDEN
+#else
+ #if __GNUC__ >= 4
+ #define GIMPACT_VISIBLE __attribute__ ((visibility ("default")))
+ #define GIMPACT_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GIMPACT_VISIBLE
+ #define GIMPACT_HIDDEN
+ #endif
+#endif
#include "GIMPACT/gim_trimesh.h"
@@ -38,8 +62,8 @@ email: projectileman at yahoo.com
*/
//! @{
//! Call this for initialize GIMPACT system structures.
-void gimpact_init();
+GIMPACT_VISIBLE void gimpact_init();
//! Call this for clean GIMPACT system structures.
-void gimpact_terminate();
+GIMPACT_VISIBLE void gimpact_terminate();
//! @}
#endif // GIMPACT_H_INCLUDED
diff --git a/deps/opende/OPCODE/CMakeLists.txt b/deps/opende/OPCODE/CMakeLists.txt
index 9c31f09..0a83485 100644
--- a/deps/opende/OPCODE/CMakeLists.txt
+++ b/deps/opende/OPCODE/CMakeLists.txt
@@ -83,6 +83,13 @@ OPC_VolumeCollider.h
Stdafx.h
)
-gz_add_library(gazebo_opcode ${sources})
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_OPCODE")
+
+if (WIN32)
+ add_library(gazebo_opcode SHARED ${sources})
+else()
+ gz_add_library(gazebo_opcode ${sources})
+endif()
+
gz_install_library(gazebo_opcode)
gz_install_includes("opcode" ${headers})
diff --git a/deps/opende/OPCODE/Ice/IceHPoint.h b/deps/opende/OPCODE/Ice/IceHPoint.h
index 34564f3..6c47670 100644
--- a/deps/opende/OPCODE/Ice/IceHPoint.h
+++ b/deps/opende/OPCODE/Ice/IceHPoint.h
@@ -1,9 +1,9 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
- * Contains code for homogeneous points.
- * \file IceHPoint.h
- * \author Pierre Terdiman
- * \date April, 4, 2000
+ * Contains code for homogeneous points.
+ * \file IceHPoint.h
+ * \author Pierre Terdiman
+ * \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -12,149 +12,157 @@
#ifndef __ICEHPOINT_H__
#define __ICEHPOINT_H__
- class __attribute__((__may_alias__)) ICEMATHS_API HPoint : public Point
- {
- public:
-
- //! Empty constructor
- inline_ HPoint() {}
- //! Constructor from floats
- inline_ HPoint(float xx, float yy, float zz, float ww=0.0f) : Point(xx, yy, zz), w(ww) {}
- //! Constructor from array
- inline_ HPoint(const float f[4]) : Point(f), w(f[3]) {}
- //! Constructor from a Point
- inline_ HPoint(const Point& p, float ww=0.0f) : Point(p), w(ww) {}
- //! Destructor
- inline_ ~HPoint() {}
-
- //! Clear the point
- inline_ HPoint& Zero() { x = y = z = w = 0.0f; return *this; }
-
- //! Assignment from values
- inline_ HPoint& Set(float xx, float yy, float zz, float ww ) { x = xx; y = yy; z = zz; w = ww; return *this; }
- //! Assignment from array
- inline_ HPoint& Set(const float f[4]) { x = f[X]; y = f[Y]; z = f[Z]; w = f[W]; return *this; }
- //! Assignment from another h-point
- inline_ HPoint& Set(const HPoint& src) { x = src.x; y = src.y; z = src.z; w = src.w; return *this; }
-
- //! Add a vector
- inline_ HPoint& Add(float xx, float yy, float zz, float ww ) { x += xx; y += yy; z += zz; w += ww; return *this; }
- //! Add a vector
- inline_ HPoint& Add(const float f[4]) { x += f[X]; y += f[Y]; z += f[Z]; w += f[W]; return *this; }
-
- //! Subtract a vector
- inline_ HPoint& Sub(float xx, float yy, float zz, float ww ) { x -= xx; y -= yy; z -= zz; w -= ww; return *this; }
- //! Subtract a vector
- inline_ HPoint& Sub(const float f[4]) { x -= f[X]; y -= f[Y]; z -= f[Z]; w -= f[W]; return *this; }
-
- //! Multiplies by a scalar
- inline_ HPoint& Mul(float s) { x *= s; y *= s; z *= s; w *= s; return *this; }
-
- //! Returns MIN(x, y, z, w);
- float Min() const { return MIN(x, MIN(y, MIN(z, w))); }
- //! Returns MAX(x, y, z, w);
- float Max() const { return MAX(x, MAX(y, MAX(z, w))); }
- //! Sets each element to be componentwise minimum
- HPoint& Min(const HPoint& p) { x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z); w = MIN(w, p.w); return *this; }
- //! Sets each element to be componentwise maximum
- HPoint& Max(const HPoint& p) { x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z); w = MAX(w, p.w); return *this; }
-
- //! Computes square magnitude
- inline_ float SquareMagnitude() const { return x*x + y*y + z*z + w*w; }
- //! Computes magnitude
- inline_ float Magnitude() const { return sqrtf(x*x + y*y + z*z + w*w); }
-
- //! Normalize the vector
- inline_ HPoint& Normalize()
- {
- float M = Magnitude();
- if(!_equal(M, 0.0))
- {
- M = 1.0f / M;
- x *= M;
- y *= M;
- z *= M;
- w *= M;
- }
- return *this;
- }
-
- // Arithmetic operators
- //! Operator for HPoint Negate = - HPoint;
- inline_ HPoint operator-() const { return HPoint(-x, -y, -z, -w); }
-
- //! Operator for HPoint Plus = HPoint + HPoint;
- inline_ HPoint operator+(const HPoint& p) const { return HPoint(x + p.x, y + p.y, z + p.z, w + p.w); }
- //! Operator for HPoint Minus = HPoint - HPoint;
- inline_ HPoint operator-(const HPoint& p) const { return HPoint(x - p.x, y - p.y, z - p.z, w - p.w); }
-
- //! Operator for HPoint Mul = HPoint * HPoint;
- inline_ HPoint operator*(const HPoint& p) const { return HPoint(x * p.x, y * p.y, z * p.z, w * p.w); }
- //! Operator for HPoint Scale = HPoint * float;
- inline_ HPoint operator*(float s) const { return HPoint(x * s, y * s, z * s, w * s); }
- //! Operator for HPoint Scale = float * HPoint;
- inline_ friend HPoint operator*(float s, const HPoint& p) { return HPoint(s * p.x, s * p.y, s * p.z, s * p.w); }
-
- //! Operator for HPoint Div = HPoint / HPoint;
- inline_ HPoint operator/(const HPoint& p) const { return HPoint(x / p.x, y / p.y, z / p.z, w / p.w); }
- //! Operator for HPoint Scale = HPoint / float;
- inline_ HPoint operator/(float s) const { s = 1.0f / s; return HPoint(x * s, y * s, z * s, w * s); }
- //! Operator for HPoint Scale = float / HPoint;
- inline_ friend HPoint operator/(float s, const HPoint& p) { return HPoint(s / p.x, s / p.y, s / p.z, s / p.w); }
-
- //! Operator for float DotProd = HPoint | HPoint;
- inline_ float operator|(const HPoint& p) const { return x*p.x + y*p.y + z*p.z + w*p.w; }
- // No cross-product in 4D
-
- //! Operator for HPoint += HPoint;
- inline_ HPoint& operator+=(const HPoint& p) { x += p.x; y += p.y; z += p.z; w += p.w; return *this; }
- //! Operator for HPoint += float;
- inline_ HPoint& operator+=(float s) { x += s; y += s; z += s; w += s; return *this; }
-
- //! Operator for HPoint -= HPoint;
- inline_ HPoint& operator-=(const HPoint& p) { x -= p.x; y -= p.y; z -= p.z; w -= p.w; return *this; }
- //! Operator for HPoint -= float;
- inline_ HPoint& operator-=(float s) { x -= s; y -= s; z -= s; w -= s; return *this; }
-
- //! Operator for HPoint *= HPoint;
- inline_ HPoint& operator*=(const HPoint& p) { x *= p.x; y *= p.y; z *= p.z; w *= p.w; return *this; }
- //! Operator for HPoint *= float;
- inline_ HPoint& operator*=(float s) { x*=s; y*=s; z*=s; w*=s; return *this; }
-
- //! Operator for HPoint /= HPoint;
- inline_ HPoint& operator/=(const HPoint& p) { x /= p.x; y /= p.y; z /= p.z; w /= p.w; return *this; }
- //! Operator for HPoint /= float;
- inline_ HPoint& operator/=(float s) { s = 1.0f / s; x*=s; y*=s; z*=s; w*=s; return *this; }
-
- // Arithmetic operators
-
- //! Operator for Point Mul = HPoint * Matrix3x3;
- Point operator*(const Matrix3x3& mat) const;
- //! Operator for HPoint Mul = HPoint * Matrix4x4;
- HPoint operator*(const Matrix4x4& mat) const;
-
- // HPoint *= Matrix3x3 doesn't exist, the matrix is first casted to a 4x4
- //! Operator for HPoint *= Matrix4x4
- HPoint& operator*=(const Matrix4x4& mat);
-
- // Logical operators
-
- //! Operator for "if(HPoint==HPoint)"
- inline_ bool operator==(const HPoint& p) const { return ( (_equal(x, p.x))&&(_equal(y, p.y))&&(_equal(z, p.z))&&(_equal(w, p.w))); }
- //! Operator for "if(HPoint!=HPoint)"
- inline_ bool operator!=(const HPoint& p) const { return ( (!_equal(x,p.x))||(!_equal(y, p.y))||(!_equal(z, p.z))||(!_equal(w, p.w))); }
-
- // Cast operators
-
- //! Cast a HPoint to a Point. w is discarded.
+#ifndef ATTRIBUTE_MAY_ALIAS
+ #if defined _WIN32
+ #define ATTRIBUTE_MAY_ALIAS
+ #else
+ #define ATTRIBUTE_MAY_ALIAS __attribute__((__may_alias__))
+ #endif
+#endif
+
+ class ATTRIBUTE_MAY_ALIAS ICEMATHS_API HPoint : public Point
+ {
+ public:
+
+ //! Empty constructor
+ inline_ HPoint() {}
+ //! Constructor from floats
+ inline_ HPoint(float xx, float yy, float zz, float ww=0.0f) : Point(xx, yy, zz), w(ww) {}
+ //! Constructor from array
+ inline_ HPoint(const float f[4]) : Point(f), w(f[3]) {}
+ //! Constructor from a Point
+ inline_ HPoint(const Point& p, float ww=0.0f) : Point(p), w(ww) {}
+ //! Destructor
+ inline_ ~HPoint() {}
+
+ //! Clear the point
+ inline_ HPoint& Zero() { x = y = z = w = 0.0f; return *this; }
+
+ //! Assignment from values
+ inline_ HPoint& Set(float xx, float yy, float zz, float ww ) { x = xx; y = yy; z = zz; w = ww; return *this; }
+ //! Assignment from array
+ inline_ HPoint& Set(const float f[4]) { x = f[X]; y = f[Y]; z = f[Z]; w = f[W]; return *this; }
+ //! Assignment from another h-point
+ inline_ HPoint& Set(const HPoint& src) { x = src.x; y = src.y; z = src.z; w = src.w; return *this; }
+
+ //! Add a vector
+ inline_ HPoint& Add(float xx, float yy, float zz, float ww ) { x += xx; y += yy; z += zz; w += ww; return *this; }
+ //! Add a vector
+ inline_ HPoint& Add(const float f[4]) { x += f[X]; y += f[Y]; z += f[Z]; w += f[W]; return *this; }
+
+ //! Subtract a vector
+ inline_ HPoint& Sub(float xx, float yy, float zz, float ww ) { x -= xx; y -= yy; z -= zz; w -= ww; return *this; }
+ //! Subtract a vector
+ inline_ HPoint& Sub(const float f[4]) { x -= f[X]; y -= f[Y]; z -= f[Z]; w -= f[W]; return *this; }
+
+ //! Multiplies by a scalar
+ inline_ HPoint& Mul(float s) { x *= s; y *= s; z *= s; w *= s; return *this; }
+
+ //! Returns MIN(x, y, z, w);
+ float Min() const { return MIN(x, MIN(y, MIN(z, w))); }
+ //! Returns MAX(x, y, z, w);
+ float Max() const { return MAX(x, MAX(y, MAX(z, w))); }
+ //! Sets each element to be componentwise minimum
+ HPoint& Min(const HPoint& p) { x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z); w = MIN(w, p.w); return *this; }
+ //! Sets each element to be componentwise maximum
+ HPoint& Max(const HPoint& p) { x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z); w = MAX(w, p.w); return *this; }
+
+ //! Computes square magnitude
+ inline_ float SquareMagnitude() const { return x*x + y*y + z*z + w*w; }
+ //! Computes magnitude
+ inline_ float Magnitude() const { return sqrtf(x*x + y*y + z*z + w*w); }
+
+ //! Normalize the vector
+ inline_ HPoint& Normalize()
+ {
+ float M = Magnitude();
+ if(!_equal(M, 0.0))
+ {
+ M = 1.0f / M;
+ x *= M;
+ y *= M;
+ z *= M;
+ w *= M;
+ }
+ return *this;
+ }
+
+ // Arithmetic operators
+ //! Operator for HPoint Negate = - HPoint;
+ inline_ HPoint operator-() const { return HPoint(-x, -y, -z, -w); }
+
+ //! Operator for HPoint Plus = HPoint + HPoint;
+ inline_ HPoint operator+(const HPoint& p) const { return HPoint(x + p.x, y + p.y, z + p.z, w + p.w); }
+ //! Operator for HPoint Minus = HPoint - HPoint;
+ inline_ HPoint operator-(const HPoint& p) const { return HPoint(x - p.x, y - p.y, z - p.z, w - p.w); }
+
+ //! Operator for HPoint Mul = HPoint * HPoint;
+ inline_ HPoint operator*(const HPoint& p) const { return HPoint(x * p.x, y * p.y, z * p.z, w * p.w); }
+ //! Operator for HPoint Scale = HPoint * float;
+ inline_ HPoint operator*(float s) const { return HPoint(x * s, y * s, z * s, w * s); }
+ //! Operator for HPoint Scale = float * HPoint;
+ inline_ friend HPoint operator*(float s, const HPoint& p) { return HPoint(s * p.x, s * p.y, s * p.z, s * p.w); }
+
+ //! Operator for HPoint Div = HPoint / HPoint;
+ inline_ HPoint operator/(const HPoint& p) const { return HPoint(x / p.x, y / p.y, z / p.z, w / p.w); }
+ //! Operator for HPoint Scale = HPoint / float;
+ inline_ HPoint operator/(float s) const { s = 1.0f / s; return HPoint(x * s, y * s, z * s, w * s); }
+ //! Operator for HPoint Scale = float / HPoint;
+ inline_ friend HPoint operator/(float s, const HPoint& p) { return HPoint(s / p.x, s / p.y, s / p.z, s / p.w); }
+
+ //! Operator for float DotProd = HPoint | HPoint;
+ inline_ float operator|(const HPoint& p) const { return x*p.x + y*p.y + z*p.z + w*p.w; }
+ // No cross-product in 4D
+
+ //! Operator for HPoint += HPoint;
+ inline_ HPoint& operator+=(const HPoint& p) { x += p.x; y += p.y; z += p.z; w += p.w; return *this; }
+ //! Operator for HPoint += float;
+ inline_ HPoint& operator+=(float s) { x += s; y += s; z += s; w += s; return *this; }
+
+ //! Operator for HPoint -= HPoint;
+ inline_ HPoint& operator-=(const HPoint& p) { x -= p.x; y -= p.y; z -= p.z; w -= p.w; return *this; }
+ //! Operator for HPoint -= float;
+ inline_ HPoint& operator-=(float s) { x -= s; y -= s; z -= s; w -= s; return *this; }
+
+ //! Operator for HPoint *= HPoint;
+ inline_ HPoint& operator*=(const HPoint& p) { x *= p.x; y *= p.y; z *= p.z; w *= p.w; return *this; }
+ //! Operator for HPoint *= float;
+ inline_ HPoint& operator*=(float s) { x*=s; y*=s; z*=s; w*=s; return *this; }
+
+ //! Operator for HPoint /= HPoint;
+ inline_ HPoint& operator/=(const HPoint& p) { x /= p.x; y /= p.y; z /= p.z; w /= p.w; return *this; }
+ //! Operator for HPoint /= float;
+ inline_ HPoint& operator/=(float s) { s = 1.0f / s; x*=s; y*=s; z*=s; w*=s; return *this; }
+
+ // Arithmetic operators
+
+ //! Operator for Point Mul = HPoint * Matrix3x3;
+ Point operator*(const Matrix3x3& mat) const;
+ //! Operator for HPoint Mul = HPoint * Matrix4x4;
+ HPoint operator*(const Matrix4x4& mat) const;
+
+ // HPoint *= Matrix3x3 doesn't exist, the matrix is first casted to a 4x4
+ //! Operator for HPoint *= Matrix4x4
+ HPoint& operator*=(const Matrix4x4& mat);
+
+ // Logical operators
+
+ //! Operator for "if(HPoint==HPoint)"
+ inline_ bool operator==(const HPoint& p) const { return ( (_equal(x, p.x))&&(_equal(y, p.y))&&(_equal(z, p.z))&&(_equal(w, p.w))); }
+ //! Operator for "if(HPoint!=HPoint)"
+ inline_ bool operator!=(const HPoint& p) const { return ( (!_equal(x,p.x))||(!_equal(y, p.y))||(!_equal(z, p.z))||(!_equal(w, p.w))); }
+
+ // Cast operators
+
+ //! Cast a HPoint to a Point. w is discarded.
#ifdef _MSC_VER
- inline_ operator Point() const { return Point(x, y, z); }
- // gcc complains that conversion to a base class will never use a type conversion operator
+ inline_ operator Point() const { return Point(x, y, z); }
+ // gcc complains that conversion to a base class will never use a type conversion operator
#endif
- public:
- float w;
- };
+ public:
+ float w;
+ };
#endif // __ICEHPOINT_H__
diff --git a/deps/opende/OPCODE/Ice/IcePoint.h b/deps/opende/OPCODE/Ice/IcePoint.h
index 506cfde..315b132 100644
--- a/deps/opende/OPCODE/Ice/IcePoint.h
+++ b/deps/opende/OPCODE/Ice/IcePoint.h
@@ -1,9 +1,9 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
- * Contains code for 3D vectors.
- * \file IcePoint.h
- * \author Pierre Terdiman
- * \date April, 4, 2000
+ * Contains code for 3D vectors.
+ * \file IcePoint.h
+ * \author Pierre Terdiman
+ * \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -12,517 +12,525 @@
#ifndef __ICEPOINT_H__
#define __ICEPOINT_H__
- // Forward declarations
- class __attribute__((__may_alias__)) HPoint;
- class Plane;
- class Matrix3x3;
- class Matrix4x4;
+#ifndef ATTRIBUTE_MAY_ALIAS
+ #if defined _WIN32
+ #define ATTRIBUTE_MAY_ALIAS
+ #else
+ #define ATTRIBUTE_MAY_ALIAS __attribute__((__may_alias__))
+ #endif
+#endif
- #define CROSS2D(a, b) (a.x*b.y - b.x*a.y)
+ // Forward declarations
+ class ATTRIBUTE_MAY_ALIAS HPoint;
+ class Plane;
+ class Matrix3x3;
+ class Matrix4x4;
- const float EPSILON2 = 1.0e-20f;
+ #define CROSS2D(a, b) (a.x*b.y - b.x*a.y)
- class __attribute__((__may_alias__)) ICEMATHS_API Point
- {
- public:
+ const float EPSILON2 = 1.0e-20f;
- //! Empty constructor
- inline_ Point() {}
- //! Constructor from a single float
-// inline_ Point(float val) : x(val), y(val), z(val) {}
+ class ATTRIBUTE_MAY_ALIAS ICEMATHS_API Point
+ {
+ public:
+
+ //! Empty constructor
+ inline_ Point() {}
+ //! Constructor from a single float
+// inline_ Point(float val) : x(val), y(val), z(val) {}
// Removed since it introduced the nasty "Point T = *Matrix4x4.GetTrans();" bug.......
- //! Constructor from floats
- inline_ Point(float xx, float yy, float zz) : x(xx), y(yy), z(zz) {}
- //! Constructor from array
- inline_ Point(const float f[3]) : x(f[X]), y(f[Y]), z(f[Z]) {}
- //! Copy constructor
- inline_ Point(const Point& p) : x(p.x), y(p.y), z(p.z) {}
- //! Destructor
- inline_ ~Point() {}
-
- //! Clears the vector
- inline_ Point& Zero() { x = y = z = 0.0f; return *this; }
-
- //! + infinity
- inline_ Point& SetPlusInfinity() { x = y = z = MAX_FLOAT; return *this; }
- //! - infinity
- inline_ Point& SetMinusInfinity() { x = y = z = MIN_FLOAT; return *this; }
-
- //! Sets positive unit random vector
- Point& PositiveUnitRandomVector();
- //! Sets unit random vector
- Point& UnitRandomVector();
-
- //! Assignment from values
- inline_ Point& Set(float xx, float yy, float zz) { x = xx; y = yy; z = zz; return *this; }
- //! Assignment from array
- inline_ Point& Set(const float f[3]) { x = f[X]; y = f[Y]; z = f[Z]; return *this; }
- //! Assignment from another point
- inline_ Point& Set(const Point& src) { x = src.x; y = src.y; z = src.z; return *this; }
-
- //! Adds a vector
- inline_ Point& Add(const Point& p) { x += p.x; y += p.y; z += p.z; return *this; }
- //! Adds a vector
- inline_ Point& Add(float xx, float yy, float zz) { x += xx; y += yy; z += zz; return *this; }
- //! Adds a vector
- inline_ Point& Add(const float f[3]) { x += f[X]; y += f[Y]; z += f[Z]; return *this; }
- //! Adds vectors
- inline_ Point& Add(const Point& p, const Point& q) { x = p.x+q.x; y = p.y+q.y; z = p.z+q.z; return *this; }
-
- //! Subtracts a vector
- inline_ Point& Sub(const Point& p) { x -= p.x; y -= p.y; z -= p.z; return *this; }
- //! Subtracts a vector
- inline_ Point& Sub(float xx, float yy, float zz) { x -= xx; y -= yy; z -= zz; return *this; }
- //! Subtracts a vector
- inline_ Point& Sub(const float f[3]) { x -= f[X]; y -= f[Y]; z -= f[Z]; return *this; }
- //! Subtracts vectors
- inline_ Point& Sub(const Point& p, const Point& q) { x = p.x-q.x; y = p.y-q.y; z = p.z-q.z; return *this; }
-
- //! this = -this
- inline_ Point& Neg() { x = -x; y = -y; z = -z; return *this; }
- //! this = -a
- inline_ Point& Neg(const Point& a) { x = -a.x; y = -a.y; z = -a.z; return *this; }
-
- //! Multiplies by a scalar
- inline_ Point& Mult(float s) { x *= s; y *= s; z *= s; return *this; }
-
- //! this = a * scalar
- inline_ Point& Mult(const Point& a, float scalar)
- {
- x = a.x * scalar;
- y = a.y * scalar;
- z = a.z * scalar;
- return *this;
- }
-
- //! this = a + b * scalar
- inline_ Point& Mac(const Point& a, const Point& b, float scalar)
- {
- x = a.x + b.x * scalar;
- y = a.y + b.y * scalar;
- z = a.z + b.z * scalar;
- return *this;
- }
-
- //! this = this + a * scalar
- inline_ Point& Mac(const Point& a, float scalar)
- {
- x += a.x * scalar;
- y += a.y * scalar;
- z += a.z * scalar;
- return *this;
- }
-
- //! this = a - b * scalar
- inline_ Point& Msc(const Point& a, const Point& b, float scalar)
- {
- x = a.x - b.x * scalar;
- y = a.y - b.y * scalar;
- z = a.z - b.z * scalar;
- return *this;
- }
-
- //! this = this - a * scalar
- inline_ Point& Msc(const Point& a, float scalar)
- {
- x -= a.x * scalar;
- y -= a.y * scalar;
- z -= a.z * scalar;
- return *this;
- }
-
- //! this = a + b * scalarb + c * scalarc
- inline_ Point& Mac2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc)
- {
- x = a.x + b.x * scalarb + c.x * scalarc;
- y = a.y + b.y * scalarb + c.y * scalarc;
- z = a.z + b.z * scalarb + c.z * scalarc;
- return *this;
- }
-
- //! this = a - b * scalarb - c * scalarc
- inline_ Point& Msc2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc)
- {
- x = a.x - b.x * scalarb - c.x * scalarc;
- y = a.y - b.y * scalarb - c.y * scalarc;
- z = a.z - b.z * scalarb - c.z * scalarc;
- return *this;
- }
-
- //! this = mat * a
- inline_ Point& Mult(const Matrix3x3& mat, const Point& a);
-
- //! this = mat1 * a1 + mat2 * a2
- inline_ Point& Mult2(const Matrix3x3& mat1, const Point& a1, const Matrix3x3& mat2, const Point& a2);
-
- //! this = this + mat * a
- inline_ Point& Mac(const Matrix3x3& mat, const Point& a);
-
- //! this = transpose(mat) * a
- inline_ Point& TransMult(const Matrix3x3& mat, const Point& a);
-
- //! Linear interpolate between two vectors: this = a + t * (b - a)
- inline_ Point& Lerp(const Point& a, const Point& b, float t)
- {
- x = a.x + t * (b.x - a.x);
- y = a.y + t * (b.y - a.y);
- z = a.z + t * (b.z - a.z);
- return *this;
- }
-
- //! Hermite interpolate between p1 and p2. p0 and p3 are used for finding gradient at p1 and p2.
- //! this = p0 * (2t^2 - t^3 - t)/2
- //! + p1 * (3t^3 - 5t^2 + 2)/2
- //! + p2 * (4t^2 - 3t^3 + t)/2
- //! + p3 * (t^3 - t^2)/2
- inline_ Point& Herp(const Point& p0, const Point& p1, const Point& p2, const Point& p3, float t)
- {
- float t2 = t * t;
- float t3 = t2 * t;
- float kp0 = (2.0f * t2 - t3 - t) * 0.5f;
- float kp1 = (3.0f * t3 - 5.0f * t2 + 2.0f) * 0.5f;
- float kp2 = (4.0f * t2 - 3.0f * t3 + t) * 0.5f;
- float kp3 = (t3 - t2) * 0.5f;
- x = p0.x * kp0 + p1.x * kp1 + p2.x * kp2 + p3.x * kp3;
- y = p0.y * kp0 + p1.y * kp1 + p2.y * kp2 + p3.y * kp3;
- z = p0.z * kp0 + p1.z * kp1 + p2.z * kp2 + p3.z * kp3;
- return *this;
- }
-
- //! this = rotpos * r + linpos
- inline_ Point& Transform(const Point& r, const Matrix3x3& rotpos, const Point& linpos);
-
- //! this = trans(rotpos) * (r - linpos)
- inline_ Point& InvTransform(const Point& r, const Matrix3x3& rotpos, const Point& linpos);
-
- //! Returns MIN(x, y, z);
- inline_ float Min() const { return MIN(x, MIN(y, z)); }
- //! Returns MAX(x, y, z);
- inline_ float Max() const { return MAX(x, MAX(y, z)); }
- //! Sets each element to be componentwise minimum
- inline_ Point& Min(const Point& p) { x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z); return *this; }
- //! Sets each element to be componentwise maximum
- inline_ Point& Max(const Point& p) { x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z); return *this; }
-
- //! Clamps each element
- inline_ Point& Clamp(float min, float max)
- {
- if(x<min) x=min; if(x>max) x=max;
- if(y<min) y=min; if(y>max) y=max;
- if(z<min) z=min; if(z>max) z=max;
- return *this;
- }
-
- //! Computes square magnitude
- inline_ float SquareMagnitude() const { return x*x + y*y + z*z; }
- //! Computes magnitude
- inline_ float Magnitude() const { return sqrtf(x*x + y*y + z*z); }
- //! Computes volume
- inline_ float Volume() const { return x * y * z; }
-
- //! Checks the point is near zero
- inline_ bool ApproxZero() const { return SquareMagnitude() < EPSILON2; }
-
- //! Tests for exact zero vector
- inline_ BOOL IsZero() const
- {
- if(IR(x) || IR(y) || IR(z)) return FALSE;
- return TRUE;
- }
-
- //! Checks point validity
- inline_ BOOL IsValid() const
- {
- if(!IsValidFloat(x)) return FALSE;
- if(!IsValidFloat(y)) return FALSE;
- if(!IsValidFloat(z)) return FALSE;
- return TRUE;
- }
-
- //! Slighty moves the point
- void Tweak(udword coord_mask, udword tweak_mask)
- {
- if(coord_mask&1) { udword Dummy = IR(x); Dummy^=tweak_mask; x = FR(Dummy); }
- if(coord_mask&2) { udword Dummy = IR(y); Dummy^=tweak_mask; y = FR(Dummy); }
- if(coord_mask&4) { udword Dummy = IR(z); Dummy^=tweak_mask; z = FR(Dummy); }
- }
-
- #define TWEAKMASK 0x3fffff
- #define TWEAKNOTMASK ~TWEAKMASK
- //! Slighty moves the point out
- inline_ void TweakBigger()
- {
- udword Dummy = (IR(x)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(x)) Dummy+=TWEAKMASK+1; x = FR(Dummy);
- Dummy = (IR(y)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(y)) Dummy+=TWEAKMASK+1; y = FR(Dummy);
- Dummy = (IR(z)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(z)) Dummy+=TWEAKMASK+1; z = FR(Dummy);
- }
-
- //! Slighty moves the point in
- inline_ void TweakSmaller()
- {
- udword Dummy = (IR(x)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(x)) Dummy+=TWEAKMASK+1; x = FR(Dummy);
- Dummy = (IR(y)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(y)) Dummy+=TWEAKMASK+1; y = FR(Dummy);
- Dummy = (IR(z)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(z)) Dummy+=TWEAKMASK+1; z = FR(Dummy);
- }
-
- //! Normalizes the vector
- inline_ Point& Normalize()
- {
- float M = x*x + y*y + z*z;
- if(!_equal(M, 0.0))
- {
- M = 1.0f / sqrtf(M);
- x *= M;
- y *= M;
- z *= M;
- }
- return *this;
- }
-
- //! Sets vector length
- inline_ Point& SetLength(float length)
- {
- float NewLength = length / Magnitude();
- x *= NewLength;
- y *= NewLength;
- z *= NewLength;
- return *this;
- }
-
- //! Clamps vector length
- inline_ Point& ClampLength(float limit_length)
- {
- if(limit_length>=0.0f) // Magnitude must be positive
- {
- float CurrentSquareLength = SquareMagnitude();
-
- if(CurrentSquareLength > limit_length * limit_length)
- {
- float Coeff = limit_length / sqrtf(CurrentSquareLength);
- x *= Coeff;
- y *= Coeff;
- z *= Coeff;
- }
- }
- return *this;
- }
-
- //! Computes distance to another point
- inline_ float Distance(const Point& b) const
- {
- return sqrtf((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
- }
-
- //! Computes square distance to another point
- inline_ float SquareDistance(const Point& b) const
- {
- return ((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
- }
-
- //! Dot product dp = this|a
- inline_ float Dot(const Point& p) const { return p.x * x + p.y * y + p.z * z; }
-
- //! Cross product this = a x b
- inline_ Point& Cross(const Point& a, const Point& b)
- {
- x = a.y * b.z - a.z * b.y;
- y = a.z * b.x - a.x * b.z;
- z = a.x * b.y - a.y * b.x;
- return *this;
- }
-
- //! Vector code ( bitmask = sign(z) | sign(y) | sign(x) )
- inline_ udword VectorCode() const
- {
- return (IR(x)>>31) | ((IR(y)&SIGN_BITMASK)>>30) | ((IR(z)&SIGN_BITMASK)>>29);
- }
-
- //! Returns largest axis
- inline_ PointComponent LargestAxis() const
- {
- const float* Vals = &x;
- PointComponent m = X;
- if(Vals[Y] > Vals[m]) m = Y;
- if(Vals[Z] > Vals[m]) m = Z;
- return m;
- }
-
- //! Returns closest axis
- inline_ PointComponent ClosestAxis() const
- {
- const float* Vals = &x;
- PointComponent m = X;
- if(AIR(Vals[Y]) > AIR(Vals[m])) m = Y;
- if(AIR(Vals[Z]) > AIR(Vals[m])) m = Z;
- return m;
- }
-
- //! Returns smallest axis
- inline_ PointComponent SmallestAxis() const
- {
- const float* Vals = &x;
- PointComponent m = X;
- if(Vals[Y] < Vals[m]) m = Y;
- if(Vals[Z] < Vals[m]) m = Z;
- return m;
- }
-
- //! Refracts the point
- Point& Refract(const Point& eye, const Point& n, float refractindex, Point& refracted);
-
- //! Projects the point onto a plane
- Point& ProjectToPlane(const Plane& p);
-
- //! Projects the point onto the screen
- void ProjectToScreen(float halfrenderwidth, float halfrenderheight, const Matrix4x4& mat, HPoint& projected) const;
-
- //! Unfolds the point onto a plane according to edge(a,b)
- Point& Unfold(Plane& p, Point& a, Point& b);
-
- //! Hash function from Ville Miettinen
- inline_ udword GetHashValue() const
- {
- const udword* h = (const udword*)(this);
- udword f = (h[0]+h[1]*11-(h[2]*17)) & 0x7fffffff; // avoid problems with +-0
- return (f>>22)^(f>>12)^(f);
- }
-
- //! Stuff magic values in the point, marking it as explicitely not used.
- void SetNotUsed();
- //! Checks the point is marked as not used
- BOOL IsNotUsed() const;
-
- // Arithmetic operators
-
- //! Unary operator for Point Negate = - Point
- inline_ Point operator-() const { return Point(-x, -y, -z); }
-
- //! Operator for Point Plus = Point + Point.
- inline_ Point operator+(const Point& p) const { return Point(x + p.x, y + p.y, z + p.z); }
- //! Operator for Point Minus = Point - Point.
- inline_ Point operator-(const Point& p) const { return Point(x - p.x, y - p.y, z - p.z); }
-
- //! Operator for Point Mul = Point * Point.
- inline_ Point operator*(const Point& p) const { return Point(x * p.x, y * p.y, z * p.z); }
- //! Operator for Point Scale = Point * float.
- inline_ Point operator*(float s) const { return Point(x * s, y * s, z * s ); }
- //! Operator for Point Scale = float * Point.
- inline_ friend Point operator*(float s, const Point& p) { return Point(s * p.x, s * p.y, s * p.z); }
-
- //! Operator for Point Div = Point / Point.
- inline_ Point operator/(const Point& p) const { return Point(x / p.x, y / p.y, z / p.z); }
- //! Operator for Point Scale = Point / float.
- inline_ Point operator/(float s) const { s = 1.0f / s; return Point(x * s, y * s, z * s); }
- //! Operator for Point Scale = float / Point.
- inline_ friend Point operator/(float s, const Point& p) { return Point(s / p.x, s / p.y, s / p.z); }
-
- //! Operator for float DotProd = Point | Point.
- inline_ float operator|(const Point& p) const { return x*p.x + y*p.y + z*p.z; }
- //! Operator for Point VecProd = Point ^ Point.
- inline_ Point operator^(const Point& p) const
- {
- return Point(
- y * p.z - z * p.y,
- z * p.x - x * p.z,
- x * p.y - y * p.x );
- }
-
- //! Operator for Point += Point.
- inline_ Point& operator+=(const Point& p) { x += p.x; y += p.y; z += p.z; return *this; }
- //! Operator for Point += float.
- inline_ Point& operator+=(float s) { x += s; y += s; z += s; return *this; }
-
- //! Operator for Point -= Point.
- inline_ Point& operator-=(const Point& p) { x -= p.x; y -= p.y; z -= p.z; return *this; }
- //! Operator for Point -= float.
- inline_ Point& operator-=(float s) { x -= s; y -= s; z -= s; return *this; }
-
- //! Operator for Point *= Point.
- inline_ Point& operator*=(const Point& p) { x *= p.x; y *= p.y; z *= p.z; return *this; }
- //! Operator for Point *= float.
- inline_ Point& operator*=(float s) { x *= s; y *= s; z *= s; return *this; }
-
- //! Operator for Point /= Point.
- inline_ Point& operator/=(const Point& p) { x /= p.x; y /= p.y; z /= p.z; return *this; }
- //! Operator for Point /= float.
- inline_ Point& operator/=(float s) { s = 1.0f/s; x *= s; y *= s; z *= s; return *this; }
-
- // Logical operators
-
- //! Operator for "if(Point==Point)"
- inline_ bool operator==(const Point& p) const { return ( (IR(x)==IR(p.x))&&(IR(y)==IR(p.y))&&(IR(z)==IR(p.z))); }
- //! Operator for "if(Point!=Point)"
- inline_ bool operator!=(const Point& p) const { return ( (IR(x)!=IR(p.x))||(IR(y)!=IR(p.y))||(IR(z)!=IR(p.z))); }
-
- // Arithmetic operators
-
- //! Operator for Point Mul = Point * Matrix3x3.
- inline_ Point operator*(const Matrix3x3& mat) const
- {
- class ShadowMatrix3x3{ public: float m[3][3]; }; // To allow inlining
- const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;
-
- return Point(
- x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0],
- x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1],
- x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] );
- }
-
- //! Operator for Point Mul = Point * Matrix4x4.
- inline_ Point operator*(const Matrix4x4& mat) const
- {
- class ShadowMatrix4x4{ public: float m[4][4]; }; // To allow inlining
- const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;
-
- return Point(
- x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0],
- x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1],
- x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2]);
- }
-
- //! Operator for Point *= Matrix3x3.
- inline_ Point& operator*=(const Matrix3x3& mat)
- {
- class ShadowMatrix3x3{ public: float m[3][3]; }; // To allow inlining
- const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;
-
- float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0];
- float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1];
- float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2];
-
- x = xp; y = yp; z = zp;
-
- return *this;
- }
-
- //! Operator for Point *= Matrix4x4.
- inline_ Point& operator*=(const Matrix4x4& mat)
- {
- class ShadowMatrix4x4{ public: float m[4][4]; }; // To allow inlining
- const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;
-
- float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0];
- float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1];
- float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2];
-
- x = xp; y = yp; z = zp;
-
- return *this;
- }
-
- // Cast operators
-
- //! Cast a Point to a HPoint. w is set to zero.
- operator HPoint() const;
-
- inline_ operator const float*() const { return &x; }
- inline_ operator float*() { return &x; }
-
- public:
- float x, y, z;
- };
-
- FUNCTION ICEMATHS_API void Normalize1(Point& a);
- FUNCTION ICEMATHS_API void Normalize2(Point& a);
+ //! Constructor from floats
+ inline_ Point(float xx, float yy, float zz) : x(xx), y(yy), z(zz) {}
+ //! Constructor from array
+ inline_ Point(const float f[3]) : x(f[X]), y(f[Y]), z(f[Z]) {}
+ //! Copy constructor
+ inline_ Point(const Point& p) : x(p.x), y(p.y), z(p.z) {}
+ //! Destructor
+ inline_ ~Point() {}
+
+ //! Clears the vector
+ inline_ Point& Zero() { x = y = z = 0.0f; return *this; }
+
+ //! + infinity
+ inline_ Point& SetPlusInfinity() { x = y = z = MAX_FLOAT; return *this; }
+ //! - infinity
+ inline_ Point& SetMinusInfinity() { x = y = z = MIN_FLOAT; return *this; }
+
+ //! Sets positive unit random vector
+ Point& PositiveUnitRandomVector();
+ //! Sets unit random vector
+ Point& UnitRandomVector();
+
+ //! Assignment from values
+ inline_ Point& Set(float xx, float yy, float zz) { x = xx; y = yy; z = zz; return *this; }
+ //! Assignment from array
+ inline_ Point& Set(const float f[3]) { x = f[X]; y = f[Y]; z = f[Z]; return *this; }
+ //! Assignment from another point
+ inline_ Point& Set(const Point& src) { x = src.x; y = src.y; z = src.z; return *this; }
+
+ //! Adds a vector
+ inline_ Point& Add(const Point& p) { x += p.x; y += p.y; z += p.z; return *this; }
+ //! Adds a vector
+ inline_ Point& Add(float xx, float yy, float zz) { x += xx; y += yy; z += zz; return *this; }
+ //! Adds a vector
+ inline_ Point& Add(const float f[3]) { x += f[X]; y += f[Y]; z += f[Z]; return *this; }
+ //! Adds vectors
+ inline_ Point& Add(const Point& p, const Point& q) { x = p.x+q.x; y = p.y+q.y; z = p.z+q.z; return *this; }
+
+ //! Subtracts a vector
+ inline_ Point& Sub(const Point& p) { x -= p.x; y -= p.y; z -= p.z; return *this; }
+ //! Subtracts a vector
+ inline_ Point& Sub(float xx, float yy, float zz) { x -= xx; y -= yy; z -= zz; return *this; }
+ //! Subtracts a vector
+ inline_ Point& Sub(const float f[3]) { x -= f[X]; y -= f[Y]; z -= f[Z]; return *this; }
+ //! Subtracts vectors
+ inline_ Point& Sub(const Point& p, const Point& q) { x = p.x-q.x; y = p.y-q.y; z = p.z-q.z; return *this; }
+
+ //! this = -this
+ inline_ Point& Neg() { x = -x; y = -y; z = -z; return *this; }
+ //! this = -a
+ inline_ Point& Neg(const Point& a) { x = -a.x; y = -a.y; z = -a.z; return *this; }
+
+ //! Multiplies by a scalar
+ inline_ Point& Mult(float s) { x *= s; y *= s; z *= s; return *this; }
+
+ //! this = a * scalar
+ inline_ Point& Mult(const Point& a, float scalar)
+ {
+ x = a.x * scalar;
+ y = a.y * scalar;
+ z = a.z * scalar;
+ return *this;
+ }
+
+ //! this = a + b * scalar
+ inline_ Point& Mac(const Point& a, const Point& b, float scalar)
+ {
+ x = a.x + b.x * scalar;
+ y = a.y + b.y * scalar;
+ z = a.z + b.z * scalar;
+ return *this;
+ }
+
+ //! this = this + a * scalar
+ inline_ Point& Mac(const Point& a, float scalar)
+ {
+ x += a.x * scalar;
+ y += a.y * scalar;
+ z += a.z * scalar;
+ return *this;
+ }
+
+ //! this = a - b * scalar
+ inline_ Point& Msc(const Point& a, const Point& b, float scalar)
+ {
+ x = a.x - b.x * scalar;
+ y = a.y - b.y * scalar;
+ z = a.z - b.z * scalar;
+ return *this;
+ }
+
+ //! this = this - a * scalar
+ inline_ Point& Msc(const Point& a, float scalar)
+ {
+ x -= a.x * scalar;
+ y -= a.y * scalar;
+ z -= a.z * scalar;
+ return *this;
+ }
+
+ //! this = a + b * scalarb + c * scalarc
+ inline_ Point& Mac2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc)
+ {
+ x = a.x + b.x * scalarb + c.x * scalarc;
+ y = a.y + b.y * scalarb + c.y * scalarc;
+ z = a.z + b.z * scalarb + c.z * scalarc;
+ return *this;
+ }
+
+ //! this = a - b * scalarb - c * scalarc
+ inline_ Point& Msc2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc)
+ {
+ x = a.x - b.x * scalarb - c.x * scalarc;
+ y = a.y - b.y * scalarb - c.y * scalarc;
+ z = a.z - b.z * scalarb - c.z * scalarc;
+ return *this;
+ }
+
+ //! this = mat * a
+ inline_ Point& Mult(const Matrix3x3& mat, const Point& a);
+
+ //! this = mat1 * a1 + mat2 * a2
+ inline_ Point& Mult2(const Matrix3x3& mat1, const Point& a1, const Matrix3x3& mat2, const Point& a2);
+
+ //! this = this + mat * a
+ inline_ Point& Mac(const Matrix3x3& mat, const Point& a);
+
+ //! this = transpose(mat) * a
+ inline_ Point& TransMult(const Matrix3x3& mat, const Point& a);
+
+ //! Linear interpolate between two vectors: this = a + t * (b - a)
+ inline_ Point& Lerp(const Point& a, const Point& b, float t)
+ {
+ x = a.x + t * (b.x - a.x);
+ y = a.y + t * (b.y - a.y);
+ z = a.z + t * (b.z - a.z);
+ return *this;
+ }
+
+ //! Hermite interpolate between p1 and p2. p0 and p3 are used for finding gradient at p1 and p2.
+ //! this = p0 * (2t^2 - t^3 - t)/2
+ //! + p1 * (3t^3 - 5t^2 + 2)/2
+ //! + p2 * (4t^2 - 3t^3 + t)/2
+ //! + p3 * (t^3 - t^2)/2
+ inline_ Point& Herp(const Point& p0, const Point& p1, const Point& p2, const Point& p3, float t)
+ {
+ float t2 = t * t;
+ float t3 = t2 * t;
+ float kp0 = (2.0f * t2 - t3 - t) * 0.5f;
+ float kp1 = (3.0f * t3 - 5.0f * t2 + 2.0f) * 0.5f;
+ float kp2 = (4.0f * t2 - 3.0f * t3 + t) * 0.5f;
+ float kp3 = (t3 - t2) * 0.5f;
+ x = p0.x * kp0 + p1.x * kp1 + p2.x * kp2 + p3.x * kp3;
+ y = p0.y * kp0 + p1.y * kp1 + p2.y * kp2 + p3.y * kp3;
+ z = p0.z * kp0 + p1.z * kp1 + p2.z * kp2 + p3.z * kp3;
+ return *this;
+ }
+
+ //! this = rotpos * r + linpos
+ inline_ Point& Transform(const Point& r, const Matrix3x3& rotpos, const Point& linpos);
+
+ //! this = trans(rotpos) * (r - linpos)
+ inline_ Point& InvTransform(const Point& r, const Matrix3x3& rotpos, const Point& linpos);
+
+ //! Returns MIN(x, y, z);
+ inline_ float Min() const { return MIN(x, MIN(y, z)); }
+ //! Returns MAX(x, y, z);
+ inline_ float Max() const { return MAX(x, MAX(y, z)); }
+ //! Sets each element to be componentwise minimum
+ inline_ Point& Min(const Point& p) { x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z); return *this; }
+ //! Sets each element to be componentwise maximum
+ inline_ Point& Max(const Point& p) { x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z); return *this; }
+
+ //! Clamps each element
+ inline_ Point& Clamp(float min, float max)
+ {
+ if(x<min) x=min; if(x>max) x=max;
+ if(y<min) y=min; if(y>max) y=max;
+ if(z<min) z=min; if(z>max) z=max;
+ return *this;
+ }
+
+ //! Computes square magnitude
+ inline_ float SquareMagnitude() const { return x*x + y*y + z*z; }
+ //! Computes magnitude
+ inline_ float Magnitude() const { return sqrtf(x*x + y*y + z*z); }
+ //! Computes volume
+ inline_ float Volume() const { return x * y * z; }
+
+ //! Checks the point is near zero
+ inline_ bool ApproxZero() const { return SquareMagnitude() < EPSILON2; }
+
+ //! Tests for exact zero vector
+ inline_ BOOL IsZero() const
+ {
+ if(IR(x) || IR(y) || IR(z)) return FALSE;
+ return TRUE;
+ }
+
+ //! Checks point validity
+ inline_ BOOL IsValid() const
+ {
+ if(!IsValidFloat(x)) return FALSE;
+ if(!IsValidFloat(y)) return FALSE;
+ if(!IsValidFloat(z)) return FALSE;
+ return TRUE;
+ }
+
+ //! Slighty moves the point
+ void Tweak(udword coord_mask, udword tweak_mask)
+ {
+ if(coord_mask&1) { udword Dummy = IR(x); Dummy^=tweak_mask; x = FR(Dummy); }
+ if(coord_mask&2) { udword Dummy = IR(y); Dummy^=tweak_mask; y = FR(Dummy); }
+ if(coord_mask&4) { udword Dummy = IR(z); Dummy^=tweak_mask; z = FR(Dummy); }
+ }
+
+ #define TWEAKMASK 0x3fffff
+ #define TWEAKNOTMASK ~TWEAKMASK
+ //! Slighty moves the point out
+ inline_ void TweakBigger()
+ {
+ udword Dummy = (IR(x)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(x)) Dummy+=TWEAKMASK+1; x = FR(Dummy);
+ Dummy = (IR(y)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(y)) Dummy+=TWEAKMASK+1; y = FR(Dummy);
+ Dummy = (IR(z)&TWEAKNOTMASK); if(!IS_NEGATIVE_FLOAT(z)) Dummy+=TWEAKMASK+1; z = FR(Dummy);
+ }
+
+ //! Slighty moves the point in
+ inline_ void TweakSmaller()
+ {
+ udword Dummy = (IR(x)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(x)) Dummy+=TWEAKMASK+1; x = FR(Dummy);
+ Dummy = (IR(y)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(y)) Dummy+=TWEAKMASK+1; y = FR(Dummy);
+ Dummy = (IR(z)&TWEAKNOTMASK); if(IS_NEGATIVE_FLOAT(z)) Dummy+=TWEAKMASK+1; z = FR(Dummy);
+ }
+
+ //! Normalizes the vector
+ inline_ Point& Normalize()
+ {
+ float M = x*x + y*y + z*z;
+ if(!_equal(M, 0.0))
+ {
+ M = 1.0f / sqrtf(M);
+ x *= M;
+ y *= M;
+ z *= M;
+ }
+ return *this;
+ }
+
+ //! Sets vector length
+ inline_ Point& SetLength(float length)
+ {
+ float NewLength = length / Magnitude();
+ x *= NewLength;
+ y *= NewLength;
+ z *= NewLength;
+ return *this;
+ }
+
+ //! Clamps vector length
+ inline_ Point& ClampLength(float limit_length)
+ {
+ if(limit_length>=0.0f) // Magnitude must be positive
+ {
+ float CurrentSquareLength = SquareMagnitude();
+
+ if(CurrentSquareLength > limit_length * limit_length)
+ {
+ float Coeff = limit_length / sqrtf(CurrentSquareLength);
+ x *= Coeff;
+ y *= Coeff;
+ z *= Coeff;
+ }
+ }
+ return *this;
+ }
+
+ //! Computes distance to another point
+ inline_ float Distance(const Point& b) const
+ {
+ return sqrtf((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
+ }
+
+ //! Computes square distance to another point
+ inline_ float SquareDistance(const Point& b) const
+ {
+ return ((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
+ }
+
+ //! Dot product dp = this|a
+ inline_ float Dot(const Point& p) const { return p.x * x + p.y * y + p.z * z; }
+
+ //! Cross product this = a x b
+ inline_ Point& Cross(const Point& a, const Point& b)
+ {
+ x = a.y * b.z - a.z * b.y;
+ y = a.z * b.x - a.x * b.z;
+ z = a.x * b.y - a.y * b.x;
+ return *this;
+ }
+
+ //! Vector code ( bitmask = sign(z) | sign(y) | sign(x) )
+ inline_ udword VectorCode() const
+ {
+ return (IR(x)>>31) | ((IR(y)&SIGN_BITMASK)>>30) | ((IR(z)&SIGN_BITMASK)>>29);
+ }
+
+ //! Returns largest axis
+ inline_ PointComponent LargestAxis() const
+ {
+ const float* Vals = &x;
+ PointComponent m = X;
+ if(Vals[Y] > Vals[m]) m = Y;
+ if(Vals[Z] > Vals[m]) m = Z;
+ return m;
+ }
+
+ //! Returns closest axis
+ inline_ PointComponent ClosestAxis() const
+ {
+ const float* Vals = &x;
+ PointComponent m = X;
+ if(AIR(Vals[Y]) > AIR(Vals[m])) m = Y;
+ if(AIR(Vals[Z]) > AIR(Vals[m])) m = Z;
+ return m;
+ }
+
+ //! Returns smallest axis
+ inline_ PointComponent SmallestAxis() const
+ {
+ const float* Vals = &x;
+ PointComponent m = X;
+ if(Vals[Y] < Vals[m]) m = Y;
+ if(Vals[Z] < Vals[m]) m = Z;
+ return m;
+ }
+
+ //! Refracts the point
+ Point& Refract(const Point& eye, const Point& n, float refractindex, Point& refracted);
+
+ //! Projects the point onto a plane
+ Point& ProjectToPlane(const Plane& p);
+
+ //! Projects the point onto the screen
+ void ProjectToScreen(float halfrenderwidth, float halfrenderheight, const Matrix4x4& mat, HPoint& projected) const;
+
+ //! Unfolds the point onto a plane according to edge(a,b)
+ Point& Unfold(Plane& p, Point& a, Point& b);
+
+ //! Hash function from Ville Miettinen
+ inline_ udword GetHashValue() const
+ {
+ const udword* h = (const udword*)(this);
+ udword f = (h[0]+h[1]*11-(h[2]*17)) & 0x7fffffff; // avoid problems with +-0
+ return (f>>22)^(f>>12)^(f);
+ }
+
+ //! Stuff magic values in the point, marking it as explicitely not used.
+ void SetNotUsed();
+ //! Checks the point is marked as not used
+ BOOL IsNotUsed() const;
+
+ // Arithmetic operators
+
+ //! Unary operator for Point Negate = - Point
+ inline_ Point operator-() const { return Point(-x, -y, -z); }
+
+ //! Operator for Point Plus = Point + Point.
+ inline_ Point operator+(const Point& p) const { return Point(x + p.x, y + p.y, z + p.z); }
+ //! Operator for Point Minus = Point - Point.
+ inline_ Point operator-(const Point& p) const { return Point(x - p.x, y - p.y, z - p.z); }
+
+ //! Operator for Point Mul = Point * Point.
+ inline_ Point operator*(const Point& p) const { return Point(x * p.x, y * p.y, z * p.z); }
+ //! Operator for Point Scale = Point * float.
+ inline_ Point operator*(float s) const { return Point(x * s, y * s, z * s ); }
+ //! Operator for Point Scale = float * Point.
+ inline_ friend Point operator*(float s, const Point& p) { return Point(s * p.x, s * p.y, s * p.z); }
+
+ //! Operator for Point Div = Point / Point.
+ inline_ Point operator/(const Point& p) const { return Point(x / p.x, y / p.y, z / p.z); }
+ //! Operator for Point Scale = Point / float.
+ inline_ Point operator/(float s) const { s = 1.0f / s; return Point(x * s, y * s, z * s); }
+ //! Operator for Point Scale = float / Point.
+ inline_ friend Point operator/(float s, const Point& p) { return Point(s / p.x, s / p.y, s / p.z); }
+
+ //! Operator for float DotProd = Point | Point.
+ inline_ float operator|(const Point& p) const { return x*p.x + y*p.y + z*p.z; }
+ //! Operator for Point VecProd = Point ^ Point.
+ inline_ Point operator^(const Point& p) const
+ {
+ return Point(
+ y * p.z - z * p.y,
+ z * p.x - x * p.z,
+ x * p.y - y * p.x );
+ }
+
+ //! Operator for Point += Point.
+ inline_ Point& operator+=(const Point& p) { x += p.x; y += p.y; z += p.z; return *this; }
+ //! Operator for Point += float.
+ inline_ Point& operator+=(float s) { x += s; y += s; z += s; return *this; }
+
+ //! Operator for Point -= Point.
+ inline_ Point& operator-=(const Point& p) { x -= p.x; y -= p.y; z -= p.z; return *this; }
+ //! Operator for Point -= float.
+ inline_ Point& operator-=(float s) { x -= s; y -= s; z -= s; return *this; }
+
+ //! Operator for Point *= Point.
+ inline_ Point& operator*=(const Point& p) { x *= p.x; y *= p.y; z *= p.z; return *this; }
+ //! Operator for Point *= float.
+ inline_ Point& operator*=(float s) { x *= s; y *= s; z *= s; return *this; }
+
+ //! Operator for Point /= Point.
+ inline_ Point& operator/=(const Point& p) { x /= p.x; y /= p.y; z /= p.z; return *this; }
+ //! Operator for Point /= float.
+ inline_ Point& operator/=(float s) { s = 1.0f/s; x *= s; y *= s; z *= s; return *this; }
+
+ // Logical operators
+
+ //! Operator for "if(Point==Point)"
+ inline_ bool operator==(const Point& p) const { return ( (IR(x)==IR(p.x))&&(IR(y)==IR(p.y))&&(IR(z)==IR(p.z))); }
+ //! Operator for "if(Point!=Point)"
+ inline_ bool operator!=(const Point& p) const { return ( (IR(x)!=IR(p.x))||(IR(y)!=IR(p.y))||(IR(z)!=IR(p.z))); }
+
+ // Arithmetic operators
+
+ //! Operator for Point Mul = Point * Matrix3x3.
+ inline_ Point operator*(const Matrix3x3& mat) const
+ {
+ class ShadowMatrix3x3{ public: float m[3][3]; }; // To allow inlining
+ const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;
+
+ return Point(
+ x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0],
+ x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1],
+ x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] );
+ }
+
+ //! Operator for Point Mul = Point * Matrix4x4.
+ inline_ Point operator*(const Matrix4x4& mat) const
+ {
+ class ShadowMatrix4x4{ public: float m[4][4]; }; // To allow inlining
+ const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;
+
+ return Point(
+ x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0],
+ x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1],
+ x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2]);
+ }
+
+ //! Operator for Point *= Matrix3x3.
+ inline_ Point& operator*=(const Matrix3x3& mat)
+ {
+ class ShadowMatrix3x3{ public: float m[3][3]; }; // To allow inlining
+ const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;
+
+ float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0];
+ float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1];
+ float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2];
+
+ x = xp; y = yp; z = zp;
+
+ return *this;
+ }
+
+ //! Operator for Point *= Matrix4x4.
+ inline_ Point& operator*=(const Matrix4x4& mat)
+ {
+ class ShadowMatrix4x4{ public: float m[4][4]; }; // To allow inlining
+ const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;
+
+ float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0];
+ float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1];
+ float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2];
+
+ x = xp; y = yp; z = zp;
+
+ return *this;
+ }
+
+ // Cast operators
+
+ //! Cast a Point to a HPoint. w is set to zero.
+ operator HPoint() const;
+
+ inline_ operator const float*() const { return &x; }
+ inline_ operator float*() { return &x; }
+
+ public:
+ float x, y, z;
+ };
+
+ FUNCTION ICEMATHS_API void Normalize1(Point& a);
+ FUNCTION ICEMATHS_API void Normalize2(Point& a);
#endif //__ICEPOINT_H__
diff --git a/deps/opende/OPCODE/Ice/IceTypes.h b/deps/opende/OPCODE/Ice/IceTypes.h
index c30890c..e130871 100644
--- a/deps/opende/OPCODE/Ice/IceTypes.h
+++ b/deps/opende/OPCODE/Ice/IceTypes.h
@@ -1,9 +1,9 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
- * Contains custom types.
- * \file IceTypes.h
- * \author Pierre Terdiman
- * \date April, 4, 2000
+ * Contains custom types.
+ * \file IceTypes.h
+ * \author Pierre Terdiman
+ * \date April, 4, 2000
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -17,144 +17,146 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- #define USE_HANDLE_MANAGER
-
- // Constants
- #define PI 3.1415926535897932384626433832795028841971693993751f //!< PI
- #define HALFPI 1.57079632679489661923f //!< 0.5 * PI
- #define TWOPI 6.28318530717958647692f //!< 2.0 * PI
- #define INVPI 0.31830988618379067154f //!< 1.0 / PI
-
- #define RADTODEG 57.2957795130823208768f //!< 180.0 / PI, convert radians to degrees
- #define DEGTORAD 0.01745329251994329577f //!< PI / 180.0, convert degrees to radians
-
- #define EXP 2.71828182845904523536f //!< e
- #define INVLOG2 3.32192809488736234787f //!< 1.0 / log10(2)
- #define LN2 0.693147180559945f //!< ln(2)
- #define INVLN2 1.44269504089f //!< 1.0f / ln(2)
-
- #define INV3 0.33333333333333333333f //!< 1/3
- #define INV6 0.16666666666666666666f //!< 1/6
- #define INV7 0.14285714285714285714f //!< 1/7
- #define INV9 0.11111111111111111111f //!< 1/9
- #define INV255 0.00392156862745098039f //!< 1/255
-
- #define SQRT2 1.41421356237f //!< sqrt(2)
- #define INVSQRT2 0.707106781188f //!< 1 / sqrt(2)
-
- #define SQRT3 1.73205080757f //!< sqrt(3)
- #define INVSQRT3 0.577350269189f //!< 1 / sqrt(3)
-
- #define null 0 //!< our own NULL pointer
-
- // Custom types used in ICE
- typedef signed char sbyte; //!< sizeof(sbyte) must be 1
- typedef unsigned char ubyte; //!< sizeof(ubyte) must be 1
- typedef signed short sword; //!< sizeof(sword) must be 2
- typedef unsigned short uword; //!< sizeof(uword) must be 2
- typedef signed int sdword; //!< sizeof(sdword) must be 4
- typedef unsigned int udword; //!< sizeof(udword) must be 4
- typedef signed __int64 sqword; //!< sizeof(sqword) must be 8
- typedef unsigned __int64 uqword; //!< sizeof(uqword) must be 8
- typedef float float32; //!< sizeof(float32) must be 4
- typedef double float64; //!< sizeof(float64) must be 4
-
- ICE_COMPILE_TIME_ASSERT(sizeof(ubyte)==1);
- ICE_COMPILE_TIME_ASSERT(sizeof(sbyte)==1);
- ICE_COMPILE_TIME_ASSERT(sizeof(sword)==2);
- ICE_COMPILE_TIME_ASSERT(sizeof(uword)==2);
- ICE_COMPILE_TIME_ASSERT(sizeof(udword)==4);
- ICE_COMPILE_TIME_ASSERT(sizeof(sdword)==4);
- ICE_COMPILE_TIME_ASSERT(sizeof(uqword)==8);
- ICE_COMPILE_TIME_ASSERT(sizeof(sqword)==8);
-
- //! TO BE DOCUMENTED
- #define DECLARE_ICE_HANDLE(name) struct name##__ { int unused; }; typedef struct name##__ *name
-
- typedef udword DynID; //!< Dynamic identifier
+ #define USE_HANDLE_MANAGER
+
+ // Constants
+ #define PI 3.1415926535897932384626433832795028841971693993751f //!< PI
+ #define HALFPI 1.57079632679489661923f //!< 0.5 * PI
+ #define TWOPI 6.28318530717958647692f //!< 2.0 * PI
+ #define INVPI 0.31830988618379067154f //!< 1.0 / PI
+
+ #define RADTODEG 57.2957795130823208768f //!< 180.0 / PI, convert radians to degrees
+ #define DEGTORAD 0.01745329251994329577f //!< PI / 180.0, convert degrees to radians
+
+ #define EXP 2.71828182845904523536f //!< e
+ #define INVLOG2 3.32192809488736234787f //!< 1.0 / log10(2)
+ #define LN2 0.693147180559945f //!< ln(2)
+ #define INVLN2 1.44269504089f //!< 1.0f / ln(2)
+
+ #define INV3 0.33333333333333333333f //!< 1/3
+ #define INV6 0.16666666666666666666f //!< 1/6
+ #define INV7 0.14285714285714285714f //!< 1/7
+ #define INV9 0.11111111111111111111f //!< 1/9
+ #define INV255 0.00392156862745098039f //!< 1/255
+
+ #define SQRT2 1.41421356237f //!< sqrt(2)
+ #define INVSQRT2 0.707106781188f //!< 1 / sqrt(2)
+
+ #define SQRT3 1.73205080757f //!< sqrt(3)
+ #define INVSQRT3 0.577350269189f //!< 1 / sqrt(3)
+
+ #define null 0 //!< our own NULL pointer
+
+ // Custom types used in ICE
+ typedef signed char sbyte; //!< sizeof(sbyte) must be 1
+ typedef unsigned char ubyte; //!< sizeof(ubyte) must be 1
+ typedef signed short sword; //!< sizeof(sword) must be 2
+ typedef unsigned short uword; //!< sizeof(uword) must be 2
+ typedef signed int sdword; //!< sizeof(sdword) must be 4
+ typedef unsigned int udword; //!< sizeof(udword) must be 4
+ typedef signed __int64 sqword; //!< sizeof(sqword) must be 8
+ typedef unsigned __int64 uqword; //!< sizeof(uqword) must be 8
+ typedef float float32; //!< sizeof(float32) must be 4
+ typedef double float64; //!< sizeof(float64) must be 4
+
+ ICE_COMPILE_TIME_ASSERT(sizeof(ubyte)==1);
+ ICE_COMPILE_TIME_ASSERT(sizeof(sbyte)==1);
+ ICE_COMPILE_TIME_ASSERT(sizeof(sword)==2);
+ ICE_COMPILE_TIME_ASSERT(sizeof(uword)==2);
+ ICE_COMPILE_TIME_ASSERT(sizeof(udword)==4);
+ ICE_COMPILE_TIME_ASSERT(sizeof(sdword)==4);
+ ICE_COMPILE_TIME_ASSERT(sizeof(uqword)==8);
+ ICE_COMPILE_TIME_ASSERT(sizeof(sqword)==8);
+
+ //! TO BE DOCUMENTED
+ #define DECLARE_ICE_HANDLE(name) struct name##__ { int unused; }; typedef struct name##__ *name
+
+ typedef udword DynID; //!< Dynamic identifier
#ifdef USE_HANDLE_MANAGER
- typedef udword KID; //!< Kernel ID
-// DECLARE_ICE_HANDLE(KID);
+ typedef udword KID; //!< Kernel ID
+// DECLARE_ICE_HANDLE(KID);
#else
- typedef uword KID; //!< Kernel ID
+ typedef uword KID; //!< Kernel ID
#endif
- typedef udword RTYPE; //!< Relationship-type (!) between owners and references
- #define INVALID_ID 0xffffffff //!< Invalid dword ID (counterpart of null pointers)
+ typedef udword RTYPE; //!< Relationship-type (!) between owners and references
+ #define INVALID_ID 0xffffffff //!< Invalid dword ID (counterpart of null pointers)
#ifdef USE_HANDLE_MANAGER
- #define INVALID_KID 0xffffffff //!< Invalid Kernel ID
+ #define INVALID_KID 0xffffffff //!< Invalid Kernel ID
#else
- #define INVALID_KID 0xffff //!< Invalid Kernel ID
+ #define INVALID_KID 0xffff //!< Invalid Kernel ID
#endif
- #define INVALID_NUMBER 0xDEADBEEF //!< Standard junk value
-
- // Define BOOL if needed
- #ifndef BOOL
- typedef int BOOL; //!< Another boolean type.
- #endif
-
- //! Union of a float and a sdword
- typedef union {
- float f; //!< The float
- sdword d; //!< The integer
- }scell;
-
- //! Union of a float and a udword
- typedef union {
- float f; //!< The float
- udword d; //!< The integer
- }ucell;
-
- // Type ranges
- #define MAX_SBYTE 0x7f //!< max possible sbyte value
- #define MIN_SBYTE 0x80 //!< min possible sbyte value
- #define MAX_UBYTE 0xff //!< max possible ubyte value
- #define MIN_UBYTE 0x00 //!< min possible ubyte value
- #define MAX_SWORD 0x7fff //!< max possible sword value
- #define MIN_SWORD 0x8000 //!< min possible sword value
- #define MAX_UWORD 0xffff //!< max possible uword value
- #define MIN_UWORD 0x0000 //!< min possible uword value
- #define MAX_SDWORD 0x7fffffff //!< max possible sdword value
- #define MIN_SDWORD 0x80000000 //!< min possible sdword value
- #define MAX_UDWORD 0xffffffff //!< max possible udword value
- #define MIN_UDWORD 0x00000000 //!< min possible udword value
- #define MAX_FLOAT FLT_MAX //!< max possible float value
- #define MIN_FLOAT (-FLT_MAX) //!< min possible loat value
- #define IEEE_1_0 0x3f800000 //!< integer representation of 1.0
- #define IEEE_255_0 0x437f0000 //!< integer representation of 255.0
- #define IEEE_MAX_FLOAT 0x7f7fffff //!< integer representation of MAX_FLOAT
- #define IEEE_MIN_FLOAT 0xff7fffff //!< integer representation of MIN_FLOAT
- #define IEEE_UNDERFLOW_LIMIT 0x1a000000
-
- #define ONE_OVER_RAND_MAX (1.0f / float(RAND_MAX)) //!< Inverse of the max possible value returned by rand()
-
- typedef int (__stdcall* PROC)(); //!< A standard procedure call.
- typedef bool (*ENUMERATION)(udword value, udword param, udword context); //!< ICE standard enumeration call
- typedef void** VTABLE; //!< A V-Table.
-
- #undef MIN
- #undef MAX
- #define MIN(a, b) ((a) < (b) ? (a) : (b)) //!< Returns the min value between a and b
- #define MAX(a, b) ((a) > (b) ? (a) : (b)) //!< Returns the max value between a and b
- #define MAXMAX(a,b,c) ((a) > (b) ? MAX (a,c) : MAX (b,c)) //!< Returns the max value between a, b and c
-
- template<class T> inline_ const T& TMin (const T& a, const T& b) { return b < a ? b : a; }
- template<class T> inline_ const T& TMax (const T& a, const T& b) { return a < b ? b : a; }
- template<class T> inline_ void TSetMin (T& a, const T& b) { if(a>b) a = b; }
- template<class T> inline_ void TSetMax (T& a, const T& b) { if(a<b) a = b; }
-
- #define SQR(x) ((x)*(x)) //!< Returns x square
- #define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube
-
- #define AND & //!< ...
- #define OR | //!< ...
- #define XOR ^ //!< ...
-
- #define QUADRAT(x) ((x)*(x)) //!< Returns x square
+ #define INVALID_NUMBER 0xDEADBEEF //!< Standard junk value
+
+ // Define BOOL if needed
+ #ifndef BOOL
+ typedef int BOOL; //!< Another boolean type.
+ #endif
+
+ //! Union of a float and a sdword
+ typedef union {
+ float f; //!< The float
+ sdword d; //!< The integer
+ }scell;
+
+ //! Union of a float and a udword
+ typedef union {
+ float f; //!< The float
+ udword d; //!< The integer
+ }ucell;
+
+ // Type ranges
+ #define MAX_SBYTE 0x7f //!< max possible sbyte value
+ #define MIN_SBYTE 0x80 //!< min possible sbyte value
+ #define MAX_UBYTE 0xff //!< max possible ubyte value
+ #define MIN_UBYTE 0x00 //!< min possible ubyte value
+ #define MAX_SWORD 0x7fff //!< max possible sword value
+ #define MIN_SWORD 0x8000 //!< min possible sword value
+ #define MAX_UWORD 0xffff //!< max possible uword value
+ #define MIN_UWORD 0x0000 //!< min possible uword value
+ #define MAX_SDWORD 0x7fffffff //!< max possible sdword value
+ #define MIN_SDWORD 0x80000000 //!< min possible sdword value
+ #define MAX_UDWORD 0xffffffff //!< max possible udword value
+ #define MIN_UDWORD 0x00000000 //!< min possible udword value
+ #define MAX_FLOAT FLT_MAX //!< max possible float value
+ #define MIN_FLOAT (-FLT_MAX) //!< min possible loat value
+ #define IEEE_1_0 0x3f800000 //!< integer representation of 1.0
+ #define IEEE_255_0 0x437f0000 //!< integer representation of 255.0
+ #define IEEE_MAX_FLOAT 0x7f7fffff //!< integer representation of MAX_FLOAT
+ #define IEEE_MIN_FLOAT 0xff7fffff //!< integer representation of MIN_FLOAT
+ #define IEEE_UNDERFLOW_LIMIT 0x1a000000
+
+ #define ONE_OVER_RAND_MAX (1.0f / float(RAND_MAX)) //!< Inverse of the max possible value returned by rand()
+
+#ifndef _WIN32
+ typedef int (__stdcall* PROC)(); //!< A standard procedure call.
+#endif
+ typedef bool (*ENUMERATION)(udword value, udword param, udword context); //!< ICE standard enumeration call
+ typedef void** VTABLE; //!< A V-Table.
+
+ #undef MIN
+ #undef MAX
+ #define MIN(a, b) ((a) < (b) ? (a) : (b)) //!< Returns the min value between a and b
+ #define MAX(a, b) ((a) > (b) ? (a) : (b)) //!< Returns the max value between a and b
+ #define MAXMAX(a,b,c) ((a) > (b) ? MAX (a,c) : MAX (b,c)) //!< Returns the max value between a, b and c
+
+ template<class T> inline_ const T& TMin (const T& a, const T& b) { return b < a ? b : a; }
+ template<class T> inline_ const T& TMax (const T& a, const T& b) { return a < b ? b : a; }
+ template<class T> inline_ void TSetMin (T& a, const T& b) { if(a>b) a = b; }
+ template<class T> inline_ void TSetMax (T& a, const T& b) { if(a<b) a = b; }
+
+ #define SQR(x) ((x)*(x)) //!< Returns x square
+ #define CUBE(x) ((x)*(x)*(x)) //!< Returns x cube
+
+ #define AND & //!< ...
+ #define OR | //!< ...
+ #define XOR ^ //!< ...
+
+ #define QUADRAT(x) ((x)*(x)) //!< Returns x square
#ifdef _WIN32
# define srand48(x) srand((unsigned int) (x))
-# define srandom(x) srand((unsigned int) (x))
-# define random() ((double) rand())
+# define srandom(x) srand((unsigned int) (x))
+# define random() ((double) rand())
# define drand48() ((double) (((double) rand()) / ((double) RAND_MAX)))
#endif
diff --git a/deps/opende/OPCODE/Opcode.h b/deps/opende/OPCODE/Opcode.h
index 288156a..03e4dd6 100644
--- a/deps/opende/OPCODE/Opcode.h
+++ b/deps/opende/OPCODE/Opcode.h
@@ -9,9 +9,9 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Main file for Opcode.dll.
- * \file Opcode.h
- * \author Pierre Terdiman
- * \date March, 20, 2001
+ * \file Opcode.h
+ * \author Pierre Terdiman
+ * \date March, 20, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -25,8 +25,6 @@
// may not compile being not able to find std::va_list
#include <stdarg.h>
-#include <gazebo/util/system.hh>
-
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Things to help us compile on non-windows platforms
@@ -59,11 +57,11 @@
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Automatic linking
#ifndef BAN_OPCODE_AUTOLINK
- #ifdef _DEBUG
- //#pragma comment(lib, "Opcode_D.lib")
- #else
- //#pragma comment(lib, "Opcode.lib")
- #endif
+ #ifdef _DEBUG
+ //#pragma comment(lib, "Opcode_D.lib")
+ #else
+ //#pragma comment(lib, "Opcode.lib")
+ #endif
#endif
#endif
#endif
@@ -81,7 +79,31 @@
// #endif
// Preprocessor__
-#define OPCODE_API GAZEBO_VISIBLE
+
+#if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_OPCODE
+ #ifdef __GNUC__
+ #define OPCODE_API __attribute__ ((dllexport))
+ #else
+ #define OPCODE_API __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define OPCODE_API __attribute__ ((dllimport))
+ #else
+ #define OPCODE_API __declspec(dllimport)
+ #endif
+ #endif
+ #define OPCODE_HIDDEN
+#else
+ #if __GNUC__ >= 4
+ #define OPCODE_API __attribute__ ((visibility ("default")))
+ #define OPCODE_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define OPCODE_API
+ #define OPCODE_HIDDEN
+ #endif
+#endif
#include "OPC_Settings.h"
#include "OPC_IceHook.h"
diff --git a/deps/opende/include/ode/h5dump.h b/deps/opende/include/ode/h5dump.h
new file mode 100644
index 0000000..575dc00
--- /dev/null
+++ b/deps/opende/include/ode/h5dump.h
@@ -0,0 +1,45 @@
+/*
+Copyright (c) 2013 Claude Lacoursiere
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any damages
+arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you must not
+ claim that you wrote the original software. If you use this software
+ in a product, an acknowledgment in the product documentation would be
+ appreciated but is not required.
+
+ 2. Altered source versions must be plainly marked as such, and must not be
+ misrepresented as being the original software.
+
+ 3. This notice may not be removed or altered from any source
+ distribution.
+
+*/
+
+#include "gazebo/gazebo_config.h"
+#ifdef HDF5_INSTRUMENT
+
+
+#ifndef H5_IO_C
+#define H5_IO_C
+
+struct dxWorld;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+ void h5dump_world(const char * file, dxWorld * w, dReal stepsize);
+ void h5_write_errors(const char *, dReal * errors, unsigned int n);
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+#endif // HDF5_INSTRUMENT
diff --git a/deps/opende/include/ode/misc.h b/deps/opende/include/ode/misc.h
index fb89369..e46a807 100644
--- a/deps/opende/include/ode/misc.h
+++ b/deps/opende/include/ode/misc.h
@@ -56,9 +56,9 @@ ODE_API dReal dRandReal(void);
/* print out a matrix */
#ifdef __cplusplus
ODE_API void dPrintMatrix (const dReal *A, int n, int m, char *fmt = "%10.4f ",
- FILE *f=stdout);
+ FILE *f=stdout);
ODE_API void dPrintIntMatrix (const int *A, int n, int m, char *fmt = "%5d ",
- FILE *f=stdout);
+ FILE *f=stdout);
#else
ODE_API void dPrintMatrix (const dReal *A, int n, int m, char *fmt, FILE *f);
ODE_API void dPrintIntMatrix (const int *A, int n, int m, char *fmt, FILE *f);
@@ -86,7 +86,7 @@ ODE_API dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n
* Normalizes the angle to be 0 to 2*M_PI
* It takes and returns radians.
*/
-ODE_API static inline dReal dNormalizeAnglePositive(dReal angle)
+static inline dReal dNormalizeAnglePositive(dReal angle)
{
return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
}
@@ -98,8 +98,8 @@ ODE_API static inline dReal dNormalizeAnglePositive(dReal angle)
* Normalizes the angle from [0, 2*M_PI] to [-M_PI, +M_PI] circle
* It takes and returns radians.
*
- */
-ODE_API static inline dReal dNormalizeAngle(dReal angle)
+ */
+static inline dReal dNormalizeAngle(dReal angle)
{
dReal a = dNormalizeAnglePositive(angle);
if (a > M_PI)
@@ -107,7 +107,7 @@ ODE_API static inline dReal dNormalizeAngle(dReal angle)
return a;
}
-
+
/*!
* \function
* \brief dShortestAngularDistance
@@ -119,7 +119,7 @@ ODE_API static inline dReal dNormalizeAngle(dReal angle)
* would always be -pi <= result <= pi. Adding the result
* to "from" will always get you an equivelent angle to "to".
*/
-ODE_API static inline dReal dShortestAngularDistance(dReal from, dReal to)
+static inline dReal dShortestAngularDistance(dReal from, dReal to)
{
dReal result = dNormalizeAngle(dNormalizeAnglePositive(dNormalizeAnglePositive(to) -
dNormalizeAnglePositive(from)));
@@ -138,7 +138,7 @@ ODE_API static inline dReal dShortestAngularDistance(dReal from, dReal to)
* However, if |delta| > tol, then this function simply returns incoming parameter "to".
*
*/
-ODE_API static inline dReal dShortestAngularDistanceUpdate(dReal from, dReal to, dReal tol = 0.3)
+static inline dReal dShortestAngularDistanceUpdate(dReal from, dReal to, dReal tol = 0.3)
{
dReal result = dShortestAngularDistance(from, to);
diff --git a/deps/opende/include/ode/objects.h b/deps/opende/include/ode/objects.h
index 24b6d97..27f4c7c 100644
--- a/deps/opende/include/ode/objects.h
+++ b/deps/opende/include/ode/objects.h
@@ -1,4 +1,3 @@
-
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
@@ -32,6 +31,22 @@
extern "C" {
#endif
+/// \brief Enum for Friction_Model
+/// Enum of different friction models
+enum Friction_Model {
+ pyramid_friction,
+ cone_friction,
+ box_friction
+};
+
+/// \brief Enum for World_Solver_Type
+/// Enum of world stepper LCP solver choices
+enum World_Solver_Type{
+ ODE_DEFAULT,
+ DART_PGS,
+ BULLET_PGS
+};
+
/**
* @defgroup world World
*
@@ -137,24 +152,24 @@ ODE_API dReal dWorldGetCFM (dWorldID);
* @brief Set the world to use shared working memory along with another world.
*
* The worlds allocate working memory internally for simulation stepping. This
- * memory is cached among the calls to @c dWordStep and @c dWorldQuickStep.
- * Similarly, several worlds can be set up to share this memory caches thus
- * reducing overall memory usage by cost of making worlds inappropriate for
+ * memory is cached among the calls to @c dWordStep and @c dWorldQuickStep.
+ * Similarly, several worlds can be set up to share this memory caches thus
+ * reducing overall memory usage by cost of making worlds inappropriate for
* simultaneous simulation in multiple threads.
*
- * If null value is passed for @a from_world parameter the world is detached from
- * sharing and returns to defaults for working memory, reservation policy and
- * memory manager as if just created. This can also be used to enable use of shared
+ * If null value is passed for @a from_world parameter the world is detached from
+ * sharing and returns to defaults for working memory, reservation policy and
+ * memory manager as if just created. This can also be used to enable use of shared
* memory for a world that has already had working memory allocated privately.
* Normally using shared memory after a world has its private working memory allocated
* is prohibited.
*
* Allocation policy used can only increase world's internal reserved memory size
- * and never decreases it. @c dWorldCleanupWorkingMemory can be used to release
- * working memory for a world in case if number of objects/joint decreases
+ * and never decreases it. @c dWorldCleanupWorkingMemory can be used to release
+ * working memory for a world in case if number of objects/joint decreases
* significantly in it.
*
- * With sharing working memory worlds also automatically share memory reservation
+ * With sharing working memory worlds also automatically share memory reservation
* policy and memory manager. Thus, these parameters need to be customized for
* initial world to be used as sharing source only.
*
@@ -174,13 +189,13 @@ ODE_API int dWorldUseSharedWorkingMemory(dWorldID w, dWorldID from_world/*=NULL*
/**
* @brief Release internal working memory allocated for world
*
- * The worlds allocate working memory internally for simulation stepping. This
+ * The worlds allocate working memory internally for simulation stepping. This
* function can be used to free world's internal memory cache in case if number of
- * objects/joints in the world decreases significantly. By default, internal
- * allocation policy is used to only increase cache size as necessary and never
+ * objects/joints in the world decreases significantly. By default, internal
+ * allocation policy is used to only increase cache size as necessary and never
* decrease it.
*
- * If a world shares its working memory with other worlds the cache deletion
+ * If a world shares its working memory with other worlds the cache deletion
* affects all the linked worlds. However the shared status itself remains intact.
*
* The function call does affect neither memory reservation policy nor memory manager.
@@ -206,8 +221,8 @@ ODE_API void dWorldCleanupWorkingMemory(dWorldID w);
* @c reserve_factor is a quotient that is multiplied by required memory size
* to allocate extra reserve whenever reallocation is needed.
*
- * @c reserve_minimum is a minimum size that is checked against whenever reallocation
- * is needed to allocate expected working memory minimum at once without extra
+ * @c reserve_minimum is a minimum size that is checked against whenever reallocation
+ * is needed to allocate expected working memory minimum at once without extra
* reallocations as number of bodies/joints grows.
*
* @ingroup world
@@ -230,7 +245,7 @@ typedef struct
* are used.
*
* Passing @a policyinfo argument as NULL results in reservation policy being
- * reset to defaults as if the world has been just created. The content of
+ * reset to defaults as if the world has been just created. The content of
* @a policyinfo structure is copied internally and does not need to remain valid
* after the call returns.
*
@@ -268,7 +283,7 @@ ODE_API int dWorldSetStepMemoryReservationPolicy(dWorldID w, const dWorldStepRes
* @ingroup init
* @see dWorldSetStepMemoryManager
*/
-typedef struct
+typedef struct
{
unsigned struct_size;
void *(*alloc_block)(size_t block_size);
@@ -285,12 +300,12 @@ typedef struct
* based memory manager is used.
*
* Passing @a memfuncs argument as NULL results in memory manager being
-* reset to default one as if the world has been just created. The content of
+* reset to default one as if the world has been just created. The content of
* @a memfuncs structure is copied internally and does not need to remain valid
* after the call returns.
*
* If the world uses working memory sharing, changing memory manager
-* affects all the worlds linked together.
+* affects all the worlds linked together.
*
* Failure result status means a memory allocation failure.
*
@@ -398,7 +413,7 @@ ODE_API void dWorldImpulseToForce
);
/**
- * @brief Set the maximum number of iterations that the RobustStep method
+ * @brief Set the maximum number of iterations that the RobustStep method
* performs per step.
* @ingroup world
* @remarks
@@ -440,7 +455,7 @@ ODE_API void dWorldSetQuickStepNumChunks (dWorldID, int num);
ODE_API void dWorldSetQuickStepNumOverlap (dWorldID, int num);
/**
- * @brief Get the maximum number of iterations that the RobustStep method
+ * @brief Get the maximum number of iterations that the RobustStep method
* performs per step.
* @ingroup world
* @return nr of iterations
@@ -553,6 +568,12 @@ ODE_API bool dWorldGetQuickStepInertiaRatioReduction (dWorldID);
ODE_API dReal dWorldGetQuickStepContactResidualSmoothing (dWorldID);
/**
+ * @brief Get option to turn on experimental threaded position correction.
+ * @ingroup world
+ */
+ODE_API bool dWorldGetQuickStepThreadPositionCorrection (dWorldID);
+
+/**
* @brief Get option to turn on experimental row reordering.
* see dWorldGetQuickStepExperimentalRowReordering for details.
* @ingroup world
@@ -572,6 +593,18 @@ ODE_API dReal dWorldGetQuickStepWarmStartFactor (dWorldID);
ODE_API int dWorldGetQuickStepExtraFrictionIterations (dWorldID);
/**
+ * @brief Get the friction model.
+ * @ingroup world
+ */
+ODE_API Friction_Model dWorldGetQuickStepFrictionModel(dWorldID);
+
+/**
+ * @brief Get the LCP Solver for world step.
+ * @ingroup world
+ */
+ODE_API World_Solver_Type dWorldGetWorldStepSolverType(dWorldID);
+
+/**
* @brief Option to turn on inertia ratio reduction.
* @ingroup world
* @param irr set to true to turn on inertia ratio reduction.
@@ -586,6 +619,14 @@ ODE_API void dWorldSetQuickStepInertiaRatioReduction (dWorldID, bool irr);
ODE_API void dWorldSetQuickStepContactResidualSmoothing (dWorldID, dReal smoo);
/**
+ * @brief Set threaded projected position correction computations
+ * for potential solver speedup.
+ * @ingroup world
+ * @param thread (false: no threading ~ true: threaded parallel update)
+ */
+ODE_API void dWorldSetQuickStepThreadPositionCorrection (dWorldID, bool thread);
+
+/**
* @brief Turn on experimental row reordering, so within one sweep,
* following ordering of constraints are used:
* 1. bilateral constraints
@@ -617,6 +658,21 @@ ODE_API void dWorldSetQuickStepWarmStartFactor (dWorldID, dReal warm);
*/
ODE_API void dWorldSetQuickStepExtraFrictionIterations (dWorldID, int iters);
+/**
+ * @brief Set the friction model from: cone friction, pyramid friction
+ * and box friction.
+ * @ingroup world
+ * @param enum for friction model
+ */
+ODE_API void dWorldSetQuickStepFrictionModel(dWorldID, Friction_Model fricmodel);
+
+/**
+ * @brief Set the LCP Solver from: ODE_DEFAULT, DART_PGS, BULLET_PGS
+ * @ingroup world
+ * @param enum for LCP Solver
+ */
+ODE_API void dWorldSetWorldStepSolverType(dWorldID, World_Solver_Type solverType);
+
/* PGS experimental parameters */
/**
@@ -696,7 +752,7 @@ ODE_API dReal dWorldGetContactSurfaceLayer (dWorldID);
* A body is considered to be idle when the magnitudes of both its
* linear average velocity and angular average velocity are below given thresholds.
* The sample size for the average defaults to one and can be disabled by setting
- * to zero with
+ * to zero with
*
* Thus, every body has six auto-disable parameters: an enabled flag, a idle step
* count, an idle time, linear/angular average velocity thresholds, and the
@@ -849,7 +905,7 @@ ODE_API void dWorldSetAutoDisableFlag (dWorldID, int do_auto_disable);
* joint constraints are processed by the stepper (moving the body), then
* the damping is applied.
*
- * @note The damping happens right after the moved callback is called; this way
+ * @note The damping happens right after the moved callback is called; this way
* it still possible use the exact velocities the body has acquired during the
* step. You can even use the callback to create your own customized damping.
*/
@@ -1068,7 +1124,7 @@ ODE_API void dBodySetAutoDisableDefaults (dBodyID);
/**
* @brief Retrieves the world attached to te given body.
* @remarks
- *
+ *
* @ingroup bodies
*/
ODE_API dWorldID dBodyGetWorld (dBodyID);
@@ -1565,7 +1621,7 @@ ODE_API void dBodySetDisabledCallback(dBodyID b, void (*callback)(dBodyID));
/**
* @brief Return the first geom associated with the body.
- *
+ *
* You can traverse through the geoms by repeatedly calling
* dBodyGetNextGeom().
*
@@ -1801,7 +1857,7 @@ ODE_API dJointID dJointCreateHinge (dWorldID, dJointGroupID);
* If it is nonzero the joint is allocated in the given joint group.
*/
ODE_API dJointID dJointCreateGearbox (dWorldID, dJointGroupID);
-
+
/**
* @brief Create a new joint of the slider type.
* @ingroup joints
@@ -2001,12 +2057,6 @@ ODE_API void dJointSetData (dJointID, void *data);
ODE_API void dJointSetScrewThreadPitch (dJointID, dReal thread_pitch);
/**
- * @brief Set joint viscous damping coefficient
- * @ingroup joints
- */
-ODE_API void dJointSetDamping (dJointID, dReal damping);
-
-/**
* @brief Get the user-data pointer
* @ingroup joints
*/
@@ -2264,12 +2314,12 @@ ODE_API void dJointSetUniversalAnchor (dJointID, dReal x, dReal y, dReal z);
ODE_API void dJointSetUniversalAxis1 (dJointID, dReal x, dReal y, dReal z);
/**
- * @brief Set the Universal axis1 as if the 2 bodies were already at
+ * @brief Set the Universal axis1 as if the 2 bodies were already at
* offset1 and offset2 appart with respect to axis1 and axis2.
* @ingroup joints
*
- * This function initialize the axis1 and the relative orientation of
- * each body as if body1 was rotated around the new axis1 by the offset1
+ * This function initialize the axis1 and the relative orientation of
+ * each body as if body1 was rotated around the new axis1 by the offset1
* value and as if body2 was rotated around the axis2 by offset2. \br
* Ex:
* <PRE>
@@ -2295,8 +2345,8 @@ ODE_API void dJointSetUniversalAxis1 (dJointID, dReal x, dReal y, dReal z);
*
* @note Any previous offsets are erased.
*
- * @warning Calling dJointSetUniversalAnchor, dJointSetUnivesalAxis1,
- * dJointSetUniversalAxis2, dJointSetUniversalAxis2Offset
+ * @warning Calling dJointSetUniversalAnchor, dJointSetUnivesalAxis1,
+ * dJointSetUniversalAxis2, dJointSetUniversalAxis2Offset
* will reset the "zero" angle position.
*/
ODE_API void dJointSetUniversalAxis1Offset (dJointID, dReal x, dReal y, dReal z,
@@ -2309,12 +2359,12 @@ ODE_API void dJointSetUniversalAxis1Offset (dJointID, dReal x, dReal y, dReal z,
ODE_API void dJointSetUniversalAxis2 (dJointID, dReal x, dReal y, dReal z);
/**
- * @brief Set the Universal axis2 as if the 2 bodies were already at
+ * @brief Set the Universal axis2 as if the 2 bodies were already at
* offset1 and offset2 appart with respect to axis1 and axis2.
* @ingroup joints
*
- * This function initialize the axis2 and the relative orientation of
- * each body as if body1 was rotated around the axis1 by the offset1
+ * This function initialize the axis2 and the relative orientation of
+ * each body as if body1 was rotated around the axis1 by the offset1
* value and as if body2 was rotated around the new axis2 by offset2. \br
* Ex:
* <PRE>
@@ -2340,8 +2390,8 @@ ODE_API void dJointSetUniversalAxis2 (dJointID, dReal x, dReal y, dReal z);
*
* @note Any previous offsets are erased.
*
- * @warning Calling dJointSetUniversalAnchor, dJointSetUnivesalAxis1,
- * dJointSetUniversalAxis2, dJointSetUniversalAxis2Offset
+ * @warning Calling dJointSetUniversalAnchor, dJointSetUnivesalAxis1,
+ * dJointSetUniversalAxis2, dJointSetUniversalAxis2Offset
* will reset the "zero" angle position.
*/
@@ -2393,7 +2443,7 @@ ODE_API void dJointSetPRParam (dJointID, int parameter, dReal value);
/**
* @brief Applies the torque about the rotoide axis of the PR joint
*
- * That is, it applies a torque with specified magnitude in the direction
+ * That is, it applies a torque with specified magnitude in the direction
* of the rotoide axis, to body 1, and with the same magnitude but in opposite
* direction to body 2. This function is just a wrapper for dBodyAddTorque()}
* @ingroup joints
@@ -3029,7 +3079,7 @@ ODE_API dReal dJointGetUniversalAngle2Rate (dJointID);
/**
* @brief Get the joint anchor point, in world coordinates.
- * @return the point on body 1. If the joint is perfectly satisfied,
+ * @return the point on body 1. If the joint is perfectly satisfied,
* this will be the same as the point on body 2.
* @ingroup joints
*/
@@ -3091,8 +3141,8 @@ ODE_API void dJointGetPRAxis2 (dJointID, dVector3 result);
*/
ODE_API dReal dJointGetPRParam (dJointID, int parameter);
-
-
+
+
/**
* @brief Get the joint anchor point, in world coordinates.
* @return the point on body 1. If the joint is perfectly satisfied,
diff --git a/deps/opende/include/ode/odecpp.h b/deps/opende/include/ode/odecpp.h
index 51265f1..8459157 100644
--- a/deps/opende/include/ode/odecpp.h
+++ b/deps/opende/include/ode/odecpp.h
@@ -126,22 +126,30 @@ public:
{ return dWorldGetQuickStepInertiaRatioReduction (get_id()); }
dReal getQuickStepContactResidualSmoothing() const
{ return dWorldGetQuickStepContactResidualSmoothing (get_id()); }
+ bool getQuickStepThreadPositionCorrection() const
+ { return dWorldGetQuickStepThreadPositionCorrection (get_id()); }
bool getQuickStepExperimentalRowReordering() const
{ return dWorldGetQuickStepExperimentalRowReordering (get_id()); }
dReal getQuickStepWarmStartFactor() const
{ return dWorldGetQuickStepWarmStartFactor (get_id()); }
int getQuickStepExtraFrictionIterations() const
{ return dWorldGetQuickStepExtraFrictionIterations (get_id()); }
+ int getQuickStepFrictionModel() const
+ { return dWorldGetQuickStepFrictionModel(get_id()); }
void setQuickStepInertiaRatioReduction(bool irr)
{ dWorldSetQuickStepInertiaRatioReduction (get_id(), irr); }
void setQuickStepContactResidualSmoothing(dReal smoo)
{ dWorldSetQuickStepContactResidualSmoothing (get_id(), smoo); }
+ void setQuickStepThreadPositionCorrection(bool thread)
+ { dWorldSetQuickStepThreadPositionCorrection (get_id(), thread); }
void setQuickStepExperimentalRowReordering(bool order)
{ dWorldSetQuickStepExperimentalRowReordering (get_id(), order); }
void setQuickStepWarmStartFactor(dReal warm)
{ dWorldSetQuickStepWarmStartFactor (get_id(), warm); }
void setQuickStepExtraFrictionIterations(int iters)
{ dWorldSetQuickStepExtraFrictionIterations (get_id(), iters); }
+ void setQuickStepFrictionModel(int fricmodel)
+ { dWorldSetQuickStepFrictionModel(get_id(), fricmodel); }
void setAutoDisableLinearThreshold (dReal threshold)
{ dWorldSetAutoDisableLinearThreshold (get_id(), threshold); }
diff --git a/deps/opende/ou/CMakeLists.txt b/deps/opende/ou/CMakeLists.txt
index e83bde3..02a9e33 100644
--- a/deps/opende/ou/CMakeLists.txt
+++ b/deps/opende/ou/CMakeLists.txt
@@ -12,8 +12,12 @@ include_directories(
${CMAKE_SOURCE_DIR}/deps/opende/ou/include
)
-set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DNDEBUG -DdNODEBUG -DdDOUBLE -DHAVE_CONFIG_H -DPIC -D_OU_NAMESPACE=gazebo_odeou")
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DNDEBUG -DdNODEBUG -DdDOUBLE -DHAVE_CONFIG_H -DPIC -D_OU_NAMESPACE=gazebo_odeou -DBUILDING_DLL_OU")
+
+if (WIN32)
+ add_library(gazebo_opende_ou SHARED ${sources})
+else()
+ gz_add_library(gazebo_opende_ou ${sources})
+endif()
-gz_add_library(gazebo_opende_ou ${sources})
-#target_link_libraries(gazebo_fcl gazebo_ccd gazebo_ann)
gz_install_library(gazebo_opende_ou)
diff --git a/deps/opende/ou/include/ou/atomic.h b/deps/opende/ou/include/ou/atomic.h
index ef36e29..6545edb 100644
--- a/deps/opende/ou/include/ou/atomic.h
+++ b/deps/opende/ou/include/ou/atomic.h
@@ -98,7 +98,7 @@
#include <ou/namespace.h>
#include <ou/platform.h>
-#include <gazebo/util/system.hh>
+#include <ou/ou_dll.h>
/**
@@ -412,32 +412,32 @@ typedef PVOID atomicptr;
#define __OU_ATOMIC_ORD32_FUNCTIONS_DEFINED
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicIncrement(volatile atomicord32 *paoDestination)
{
return ::InterlockedIncrement((__ou_intlck_target_t)paoDestination);
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicDecrement(volatile atomicord32 *paoDestination)
{
return ::InterlockedDecrement((__ou_intlck_target_t)paoDestination);
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicExchange(volatile atomicord32 *paoDestination, atomicord32 aoExchange)
{
return ::InterlockedExchange((__ou_intlck_target_t)paoDestination, aoExchange);
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicExchangeAdd(volatile atomicord32 *paoDestination, atomicord32 aoAddend)
{
return ::InterlockedExchangeAdd((__ou_xchgadd_target_t)paoDestination, aoAddend);
}
-static _OU_ALWAYSINLINE_PRE bool _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE bool _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*bool */AtomicCompareExchange(volatile atomicord32 *paoDestination, atomicord32 aoComparand, atomicord32 aoExchange)
{
return (aoComparand == (atomicord32)::InterlockedCompareExchange((__ou_cmpxchg_target_t)paoDestination, (__ou_cmpxchg_value_t)aoExchange, (__ou_cmpxchg_value_t)aoComparand));
@@ -446,7 +446,7 @@ static _OU_ALWAYSINLINE_PRE bool _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_V
#define __OU_ATOMIC_BIT_FUNCTIONS_DEFINED
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicAnd(volatile atomicord32 *paoDestination, atomicord32 aoBitMask)
{
atomicord32 aoOldValue = *paoDestination;
@@ -466,7 +466,7 @@ static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API G
return aoOldValue;
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicOr(volatile atomicord32 *paoDestination, atomicord32 aoBitMask)
{
atomicord32 aoOldValue = *paoDestination;
@@ -486,7 +486,7 @@ static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API G
return aoOldValue;
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicXor(volatile atomicord32 *paoDestination, atomicord32 aoBitMask)
{
atomicord32 aoOldValue = *paoDestination;
@@ -509,7 +509,7 @@ static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API G
#define __OU_ATOMIC_PTR_FUNCTIONS_DEFINED
-static _OU_ALWAYSINLINE_PRE atomicptr _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicptr _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicptr */AtomicExchangePointer(volatile atomicptr *papDestination, atomicptr apExchange)
{
#if _OU_TARGET_BITS == _OU_TARGET_BITS_32
@@ -525,7 +525,7 @@ static _OU_ALWAYSINLINE_PRE atomicptr _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZ
#endif // #if _OU_TARGET_BITS == ...
}
-static _OU_ALWAYSINLINE_PRE bool _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE bool _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*bool */AtomicCompareExchangePointer(volatile atomicptr *papDestination, atomicptr apComparand, atomicptr apExchange)
{
#if _OU_TARGET_BITS == _OU_TARGET_BITS_32
@@ -572,32 +572,32 @@ typedef void *atomicptr;
#define __OU_ATOMIC_ORD32_FUNCTIONS_DEFINED
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicIncrement(volatile atomicord32 *paoDestination)
{
return (atomic_add_value(paoDestination, 1U) + 1U);
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicDecrement(volatile atomicord32 *paoDestination)
{
return (atomic_sub_value(paoDestination, 1U) - 1U);
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicExchange(volatile atomicord32 *paoDestination, atomicord32 aoExchange)
{
return _smp_xchg(paoDestination, aoExchange);
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicExchangeAdd(volatile atomicord32 *paoDestination, atomicord32 aoAddend)
{
return atomic_add_value(paoDestination, aoAddend);
}
-static _OU_ALWAYSINLINE_PRE bool _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE bool _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*bool */AtomicCompareExchange(volatile atomicord32 *paoDestination, atomicord32 aoComparand, atomicord32 aoExchange)
{
return (aoComparand == (atomicord32)_smp_cmpxchg(paoDestination, aoComparand, aoExchange));
@@ -606,19 +606,19 @@ static _OU_ALWAYSINLINE_PRE bool _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_V
#define __OU_ATOMIC_BIT_FUNCTIONS_DEFINED
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicAnd(volatile atomicord32 *paoDestination, atomicord32 aoBitMask)
{
return atomic_clr_value(paoDestination, ~aoBitMask);
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicOr(volatile atomicord32 *paoDestination, atomicord32 aoBitMask)
{
return atomic_set_value(paoDestination, aoBitMask);
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicXor(volatile atomicord32 *paoDestination, atomicord32 aoBitMask)
{
return atomic_toggle_value(paoDestination, aoBitMask);
@@ -627,37 +627,37 @@ static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API G
#define __OU_ATOMIC_ORD32_NORESULT_FUNCTIONS_DEFINED
-static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*void */AtomicIncrementNoResult(volatile atomicord32 *paoDestination)
{
atomic_add(paoDestination, 1U);
}
-static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*void */AtomicDecrementNoResult(volatile atomicord32 *paoDestination)
{
atomic_sub(paoDestination, 1U);
}
-static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*void */AtomicExchangeAddNoResult(volatile atomicord32 *paoDestination, atomicord32 aoAddend)
{
atomic_add(paoDestination, aoAddend);
}
-static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*void */AtomicAndNoResult(volatile atomicord32 *paoDestination, atomicord32 aoBitMask)
{
atomic_clr(paoDestination, ~aoBitMask);
}
-static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*void */AtomicOrNoResult(volatile atomicord32 *paoDestination, atomicord32 aoBitMask)
{
atomic_set(paoDestination, aoBitMask);
}
-static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*void */AtomicXorNoResult(volatile atomicord32 *paoDestination, atomicord32 aoBitMask)
{
atomic_toggle(paoDestination, aoBitMask);
@@ -693,20 +693,20 @@ typedef void *atomicptr;
#define __OU_ATOMIC_ORD32_FUNCTIONS_DEFINED
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicIncrement(volatile atomicord32 *paoDestination)
{
return OSAtomicIncrement32Barrier(paoDestination);
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicDecrement(volatile atomicord32 *paoDestination)
{
return OSAtomicDecrement32Barrier(paoDestination);
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicExchange(volatile atomicord32 *paoDestination, atomicord32 aoExchange)
{
atomicord32 aoOldValue = *paoDestination;
@@ -728,13 +728,13 @@ static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API G
return aoOldValue;
}
-static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE atomicord32 _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*atomicord32 */AtomicExchangeAdd(volatile atomicord32 *paoDestination, atomicord32 aoAddend)
{
return (OSAtomicAdd32Barrier(aoAddend, paoDestination) - aoAddend);
}
-static _OU_ALWAYSINLINE_PRE bool _OU_ALWAYSINLINE_IN _OU_CONVENTION_API GAZEBO_VISIBLE
+static _OU_ALWAYSINLINE_PRE bool _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
/*bool */AtomicCompareExchange(volatile atomicord32 *paoDestination, atomicord32 aoComparand, atomicord32 aoExchange)
{
return OSAtomicCompareAndSwap32Barrier(aoComparand, aoExchange, paoDestination);
@@ -1502,19 +1502,19 @@ atomicord32 _OU_CONVENTION_API AtomicXor(volatile atomicord32 *paoDestination, a
#if !defined(__OU_ATOMIC_PTR_FUNCTIONS_DEFINED)
-atomicptr _OU_CONVENTION_API GAZEBO_VISIBLE AtomicExchangePointer(volatile atomicptr *papDestination, atomicptr apExchange);
-bool _OU_CONVENTION_API GAZEBO_VISIBLE AtomicCompareExchangePointer(volatile atomicptr *papDestination, atomicptr apComparand, atomicptr apExchange);
+atomicptr _OU_CONVENTION_API OU_VISIBLE AtomicExchangePointer(volatile atomicptr *papDestination, atomicptr apExchange);
+bool _OU_CONVENTION_API OU_VISIBLE AtomicCompareExchangePointer(volatile atomicptr *papDestination, atomicptr apComparand, atomicptr apExchange);
#if defined(__OU_DOXYGEN__)
// Doxygen fooling declarations (used for documentation generation only)
-void _OU_CONVENTION_API GAZEBO_VISIBLE AtomicIncrementNoResult(volatile atomicord32 *paoDestination);
-void _OU_CONVENTION_API GAZEBO_VISIBLE AtomicDecrementNoResult(volatile atomicord32 *paoDestination);
-void _OU_CONVENTION_API GAZEBO_VISIBLE AtomicExchangeAddNoResult(volatile atomicord32 *paoDestination, atomicord32 aoAddend);
-void _OU_CONVENTION_API GAZEBO_VISIBLE AtomicAndNoResult(volatile atomicord32 *paoDestination, atomicord32 aoBitMask);
-void _OU_CONVENTION_API GAZEBO_VISIBLE AtomicOrNoResult(volatile atomicord32 *paoDestination, atomicord32 aoBitMask);
-void _OU_CONVENTION_API GAZEBO_VISIBLE AtomicXorNoResult(volatile atomicord32 *paoDestination, atomicord32 aoBitMask);
+void _OU_CONVENTION_API OU_VISIBLE AtomicIncrementNoResult(volatile atomicord32 *paoDestination);
+void _OU_CONVENTION_API OU_VISIBLE AtomicDecrementNoResult(volatile atomicord32 *paoDestination);
+void _OU_CONVENTION_API OU_VISIBLE AtomicExchangeAddNoResult(volatile atomicord32 *paoDestination, atomicord32 aoAddend);
+void _OU_CONVENTION_API OU_VISIBLE AtomicAndNoResult(volatile atomicord32 *paoDestination, atomicord32 aoBitMask);
+void _OU_CONVENTION_API OU_VISIBLE AtomicOrNoResult(volatile atomicord32 *paoDestination, atomicord32 aoBitMask);
+void _OU_CONVENTION_API OU_VISIBLE AtomicXorNoResult(volatile atomicord32 *paoDestination, atomicord32 aoBitMask);
#endif // #if defined(__OU_DOXYGEN__)
@@ -1524,8 +1524,8 @@ void _OU_CONVENTION_API GAZEBO_VISIBLE AtomicXorNoResult(volatile atomicord32 *p
#define __OU_ATOMIC_INITIALIZATION_FUNCTIONS_REQUIRED
// Initialization must be performed from main thread
-bool _OU_CONVENTION_API GAZEBO_VISIBLE InitializeAtomicAPI();
-void _OU_CONVENTION_API GAZEBO_VISIBLE FinalizeAtomicAPI();
+bool _OU_CONVENTION_API OU_VISIBLE InitializeAtomicAPI();
+void _OU_CONVENTION_API OU_VISIBLE FinalizeAtomicAPI();
#endif // #if !defined(__OU_ATOMIC_PTR_FUNCTIONS_DEFINED)
@@ -1582,14 +1582,14 @@ static _OU_ALWAYSINLINE_PRE void _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
#if !defined(__OU_ATOMIC_INITIALIZATION_FUNCTIONS_REQUIRED)
// Initialization must be performed from main thread
-static _OU_INLINE bool _OU_CONVENTION_API GAZEBO_VISIBLE InitializeAtomicAPI()
+static _OU_INLINE bool _OU_CONVENTION_API InitializeAtomicAPI()
{
// Do nothing
return true;
}
-static _OU_INLINE void _OU_CONVENTION_API GAZEBO_VISIBLE FinalizeAtomicAPI()
+static _OU_INLINE void _OU_CONVENTION_API FinalizeAtomicAPI()
{
// Do nothing
}
diff --git a/deps/opende/ou/include/ou/atomicflags.h b/deps/opende/ou/include/ou/atomicflags.h
index 5c6fd6e..7824aef 100644
--- a/deps/opende/ou/include/ou/atomicflags.h
+++ b/deps/opende/ou/include/ou/atomicflags.h
@@ -32,7 +32,7 @@
#include <ou/atomic.h>
#include <ou/assert.h>
#include <ou/namespace.h>
-#include <gazebo/util/system.hh>
+#include <ou/ou_dll.h>
BEGIN_NAMESPACE_OU()
@@ -47,7 +47,7 @@ BEGIN_NAMESPACE_OU()
* knowledge in field, of course).
*/
-class GAZEBO_VISIBLE CAtomicFlags
+class OU_VISIBLE CAtomicFlags
{
public:
_OU_INLINE _OU_CONVENTION_METHOD CAtomicFlags():
diff --git a/deps/opende/ou/include/ou/customization.h b/deps/opende/ou/include/ou/customization.h
index c1906e1..b3b4b1f 100644
--- a/deps/opende/ou/include/ou/customization.h
+++ b/deps/opende/ou/include/ou/customization.h
@@ -31,9 +31,9 @@
#include <ou/namespace.h>
#include <ou/inttypes.h>
#include <ou/platform.h>
+#include <ou/ou_dll.h>
#include <stddef.h>
-#include <gazebo/util/system.hh>
BEGIN_NAMESPACE_OU()
@@ -78,7 +78,7 @@ typedef void (_OU_CONVENTION_CALLBACK *CAssertionFailedProcedure)(EASSERTIONFAIL
const char *szAssertionExpression, const char *szAssertionFileName, unsigned int uiAssertionSourceLine);
-class GAZEBO_VISIBLE CAssertionCheckCustomization
+class OU_VISIBLE CAssertionCheckCustomization
{
public:
static _OU_ALWAYSINLINE_PRE CAssertionFailedProcedure _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
@@ -108,7 +108,7 @@ typedef void *(_OU_CONVENTION_CALLBACK *CMemoryReallocationProcedure)(void *pv_E
typedef void (_OU_CONVENTION_CALLBACK *CMemoryDeallocationProcedure)(void *pv_ExistingBlock);
-class GAZEBO_VISIBLE CMemoryManagerCustomization
+class OU_VISIBLE CMemoryManagerCustomization
{
public:
static _OU_ALWAYSINLINE_PRE CMemoryAllocationProcedure _OU_ALWAYSINLINE_IN _OU_CONVENTION_API
diff --git a/deps/opende/ou/include/ou/enumarrays.h b/deps/opende/ou/include/ou/enumarrays.h
index 8e3b4b6..bfdb61f 100644
--- a/deps/opende/ou/include/ou/enumarrays.h
+++ b/deps/opende/ou/include/ou/enumarrays.h
@@ -32,7 +32,7 @@
#include <ou/macros.h>
#include <ou/platform.h>
#include <ou/namespace.h>
-#include <gazebo/util/system.hh>
+#include <ou/ou_dll.h>
BEGIN_NAMESPACE_OU()
@@ -70,8 +70,20 @@ struct CTypeStandardLess
* and it is acceptable to have it as template parameter.
*/
+// The visibility on this class is tricky because a particular templated
+// instance is declared inside the ODE library (in odeou.cpp).
template<typename EnumType, const EnumType EnumMax, typename ElementType, const int Instance=0, class CElementEqualType=CTypeStandardEqual<ElementType> >
-class GAZEBO_VISIBLE CEnumUnsortedElementArray
+class
+#if defined ODE_API
+ ODE_API
+#else
+ #if defined(_MSC_VER)
+ __declspec(dllimport)
+ #else
+ OU_VISIBLE
+ #endif
+#endif
+CEnumUnsortedElementArray
{
public:
_OU_CONVENTION_METHOD CEnumUnsortedElementArray()
@@ -155,7 +167,7 @@ private:
*/
template<typename EnumType, const EnumType EnumMax, typename ElementType, const int Instance=0, class CElementLessType=CTypeStandardLess<ElementType> >
-class GAZEBO_VISIBLE CEnumSortedElementArray
+class OU_VISIBLE CEnumSortedElementArray
{
public:
_OU_INLINE _OU_CONVENTION_METHOD CEnumSortedElementArray()
diff --git a/deps/opende/ou/include/ou/ou_dll.h b/deps/opende/ou/include/ou/ou_dll.h
new file mode 100644
index 0000000..b855060
--- /dev/null
+++ b/deps/opende/ou/include/ou/ou_dll.h
@@ -0,0 +1,29 @@
+#ifndef __OU_OU_DLL_H_INCLUDED
+#define __OU_OU_DLLASSERT_H_INCLUDED
+
+#if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_OU
+ #ifdef __GNUC__
+ #define OU_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define OU_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define OU_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define OU_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define OU_HIDDEN
+#else
+ #if __GNUC__ >= 4
+ #define OU_VISIBLE __attribute__ ((visibility ("default")))
+ #define OU_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define OU_VISIBLE
+ #define OU_HIDDEN
+ #endif
+#endif
+
+#endif
diff --git a/deps/opende/ou/include/ou/simpleflags.h b/deps/opende/ou/include/ou/simpleflags.h
index 5f782a2..19b20b6 100644
--- a/deps/opende/ou/include/ou/simpleflags.h
+++ b/deps/opende/ou/include/ou/simpleflags.h
@@ -32,16 +32,16 @@
#include <ou/assert.h>
#include <ou/inttypes.h>
#include <ou/namespace.h>
+#include <ou/ou_dll.h>
#include <stddef.h>
-#include <gazebo/util/system.hh>
BEGIN_NAMESPACE_OU()
template<typename ContainerType>
-class GAZEBO_VISIBLE CSimpleFlagsTemplate
+class OU_VISIBLE CSimpleFlagsTemplate
{
public:
_OU_INLINE _OU_CONVENTION_METHOD CSimpleFlagsTemplate():
diff --git a/deps/opende/ou/include/ou/threadlocalstorage.h b/deps/opende/ou/include/ou/threadlocalstorage.h
index e162a68..1aea56e 100644
--- a/deps/opende/ou/include/ou/threadlocalstorage.h
+++ b/deps/opende/ou/include/ou/threadlocalstorage.h
@@ -33,6 +33,7 @@
#include <ou/assert.h>
#include <ou/platform.h>
#include <ou/namespace.h>
+#include <ou/ou_dll.h>
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
@@ -42,7 +43,6 @@
#else // #if _OU_TARGET_OS != _OU_TARGET_OS_WINDOWS
#include <pthread.h>
-#include <gazebo/util/system.hh>
#endif // #if _OU_TARGET_OS == ...
@@ -156,7 +156,7 @@ private:
//////////////////////////////////////////////////////////////////////////
// API declaration
-class GAZEBO_VISIBLE CThreadLocalStorage
+class OU_VISIBLE CThreadLocalStorage
{
public: // Safe methods
/*
@@ -261,7 +261,7 @@ private:
//////////////////////////////////////////////////////////////////////////
// Initialization/finalization
-class GAZEBO_VISIBLE CTLSInitialization
+class OU_VISIBLE CTLSInitialization
{
public:
enum EINITIALIZATIONFLAGS
diff --git a/deps/opende/ou/src/threadlocalstorage.cpp b/deps/opende/ou/src/threadlocalstorage.cpp
index 9d2a796..9573694 100644
--- a/deps/opende/ou/src/threadlocalstorage.cpp
+++ b/deps/opende/ou/src/threadlocalstorage.cpp
@@ -38,7 +38,7 @@
#if !defined(EOK)
-#define EOK 0
+#define EOK 0
#endif
@@ -59,12 +59,12 @@ class CTLSStorageInstance;
enum ESTORAGEINSTANCEKIND
{
- SIK__MIN,
+ SIK__MIN,
- SIK_AUTOCLEANUP = SIK__MIN,
- SIK_MANUALCLEANUP,
+ SIK_AUTOCLEANUP = SIK__MIN,
+ SIK_MANUALCLEANUP,
- SIK__MAX
+ SIK__MAX
};
@@ -75,19 +75,19 @@ static HTLSKEYVALUE g_ahkvStorageGlobalKeyValues[SIK__MAX] = { NULL };
static inline size_t DecodeInstanceKindFromKeySelector(const HTLSKEYSELECTOR &hksKeySelector)
{
- return (HTLSKEYSELECTOR::value_type)hksKeySelector - g_ahkvStorageGlobalKeyValues;
+ return (HTLSKEYSELECTOR::value_type)hksKeySelector - g_ahkvStorageGlobalKeyValues;
}
static inline HTLSKEYSELECTOR EncodeKeySelectorFromStorageKind(ESTORAGEINSTANCEKIND ikInstanceKind)
{
- return g_ahkvStorageGlobalKeyValues + ikInstanceKind;
+ return g_ahkvStorageGlobalKeyValues + ikInstanceKind;
}
#if !defined(_OU_TLS_ARRAY_ELEMENT_COUNT)
// Default TLS array element count
-#define _OU_TLS_ARRAY_ELEMENT_COUNT 8
+#define _OU_TLS_ARRAY_ELEMENT_COUNT 8
#endif // #if !defined(_OU_TLS_ARRAY_ELEMENT_COUNT)
@@ -104,7 +104,7 @@ static inline HTLSKEYSELECTOR EncodeKeySelectorFromStorageKind(ESTORAGEINSTANCEK
enum
{
- TLS_ARRAY_ELEMENT__MAX = _OU_TLS_ARRAY_ELEMENT_COUNT // 16 threads with 8 values each using 4 + 4 bytes is ~1 kb of memory
+ TLS_ARRAY_ELEMENT__MAX = _OU_TLS_ARRAY_ELEMENT_COUNT // 16 threads with 8 values each using 4 + 4 bytes is ~1 kb of memory
};
@@ -112,253 +112,253 @@ struct CTLSStorageArray
{
private:
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-
- typedef HANDLE CClientHandleArray[TLS_ARRAY_ELEMENT__MAX];
- typedef unsigned int CHandleTranslationMap[TLS_ARRAY_ELEMENT__MAX];
-
-
+
+ typedef HANDLE CClientHandleArray[TLS_ARRAY_ELEMENT__MAX];
+ typedef unsigned int CHandleTranslationMap[TLS_ARRAY_ELEMENT__MAX];
+
+
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-
+
public:
- static inline size_t GetHeaderSize() { return OU_ALIGNED_SIZE(sizeof(CTLSStorageArray), CTLSStorageBlock::TSB_LARGEST_ALIGNMENT); }
+ static inline size_t GetHeaderSize() { return OU_ALIGNED_SIZE(sizeof(CTLSStorageArray), CTLSStorageBlock::TSB_LARGEST_ALIGNMENT); }
public:
- static CTLSStorageArray *AllocateInstance(tlsindextype iValueCount);
- void FreeInstance(tlsindextype iValueCount);
+ static CTLSStorageArray *AllocateInstance(tlsindextype iValueCount);
+ void FreeInstance(tlsindextype iValueCount);
protected:
- inline CTLSStorageArray(); // Use AllocateInstance()
- inline ~CTLSStorageArray(); // Use FreeInstance()
+ inline CTLSStorageArray(); // Use AllocateInstance()
+ inline ~CTLSStorageArray(); // Use FreeInstance()
public:
- void FreeStorageBlockOnThreadExit(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount);
-
+ void FreeStorageBlockOnThreadExit(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount);
+
public:
- bool FindFreeStorageBlock(CTLSStorageBlock *&psbOutFreeStorageBlock,
- tlsindextype iValueCount, bool bIsManualCleanup);
-
+ bool FindFreeStorageBlock(CTLSStorageBlock *&psbOutFreeStorageBlock,
+ tlsindextype iValueCount, bool bIsManualCleanup);
+
private:
- bool FindFreeStorageBlockIndex(unsigned int &nOutFreeBlockIndex, tlsindextype iValueCount, bool bIsManualCleanup);
- bool FindFreeStorageBlockIndexWithPossibilityVerified(unsigned int &nOutFreeBlockIndex, bool bIsManualCleanup);
+ bool FindFreeStorageBlockIndex(unsigned int &nOutFreeBlockIndex, tlsindextype iValueCount, bool bIsManualCleanup);
+ bool FindFreeStorageBlockIndexWithPossibilityVerified(unsigned int &nOutFreeBlockIndex, bool bIsManualCleanup);
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-
- bool FindAbandonedStorageBlockIndex(unsigned int &nOutFreeBlockIndex, tlsindextype iValueCount);
- unsigned int TranslateClientHandles(CClientHandleArray haTranslatedHandlesStorage, CHandleTranslationMap tmTranslationMapStorage,
- const HANDLE *&ph_OutTranslatedHandles, const unsigned int *&puiOutTranslationMap) const;
+
+ bool FindAbandonedStorageBlockIndex(unsigned int &nOutFreeBlockIndex, tlsindextype iValueCount);
+ unsigned int TranslateClientHandles(CClientHandleArray haTranslatedHandlesStorage, CHandleTranslationMap tmTranslationMapStorage,
+ const HANDLE *&ph_OutTranslatedHandles, const unsigned int *&puiOutTranslationMap) const;
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
private:
- void FreeStorageAllBlocks(tlsindextype iValueCount);
- void ReinitializeStorageSingleBlock(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount);
- static void FinalizeStorageSingleBlock(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount);
+ void FreeStorageAllBlocks(tlsindextype iValueCount);
+ void ReinitializeStorageSingleBlock(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount);
+ static void FinalizeStorageSingleBlock(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount);
+
+ void AssignAllBlocksHostArray(tlsindextype iValueCount);
+ inline void AssignSingleBlockHostArray(CTLSStorageBlock *psbStorageBlock);
- void AssignAllBlocksHostArray(tlsindextype iValueCount);
- inline void AssignSingleBlockHostArray(CTLSStorageBlock *psbStorageBlock);
-
private:
- inline CTLSStorageBlock *GetStorageBlockPointer(unsigned int nBlockIndex, tlsindextype iValueCount) const;
- inline unsigned int GetStorageBlockIndex(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount) const;
- inline static void ZeroStorageBlockMemory(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount);
+ inline CTLSStorageBlock *GetStorageBlockPointer(unsigned int nBlockIndex, tlsindextype iValueCount) const;
+ inline unsigned int GetStorageBlockIndex(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount) const;
+ inline static void ZeroStorageBlockMemory(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount);
private:
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
- void AllocateBlockThreadHandle(unsigned int nBlockIndex);
- void FreeStorageThreadHandle(unsigned int nBlockIndex);
-
- void AssignAllBlocksInvalidThreads();
- bool CheckIfAllBlocksHaveInvalidThreads();
-
+ void AllocateBlockThreadHandle(unsigned int nBlockIndex);
+ void FreeStorageThreadHandle(unsigned int nBlockIndex);
+
+ void AssignAllBlocksInvalidThreads();
+ bool CheckIfAllBlocksHaveInvalidThreads();
+
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-
+
private:
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-
- inline void SetBlockThreadHandle(unsigned int nBlockIndex, HANDLE hValue)
- {
- m_haBlockThreads[nBlockIndex] = hValue;
- }
-
- inline HANDLE GetBlockThreadHandle(unsigned int nBlockIndex) const
- {
- return m_haBlockThreads[nBlockIndex];
- }
-
- inline const HANDLE *GetBlockThreadHandlesStorage() const
- {
- return m_haBlockThreads;
- }
-
+
+ inline void SetBlockThreadHandle(unsigned int nBlockIndex, HANDLE hValue)
+ {
+ m_haBlockThreads[nBlockIndex] = hValue;
+ }
+
+ inline HANDLE GetBlockThreadHandle(unsigned int nBlockIndex) const
+ {
+ return m_haBlockThreads[nBlockIndex];
+ }
+
+ inline const HANDLE *GetBlockThreadHandlesStorage() const
+ {
+ return m_haBlockThreads;
+ }
+
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
public:
- inline void SetNextArray(CTLSStorageArray *psaInstance)
- {
- m_psaNextArray = (atomicptr)psaInstance;
- }
-
- inline CTLSStorageArray *GetNextArray() const
- {
- return (CTLSStorageArray *)m_psaNextArray;
- }
-
+ inline void SetNextArray(CTLSStorageArray *psaInstance)
+ {
+ m_psaNextArray = (atomicptr)psaInstance;
+ }
+
+ inline CTLSStorageArray *GetNextArray() const
+ {
+ return (CTLSStorageArray *)m_psaNextArray;
+ }
+
private:
- enum
- {
- FL_OCCUPANCY_FLAGS__START = 0x00000001,
- FL_OCCUPANCY_FLAGS__END = FL_OCCUPANCY_FLAGS__START << TLS_ARRAY_ELEMENT__MAX,
-
- FL_ARRAY_LOCKED = FL_OCCUPANCY_FLAGS__END
- };
-
- inline bool GetAreAllBlocksOccupied() const
- {
- return m_afOccupancyFlags.EnumAllQueryEnumeratedFlags(FL_OCCUPANCY_FLAGS__START, TLS_ARRAY_ELEMENT__MAX) == OU_FLAGS_ENUMFLAGS_MASK(COccupancyFlagsType::value_type, FL_OCCUPANCY_FLAGS__START, TLS_ARRAY_ELEMENT__MAX);
- }
-
- inline bool GetIsAnyBlockOccupied() const
- {
- return m_afOccupancyFlags.EnumAnyGetEnumeratedFlagValue(FL_OCCUPANCY_FLAGS__START, TLS_ARRAY_ELEMENT__MAX);
- }
-
- inline bool SetBlockOccupiedFlag(unsigned int nBlockIndex)
- {
- return m_afOccupancyFlags.EnumModifyEnumeratedFlagValue(FL_OCCUPANCY_FLAGS__START, nBlockIndex, TLS_ARRAY_ELEMENT__MAX, true);
- }
-
- inline void ResetBlockOccupiedFlag(unsigned int nBlockIndex)
- {
- m_afOccupancyFlags.EnumDropEnumeratedFlagValue(FL_OCCUPANCY_FLAGS__START, nBlockIndex, TLS_ARRAY_ELEMENT__MAX);
- }
-
- inline bool GetBlockOccupiedFlag(unsigned int nBlockIndex) const
- {
- return m_afOccupancyFlags.EnumGetEnumeratedFlagValue(FL_OCCUPANCY_FLAGS__START, nBlockIndex, TLS_ARRAY_ELEMENT__MAX);
- }
-
- inline bool SetArrayLockedFlag()
- {
- return m_afOccupancyFlags.ModifySingleFlagValue(FL_ARRAY_LOCKED, true);
- }
-
- inline void ResetArrayLockedFlag()
- {
- m_afOccupancyFlags.DropFlagsMaskValue(FL_ARRAY_LOCKED);
- }
+ enum
+ {
+ FL_OCCUPANCY_FLAGS__START = 0x00000001,
+ FL_OCCUPANCY_FLAGS__END = FL_OCCUPANCY_FLAGS__START << TLS_ARRAY_ELEMENT__MAX,
+
+ FL_ARRAY_LOCKED = FL_OCCUPANCY_FLAGS__END
+ };
+
+ inline bool GetAreAllBlocksOccupied() const
+ {
+ return m_afOccupancyFlags.EnumAllQueryEnumeratedFlags(FL_OCCUPANCY_FLAGS__START, TLS_ARRAY_ELEMENT__MAX) == OU_FLAGS_ENUMFLAGS_MASK(COccupancyFlagsType::value_type, FL_OCCUPANCY_FLAGS__START, TLS_ARRAY_ELEMENT__MAX);
+ }
+
+ inline bool GetIsAnyBlockOccupied() const
+ {
+ return m_afOccupancyFlags.EnumAnyGetEnumeratedFlagValue(FL_OCCUPANCY_FLAGS__START, TLS_ARRAY_ELEMENT__MAX);
+ }
+
+ inline bool SetBlockOccupiedFlag(unsigned int nBlockIndex)
+ {
+ return m_afOccupancyFlags.EnumModifyEnumeratedFlagValue(FL_OCCUPANCY_FLAGS__START, nBlockIndex, TLS_ARRAY_ELEMENT__MAX, true);
+ }
+
+ inline void ResetBlockOccupiedFlag(unsigned int nBlockIndex)
+ {
+ m_afOccupancyFlags.EnumDropEnumeratedFlagValue(FL_OCCUPANCY_FLAGS__START, nBlockIndex, TLS_ARRAY_ELEMENT__MAX);
+ }
+
+ inline bool GetBlockOccupiedFlag(unsigned int nBlockIndex) const
+ {
+ return m_afOccupancyFlags.EnumGetEnumeratedFlagValue(FL_OCCUPANCY_FLAGS__START, nBlockIndex, TLS_ARRAY_ELEMENT__MAX);
+ }
+
+ inline bool SetArrayLockedFlag()
+ {
+ return m_afOccupancyFlags.ModifySingleFlagValue(FL_ARRAY_LOCKED, true);
+ }
+
+ inline void ResetArrayLockedFlag()
+ {
+ m_afOccupancyFlags.DropFlagsMaskValue(FL_ARRAY_LOCKED);
+ }
private:
- typedef CAtomicFlags COccupancyFlagsType;
+ typedef CAtomicFlags COccupancyFlagsType;
- volatile atomicptr m_psaNextArray; // CTLSStorageArray *
- COccupancyFlagsType m_afOccupancyFlags;
+ volatile atomicptr m_psaNextArray; // CTLSStorageArray *
+ COccupancyFlagsType m_afOccupancyFlags;
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
- CClientHandleArray m_haBlockThreads;
+ CClientHandleArray m_haBlockThreads;
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
- // CTLSStorageBlock m_asbStorageBlocks[];
+ // CTLSStorageBlock m_asbStorageBlocks[];
};
class CTLSStorageInstance
{
public:
- static CTLSStorageInstance *AllocateInstance(tlsindextype iValueCount, unsigned int uiInitializationFlags);
- void FreeInstance();
+ static CTLSStorageInstance *AllocateInstance(tlsindextype iValueCount, unsigned int uiInitializationFlags);
+ void FreeInstance();
protected:
- CTLSStorageInstance(tlsindextype iValueCount, unsigned int uiInitializationFlags);
- ~CTLSStorageInstance();
+ CTLSStorageInstance(tlsindextype iValueCount, unsigned int uiInitializationFlags);
+ ~CTLSStorageInstance();
public:
- bool Init(ESTORAGEINSTANCEKIND ikInstanceKind);
+ bool Init(ESTORAGEINSTANCEKIND ikInstanceKind);
private:
- void Finit();
+ void Finit();
public:
- inline const HTLSKEYVALUE &RetrieveStorageKey() const { return GetStorageKey(); }
- inline tlsindextype RetrieveValueCount() const { return GetValueCount(); }
- inline unsigned int RetrieveInitializationFlags() const { return GetInitializationFlags(); }
-
- inline bool GetIsThreadManualCleanup() const { return GetThreadManualCleanupFlag(); }
+ inline const HTLSKEYVALUE &RetrieveStorageKey() const { return GetStorageKey(); }
+ inline tlsindextype RetrieveValueCount() const { return GetValueCount(); }
+ inline unsigned int RetrieveInitializationFlags() const { return GetInitializationFlags(); }
+
+ inline bool GetIsThreadManualCleanup() const { return GetThreadManualCleanupFlag(); }
public:
- void FreeStorageBlockOnThreadExit(CTLSStorageBlock *psbStorageBlock);
+ void FreeStorageBlockOnThreadExit(CTLSStorageBlock *psbStorageBlock);
public:
- bool FindFreeStorageBlock(CTLSStorageBlock *&psbOutStorageBlock);
+ bool FindFreeStorageBlock(CTLSStorageBlock *&psbOutStorageBlock);
private:
- bool FindFreeStorageBlockInArrayList(CTLSStorageBlock *&psbOutStorageBlock);
- bool FindFreeStorageBlockInArrayListSegment(CTLSStorageBlock *&psbOutStorageBlock,
- CTLSStorageArray *psaListSegmentBegin, CTLSStorageArray *psaListSegmentEnd);
- bool FindFreeStorageBlockFromArray(CTLSStorageBlock *&psbOutStorageBlock,
- CTLSStorageArray *psaArrayInstance);
+ bool FindFreeStorageBlockInArrayList(CTLSStorageBlock *&psbOutStorageBlock);
+ bool FindFreeStorageBlockInArrayListSegment(CTLSStorageBlock *&psbOutStorageBlock,
+ CTLSStorageArray *psaListSegmentBegin, CTLSStorageArray *psaListSegmentEnd);
+ bool FindFreeStorageBlockFromArray(CTLSStorageBlock *&psbOutStorageBlock,
+ CTLSStorageArray *psaArrayInstance);
- void AddStorageArrayToArrayList(CTLSStorageArray *psaStorageArray);
+ void AddStorageArrayToArrayList(CTLSStorageArray *psaStorageArray);
private:
- static bool AllocateStorageKey(HTLSKEYVALUE &hkvOutStorageKey, ESTORAGEINSTANCEKIND ikInstanceKind);
- static void FreeStorageKey(const HTLSKEYVALUE &hkvStorageKey);
+ static bool AllocateStorageKey(HTLSKEYVALUE &hkvOutStorageKey, ESTORAGEINSTANCEKIND ikInstanceKind);
+ static void FreeStorageKey(const HTLSKEYVALUE &hkvStorageKey);
#if _OU_TARGET_OS != _OU_TARGET_OS_WINDOWS
- static void FreeStorageBlock_Callback_Automatic(void *pv_DataValue);
- static void FreeStorageBlock_Callback_Manual(void *pv_DataValue);
+ static void FreeStorageBlock_Callback_Automatic(void *pv_DataValue);
+ static void FreeStorageBlock_Callback_Manual(void *pv_DataValue);
#endif // #if _OU_TARGET_OS != _OU_TARGET_OS_WINDOWS
- void FreeStorageBlock(CTLSStorageBlock *psbStorageBlock);
-
- CTLSStorageArray *AllocateStorageArray();
- void FreeStorageArrayList(CTLSStorageArray *psaStorageArrayList);
-
+ void FreeStorageBlock(CTLSStorageBlock *psbStorageBlock);
+
+ CTLSStorageArray *AllocateStorageArray();
+ void FreeStorageArrayList(CTLSStorageArray *psaStorageArrayList);
+
private:
- inline bool TrySettingStorageArrayList(CTLSStorageArray *psaInstance, CTLSStorageArray *psaCurrentList)
- {
- return AtomicCompareExchangePointer(&m_psaStorageList, (atomicptr)psaCurrentList, (atomicptr)psaInstance);
- }
+ inline bool TrySettingStorageArrayList(CTLSStorageArray *psaInstance, CTLSStorageArray *psaCurrentList)
+ {
+ return AtomicCompareExchangePointer(&m_psaStorageList, (atomicptr)psaCurrentList, (atomicptr)psaInstance);
+ }
- inline CTLSStorageArray *GetStorageArrayList() const
- {
- return (CTLSStorageArray *)m_psaStorageList;
- }
+ inline CTLSStorageArray *GetStorageArrayList() const
+ {
+ return (CTLSStorageArray *)m_psaStorageList;
+ }
- inline void SetStorageKey(const HTLSKEYVALUE &hskValue) { m_hskStorageKey = hskValue; }
- inline const HTLSKEYVALUE &GetStorageKey() const { return m_hskStorageKey; }
+ inline void SetStorageKey(const HTLSKEYVALUE &hskValue) { m_hskStorageKey = hskValue; }
+ inline const HTLSKEYVALUE &GetStorageKey() const { return m_hskStorageKey; }
- inline tlsindextype GetValueCount() const { return m_iValueCount; }
+ inline tlsindextype GetValueCount() const { return m_iValueCount; }
private:
- enum
- {
- FL_STORAGE_KEY_VALID = 0x00000001,
+ enum
+ {
+ FL_STORAGE_KEY_VALID = 0x00000001,
- FLM_INITIALIZATION_FLAGS_MASK = 0x0000FFFF,
- FLS_INITIALIZATION_FLAGS_SHIFT = 16,
+ FLM_INITIALIZATION_FLAGS_MASK = 0x0000FFFF,
+ FLS_INITIALIZATION_FLAGS_SHIFT = 16,
- FL_INITIALIZATION_THREAD_MANUAL_CLEANUP = CTLSInitialization::SIF_MANUAL_CLEANUP_ON_THREAD_EXIT << FLS_INITIALIZATION_FLAGS_SHIFT
- };
+ FL_INITIALIZATION_THREAD_MANUAL_CLEANUP = CTLSInitialization::SIF_MANUAL_CLEANUP_ON_THREAD_EXIT << FLS_INITIALIZATION_FLAGS_SHIFT
+ };
- inline void SetStorageKeyValidFlag() { m_sfInstanceFlags.SignalFlagsMaskValue(FL_STORAGE_KEY_VALID); }
- inline void ResetStorageKeyValidFlag() { m_sfInstanceFlags.DropFlagsMaskValue(FL_STORAGE_KEY_VALID); }
- inline bool GetStorageKeyValidFlag() const { return m_sfInstanceFlags.GetFlagsMaskValue(FL_STORAGE_KEY_VALID); }
+ inline void SetStorageKeyValidFlag() { m_sfInstanceFlags.SignalFlagsMaskValue(FL_STORAGE_KEY_VALID); }
+ inline void ResetStorageKeyValidFlag() { m_sfInstanceFlags.DropFlagsMaskValue(FL_STORAGE_KEY_VALID); }
+ inline bool GetStorageKeyValidFlag() const { return m_sfInstanceFlags.GetFlagsMaskValue(FL_STORAGE_KEY_VALID); }
- inline void SetInitializationFlags(unsigned int uiValue) { m_sfInstanceFlags.StoreFlagsEnumeratedValue(FLM_INITIALIZATION_FLAGS_MASK, FLS_INITIALIZATION_FLAGS_SHIFT, uiValue); }
- inline unsigned int GetInitializationFlags() const { return m_sfInstanceFlags.RetrieveFlagsEnumeratedValue(FLM_INITIALIZATION_FLAGS_MASK, FLS_INITIALIZATION_FLAGS_SHIFT); }
+ inline void SetInitializationFlags(unsigned int uiValue) { m_sfInstanceFlags.StoreFlagsEnumeratedValue(FLM_INITIALIZATION_FLAGS_MASK, FLS_INITIALIZATION_FLAGS_SHIFT, uiValue); }
+ inline unsigned int GetInitializationFlags() const { return m_sfInstanceFlags.RetrieveFlagsEnumeratedValue(FLM_INITIALIZATION_FLAGS_MASK, FLS_INITIALIZATION_FLAGS_SHIFT); }
- inline bool GetThreadManualCleanupFlag() const { return m_sfInstanceFlags.GetFlagsMaskValue(FL_INITIALIZATION_THREAD_MANUAL_CLEANUP); }
+ inline bool GetThreadManualCleanupFlag() const { return m_sfInstanceFlags.GetFlagsMaskValue(FL_INITIALIZATION_THREAD_MANUAL_CLEANUP); }
private:
- volatile atomicptr m_psaStorageList; // CTLSStorageArray *
- HTLSKEYVALUE m_hskStorageKey;
- CSimpleFlags m_sfInstanceFlags;
- tlsindextype m_iValueCount;
+ volatile atomicptr m_psaStorageList; // CTLSStorageArray *
+ HTLSKEYVALUE m_hskStorageKey;
+ CSimpleFlags m_sfInstanceFlags;
+ tlsindextype m_iValueCount;
};
@@ -367,268 +367,278 @@ private:
CTLSStorageArray *CTLSStorageArray::AllocateInstance(tlsindextype iValueCount)
{
- const size_t nHeaderSize = CTLSStorageArray::GetHeaderSize();
- const size_t nBlockSize = CTLSStorageBlock::GetRequiredSize(iValueCount);
- size_t nRequiredSize = nHeaderSize + nBlockSize * TLS_ARRAY_ELEMENT__MAX;
+ const size_t nHeaderSize = CTLSStorageArray::GetHeaderSize();
+ const size_t nBlockSize = CTLSStorageBlock::GetRequiredSize(iValueCount);
+ size_t nRequiredSize = nHeaderSize + nBlockSize * TLS_ARRAY_ELEMENT__MAX;
+
+ CTLSStorageArray *psaNewInstance = (CTLSStorageArray *)AllocateMemoryBlock(nRequiredSize);
- CTLSStorageArray *psaNewInstance = (CTLSStorageArray *)AllocateMemoryBlock(nRequiredSize);
-
- if (psaNewInstance)
- {
- memset(psaNewInstance, 0, nRequiredSize);
- new((CTLSStorageArray *)psaNewInstance) CTLSStorageArray();
+ if (psaNewInstance)
+ {
+ memset(psaNewInstance, 0, nRequiredSize);
+ new((CTLSStorageArray *)psaNewInstance) CTLSStorageArray();
- psaNewInstance->AssignAllBlocksHostArray(iValueCount);
- }
+ psaNewInstance->AssignAllBlocksHostArray(iValueCount);
+ }
- return psaNewInstance;
+ return psaNewInstance;
}
void CTLSStorageArray::FreeInstance(tlsindextype iValueCount)
{
- if (GetIsAnyBlockOccupied())
- {
- FreeStorageAllBlocks(iValueCount);
- }
+ if (GetIsAnyBlockOccupied())
+ {
+ FreeStorageAllBlocks(iValueCount);
+ }
- this->CTLSStorageArray::~CTLSStorageArray();
- FreeMemoryBlock((void *)this);
+ this->CTLSStorageArray::~CTLSStorageArray();
+ FreeMemoryBlock((void *)this);
}
CTLSStorageArray::CTLSStorageArray()
{
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-
- AssignAllBlocksInvalidThreads();
-
-
+
+ AssignAllBlocksInvalidThreads();
+
+
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
}
CTLSStorageArray::~CTLSStorageArray()
{
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-
- OU_ASSERT(CheckIfAllBlocksHaveInvalidThreads());
-
-
+
+ OU_ASSERT(CheckIfAllBlocksHaveInvalidThreads());
+
+
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
}
void CTLSStorageArray::FreeStorageBlockOnThreadExit(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount)
{
- ReinitializeStorageSingleBlock(psbStorageBlock, iValueCount);
- // OU_ASSERT(GetBlockThreadHandle(nBlockIndex) == INVALID_HANDLE_VALUE) -- assertion further in the code
+ ReinitializeStorageSingleBlock(psbStorageBlock, iValueCount);
+ // OU_ASSERT(GetBlockThreadHandle(nBlockIndex) == INVALID_HANDLE_VALUE) -- assertion further in the code
- unsigned int nBlockIndex = GetStorageBlockIndex(psbStorageBlock, iValueCount);
- OU_ASSERT(GetBlockOccupiedFlag(nBlockIndex));
+ unsigned int nBlockIndex = GetStorageBlockIndex(psbStorageBlock, iValueCount);
+ OU_ASSERT(GetBlockOccupiedFlag(nBlockIndex));
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
- OU_ASSERT(GetBlockThreadHandle(nBlockIndex) == INVALID_HANDLE_VALUE); // The method is not to be called if automatic cleanup is enabled
+ OU_ASSERT(GetBlockThreadHandle(nBlockIndex) == INVALID_HANDLE_VALUE); // The method is not to be called if automatic cleanup is enabled
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
- ResetBlockOccupiedFlag(nBlockIndex);
+ ResetBlockOccupiedFlag(nBlockIndex);
}
-bool CTLSStorageArray::FindFreeStorageBlock(CTLSStorageBlock *&psbOutFreeStorageBlock,
- tlsindextype iValueCount, bool bIsManualCleanup)
+bool CTLSStorageArray::FindFreeStorageBlock(CTLSStorageBlock *&psbOutFreeStorageBlock,
+ tlsindextype iValueCount, bool bIsManualCleanup)
{
- bool bResult = false;
+ bool bResult = false;
- unsigned int nFreeBlockIndex;
+ unsigned int nFreeBlockIndex;
- if (FindFreeStorageBlockIndex(nFreeBlockIndex, iValueCount, bIsManualCleanup))
- {
- CTLSStorageBlock *psbFreeStorageBlock = GetStorageBlockPointer(nFreeBlockIndex, iValueCount);
-
- psbOutFreeStorageBlock = psbFreeStorageBlock;
- bResult = true;
- }
+ if (FindFreeStorageBlockIndex(nFreeBlockIndex, iValueCount, bIsManualCleanup))
+ {
+ CTLSStorageBlock *psbFreeStorageBlock = GetStorageBlockPointer(nFreeBlockIndex, iValueCount);
- return bResult;
+ psbOutFreeStorageBlock = psbFreeStorageBlock;
+ bResult = true;
+ }
+
+ return bResult;
}
-bool CTLSStorageArray::FindFreeStorageBlockIndex(unsigned int &nOutFreeBlockIndex,
- tlsindextype /*iValueCount*/, bool bIsManualCleanup)
+#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
+bool CTLSStorageArray::FindFreeStorageBlockIndex(unsigned int &nOutFreeBlockIndex, tlsindextype iValueCount, bool bIsManualCleanup)
+#else
+
+bool CTLSStorageArray::FindFreeStorageBlockIndex(unsigned int &nOutFreeBlockIndex,
+ tlsindextype /*iValueCount*/, bool bIsManualCleanup)
+#endif
{
- bool bResult = false;
+ bool bResult = false;
- if (!GetAreAllBlocksOccupied() && FindFreeStorageBlockIndexWithPossibilityVerified(nOutFreeBlockIndex, bIsManualCleanup))
- {
- bResult = true;
- }
+ if (!GetAreAllBlocksOccupied() && FindFreeStorageBlockIndexWithPossibilityVerified(nOutFreeBlockIndex, bIsManualCleanup))
+ {
+ bResult = true;
+ }
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
- else if (!bIsManualCleanup)
- {
- // Execution gets here is all slots were already occupied or
- // they become occupied during search (otherwise why
- // FindFreeStorageBlockIndexWithPossibilityVerified call failed???).
- // In Automatic cleanup mode a block can't become free by itself -
- // it is just re-allocated for new thread and remains busy.
- OU_ASSERT(GetAreAllBlocksOccupied());
-
- // The locking is performed to avoid more than one threads checking
- // for abandoned handles simultaneously.
- // If locking fails, execution just proceeds to next array in the chain
- if (SetArrayLockedFlag())
- {
- bResult = FindAbandonedStorageBlockIndex(nOutFreeBlockIndex, iValueCount);
-
- ResetArrayLockedFlag();
- }
- }
+ else if (!bIsManualCleanup)
+ {
+ // Execution gets here is all slots were already occupied or
+ // they become occupied during search (otherwise why
+ // FindFreeStorageBlockIndexWithPossibilityVerified call failed???).
+ // In Automatic cleanup mode a block can't become free by itself -
+ // it is just re-allocated for new thread and remains busy.
+ OU_ASSERT(GetAreAllBlocksOccupied());
+
+ // The locking is performed to avoid more than one threads checking
+ // for abandoned handles simultaneously.
+ // If locking fails, execution just proceeds to next array in the chain
+ if (SetArrayLockedFlag())
+ {
+ bResult = FindAbandonedStorageBlockIndex(nOutFreeBlockIndex, iValueCount);
+
+ ResetArrayLockedFlag();
+ }
+ }
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
- return bResult;
+ return bResult;
}
-bool CTLSStorageArray::FindFreeStorageBlockIndexWithPossibilityVerified(unsigned int &nOutFreeBlockIndex,
- bool /*bIsManualCleanup*/)
+#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
+bool CTLSStorageArray::FindFreeStorageBlockIndexWithPossibilityVerified(unsigned int &nOutFreeBlockIndex,
+ bool bIsManualCleanup)
+#else
+bool CTLSStorageArray::FindFreeStorageBlockIndexWithPossibilityVerified(unsigned int &nOutFreeBlockIndex,
+ bool /*bIsManualCleanup*/)
+#endif
{
- unsigned int nBlockIndex = 0;
+ unsigned int nBlockIndex = 0;
- for (; nBlockIndex != TLS_ARRAY_ELEMENT__MAX; ++nBlockIndex)
- {
- if (SetBlockOccupiedFlag(nBlockIndex))
- {
+ for (; nBlockIndex != TLS_ARRAY_ELEMENT__MAX; ++nBlockIndex)
+ {
+ if (SetBlockOccupiedFlag(nBlockIndex))
+ {
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
- if (!bIsManualCleanup)
- {
- AllocateBlockThreadHandle(nBlockIndex);
- }
-
+ if (!bIsManualCleanup)
+ {
+ AllocateBlockThreadHandle(nBlockIndex);
+ }
+
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
- nOutFreeBlockIndex = nBlockIndex;
- break;
- }
- }
+ nOutFreeBlockIndex = nBlockIndex;
+ break;
+ }
+ }
- bool bResult = nBlockIndex != TLS_ARRAY_ELEMENT__MAX;
- return bResult;
+ bool bResult = nBlockIndex != TLS_ARRAY_ELEMENT__MAX;
+ return bResult;
}
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-bool CTLSStorageArray::FindAbandonedStorageBlockIndex(unsigned int &nOutFreeBlockIndex,
- tlsindextype iValueCount)
+bool CTLSStorageArray::FindAbandonedStorageBlockIndex(unsigned int &nOutFreeBlockIndex,
+ tlsindextype iValueCount)
{
- bool bResult = false;
-
- do
- {
- CClientHandleArray haTranslatedHandlesStorage;
- CHandleTranslationMap tmTranslationMapStorage;
-
- const HANDLE *ph_TranslatedHandles;
- const unsigned int *puiTranslationMap;
-
- // Translate handles into array for the case if there are invalids
- unsigned int nHandleCount = TranslateClientHandles(haTranslatedHandlesStorage, tmTranslationMapStorage,
- ph_TranslatedHandles, puiTranslationMap);
- OU_ASSERT(OU_IN_INT_RANGE(nHandleCount, 0, MAXIMUM_WAIT_OBJECTS + 1));
-
- if (nHandleCount == 0)
- {
- break;
- }
-
- // Since allocating a new storage block is a relatively slow operation
- // it is acceptable to enter kernel for checking for exited threads.
- DWORD dwWaitResult = ::WaitForMultipleObjects(nHandleCount, ph_TranslatedHandles, FALSE, 0);
-
- if (!OU_IN_INT_RANGE(dwWaitResult - WAIT_OBJECT_0, 0, nHandleCount))
- {
- // Wait should not normally fail. If it does it's in most cases an indication
- // of invalid handle passed as parameter. However it may fail because of other
- // reasons as well. If this assertion fails too often and you are sure all the
- // handles are valid, it is safe to comment it.
- OU_ASSERT(dwWaitResult != WAIT_FAILED);
-
- break;
- }
-
- unsigned int nTranslatedBlockIndex = (unsigned int)(dwWaitResult - WAIT_OBJECT_0);
- unsigned int nBlockIndex = !puiTranslationMap ? nTranslatedBlockIndex : puiTranslationMap[nTranslatedBlockIndex];
-
- CTLSStorageBlock *psbStorageBlock = GetStorageBlockPointer(nBlockIndex, iValueCount);
- ReinitializeStorageSingleBlock(psbStorageBlock, iValueCount);
-
- // Close old handle and make a duplicate of current thread handle
- FreeStorageThreadHandle(nBlockIndex);
- AllocateBlockThreadHandle(nBlockIndex);
-
- nOutFreeBlockIndex = nBlockIndex;
- bResult = true;
- }
- while (false);
-
- return bResult;
+ bool bResult = false;
+
+ do
+ {
+ CClientHandleArray haTranslatedHandlesStorage;
+ CHandleTranslationMap tmTranslationMapStorage;
+
+ const HANDLE *ph_TranslatedHandles;
+ const unsigned int *puiTranslationMap;
+
+ // Translate handles into array for the case if there are invalids
+ unsigned int nHandleCount = TranslateClientHandles(haTranslatedHandlesStorage, tmTranslationMapStorage,
+ ph_TranslatedHandles, puiTranslationMap);
+ OU_ASSERT(OU_IN_INT_RANGE(nHandleCount, 0, MAXIMUM_WAIT_OBJECTS + 1));
+
+ if (nHandleCount == 0)
+ {
+ break;
+ }
+
+ // Since allocating a new storage block is a relatively slow operation
+ // it is acceptable to enter kernel for checking for exited threads.
+ DWORD dwWaitResult = ::WaitForMultipleObjects(nHandleCount, ph_TranslatedHandles, FALSE, 0);
+
+ if (!OU_IN_INT_RANGE(dwWaitResult - WAIT_OBJECT_0, 0, nHandleCount))
+ {
+ // Wait should not normally fail. If it does it's in most cases an indication
+ // of invalid handle passed as parameter. However it may fail because of other
+ // reasons as well. If this assertion fails too often and you are sure all the
+ // handles are valid, it is safe to comment it.
+ OU_ASSERT(dwWaitResult != WAIT_FAILED);
+
+ break;
+ }
+
+ unsigned int nTranslatedBlockIndex = (unsigned int)(dwWaitResult - WAIT_OBJECT_0);
+ unsigned int nBlockIndex = !puiTranslationMap ? nTranslatedBlockIndex : puiTranslationMap[nTranslatedBlockIndex];
+
+ CTLSStorageBlock *psbStorageBlock = GetStorageBlockPointer(nBlockIndex, iValueCount);
+ ReinitializeStorageSingleBlock(psbStorageBlock, iValueCount);
+
+ // Close old handle and make a duplicate of current thread handle
+ FreeStorageThreadHandle(nBlockIndex);
+ AllocateBlockThreadHandle(nBlockIndex);
+
+ nOutFreeBlockIndex = nBlockIndex;
+ bResult = true;
+ }
+ while (false);
+
+ return bResult;
}
unsigned int CTLSStorageArray::TranslateClientHandles(CClientHandleArray haTranslatedHandlesStorage, CHandleTranslationMap tmTranslationMapStorage,
- const HANDLE *&ph_OutTranslatedHandles, const unsigned int *&puiOutTranslationMap) const
+ const HANDLE *&ph_OutTranslatedHandles, const unsigned int *&puiOutTranslationMap) const
{
- ph_OutTranslatedHandles = haTranslatedHandlesStorage;
- puiOutTranslationMap = tmTranslationMapStorage;
-
- unsigned int nTargetStartIndex = 0;
- unsigned int nSourceStartIndex = 0, nSourceCurrentIndex = 0;
-
- while (true)
- {
- if (GetBlockThreadHandle(nSourceCurrentIndex) == INVALID_HANDLE_VALUE)
- {
- const HANDLE *ph_BlockThreadHandles = GetBlockThreadHandlesStorage();
-
- unsigned int nTargetIncrement = nSourceCurrentIndex - nSourceStartIndex;
-
- memcpy(&haTranslatedHandlesStorage[nTargetStartIndex], &ph_BlockThreadHandles[nSourceStartIndex], nTargetIncrement * sizeof(HANDLE));
- for (; nTargetIncrement != 0; ++nTargetStartIndex, ++nSourceStartIndex, --nTargetIncrement) { tmTranslationMapStorage[nTargetStartIndex] = nSourceStartIndex; }
-
- // Skip invalid handle (at this point nSourceStartIndex is equal to nSourceCurrentIndex)
- ++nSourceStartIndex;
- }
-
- ++nSourceCurrentIndex;
-
- if (nSourceCurrentIndex == TLS_ARRAY_ELEMENT__MAX)
- {
- // Start indice can be equal if and only if no invalid handles have been found
- if (nSourceStartIndex != nTargetStartIndex)
- {
- const HANDLE *ph_BlockThreadHandles = GetBlockThreadHandlesStorage();
-
- unsigned int nTargetIncrement = nSourceCurrentIndex - nSourceStartIndex;
-
- memcpy(&haTranslatedHandlesStorage[nTargetStartIndex], &ph_BlockThreadHandles[nSourceStartIndex], nTargetIncrement * sizeof(HANDLE));
- for (; nTargetIncrement != 0; ++nTargetStartIndex, ++nSourceStartIndex, --nTargetIncrement) { tmTranslationMapStorage[nTargetStartIndex] = nSourceStartIndex; }
- }
-
- break;
- }
- }
-
- // If all the handles are valid...
- if (nTargetStartIndex == 0)
- {
- // ...just return original handle array as no copying was performed
- ph_OutTranslatedHandles = GetBlockThreadHandlesStorage();
- puiOutTranslationMap = NULL;
- }
-
- return nTargetStartIndex;
+ ph_OutTranslatedHandles = haTranslatedHandlesStorage;
+ puiOutTranslationMap = tmTranslationMapStorage;
+
+ unsigned int nTargetStartIndex = 0;
+ unsigned int nSourceStartIndex = 0, nSourceCurrentIndex = 0;
+
+ while (true)
+ {
+ if (GetBlockThreadHandle(nSourceCurrentIndex) == INVALID_HANDLE_VALUE)
+ {
+ const HANDLE *ph_BlockThreadHandles = GetBlockThreadHandlesStorage();
+
+ unsigned int nTargetIncrement = nSourceCurrentIndex - nSourceStartIndex;
+
+ memcpy(&haTranslatedHandlesStorage[nTargetStartIndex], &ph_BlockThreadHandles[nSourceStartIndex], nTargetIncrement * sizeof(HANDLE));
+ for (; nTargetIncrement != 0; ++nTargetStartIndex, ++nSourceStartIndex, --nTargetIncrement) { tmTranslationMapStorage[nTargetStartIndex] = nSourceStartIndex; }
+
+ // Skip invalid handle (at this point nSourceStartIndex is equal to nSourceCurrentIndex)
+ ++nSourceStartIndex;
+ }
+
+ ++nSourceCurrentIndex;
+
+ if (nSourceCurrentIndex == TLS_ARRAY_ELEMENT__MAX)
+ {
+ // Start indice can be equal if and only if no invalid handles have been found
+ if (nSourceStartIndex != nTargetStartIndex)
+ {
+ const HANDLE *ph_BlockThreadHandles = GetBlockThreadHandlesStorage();
+
+ unsigned int nTargetIncrement = nSourceCurrentIndex - nSourceStartIndex;
+
+ memcpy(&haTranslatedHandlesStorage[nTargetStartIndex], &ph_BlockThreadHandles[nSourceStartIndex], nTargetIncrement * sizeof(HANDLE));
+ for (; nTargetIncrement != 0; ++nTargetStartIndex, ++nSourceStartIndex, --nTargetIncrement) { tmTranslationMapStorage[nTargetStartIndex] = nSourceStartIndex; }
+ }
+
+ break;
+ }
+ }
+
+ // If all the handles are valid...
+ if (nTargetStartIndex == 0)
+ {
+ // ...just return original handle array as no copying was performed
+ ph_OutTranslatedHandles = GetBlockThreadHandlesStorage();
+ puiOutTranslationMap = NULL;
+ }
+
+ return nTargetStartIndex;
}
@@ -637,106 +647,106 @@ unsigned int CTLSStorageArray::TranslateClientHandles(CClientHandleArray haTrans
void CTLSStorageArray::FreeStorageAllBlocks(tlsindextype iValueCount)
{
- for (unsigned int nBlockIndex = 0; nBlockIndex != TLS_ARRAY_ELEMENT__MAX; ++nBlockIndex)
- {
- if (GetBlockOccupiedFlag(nBlockIndex))
- {
- CTLSStorageBlock *psbStorageBlock = GetStorageBlockPointer(nBlockIndex, iValueCount);
+ for (unsigned int nBlockIndex = 0; nBlockIndex != TLS_ARRAY_ELEMENT__MAX; ++nBlockIndex)
+ {
+ if (GetBlockOccupiedFlag(nBlockIndex))
+ {
+ CTLSStorageBlock *psbStorageBlock = GetStorageBlockPointer(nBlockIndex, iValueCount);
- FinalizeStorageSingleBlock(psbStorageBlock, iValueCount);
+ FinalizeStorageSingleBlock(psbStorageBlock, iValueCount);
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-
- FreeStorageThreadHandle(nBlockIndex);
+
+ FreeStorageThreadHandle(nBlockIndex);
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
- }
- else
- {
+ }
+ else
+ {
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-
- OU_ASSERT(GetBlockThreadHandle(nBlockIndex) == INVALID_HANDLE_VALUE); // Where did the handle come from if block is not occupied?
-
-
+
+ OU_ASSERT(GetBlockThreadHandle(nBlockIndex) == INVALID_HANDLE_VALUE); // Where did the handle come from if block is not occupied?
+
+
#endif // #if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
- }
- }
+ }
+ }
}
void CTLSStorageArray::ReinitializeStorageSingleBlock(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount)
{
- FinalizeStorageSingleBlock(psbStorageBlock, iValueCount);
-
- ZeroStorageBlockMemory(psbStorageBlock, iValueCount);
- AssignSingleBlockHostArray(psbStorageBlock);
+ FinalizeStorageSingleBlock(psbStorageBlock, iValueCount);
+
+ ZeroStorageBlockMemory(psbStorageBlock, iValueCount);
+ AssignSingleBlockHostArray(psbStorageBlock);
}
void CTLSStorageArray::FinalizeStorageSingleBlock(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount)
{
- for (tlsindextype iValueIndex = 0; iValueIndex != iValueCount; ++iValueIndex)
- {
- tlsvaluetype vValueData = psbStorageBlock->GetValueData(iValueIndex);
-
- if (vValueData)
- {
- CTLSValueDestructor fnValueDestructor = psbStorageBlock->GetValueDestructor(iValueIndex);
-
- if (fnValueDestructor)
- {
- fnValueDestructor(vValueData);
- }
- }
- }
+ for (tlsindextype iValueIndex = 0; iValueIndex != iValueCount; ++iValueIndex)
+ {
+ tlsvaluetype vValueData = psbStorageBlock->GetValueData(iValueIndex);
+
+ if (vValueData)
+ {
+ CTLSValueDestructor fnValueDestructor = psbStorageBlock->GetValueDestructor(iValueIndex);
+
+ if (fnValueDestructor)
+ {
+ fnValueDestructor(vValueData);
+ }
+ }
+ }
}
void CTLSStorageArray::AssignAllBlocksHostArray(tlsindextype iValueCount)
{
- for (unsigned int nBlockIndex = 0; nBlockIndex != TLS_ARRAY_ELEMENT__MAX; ++nBlockIndex)
- {
- CTLSStorageBlock *psbStorageBlock = GetStorageBlockPointer(nBlockIndex, iValueCount);
+ for (unsigned int nBlockIndex = 0; nBlockIndex != TLS_ARRAY_ELEMENT__MAX; ++nBlockIndex)
+ {
+ CTLSStorageBlock *psbStorageBlock = GetStorageBlockPointer(nBlockIndex, iValueCount);
- AssignSingleBlockHostArray(psbStorageBlock);
- }
+ AssignSingleBlockHostArray(psbStorageBlock);
+ }
}
void CTLSStorageArray::AssignSingleBlockHostArray(CTLSStorageBlock *psbStorageBlock)
{
- psbStorageBlock->SetHostArray(this);
+ psbStorageBlock->SetHostArray(this);
}
CTLSStorageBlock *CTLSStorageArray::GetStorageBlockPointer(unsigned int nBlockIndex, tlsindextype iValueCount) const
{
- OU_ASSERT(OU_IN_INT_RANGE(nBlockIndex, 0, TLS_ARRAY_ELEMENT__MAX));
-
- const size_t nHeaderSize = CTLSStorageArray::GetHeaderSize();
- const size_t nBlockSize = CTLSStorageBlock::GetRequiredSize(iValueCount);
- const size_t nBlockZeroOffset = CTLSStorageBlock::GetZeroOffset(iValueCount);
-
- CTLSStorageBlock *psbStorageBlock = (CTLSStorageBlock *)(((int8ou *)this) + nHeaderSize + nBlockIndex * nBlockSize + nBlockZeroOffset);
- return psbStorageBlock;
+ OU_ASSERT(OU_IN_INT_RANGE(nBlockIndex, 0, TLS_ARRAY_ELEMENT__MAX));
+
+ const size_t nHeaderSize = CTLSStorageArray::GetHeaderSize();
+ const size_t nBlockSize = CTLSStorageBlock::GetRequiredSize(iValueCount);
+ const size_t nBlockZeroOffset = CTLSStorageBlock::GetZeroOffset(iValueCount);
+
+ CTLSStorageBlock *psbStorageBlock = (CTLSStorageBlock *)(((int8ou *)this) + nHeaderSize + nBlockIndex * nBlockSize + nBlockZeroOffset);
+ return psbStorageBlock;
}
unsigned int CTLSStorageArray::GetStorageBlockIndex(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount) const
{
- const size_t nHeaderSize = CTLSStorageArray::GetHeaderSize();
- const size_t nBlockSize = CTLSStorageBlock::GetRequiredSize(iValueCount);
- const size_t nBlockZeroOffset = CTLSStorageBlock::GetZeroOffset(iValueCount);
+ const size_t nHeaderSize = CTLSStorageArray::GetHeaderSize();
+ const size_t nBlockSize = CTLSStorageBlock::GetRequiredSize(iValueCount);
+ const size_t nBlockZeroOffset = CTLSStorageBlock::GetZeroOffset(iValueCount);
- unsigned int uiBlockIndex = (unsigned int)((((int8ou *)psbStorageBlock) - nBlockZeroOffset - nHeaderSize - ((int8ou *)this)) / nBlockSize);
- OU_ASSERT((((int8ou *)psbStorageBlock) - nBlockZeroOffset - nHeaderSize - ((int8ou *)this)) % nBlockSize == 0);
- OU_ASSERT(OU_IN_INT_RANGE(uiBlockIndex, 0, TLS_ARRAY_ELEMENT__MAX));
+ unsigned int uiBlockIndex = (unsigned int)((((int8ou *)psbStorageBlock) - nBlockZeroOffset - nHeaderSize - ((int8ou *)this)) / nBlockSize);
+ OU_ASSERT((((int8ou *)psbStorageBlock) - nBlockZeroOffset - nHeaderSize - ((int8ou *)this)) % nBlockSize == 0);
+ OU_ASSERT(OU_IN_INT_RANGE(uiBlockIndex, 0, TLS_ARRAY_ELEMENT__MAX));
- return uiBlockIndex;
+ return uiBlockIndex;
}
void CTLSStorageArray::ZeroStorageBlockMemory(CTLSStorageBlock *psbStorageBlock, tlsindextype iValueCount)
{
- const size_t nBlockSize = CTLSStorageBlock::GetRequiredSize(iValueCount);
- const size_t nBlockZeroOffset = CTLSStorageBlock::GetZeroOffset(iValueCount);
+ const size_t nBlockSize = CTLSStorageBlock::GetRequiredSize(iValueCount);
+ const size_t nBlockZeroOffset = CTLSStorageBlock::GetZeroOffset(iValueCount);
- memset(((int8ou *)psbStorageBlock) - nBlockZeroOffset, 0, nBlockSize);
+ memset(((int8ou *)psbStorageBlock) - nBlockZeroOffset, 0, nBlockSize);
}
@@ -744,62 +754,62 @@ void CTLSStorageArray::ZeroStorageBlockMemory(CTLSStorageBlock *psbStorageBlock,
void CTLSStorageArray::AllocateBlockThreadHandle(unsigned int nBlockIndex)
{
- OU_ASSERT(GetBlockThreadHandle(nBlockIndex) == INVALID_HANDLE_VALUE);
-
- HANDLE hCurrentThreadDuplicate;
-
- HANDLE hCurrentProcess = ::GetCurrentProcess();
- HANDLE hCurrentThread = ::GetCurrentThread();
- if (!::DuplicateHandle(hCurrentProcess, hCurrentThread, hCurrentProcess, &hCurrentThreadDuplicate, SYNCHRONIZE, FALSE, 0))
- {
- // Handle duplication should not normally fail.
- // Thread and process pseudo-handles have full access allowed.
- // The duplication may only fail in case of kernel internal problems
- // (like lack of the resources or resource limit hits).
- // Well, in this case thread data will remain in memory until
- // CTLSInitialization::FinalizeTLSAPI() is called.
- hCurrentThreadDuplicate = INVALID_HANDLE_VALUE;
- }
-
- SetBlockThreadHandle(nBlockIndex, hCurrentThreadDuplicate);
+ OU_ASSERT(GetBlockThreadHandle(nBlockIndex) == INVALID_HANDLE_VALUE);
+
+ HANDLE hCurrentThreadDuplicate;
+
+ HANDLE hCurrentProcess = ::GetCurrentProcess();
+ HANDLE hCurrentThread = ::GetCurrentThread();
+ if (!::DuplicateHandle(hCurrentProcess, hCurrentThread, hCurrentProcess, &hCurrentThreadDuplicate, SYNCHRONIZE, FALSE, 0))
+ {
+ // Handle duplication should not normally fail.
+ // Thread and process pseudo-handles have full access allowed.
+ // The duplication may only fail in case of kernel internal problems
+ // (like lack of the resources or resource limit hits).
+ // Well, in this case thread data will remain in memory until
+ // CTLSInitialization::FinalizeTLSAPI() is called.
+ hCurrentThreadDuplicate = INVALID_HANDLE_VALUE;
+ }
+
+ SetBlockThreadHandle(nBlockIndex, hCurrentThreadDuplicate);
}
void CTLSStorageArray::FreeStorageThreadHandle(unsigned int nBlockIndex)
{
- HANDLE hExistingThreadHandle = GetBlockThreadHandle(nBlockIndex);
-
- if (hExistingThreadHandle != INVALID_HANDLE_VALUE)
- {
- BOOL bHandleCloseResult = ::CloseHandle(hExistingThreadHandle);
- OU_VERIFY(bHandleCloseResult); // Closing handle should normally succeed
-
- SetBlockThreadHandle(nBlockIndex, INVALID_HANDLE_VALUE);
- }
+ HANDLE hExistingThreadHandle = GetBlockThreadHandle(nBlockIndex);
+
+ if (hExistingThreadHandle != INVALID_HANDLE_VALUE)
+ {
+ BOOL bHandleCloseResult = ::CloseHandle(hExistingThreadHandle);
+ OU_VERIFY(bHandleCloseResult); // Closing handle should normally succeed
+
+ SetBlockThreadHandle(nBlockIndex, INVALID_HANDLE_VALUE);
+ }
}
void CTLSStorageArray::AssignAllBlocksInvalidThreads()
{
- for (unsigned int nBlockIndex = 0; nBlockIndex != TLS_ARRAY_ELEMENT__MAX; ++nBlockIndex)
- {
- SetBlockThreadHandle(nBlockIndex, INVALID_HANDLE_VALUE);
- }
+ for (unsigned int nBlockIndex = 0; nBlockIndex != TLS_ARRAY_ELEMENT__MAX; ++nBlockIndex)
+ {
+ SetBlockThreadHandle(nBlockIndex, INVALID_HANDLE_VALUE);
+ }
}
bool CTLSStorageArray::CheckIfAllBlocksHaveInvalidThreads()
{
- unsigned nBlockIndex = 0;
-
- for (; nBlockIndex != TLS_ARRAY_ELEMENT__MAX; ++nBlockIndex)
- {
- if (GetBlockThreadHandle(nBlockIndex) != INVALID_HANDLE_VALUE)
- {
- break;
- }
- }
-
- bool bResult = nBlockIndex == TLS_ARRAY_ELEMENT__MAX;
- return bResult;
+ unsigned nBlockIndex = 0;
+
+ for (; nBlockIndex != TLS_ARRAY_ELEMENT__MAX; ++nBlockIndex)
+ {
+ if (GetBlockThreadHandle(nBlockIndex) != INVALID_HANDLE_VALUE)
+ {
+ break;
+ }
+ }
+
+ bool bResult = nBlockIndex == TLS_ARRAY_ELEMENT__MAX;
+ return bResult;
}
@@ -811,270 +821,270 @@ bool CTLSStorageArray::CheckIfAllBlocksHaveInvalidThreads()
CTLSStorageInstance *CTLSStorageInstance::AllocateInstance(tlsindextype iValueCount, unsigned int uiInitializationFlags)
{
- size_t nSizeRequired = sizeof(CTLSStorageInstance);
-
- CTLSStorageInstance *psiNewInstance = (CTLSStorageInstance *)AllocateMemoryBlock(nSizeRequired);
-
- if (psiNewInstance)
- {
- new(psiNewInstance) CTLSStorageInstance(iValueCount, uiInitializationFlags);
- }
-
- return psiNewInstance;
+ size_t nSizeRequired = sizeof(CTLSStorageInstance);
+
+ CTLSStorageInstance *psiNewInstance = (CTLSStorageInstance *)AllocateMemoryBlock(nSizeRequired);
+
+ if (psiNewInstance)
+ {
+ new(psiNewInstance) CTLSStorageInstance(iValueCount, uiInitializationFlags);
+ }
+
+ return psiNewInstance;
}
void CTLSStorageInstance::FreeInstance()
{
- this->CTLSStorageInstance::~CTLSStorageInstance();
- FreeMemoryBlock(this);
+ this->CTLSStorageInstance::~CTLSStorageInstance();
+ FreeMemoryBlock(this);
}
CTLSStorageInstance::CTLSStorageInstance(tlsindextype iValueCount, unsigned int uiInitializationFlags):
- m_psaStorageList((atomicptr)0),
- m_hskStorageKey((HTLSKEYVALUE::value_type)0),
- m_iValueCount(iValueCount)
+ m_psaStorageList((atomicptr)0),
+ m_hskStorageKey((HTLSKEYVALUE::value_type)0),
+ m_iValueCount(iValueCount)
{
- SetInitializationFlags(uiInitializationFlags);
+ SetInitializationFlags(uiInitializationFlags);
}
CTLSStorageInstance::~CTLSStorageInstance()
{
- Finit();
+ Finit();
}
bool CTLSStorageInstance::Init(ESTORAGEINSTANCEKIND ikInstanceKind)
{
- bool bResult = false;
-
- bool bKeyAllocationResult = false;
- HTLSKEYVALUE hkvStorageKey;
-
- do
- {
- if (!AllocateStorageKey(hkvStorageKey, ikInstanceKind))
- {
- break;
- }
-
- bKeyAllocationResult = true;
-
- CTLSStorageArray *psaFirstStorageArray = AllocateStorageArray();
-
- if (!psaFirstStorageArray)
- {
- break;
- }
-
- SetStorageKey(hkvStorageKey);
- SetStorageKeyValidFlag();
- AddStorageArrayToArrayList(psaFirstStorageArray);
-
- bResult = true;
- }
- while (false);
-
- if (!bResult)
- {
- if (bKeyAllocationResult)
- {
- FreeStorageKey(hkvStorageKey);
- }
- }
-
- return bResult;
+ bool bResult = false;
+
+ bool bKeyAllocationResult = false;
+ HTLSKEYVALUE hkvStorageKey;
+
+ do
+ {
+ if (!AllocateStorageKey(hkvStorageKey, ikInstanceKind))
+ {
+ break;
+ }
+
+ bKeyAllocationResult = true;
+
+ CTLSStorageArray *psaFirstStorageArray = AllocateStorageArray();
+
+ if (!psaFirstStorageArray)
+ {
+ break;
+ }
+
+ SetStorageKey(hkvStorageKey);
+ SetStorageKeyValidFlag();
+ AddStorageArrayToArrayList(psaFirstStorageArray);
+
+ bResult = true;
+ }
+ while (false);
+
+ if (!bResult)
+ {
+ if (bKeyAllocationResult)
+ {
+ FreeStorageKey(hkvStorageKey);
+ }
+ }
+
+ return bResult;
}
void CTLSStorageInstance::Finit()
{
- CTLSStorageArray *psaStorageArrayList = GetStorageArrayList();
+ CTLSStorageArray *psaStorageArrayList = GetStorageArrayList();
- if (psaStorageArrayList)
- {
- FreeStorageArrayList(psaStorageArrayList);
+ if (psaStorageArrayList)
+ {
+ FreeStorageArrayList(psaStorageArrayList);
- bool bListClearingResult = TrySettingStorageArrayList(NULL, psaStorageArrayList); // It could be assigned directly, but I just do not want to add an extra method
- OU_VERIFY(bListClearingResult);
- }
+ bool bListClearingResult = TrySettingStorageArrayList(NULL, psaStorageArrayList); // It could be assigned directly, but I just do not want to add an extra method
+ OU_VERIFY(bListClearingResult);
+ }
- if (GetStorageKeyValidFlag())
- {
- const HTLSKEYVALUE &hkvStorageKey = GetStorageKey();
- FreeStorageKey(hkvStorageKey);
+ if (GetStorageKeyValidFlag())
+ {
+ const HTLSKEYVALUE &hkvStorageKey = GetStorageKey();
+ FreeStorageKey(hkvStorageKey);
- ResetStorageKeyValidFlag();
- }
+ ResetStorageKeyValidFlag();
+ }
}
void CTLSStorageInstance::FreeStorageBlockOnThreadExit(CTLSStorageBlock *psbStorageBlock)
{
- FreeStorageBlock(psbStorageBlock);
+ FreeStorageBlock(psbStorageBlock);
}
bool CTLSStorageInstance::FindFreeStorageBlock(CTLSStorageBlock *&psbOutStorageBlock)
{
- bool bResult = false;
-
- do
- {
- if (!FindFreeStorageBlockInArrayList(psbOutStorageBlock))
- {
- CTLSStorageArray *psaStorageArray = AllocateStorageArray();
-
- if (!psaStorageArray)
- {
- break;
- }
-
- FindFreeStorageBlockFromArray(psbOutStorageBlock, psaStorageArray); // Must always succeed as array is not added to list yet
-
- AddStorageArrayToArrayList(psaStorageArray);
- }
-
- bResult = true;
- }
- while (false);
-
- return bResult;
+ bool bResult = false;
+
+ do
+ {
+ if (!FindFreeStorageBlockInArrayList(psbOutStorageBlock))
+ {
+ CTLSStorageArray *psaStorageArray = AllocateStorageArray();
+
+ if (!psaStorageArray)
+ {
+ break;
+ }
+
+ FindFreeStorageBlockFromArray(psbOutStorageBlock, psaStorageArray); // Must always succeed as array is not added to list yet
+
+ AddStorageArrayToArrayList(psaStorageArray);
+ }
+
+ bResult = true;
+ }
+ while (false);
+
+ return bResult;
}
bool CTLSStorageInstance::FindFreeStorageBlockInArrayList(CTLSStorageBlock *&psbOutStorageBlock)
{
- bool bResult;
-
- CTLSStorageArray *psaListOldHead = NULL;
- CTLSStorageArray *psaListCurrentHead = GetStorageArrayList();
-
- while (true)
- {
- if (FindFreeStorageBlockInArrayListSegment(psbOutStorageBlock, psaListCurrentHead, psaListOldHead))
- {
- bResult = true;
- break;
- }
-
- psaListOldHead = psaListCurrentHead;
- psaListCurrentHead = GetStorageArrayList();
-
- if (psaListOldHead == psaListCurrentHead)
- {
- bResult = false;
- break;
- }
- }
-
- return bResult;
+ bool bResult;
+
+ CTLSStorageArray *psaListOldHead = NULL;
+ CTLSStorageArray *psaListCurrentHead = GetStorageArrayList();
+
+ while (true)
+ {
+ if (FindFreeStorageBlockInArrayListSegment(psbOutStorageBlock, psaListCurrentHead, psaListOldHead))
+ {
+ bResult = true;
+ break;
+ }
+
+ psaListOldHead = psaListCurrentHead;
+ psaListCurrentHead = GetStorageArrayList();
+
+ if (psaListOldHead == psaListCurrentHead)
+ {
+ bResult = false;
+ break;
+ }
+ }
+
+ return bResult;
}
-bool CTLSStorageInstance::FindFreeStorageBlockInArrayListSegment(CTLSStorageBlock *&psbOutStorageBlock,
- CTLSStorageArray *psaListSegmentBegin, CTLSStorageArray *psaListSegmentEnd)
+bool CTLSStorageInstance::FindFreeStorageBlockInArrayListSegment(CTLSStorageBlock *&psbOutStorageBlock,
+ CTLSStorageArray *psaListSegmentBegin, CTLSStorageArray *psaListSegmentEnd)
{
- OU_ASSERT(psaListSegmentBegin != psaListSegmentEnd);
-
- bool bResult;
-
- CTLSStorageArray *psaListSegmentCurrent = psaListSegmentBegin;
-
- while (true)
- {
- if (FindFreeStorageBlockFromArray(psbOutStorageBlock, psaListSegmentCurrent))
- {
- bResult = true;
- break;
- }
-
- psaListSegmentCurrent = psaListSegmentCurrent->GetNextArray();
-
- if (psaListSegmentCurrent == psaListSegmentEnd)
- {
- bResult = false;
- break;
- }
- }
-
- return bResult;
+ OU_ASSERT(psaListSegmentBegin != psaListSegmentEnd);
+
+ bool bResult;
+
+ CTLSStorageArray *psaListSegmentCurrent = psaListSegmentBegin;
+
+ while (true)
+ {
+ if (FindFreeStorageBlockFromArray(psbOutStorageBlock, psaListSegmentCurrent))
+ {
+ bResult = true;
+ break;
+ }
+
+ psaListSegmentCurrent = psaListSegmentCurrent->GetNextArray();
+
+ if (psaListSegmentCurrent == psaListSegmentEnd)
+ {
+ bResult = false;
+ break;
+ }
+ }
+
+ return bResult;
}
-bool CTLSStorageInstance::FindFreeStorageBlockFromArray(CTLSStorageBlock *&psbOutStorageBlock,
- CTLSStorageArray *psaArrayInstance)
+bool CTLSStorageInstance::FindFreeStorageBlockFromArray(CTLSStorageBlock *&psbOutStorageBlock,
+ CTLSStorageArray *psaArrayInstance)
{
- tlsindextype iValueCount = GetValueCount();
- bool bIsManualCleanup = GetThreadManualCleanupFlag();
+ tlsindextype iValueCount = GetValueCount();
+ bool bIsManualCleanup = GetThreadManualCleanupFlag();
- return psaArrayInstance->FindFreeStorageBlock(psbOutStorageBlock, iValueCount, bIsManualCleanup);
+ return psaArrayInstance->FindFreeStorageBlock(psbOutStorageBlock, iValueCount, bIsManualCleanup);
}
void CTLSStorageInstance::AddStorageArrayToArrayList(CTLSStorageArray *psaStorageArray)
{
- while (true)
- {
- CTLSStorageArray *psaListCurrentHead = GetStorageArrayList();
- psaStorageArray->SetNextArray(psaListCurrentHead);
-
- if (TrySettingStorageArrayList(psaStorageArray, psaListCurrentHead))
- {
- break;
- }
- }
+ while (true)
+ {
+ CTLSStorageArray *psaListCurrentHead = GetStorageArrayList();
+ psaStorageArray->SetNextArray(psaListCurrentHead);
+
+ if (TrySettingStorageArrayList(psaStorageArray, psaListCurrentHead))
+ {
+ break;
+ }
+ }
}
bool CTLSStorageInstance::AllocateStorageKey(HTLSKEYVALUE &hkvOutStorageKey, ESTORAGEINSTANCEKIND ikInstanceKind)
{
- bool bResult = false;
+ bool bResult = false;
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-
- DWORD dwTlsIndex = ::TlsAlloc();
- if (dwTlsIndex != TLS_OUT_OF_INDEXES)
- {
- hkvOutStorageKey = (HTLSKEYVALUE)(HTLSKEYVALUE::value_type)(size_t)dwTlsIndex;
- bResult = true;
- }
-
+ DWORD dwTlsIndex = ::TlsAlloc();
+
+ if (dwTlsIndex != TLS_OUT_OF_INDEXES)
+ {
+ hkvOutStorageKey = (HTLSKEYVALUE)(HTLSKEYVALUE::value_type)(size_t)dwTlsIndex;
+ bResult = true;
+ }
+
#else // #if _OU_TARGET_OS != _OU_TARGET_OS_WINDOWS
-
- pthread_key_t pkThreadKey;
-
- int iKeyCreationResult = pthread_key_create(&pkThreadKey,
- (ikInstanceKind == SIK_AUTOCLEANUP) ? &CTLSStorageInstance::FreeStorageBlock_Callback_Automatic : &CTLSStorageInstance::FreeStorageBlock_Callback_Manual);
- if (iKeyCreationResult == EOK)
- {
- hkvOutStorageKey = (HTLSKEYVALUE)(HTLSKEYVALUE::value_type)(size_t)pkThreadKey;
- bResult = true;
- }
-
-
+
+ pthread_key_t pkThreadKey;
+
+ int iKeyCreationResult = pthread_key_create(&pkThreadKey,
+ (ikInstanceKind == SIK_AUTOCLEANUP) ? &CTLSStorageInstance::FreeStorageBlock_Callback_Automatic : &CTLSStorageInstance::FreeStorageBlock_Callback_Manual);
+ if (iKeyCreationResult == EOK)
+ {
+ hkvOutStorageKey = (HTLSKEYVALUE)(HTLSKEYVALUE::value_type)(size_t)pkThreadKey;
+ bResult = true;
+ }
+
+
#endif // #if _OU_TARGET_OS == ...
- return bResult;
+ return bResult;
}
void CTLSStorageInstance::FreeStorageKey(const HTLSKEYVALUE &hkvStorageKey)
{
#if _OU_TARGET_OS == _OU_TARGET_OS_WINDOWS
-
- DWORD dwTlsIndex = (DWORD)(size_t)(HTLSKEYVALUE::value_type)hkvStorageKey;
- OU_ASSERT(dwTlsIndex != TLS_OUT_OF_INDEXES);
-
- BOOL bIndexFreeingResult = ::TlsFree(dwTlsIndex);
- OU_VERIFY(bIndexFreeingResult);
-
-
+
+ DWORD dwTlsIndex = (DWORD)(size_t)(HTLSKEYVALUE::value_type)hkvStorageKey;
+ OU_ASSERT(dwTlsIndex != TLS_OUT_OF_INDEXES);
+
+ BOOL bIndexFreeingResult = ::TlsFree(dwTlsIndex);
+ OU_VERIFY(bIndexFreeingResult);
+
+
#else // #if _OU_TARGET_OS != _OU_TARGET_OS_WINDOWS
-
- pthread_key_t pkThreadKey = (pthread_key_t)(size_t)(HTLSKEYVALUE::value_type)hkvStorageKey;
-
- int iKeyDeletionResult = pthread_key_delete(pkThreadKey);
- OU_VERIFY(iKeyDeletionResult == EOK);
-
-
+
+ pthread_key_t pkThreadKey = (pthread_key_t)(size_t)(HTLSKEYVALUE::value_type)hkvStorageKey;
+
+ int iKeyDeletionResult = pthread_key_delete(pkThreadKey);
+ OU_VERIFY(iKeyDeletionResult == EOK);
+
+
#endif // #if _OU_TARGET_OS == ...
}
@@ -1083,22 +1093,22 @@ void CTLSStorageInstance::FreeStorageKey(const HTLSKEYVALUE &hkvStorageKey)
void CTLSStorageInstance::FreeStorageBlock_Callback_Automatic(void *pv_DataValue)
{
- if (pv_DataValue) // Just a precaution
- {
- CTLSStorageBlock *psbStorageBlock = (CTLSStorageBlock *)pv_DataValue;
+ if (pv_DataValue) // Just a precaution
+ {
+ CTLSStorageBlock *psbStorageBlock = (CTLSStorageBlock *)pv_DataValue;
- g_apsiStorageGlobalInstances[SIK_AUTOCLEANUP]->FreeStorageBlock(psbStorageBlock);
- }
+ g_apsiStorageGlobalInstances[SIK_AUTOCLEANUP]->FreeStorageBlock(psbStorageBlock);
+ }
}
void CTLSStorageInstance::FreeStorageBlock_Callback_Manual(void *pv_DataValue)
{
- if (pv_DataValue) // Just a precaution
- {
- CTLSStorageBlock *psbStorageBlock = (CTLSStorageBlock *)pv_DataValue;
+ if (pv_DataValue) // Just a precaution
+ {
+ CTLSStorageBlock *psbStorageBlock = (CTLSStorageBlock *)pv_DataValue;
- g_apsiStorageGlobalInstances[SIK_MANUALCLEANUP]->FreeStorageBlock(psbStorageBlock);
- }
+ g_apsiStorageGlobalInstances[SIK_MANUALCLEANUP]->FreeStorageBlock(psbStorageBlock);
+ }
}
@@ -1107,32 +1117,32 @@ void CTLSStorageInstance::FreeStorageBlock_Callback_Manual(void *pv_DataValue)
void CTLSStorageInstance::FreeStorageBlock(CTLSStorageBlock *psbStorageBlock)
{
- const int iValueCount = GetValueCount();
-
- CTLSStorageArray *psaArrayInstance = psbStorageBlock->GetHostArray();
- psaArrayInstance->FreeStorageBlockOnThreadExit(psbStorageBlock, iValueCount);
+ const int iValueCount = GetValueCount();
+
+ CTLSStorageArray *psaArrayInstance = psbStorageBlock->GetHostArray();
+ psaArrayInstance->FreeStorageBlockOnThreadExit(psbStorageBlock, iValueCount);
}
CTLSStorageArray *CTLSStorageInstance::AllocateStorageArray()
{
- const tlsindextype iValueCount = GetValueCount();
+ const tlsindextype iValueCount = GetValueCount();
- return CTLSStorageArray::AllocateInstance(iValueCount);
+ return CTLSStorageArray::AllocateInstance(iValueCount);
}
void CTLSStorageInstance::FreeStorageArrayList(CTLSStorageArray *psaStorageArrayList)
{
- const tlsindextype iValueCount = GetValueCount();
-
- while (psaStorageArrayList)
- {
- CTLSStorageArray *psaStorageNextArray = psaStorageArrayList->GetNextArray();
-
- psaStorageArrayList->FreeInstance(iValueCount);
-
- psaStorageArrayList = psaStorageNextArray;
- }
+ const tlsindextype iValueCount = GetValueCount();
+
+ while (psaStorageArrayList)
+ {
+ CTLSStorageArray *psaStorageNextArray = psaStorageArrayList->GetNextArray();
+
+ psaStorageArrayList->FreeInstance(iValueCount);
+
+ psaStorageArrayList = psaStorageNextArray;
+ }
}
@@ -1140,34 +1150,34 @@ void CTLSStorageInstance::FreeStorageArrayList(CTLSStorageArray *psaStorageArray
// CThreadLocalStorage methods
bool CThreadLocalStorage::AllocateAndSetStorageValue(const HTLSKEYSELECTOR &hksKeySelector,
- tlsindextype iValueIndex, tlsvaluetype vValueData, CTLSValueDestructor fnValueDestructor)
+ tlsindextype iValueIndex, tlsvaluetype vValueData, CTLSValueDestructor fnValueDestructor)
{
- OU_ASSERT(OU_IN_SIZET_RANGE(DecodeInstanceKindFromKeySelector(hksKeySelector), SIK__MIN, SIK__MAX));
-
- bool bResult = false;
-
- do
- {
- ESTORAGEINSTANCEKIND ikInstanceKind = (ESTORAGEINSTANCEKIND)DecodeInstanceKindFromKeySelector(hksKeySelector);
- CTLSStorageInstance *psiStorageInstance = g_apsiStorageGlobalInstances[ikInstanceKind];
-
- CTLSStorageBlock *psbStorageBlock;
-
- if (!psiStorageInstance->FindFreeStorageBlock(psbStorageBlock))
- {
- break;
- }
-
- SetKeyStorageBlock(hksKeySelector, psbStorageBlock);
-
- psbStorageBlock->SetValueData(iValueIndex, vValueData);
- psbStorageBlock->SetValueDestructor(iValueIndex, fnValueDestructor);
-
- bResult = true;
- }
- while (false);
-
- return bResult;
+ OU_ASSERT(OU_IN_SIZET_RANGE(DecodeInstanceKindFromKeySelector(hksKeySelector), SIK__MIN, SIK__MAX));
+
+ bool bResult = false;
+
+ do
+ {
+ ESTORAGEINSTANCEKIND ikInstanceKind = (ESTORAGEINSTANCEKIND)DecodeInstanceKindFromKeySelector(hksKeySelector);
+ CTLSStorageInstance *psiStorageInstance = g_apsiStorageGlobalInstances[ikInstanceKind];
+
+ CTLSStorageBlock *psbStorageBlock;
+
+ if (!psiStorageInstance->FindFreeStorageBlock(psbStorageBlock))
+ {
+ break;
+ }
+
+ SetKeyStorageBlock(hksKeySelector, psbStorageBlock);
+
+ psbStorageBlock->SetValueData(iValueIndex, vValueData);
+ psbStorageBlock->SetValueDestructor(iValueIndex, fnValueDestructor);
+
+ bResult = true;
+ }
+ while (false);
+
+ return bResult;
}
@@ -1175,152 +1185,152 @@ bool CThreadLocalStorage::AllocateAndSetStorageValue(const HTLSKEYSELECTOR &hksK
// CTLSInitialization methods
bool CTLSInitialization::InitializeTLSAPI(HTLSKEY &hskOutStorageKey, tlsindextype iValueCount,
- unsigned int uiInitializationFlags/*=0*/)
+ unsigned int uiInitializationFlags/*=0*/)
{
- OU_ASSERT(g_uiThreadLocalStorageInitializationCount != 0U - 1U);
-
- bool bResult = false;
-
- bool bAtomicAPIInitialized = false;
-
- do
- {
- const ESTORAGEINSTANCEKIND ikInstanceKind = (uiInitializationFlags & SIF_MANUAL_CLEANUP_ON_THREAD_EXIT) ? SIK_MANUALCLEANUP : SIK_AUTOCLEANUP;
-
- if (g_apsiStorageGlobalInstances[ikInstanceKind] == NULL) // Initialization/finalization must be called from main thread
- {
- if (!InitializeAtomicAPI())
- {
- break;
- }
-
- bAtomicAPIInitialized = true;
-
- if (!InitializeTLSAPIValidated(ikInstanceKind, iValueCount, uiInitializationFlags))
- {
- break;
- }
-
- const HTLSKEYVALUE &hkvStorageKey = g_apsiStorageGlobalInstances[ikInstanceKind]->RetrieveStorageKey();
- g_ahkvStorageGlobalKeyValues[ikInstanceKind] = hkvStorageKey;
- }
-
- ++g_uiThreadLocalStorageInitializationCount;
-
- hskOutStorageKey = EncodeKeySelectorFromStorageKind(ikInstanceKind);
- OU_ASSERT(iValueCount == g_apsiStorageGlobalInstances[ikInstanceKind]->RetrieveValueCount());
- OU_ASSERT(uiInitializationFlags == g_apsiStorageGlobalInstances[ikInstanceKind]->RetrieveInitializationFlags());
-
- bResult = true;
- }
- while (false);
-
- if (!bResult)
- {
- if (bAtomicAPIInitialized)
- {
- FinalizeAtomicAPI();
- }
- }
-
- return bResult;
+ OU_ASSERT(g_uiThreadLocalStorageInitializationCount != 0U - 1U);
+
+ bool bResult = false;
+
+ bool bAtomicAPIInitialized = false;
+
+ do
+ {
+ const ESTORAGEINSTANCEKIND ikInstanceKind = (uiInitializationFlags & SIF_MANUAL_CLEANUP_ON_THREAD_EXIT) ? SIK_MANUALCLEANUP : SIK_AUTOCLEANUP;
+
+ if (g_apsiStorageGlobalInstances[ikInstanceKind] == NULL) // Initialization/finalization must be called from main thread
+ {
+ if (!InitializeAtomicAPI())
+ {
+ break;
+ }
+
+ bAtomicAPIInitialized = true;
+
+ if (!InitializeTLSAPIValidated(ikInstanceKind, iValueCount, uiInitializationFlags))
+ {
+ break;
+ }
+
+ const HTLSKEYVALUE &hkvStorageKey = g_apsiStorageGlobalInstances[ikInstanceKind]->RetrieveStorageKey();
+ g_ahkvStorageGlobalKeyValues[ikInstanceKind] = hkvStorageKey;
+ }
+
+ ++g_uiThreadLocalStorageInitializationCount;
+
+ hskOutStorageKey = EncodeKeySelectorFromStorageKind(ikInstanceKind);
+ OU_ASSERT(iValueCount == g_apsiStorageGlobalInstances[ikInstanceKind]->RetrieveValueCount());
+ OU_ASSERT(uiInitializationFlags == g_apsiStorageGlobalInstances[ikInstanceKind]->RetrieveInitializationFlags());
+
+ bResult = true;
+ }
+ while (false);
+
+ if (!bResult)
+ {
+ if (bAtomicAPIInitialized)
+ {
+ FinalizeAtomicAPI();
+ }
+ }
+
+ return bResult;
}
void CTLSInitialization::FinalizeTLSAPI()
{
- OU_ASSERT(g_uiThreadLocalStorageInitializationCount != 0U);
+ OU_ASSERT(g_uiThreadLocalStorageInitializationCount != 0U);
- ESTORAGEINSTANCEKIND ikInstanceKind =
- (--g_uiThreadLocalStorageInitializationCount == 0U) ? SIK__MIN : SIK__MAX; // Initialization/finalization must be called from main thread
- for (; ikInstanceKind != SIK__MAX; ++ikInstanceKind)
- {
- if (g_apsiStorageGlobalInstances[ikInstanceKind])
- {
- g_ahkvStorageGlobalKeyValues[ikInstanceKind] = 0;
+ ESTORAGEINSTANCEKIND ikInstanceKind =
+ (--g_uiThreadLocalStorageInitializationCount == 0U) ? SIK__MIN : SIK__MAX; // Initialization/finalization must be called from main thread
+ for (; ikInstanceKind != SIK__MAX; ++ikInstanceKind)
+ {
+ if (g_apsiStorageGlobalInstances[ikInstanceKind])
+ {
+ g_ahkvStorageGlobalKeyValues[ikInstanceKind] = 0;
- FinalizeTLSAPIValidated(ikInstanceKind);
+ FinalizeTLSAPIValidated(ikInstanceKind);
- FinalizeAtomicAPI();
- }
- }
+ FinalizeAtomicAPI();
+ }
+ }
}
void CTLSInitialization::CleanupOnThreadExit()
{
- const ESTORAGEINSTANCEKIND ikInstanceKind = SIK_MANUALCLEANUP;
- CTLSStorageInstance *psiStorageInstance = g_apsiStorageGlobalInstances[ikInstanceKind];
-
- if (psiStorageInstance != NULL)
- {
- OU_ASSERT(psiStorageInstance->GetIsThreadManualCleanup());
-
- const HTLSKEYSELECTOR &hksKeySelector = EncodeKeySelectorFromStorageKind(ikInstanceKind);
- CTLSStorageBlock *psbStorageBlock = CThreadLocalStorage::gzGetKeyStorageBlock(hksKeySelector);
-
- if (psbStorageBlock)
- {
- psiStorageInstance->FreeStorageBlockOnThreadExit(psbStorageBlock);
-
- CThreadLocalStorage::SetKeyStorageBlock(hksKeySelector, NULL);
- }
- }
- else
- {
- OU_ASSERT(false); // The method is not supposed to be called if manual cleanup was not requested on initialization
- }
+ const ESTORAGEINSTANCEKIND ikInstanceKind = SIK_MANUALCLEANUP;
+ CTLSStorageInstance *psiStorageInstance = g_apsiStorageGlobalInstances[ikInstanceKind];
+
+ if (psiStorageInstance != NULL)
+ {
+ OU_ASSERT(psiStorageInstance->GetIsThreadManualCleanup());
+
+ const HTLSKEYSELECTOR &hksKeySelector = EncodeKeySelectorFromStorageKind(ikInstanceKind);
+ CTLSStorageBlock *psbStorageBlock = CThreadLocalStorage::gzGetKeyStorageBlock(hksKeySelector);
+
+ if (psbStorageBlock)
+ {
+ psiStorageInstance->FreeStorageBlockOnThreadExit(psbStorageBlock);
+
+ CThreadLocalStorage::SetKeyStorageBlock(hksKeySelector, NULL);
+ }
+ }
+ else
+ {
+ OU_ASSERT(false); // The method is not supposed to be called if manual cleanup was not requested on initialization
+ }
}
-bool CTLSInitialization::InitializeTLSAPIValidated(unsigned int uiInstanceKind,
- tlsindextype iValueCount, unsigned int uiInitializationFlags)
+bool CTLSInitialization::InitializeTLSAPIValidated(unsigned int uiInstanceKind,
+ tlsindextype iValueCount, unsigned int uiInitializationFlags)
{
- OU_ASSERT(g_apsiStorageGlobalInstances[uiInstanceKind] == NULL);
-
- bool bResult = false;
-
- CTLSStorageInstance *psiStorageInstance;
-
- do
- {
- // Use static methods instead of constructor/destructor
- // to avoid overloading operators new/delete and for
- // uniformity with CTLSStorageArray class
- psiStorageInstance = CTLSStorageInstance::AllocateInstance(iValueCount, uiInitializationFlags);
-
- if (!psiStorageInstance)
- {
- break;
- }
-
- if (!psiStorageInstance->Init((ESTORAGEINSTANCEKIND)uiInstanceKind))
- {
- break;
- }
-
- g_apsiStorageGlobalInstances[uiInstanceKind] = psiStorageInstance;
-
- bResult = true;
- }
- while (false);
-
- if (!bResult)
- {
- if (psiStorageInstance)
- {
- psiStorageInstance->FreeInstance();
- }
- }
-
- return bResult;
+ OU_ASSERT(g_apsiStorageGlobalInstances[uiInstanceKind] == NULL);
+
+ bool bResult = false;
+
+ CTLSStorageInstance *psiStorageInstance;
+
+ do
+ {
+ // Use static methods instead of constructor/destructor
+ // to avoid overloading operators new/delete and for
+ // uniformity with CTLSStorageArray class
+ psiStorageInstance = CTLSStorageInstance::AllocateInstance(iValueCount, uiInitializationFlags);
+
+ if (!psiStorageInstance)
+ {
+ break;
+ }
+
+ if (!psiStorageInstance->Init((ESTORAGEINSTANCEKIND)uiInstanceKind))
+ {
+ break;
+ }
+
+ g_apsiStorageGlobalInstances[uiInstanceKind] = psiStorageInstance;
+
+ bResult = true;
+ }
+ while (false);
+
+ if (!bResult)
+ {
+ if (psiStorageInstance)
+ {
+ psiStorageInstance->FreeInstance();
+ }
+ }
+
+ return bResult;
}
void CTLSInitialization::FinalizeTLSAPIValidated(unsigned int uiInstanceKind)
{
- OU_ASSERT(g_apsiStorageGlobalInstances[uiInstanceKind] != NULL);
+ OU_ASSERT(g_apsiStorageGlobalInstances[uiInstanceKind] != NULL);
- g_apsiStorageGlobalInstances[uiInstanceKind]->FreeInstance();
- g_apsiStorageGlobalInstances[uiInstanceKind] = NULL;
+ g_apsiStorageGlobalInstances[uiInstanceKind]->FreeInstance();
+ g_apsiStorageGlobalInstances[uiInstanceKind] = NULL;
}
diff --git a/deps/opende/src/collision_trimesh_trimesh_new.cpp b/deps/opende/src/collision_trimesh_trimesh_new.cpp
index 432b8d4..ce810f0 100644
--- a/deps/opende/src/collision_trimesh_trimesh_new.cpp
+++ b/deps/opende/src/collision_trimesh_trimesh_new.cpp
@@ -27,6 +27,12 @@
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
#endif
+// Prevents warnings about the following unused functions:
+// dMin
+#ifdef __clang__
+#pragma clang diagnostic ignored "-Wunused-function"
+#endif
+
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
diff --git a/deps/opende/src/heightfield.cpp b/deps/opende/src/heightfield.cpp
index 6caf601..5a192d2 100644
--- a/deps/opende/src/heightfield.cpp
+++ b/deps/opende/src/heightfield.cpp
@@ -5,6 +5,12 @@
// Benoit CHAPEROT 2003-2004 http://www.jstarlab.com
// Some code inspired by Magic Software
+// Prevents warnings about the following unused functions:
+// DescendingTriangleSort
+// DistancePointToLine
+#ifdef __clang__
+#pragma clang diagnostic ignored "-Wunused-function"
+#endif
#include <ode/common.h>
#include <ode/collision.h>
diff --git a/deps/opende/src/io.cpp b/deps/opende/src/io.cpp
new file mode 100644
index 0000000..d112960
--- /dev/null
+++ b/deps/opende/src/io.cpp
@@ -0,0 +1,457 @@
+/*
+Copyright (c) 2013 Claude Lacoursiere
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any damages
+arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you must not
+ claim that you wrote the original software. If you use this software
+ in a product, an acknowledgment in the product documentation would be
+ appreciated but is not required.
+
+ 2. Altered source versions must be plainly marked as such, and must not be
+ misrepresented as being the original software.
+
+ 3. This notice may not be removed or altered from any source
+ distribution.
+
+reference:
+ hdf5-based BPMD database: https://grasp.robotics.cs.rpi.edu/bpmd/
+*/
+
+
+#include "gazebo/gazebo_config.h"
+#ifdef HDF5_INSTRUMENT
+#include <iostream>
+#include <string>
+#include <string.h>
+#include <stdlib.h>
+#include "io.h"
+#include "ioh5.h"
+#include "objects.h"
+#include "joints/joint.h"
+#include "joints/contact.h"
+#include <ode/h5dump.h>
+
+using namespace H5;
+using namespace h5;
+
+// This does not use STL because the need for iterators is almost nil.
+
+// Utilities
+// Put identical values in all elements of a vector
+template <typename R, typename V>
+void set_vector(R *x, const V& v, size_t n, size_t stride = 1)
+{
+ for (size_t i=0; i<n; ++i){
+ x[ i * stride ] = v;
+ }
+}
+
+/// this wrapper saves space and typing
+void copy(dReal *dest, const dReal * source, size_t n, size_t offset)
+{
+ for (size_t i = 0; i<n; ++i) {
+ dest[i + n*offset] = source[i];
+ }
+}
+
+/// this wrapper saves space and typing
+void copy2(dReal *dest, const dReal * s1, const dReal * s2, size_t block, size_t offset)
+{
+ for (size_t i = 0; i<block; ++i) {
+ dest[i + 2 * block * offset] = s1[i];
+ dest[i + 3 * block * offset] = s2[i];
+ }
+}
+
+// ODE specific utilities.
+// ODE's matrix format for 3x3 matrices has leading
+// dimension 4 and we need this to be 3 instead.
+dReal* copy_dMatrix3_33(dReal * dest, dReal * source)
+{
+ memcpy(dest , source, 3*sizeof(dReal));
+ memcpy(dest+3, source+4, 3*sizeof(dReal));
+ memcpy(dest+6, source+8, 3*sizeof(dReal));
+ return dest;
+}
+
+// Must compute world inertia here since that is not cached in the body
+// data structure, but computed as needed in the solvers.
+
+dReal * get_world_inertia(dReal * dest, dxBody * b, size_t offset)
+{
+ dMatrix3 M0;
+ dMatrix3 tmp;
+ dMultiply2_333 (tmp, b->mass.I, b->posr.R);
+ dMultiply0_333 (M0, b->posr.R, tmp);
+ // Now, dMatrix3 is a 12D array so we need to extract the first 3
+ // elements of each row
+ return copy_dMatrix3_33(dest + offset*9, (dReal *)M0);
+}
+
+/// maybe unnecessarily verbose since this
+/// is an internal function.
+void h5dump::allocate_bodies(){
+
+ int n = nbodies;
+
+ if ( nbodies > 0 ) {
+ x = new dReal[3*n]; // positions
+ set_vector(x, 0, 3*n);
+ q = new dReal[4*n]; // quaternions
+ set_vector(q, 0,4*n); // quaternions
+
+ v = new dReal[6*n]; // velocites, linear *and* angular
+ set_vector(v, 0, 6*n); // velocites, linear *and* angular
+
+ f = new dReal[6*n]; // forces, translational and rotational
+ set_vector(f, 0, 6*n);
+
+ m = new dReal[n]; // mass
+ set_vector(m, 0, n);
+
+ M = new dReal[9*n]; // inertia tensor in world frame
+ /// this will be repacked as a 3 x (3n) matrix during dump
+ set_vector(M, 0, 9*n);
+
+ id = new int[n]; // tags for rigid bodies to match with
+ // matrices
+ state = new int[n]; // 1 if body is active, -1 otherwise.
+
+ body_data = new void*[n];
+ memset(body_data, 0, n*sizeof(void*));
+ }
+}
+
+void h5dump::free_bodies(){
+
+ if ( nbodies > 0 ) {
+ if (x) delete [] x;
+ if (q) delete [] q;
+ if (v) delete [] v;
+ if (f) delete [] f;
+ if (m) delete [] m;
+ if (M) delete [] M;
+ if (body_data) delete [] body_data;
+ nbodies = 0;
+ }
+}
+
+void h5dump::allocate_constraints()
+{
+ // now allocate and initialize
+
+ if ( nrows > 0 ) {
+ rows = new size_t[nrows];
+ memset(rows, 0, nrows*sizeof(size_t));
+
+ g = new dReal[nrows];
+ set_vector(g, 0, nrows);
+
+ gt = new dReal[nrows];
+ set_vector(gt, 0, nrows);
+
+ bounds = new dReal[2*nrows];
+ set_vector(bounds , -INFINITY, nrows, 2);
+ set_vector(bounds+1, INFINITY, nrows, 2);
+
+ constraint_pairs = new int[2*nrows];
+ set_vector(constraint_pairs, -1, 2*nrows);
+
+ G = new dReal[nrows*ldg];
+ set_vector(G, 0, nrows*ldg);
+ }
+}
+
+void h5dump::free_constraints()
+{
+ if ( nrows > 0 ) {
+ if ( g ) delete [] g;
+ if ( gt ) delete [] gt;
+ if ( bounds ) delete [] bounds;
+ if (constraint_pairs ) delete [] constraint_pairs;
+ if ( G ) delete [] G;
+ nrows = 0 ;
+ }
+}
+
+void h5dump::allocate_contacts(){
+ size_t n = ncontacts;
+ if ( n > 0 ) {
+ points = new dReal[n*3];
+ set_vector(points, 0, n*3);
+
+ normals = new dReal[n*3];
+ set_vector(normals, 0, n*3);
+
+ // change it to gaps
+ gaps = new dReal[n];
+ set_vector(gaps, 0, n);
+
+ mu_s = new dReal[2*n];
+ set_vector(mu_s, 0, 2*n);
+
+ mu_k = new dReal[2*n];
+ set_vector(mu_k, 0, 2*n);
+
+ contact_pairs = new int[n*2];
+ set_vector(contact_pairs, 0, n*2);
+ }
+}
+
+void h5dump::free_contacts()
+{
+ if ( ncontacts > 0) {
+ if ( points ) delete [] points;
+ if ( normals ) delete [] normals;
+ if ( gaps ) delete [] gaps;
+ if ( mu_s ) delete [] mu_s;
+ if ( mu_k ) delete [] mu_k;
+ if ( contact_pairs ) delete [] contact_pairs;
+ ncontacts = 0 ;
+ }
+}
+
+/// concatenate all the rigid body data into a packed array
+void h5dump::pack_bodies()
+{
+ int n = nbodies = world->nb;
+ if ( n > 0 ) {
+ allocate_bodies();
+ int i = 0;
+ for (dxBody* b = world->firstbody; b != 0 ; b = (dxBody *)b->next) {
+ // positions stored as just CM coordinates
+ copy(x, dBodyGetPosition(b), 3, i );
+
+ // Rotations as quaternions
+ copy(q, dBodyGetQuaternion(b), 4, i );
+
+ // linear and angular velocities are concatenated
+ memcpy(v+6*i, dBodyGetLinearVel(b), 3 * sizeof(dReal) );
+ memcpy(v+6*i+3, dBodyGetAngularVel(b), 3 * sizeof(dReal) );
+
+ // forces and torques are concatenated
+ //copy2(f, b->facc, b->tacc, 3, i);
+ memcpy(f+6*i, b->facc, 3 * sizeof(dReal) );
+ memcpy(f+6*i+3, b->tacc, 3 * sizeof(dReal) );
+
+ // the scalar mass
+ m[i] = b->mass.mass;
+
+ // compute and store the inertia tensor
+ get_world_inertia(M, b, i);
+
+ state[i] = ( ( b->flags & dxBodyDisabled ) == 0 ) ? 1 : -1 ;
+ id[i] = tags[(void*)b] = i+1;
+ ++i;
+ }
+ }
+ tags[NULL] = -1;
+}
+
+void h5dump::pack_constraints()
+{
+ nconstraints = 0;
+ // first cound how many active rows we have for regular constraints,
+ // i.e., not contacts
+ nrows =0;
+ for (dxJoint* c = world->firstjoint; c != 0 ; c = (dxJoint *)c->next) {
+ if ( c->type() != dJointTypeContact) {
+ ++nconstraints;
+ dxJoint::Info1 info1;
+ c->getInfo1(&info1);
+ nrows += (size_t)info1.m;
+ }
+ }
+
+ if ( nconstraints > 0 ) {
+ allocate_constraints();
+ // tmp vector for ODE
+ int * junk = new int[nrows];
+
+ // Jacobian matrix, i.e., it is a row major nrows x 12 array
+ int i = 0;
+ int row = 0;
+ for (dxJoint* c = world->firstjoint; c != 0 ; c = (dxJoint*)c->next) {
+ if ( dJointIsEnabled(c) && c->type() != dJointTypeContact) {
+
+ // find out which bodies we have
+ // for reasons unknown to me, this labeling is inverted in ODE
+ for (int j = 1; j > -1 ; --j )
+ constraint_pairs[i*2 -j + 1 ] = tags[c->node[j].body];
+
+ // Get number of rows
+ dxJoint::Info1 info1;
+ c->getInfo1(&info1);
+ // store the number of rows
+ rows[i] = (size_t )info1.m;
+
+ dxJoint::Info2 info;
+ info.rowskip = ldg; // The leading dimension of G : 12;
+ info.fps = 60; // arbitrary value: we don't store that
+ info.erp = 1; // arbitrary value: we don't store that
+
+ dReal * J0 = G + ldg * row;
+ dReal cfm[3];
+ info.J1l = J0;
+ info.J1a = J0 + 3;
+ info.J2l = J0 + 6;
+ info.J2a = J0 + 9;
+ info.c = g + row;
+ info.cfm = (dReal *)cfm;
+ info.lo = bounds + 2*row;
+ info.hi = bounds + 2*row + 1;
+ info.findex = junk + row;
+
+ c->getInfo2(&info);
+ row += rows[i];
+ ++i;
+ }
+ }
+ delete [] junk;
+ }
+}
+
+void h5dump::pack_contacts()
+{
+ ncontacts =0;
+ for (dxJoint* c = world->firstjoint; c != 0 ; c = (dxJoint *)c->next) {
+ ncontacts += ( c->type() == dJointTypeContact && dJointIsEnabled(c)) ;
+ }
+ if ( ncontacts ) {
+ allocate_contacts();
+
+ // extract data
+ int k = 0; // running index
+ for (dxJoint* c = world->firstjoint; c != 0 ; c = (dxJoint *)c->next) {
+ if ( c->type() == dJointTypeContact && dJointIsEnabled(c)) {
+ dContactGeom geo = ((dxJointContact *)c)->contact.geom;
+ dSurfaceParameters surf = ((dxJointContact *)c)->contact.surface;
+
+ // gaps is the -1*depth
+ gaps[k] = -1.0*geo.depth;
+ copy(normals, geo.normal, 3, k);
+ copy(points, geo.pos, 3, k);
+ mu_s[2 * k ] = surf.mu ;
+ mu_s[2 * k + 1] = surf.mu ;
+ if ( surf.mode&dContactMu2 ) {
+ mu_k[2*k] = surf.mu2;
+ mu_k[2*k + 1] = surf.mu2;
+ }
+ else {
+ mu_k[2 * k] = surf.mu;
+ mu_k[2 * k + 1] = surf.mu;
+ }
+
+ // find out which bodies we have
+ for (int i = 1; i > -1 ; --i )
+ contact_pairs[k*2 -i + 1 ] = tags[c->node[i].body];
+
+ ++k;
+ }
+ }
+ }
+}
+
+void h5dump::dump(bool first)
+{
+ pack_bodies();
+ pack_constraints();
+ pack_contacts();
+
+ H5File * F = append_or_create( file );
+
+ Group gp = append_problem(F, "mbs_frame");
+ if (first )
+ //dump_string(gp, "toolkit", "This example was generated by the ODE toolkit");
+
+ dump_scalar(gp, "timestep", step_size);
+
+ // bodies
+ if ( nbodies > 0 ) {
+ Group gb = gp.createGroup("bodies");
+ // store positions and quaternions as 2D arrays with 3 and 4 columns,
+ // respectively which removes possibility of confusion with the format
+ // of the generalized forces and velocities
+ dump_array(gb, "positions", x, nbodies, 3);
+ dump_array(gb, "quaternions", q, nbodies, 4);
+ // these are plain vectors which makes the manipulation of the whole
+ // system easier.
+ dump_array(gb, "velocities", v, 6*nbodies);
+ dump_array(gb, "forces", f, 6*nbodies);
+ dump_array(gb, "masses", m, nbodies);
+ // world_inertia_tensors
+ dump_array(gb, "world_inertia_tensors", M, 3*nbodies, 3);
+ dump_array(gb, "ids", id, nbodies);
+ // states is kept here to make it more flexible to recover the simulation
+ dump_array(gb, "states", state, nbodies);
+ gb.close();
+ free_bodies();
+ }
+
+ if ( nconstraints > 0 ) {
+ Group gc = gp.createGroup("constraints");
+ dump_array(gc, "rows", rows, nconstraints);
+ dump_array(gc, "violations", g, nrows);
+ dump_array(gc, "time_derivative", gt, nrows);
+ dump_array(gc, "bounds", bounds, nrows, 2);
+ dump_array(gc, "pairs", constraint_pairs, nconstraints, 2);
+ dump_array(gc, "Jacobian", G, nrows, ldg);
+ gc.close();
+ free_constraints();
+ }
+
+ if ( ncontacts > 0 ) {
+ Group gct = gp.createGroup("contacts");
+ dump_array(gct, "points", points, ncontacts, 3);
+ dump_array(gct, "normals", normals, ncontacts, 3);
+ dump_array(gct, "gaps", gaps, ncontacts);
+ dump_array(gct, "mu_s", mu_s, ncontacts, 2);
+ dump_array(gct, "mu_k", mu_k, ncontacts, 2);
+ dump_array(gct, "pairs", contact_pairs, ncontacts, 2);
+ gct.close();
+ free_contacts();
+ }
+
+ gp.close();
+ F->close();
+ delete F;
+}
+
+extern "C" {
+ void h5dump_world(const char * file, dxWorld * w, dReal stepsize)
+ {
+ static size_t frame = 0;
+
+ if ( true || getenv("DUMP_HDF5_ODE")) {
+ ++frame;
+ size_t freq = floor(1/stepsize);
+ // dump 5 times a second
+ h5dump h(file, w, stepsize, frame%(freq/5) );
+ }
+ }
+
+ void h5_write_errors(const char *file , dReal * errors, unsigned int m){
+ H5File * F = append_or_create( H5std_string(file) );
+ H5::Group root = F->openGroup("/");
+ H5::Group g = root.openGroup(root.getObjnameByIdx(root.getNumObjs()-1));
+ if ( ! g.getNumObjs() || g.getObjnameByIdx(0) != H5std_string("solver_data") ) {
+ g = g.createGroup("solver_data");
+ if ( ! g.getNumObjs() || g.getObjnameByIdx(0) != H5std_string("ODE_GS") ) {
+ g = g. createGroup("ODE_GS");
+ h5::dump_array( g, std::string("gs_errors"), errors, m );
+ }
+ }
+ F->close();
+ delete F;
+ return;
+ }
+}
+
+#endif // HDF5_INSTRUMENT
diff --git a/deps/opende/src/io.h b/deps/opende/src/io.h
new file mode 100644
index 0000000..cb4fcd5
--- /dev/null
+++ b/deps/opende/src/io.h
@@ -0,0 +1,104 @@
+/*
+ Copyright (c) 2013 Claude Lacoursiere
+
+ This software is provided 'as-is', without any express or implied
+ warranty. In no event will the authors be held liable for any damages
+ arising from the use of this software.
+
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it
+ freely, subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you must not
+ claim that you wrote the original software. If you use this software
+ in a product, an acknowledgment in the product documentation would be
+ appreciated but is not required.
+
+ 2. Altered source versions must be plainly marked as such, and must not be
+ misrepresented as being the original software.
+
+ 3. This notice may not be removed or altered from any source
+ distribution.
+
+reference:
+ hdf5-based BPMD database: https://grasp.robotics.cs.rpi.edu/bpmd/
+*/
+
+#include "gazebo/gazebo_config.h"
+#ifdef HDF5_INSTRUMENT
+
+
+#ifndef H5_IO
+#define H5_IO
+#include <string>
+#include <ode/ode.h>
+#include <unordered_map>
+class h5dump {
+ private:
+ std::string file; // data will be accumulated in this file
+ dxWorld * world;
+ dReal step_size;
+ size_t nbodies; // number of bodies in current frame
+ void **body_data; // place holder to save and restore userdata in bodies
+ int * body_labels; // body indices starting from 1 for the dump
+ dReal * x; // positions
+ dReal * q; // quaternions
+ dReal * v; // linear and angular velocities
+ dReal * f; // forces and torques
+ dReal * m; // mass
+ dReal *M; // inertia tensor
+ int *id; // body labels to match constraints
+ int *state; // body labels to match constraints
+ std::unordered_map< void*, int> tags; // tags are not guaranteed to be constant in
+ // so we need to user pointers to do that.
+
+ // data for constraints
+ size_t nconstraints; // number of constraints in current frame
+ size_t nrows; // total number of rows
+ size_t *rows; // number of rows in one constraint
+ dReal * g; // constraint violation
+ dReal * gt; // velocity for rheonomic constraints
+ dReal * bounds; // lower bound for bounded constraints
+ int * constraint_pairs; // pairs of bodies bound by constraints
+ dReal *G; // The Jacobian packed in a matrix with nrows and 12 columns. This is row major
+ bool *holonomic; // Whether a constraint is holonomic or not.
+ const size_t ldg; // the leading dimension for G
+
+ // data for contacts
+ size_t ncontacts; // number of contacts in current frame
+ dReal * points; // contact points
+ dReal * normals; // contact normals
+ dReal * gaps; // gap distance, -1*penetration depth
+ dReal * mu_s; // static friction coefficient
+ dReal * mu_k; // kinetic friction coefficient
+ int * contact_pairs; // pairs of bodies bound by contacts
+
+ // The internal API
+ // Body manipulation and extraction
+ void allocate_bodies();
+ void free_bodies();
+ void pack_bodies();
+ void dump_bodies();
+
+ // Constraint manipulation and extraction
+ void allocate_constraints();
+ void free_constraints();
+ void pack_constraints();
+ void dump_constraints();
+
+ // Contact manipulation and extraction
+ void allocate_contacts();
+ void free_contacts();
+ void pack_contacts();
+ void dump_contacts();
+ void dump( bool first );
+
+ public:
+ h5dump(const std::string & filename, dxWorld *w, dReal stepsize, bool first, size_t l = 12) :
+ file(filename), world(w), step_size(stepsize), ldg(l) {
+ dump( first );
+ }
+};
+
+#endif // H5_IO
+#endif // HDF5_INSTRUMENT
diff --git a/deps/opende/src/ioh5.cpp b/deps/opende/src/ioh5.cpp
new file mode 100644
index 0000000..e69776e
--- /dev/null
+++ b/deps/opende/src/ioh5.cpp
@@ -0,0 +1,433 @@
+/*
+ Copyright (c) 2013 Claude Lacoursiere
+
+ This software is provided 'as-is', without any express or implied
+ warranty. In no event will the authors be held liable for any damages
+ arising from the use of this software.
+
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it
+ freely, subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you must not
+ claim that you wrote the original software. If you use this software
+ in a product, an acknowledgment in the product documentation would be
+ appreciated but is not required.
+
+ 2. Altered source versions must be plainly marked as such, and must not be
+ misrepresented as being the original software.
+
+ 3. This notice may not be removed or altered from any source
+ distribution.
+
+reference:
+ hdf5-based BPMD database: https://grasp.robotics.cs.rpi.edu/bpmd/
+*/
+#include "gazebo/gazebo_config.h"
+#ifdef HDF5_INSTRUMENT
+#include "ioh5.h"
+#include <iostream>
+#include <sstream> // for string manipulations.
+#include <iomanip>
+#include <string>
+#include <typeinfo>
+#include <stdint.h>
+
+
+using namespace H5;
+namespace h5 {
+ /// Verify that a given attribute is in the given group
+ bool check_attr(const H5::H5Object &g, const std::string & name){
+ bool ret = false;
+ try {
+ Exception::dontPrint();
+ Attribute a = g.openAttribute(name);
+ a.close();
+ ret = true;
+ }
+ catch(H5::AttributeIException& error) {
+ }
+ return ret;
+ }
+
+// Below is the complete list of atomic datatypes provided by HDF5. The
+// logic here is to do runtime type identification so we have one and only
+// one templated function to write scalars, one and only one to write
+// one or two dimensional arrays. The list is static and only used for
+// lookup. This type of clumsy initialization is a necessary evil.
+ static const h5_type h5_type_list[] = {
+ {typeid(char) , PredType::NATIVE_CHAR, IntType ( PredType::NATIVE_CHAR ), "char" },
+ {typeid(signed char) , PredType::NATIVE_SCHAR, IntType ( PredType::NATIVE_SCHAR ), "signed char" },
+ {typeid(unsigned char) , PredType::NATIVE_UCHAR, IntType ( PredType::NATIVE_UCHAR ), "unsighed char" },
+ {typeid(short int) , PredType::NATIVE_SHORT, IntType ( PredType::NATIVE_SHORT ), "short int" },
+ {typeid(unsigned short int) , PredType::NATIVE_USHORT, IntType ( PredType::NATIVE_USHORT ), "unsigned short int" },
+ {typeid(int) , PredType::NATIVE_INT, IntType ( PredType::NATIVE_INT ), "int" },
+ {typeid(unsigned int) , PredType::NATIVE_UINT, IntType ( PredType::NATIVE_UINT ), "unsigned int" },
+ {typeid(long int) , PredType::NATIVE_LONG, IntType ( PredType::NATIVE_LONG ), "long int" },
+ {typeid(unsigned long int) , PredType::NATIVE_ULONG, IntType ( PredType::NATIVE_ULONG ), "unsigned long int" },
+ {typeid(long long int) , PredType::NATIVE_LLONG, IntType ( PredType::NATIVE_LLONG ), "long long int" },
+ {typeid(unsigned long long int), PredType::NATIVE_ULLONG, IntType ( PredType::NATIVE_ULLONG ), "unsigned long long int" },
+ {typeid(float) , PredType::NATIVE_FLOAT, FloatType ( PredType::NATIVE_FLOAT ), "float" },
+ {typeid(double) , PredType::NATIVE_DOUBLE, FloatType ( PredType::NATIVE_DOUBLE ), "double" },
+ {typeid(long double) , PredType::NATIVE_LDOUBLE, FloatType ( PredType::NATIVE_LDOUBLE ), "long double" },
+/* These are bit field types which are not needed yet but left here for completeness
+ PredType::NATIVE_B8
+ PredType::NATIVE_B16
+ PredType::NATIVE_B32
+ PredType::NATIVE_B64
+*/
+/* These are mostly used by HDF5 only. Again, this is here so that this list is as close as possible to the list of types provided by HDF5
+ PredType::NATIVE_OPAQUE
+ PredType::NATIVE_HSIZE
+ PredType::NATIVE_HSSIZE
+ PredType::NATIVE_HERR
+*/
+ {typeid(bool) , PredType::NATIVE_HBOOL, IntType (PredType::NATIVE_HBOOL ), "bool" },
+ {typeid(int8_t) , PredType::NATIVE_INT8, IntType (PredType::NATIVE_INT8 ), "int 8" },
+ {typeid(uint8_t) , PredType::NATIVE_UINT8, IntType (PredType::NATIVE_UINT8 ), "uint 8" },
+ {typeid(int16_t) , PredType::NATIVE_INT16, IntType (PredType::NATIVE_INT16 ), "int 16" },
+ {typeid(uint16_t) , PredType::NATIVE_UINT16, IntType (PredType::NATIVE_UINT16 ), "uint 16" },
+ {typeid(int32_t) , PredType::NATIVE_INT32, IntType (PredType::NATIVE_INT32 ), "int32" },
+ {typeid(uint32_t) , PredType::NATIVE_UINT32, IntType (PredType::NATIVE_UINT32 ), "uint32" },
+ {typeid(int64_t) , PredType::NATIVE_INT64, IntType (PredType::NATIVE_INT64 ), "int64" },
+ {typeid(uint64_t) , PredType::NATIVE_UINT64, IntType (PredType::NATIVE_UINT64 ), "uint64" },
+ {typeid(std::string) , PredType::C_S1 , StrType ( ), "string" },
+ {typeid(H5std_string) , PredType::C_S1 , StrType ( ), "string" },
+ /** though this would be nice, plain old char * strings don't quite behave so this is ignored
+ {typeid(char*), PredType::C_S1, StrType()},
+
+ Last entry is garbage indicating that we have not found the datatype.
+ That's detected at runtime by checking the typeinfo on the variable
+ you are trying to save. New entries must be entred above this one.
+ */
+ {typeid(void*) , PredType::NATIVE_UINT64, IntType(PredType::NATIVE_UINT64 ), "UNKNOWN"}
+ // I mean it, DON'T put anything below this.
+ };
+
+ // Unify creation of scalar and array data spaces.
+ H5::DataSpace create_dataspace(const h2s & h){
+ H5::DataSpace ret;
+ try {
+ if ( h[0] ==1 && h[1] == 1){
+ ret = H5::DataSpace(H5S_SCALAR);
+ }
+ else{
+ const hsize_t *hh = h;
+ ret = H5::DataSpace(2, hh);
+ }
+ } catch ( H5::DataSpaceIException& error ){
+ }
+ return ret;
+ }
+
+ // Type identification to simplify the construction of dataypes.
+ // This return the correct entry if found or garbage otherwise. If the type
+ // is not found, the error flag in the returned struct
+ // This is a linear search. A hash table might be more efficient.
+ const h5_type& h5_type_find(const std::type_info& type) {
+ // Ignore the last entry for the search
+ size_t n_items = sizeof(h5_type_list)/sizeof(h5_type_list[0]) -1 ;
+ size_t i=0;
+ for (i=0; i < n_items; ++i )
+ if ( h5_type_list[i].info == type ) break;
+ // the following is safe since if we reached i == n_items, we are
+ // returning the garbage entry which will not match the type of the
+ // argument.
+ // Error checking is left to the caller.
+ return h5_type_list[i];
+ }
+
+ const h5_type& h5_type_find(const H5::DataType& type) {
+ size_t n_items = sizeof(h5_type_list)/sizeof(h5_type_list[0]) -1 ;
+ size_t i=0;
+ for (i=0; i < n_items; ++i )
+ if ( h5_type_list[i].type == type ) break;
+ // Error checking is left to the caller.
+ return h5_type_list[i];
+ }
+
+ const std::string& h5_type_find_name(const H5::DataType& type) {
+ return h5_type_find(type).name;
+ }
+
+ // Read a string with attribute "name" from the named group or file in the buff argument
+ int get_string(const H5::CommonFG& g, const std::string &name, std::string & buff ) {
+ int ret = 0;
+ try {
+ Exception::dontPrint();
+ DataSet t = g.openDataSet(name);
+ try {
+ std::string tmp;
+ tmp.resize(t.getStorageSize());
+ t.read(tmp, t.getStrType());
+ buff = tmp;
+ t.close();
+ ret = 1;
+ } catch ( H5::DataSetIException& error ) {
+ }
+ }
+ catch (H5::GroupIException& error ) {
+ }
+ return ret;
+ }
+
+ // return an integer named "name" from a group or file
+ int get_integer(const H5::CommonFG& g, const std::string &name ) {
+ int ret = 0;
+ try {
+ Exception::dontPrint();
+ DataSet t = g.openDataSet(name);
+ try {
+ Exception::dontPrint();
+ t.read(&ret, t.getIntType());
+ t.close();
+ } catch ( H5::DataSetIException& error ) {
+ }
+ }
+ catch (H5::GroupIException& error ) {
+ }
+ return ret;
+ }
+#if 0
+ template <typename R>
+ int get_array(const H5::CommonFG& g, const std::string &name, std::valarray<R>& v ) {
+ int ret = 0;
+ try {
+ Exception::dontPrint();
+ DataSet t = g.openDataSet(name);
+ try {
+ Exception::dontPrint();
+ DataSpace ds = t.getSpace();
+ // this is to make sure that we have enough room
+ const h5_type & type = h5_type_find(typeid(v[0]));
+ size_t N = h2s().get_size(ds);
+
+ v.capacity( N * t.getDataType().getSize() / sizeof(v[0]) );
+ v.resize(N);
+ t.read(v.data(), t.getDataType() );
+ // at this point, we can't be certain that we have the correct
+ // datatype for the array that was given so we use the conversion
+ // if necessary.
+ t.getDataType().convert(type.type, N , v.data(), NULL, PropList());
+
+ t.close();
+ ret = 1;
+ } catch ( H5::DataSetIException& error ) {
+ }
+ }
+ catch (H5::GroupIException& error ) {
+ }
+ return ret;
+ }
+#endif
+
+ ///
+ /// Create a hard link so that group or dataset named orig under source
+ /// group f is linked to group or dataset target unger group g
+ ///
+ /// Curiously, the C++ API does not provide this simple and necessary
+ /// operation, and no explanation is provided as to why.
+ ///
+ herr_t hard_link(const H5::CommonFG& f, const std::string & orig, H5::Group& g, const std::string& target){
+ return H5Lcreate_hard( f.getLocId(), orig.c_str(), g.getLocId(), target.c_str(), 0, 0);
+ }
+
+ // The name says it all. This works for files or groups
+ void dump_string(H5::CommonFG&g, const std::string &name, const std::string &s){
+ try {
+ H5::DataSpace dataspace( 0, h2s());
+ StrType datatype(PredType::C_S1, s.length()+1);
+ H5::DataSet dataset = g.createDataSet( name, datatype, dataspace );
+ dataset.write( s, datatype);
+ dataset.close();
+ } catch ( const H5::DataSetIException& exception ) {
+ exception.printError();
+ }
+ }
+
+ /**
+ Open an existing H5File to append or create it from scratch if it
+ does not exist. It is the caller's responsibility to close that file
+ when writing is done.
+
+ The logic here is that datasets are valuable and one should not
+ just truncate an existing file. This library is designed so there
+ is no possibility to overwrite data committed to a file by accident.
+ */
+ H5::H5File * append_or_create(const std::string & filename) {
+ H5File *file = 0;
+ try {
+ Exception::dontPrint();
+ file = new H5File(filename, H5F_ACC_RDWR);
+ } catch( FileIException& error ) {
+ try {
+ file = new H5File(filename, H5F_ACC_TRUNC);
+ } catch (FileIException& foo){
+ }
+ }
+ return file;
+ }
+
+ /** Open a group with a unique name in an existing H5File. The groups
+ are labelled sequentially with an integer.
+
+ The H5::Group destructor closes the group automatically. However, if a
+ groupis reused, it must be closed by the caller.
+
+ This function will not ovewrite data but append at the end of
+ the list of groups in a file.
+ */
+ H5::Group append_problem(H5File *file, const std::string & name ) {
+ const std::string ioh5_h5_problem("Problem");
+ const int ioh5_problem_width = 6;
+ H5::Group root = file->openGroup("/");
+ int n_groups = root.getNumObjs();
+ std::ostringstream buffer;
+ buffer << name << std::setw(ioh5_problem_width) << std::setfill('0') << ++n_groups;
+ std::string w = buffer.str();
+ return root.createGroup(buffer.str());
+ }
+
+ hsize_t getIndexOf( const H5::CommonFG* parent, const H5std_string& childName )
+ {
+ /// \todo This cannot be the proper way to find the index of a named child.
+ /// NOTE: the correct way is to use an iterator function
+ hsize_t numObjects = parent->getNumObjs();
+ for ( size_t i = 0 ; i < numObjects ; ++i )
+ {
+ H5std_string name = parent->getObjnameByIdx( i );
+ if ( name == childName )
+ return i;
+ }
+ return numObjects;
+ }
+
+ bool hasChildNamed( const H5::H5Object& parent, const H5std_string& childName )
+ {
+ // Not sure if these two approaches are equivalent.
+
+ bool exists0 = H5Lexists( parent.getId(), childName.c_str(), H5P_DEFAULT ) > 0;
+
+ H5O_info_t frameHeaderInfo;
+ bool exists1 = H5Oget_info_by_name( parent.getId(), childName.c_str(), &frameHeaderInfo, H5P_DEFAULT ) >= 0;
+
+ if ( exists0 != exists1 )
+ abort();
+
+ return exists0;
+ }
+
+ bool hasChildNamed( const H5::CommonFG& parent, const H5std_string& childName )
+ {
+ return getIndexOf( &parent, childName ) != parent.getNumObjs();
+ }
+
+
+ H5::Group getOrCreateGroup(H5::CommonFG& parent, const H5std_string& childName )
+ {
+ hsize_t childIndex = getIndexOf( &parent, childName );
+ if ( childIndex == parent.getNumObjs() )
+ return parent.createGroup( childName ) ;
+ return parent.openGroup( childName );
+ }
+
+ void print_attributes( H5::H5Object& loc, const H5std_string attr_name, void *operator_data) {
+ // this to make gcc shut up about unused arguments
+ H5::H5Object *l = &loc; void * f = operator_data; f = (void *)l; l = (H5::H5Object *)f;
+ H5std_string tmp_attr = attr_name;
+
+ // would be nice to find the name of the parent. Dunno yet.
+ //std::cerr << "Found attribute : " << attr_name << std::endl;
+ }
+
+ //
+ // Specialization for strings.
+ //
+ template<>
+ void set_scalar_attribute(H5::H5Object& g, const std::string &name, const std::string& val){
+ try {
+ H5::StrType datatype(H5::PredType::C_S1, val.size()+1 );
+ H5::Attribute attr = g.createAttribute(name, datatype, H5::DataSpace(H5S_SCALAR));
+ attr.write(datatype, val);
+ }
+ catch ( const H5::AttributeIException& exception ) {
+ exception.printError();
+ return;
+ }
+ catch ( const H5::DataTypeIException& exception ) {
+ exception.printError();
+ return;
+ }
+ }
+
+ /**
+ Template specialization for strings.
+
+ Read the value of the attribute with the given name from the given group.
+ \return True if a write was made to val, false if there is no such attribute,
+ or if the attribute was of the wrong type.
+
+ */
+ template <>
+ bool get_scalar_attribute(H5::H5Object & g, const std::string& name, std::string & result){
+ try {
+ H5::Attribute attribute;
+ try{
+ attribute = g.openAttribute(name);
+ } catch ( const H5::AttributeIException & exception ) {
+ return false;
+ }
+ try {
+ if ( attribute.getTypeClass() == H5T_STRING){
+ attribute.read( attribute.getDataType(), result );
+ return true;
+ }
+ } catch ( const H5::DataTypeIException& exception ) {
+ return false;
+ }
+ } catch ( const H5::DataTypeIException& exception ) {
+ return false;
+ }
+ return false;
+ }
+
+ ///
+ /// Dump an array with knowledge of the datatype. This is the only
+ /// function containing details of the datalayout and controls the
+ /// possible compression of the data on disk.
+ H5::DataSet dump_array_raw( H5::CommonFG & g, const h5_type& t,
+ const std::string& name, const void *a, const size_t m, const size_t n){
+ try {
+ H5::DataSetIException::dontPrint();
+ H5::DSetCreatPropList prop;
+ //prop.setChunk(2, h2s(m, n));
+ //prop.setDeflate(6);
+ H5::DataSet dataset = g.createDataSet( name, t.type, create_dataspace(h2s(m, n)), prop);
+ dataset.write( a, t.predicate);
+ return dataset;
+ }catch ( const H5::DataSetIException& exception ) {
+ }
+ return H5::DataSet();
+ }
+
+ bool find_first(Group g, TraverserFn& fn) {
+
+ for(size_t i=0; i<g.getNumObjs(); ++i ){
+
+ if ( fn(g , i) ) {
+ return true;
+ }
+
+ else if ( g.getObjTypeByIdx( i ) == H5G_GROUP ) {
+ std::string n = g.getObjnameByIdx(i) ;
+ Group child = g.openGroup(g.getObjnameByIdx(i));
+ if ( find_first(child, fn) ){
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+
+} /* namespace H5 */
+#endif // HDF5_INSTRUMENT
diff --git a/deps/opende/src/ioh5.h b/deps/opende/src/ioh5.h
new file mode 100644
index 0000000..0a8c15d
--- /dev/null
+++ b/deps/opende/src/ioh5.h
@@ -0,0 +1,414 @@
+/*
+ Copyright (c) 2013 Claude Lacoursiere
+
+ This software is provided 'as-is', without any express or implied
+ warranty. In no event will the authors be held liable for any damages
+ arising from the use of this software.
+
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it
+ freely, subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you must not
+ claim that you wrote the original software. If you use this software
+ in a product, an acknowledgment in the product documentation would be
+ appreciated but is not required.
+
+ 2. Altered source versions must be plainly marked as such, and must not be
+ misrepresented as being the original software.
+
+ 3. This notice may not be removed or altered from any source
+ distribution.
+
+reference:
+ hdf5-based BPMD database: https://grasp.robotics.cs.rpi.edu/bpmd/
+*/
+
+
+#include "gazebo/gazebo_config.h"
+#ifdef HDF5_INSTRUMENT
+
+#ifndef IO_H5
+#define IO_H5
+
+#include<string>
+#include<vector>
+#include <iostream>
+#include <valarray>
+#include <typeinfo>
+#include <string.h>
+
+#ifdef _MSC_VER
+#include <cpp/H5Cpp.h>
+#else
+#include <H5Cpp.h>
+#endif
+#include <H5Tpublic.h>
+#include <H5Exception.h>
+
+/** TODO:
+
+ Missing in this implementation is a way to handle bitfields, and
+ some better way to handle enumerations.
+
+ The "stride" argument used in the journal should be used to set the
+ column size of the array being dumped since this is in fact valuable
+ information when time comes to read back.
+*/
+
+
+/**
+ The h5 namespace provides functionality for saving/loading data to/from HDF5 files.
+*/
+namespace h5 {
+
+ struct IteratorData {
+ IteratorData() : m_name() {}
+ std::string m_name;
+ };
+
+ /** A simple wrapper class for the annoying hsize_t dims[2] arrays which are used all the time. */
+ class h2s {
+ public:
+ hsize_t m[2];
+ h2s(hsize_t a, hsize_t b) { m[0] =a; m[1]=b;}
+ h2s(hsize_t a) { m[0] =a; m[1]=1;}
+ h2s() { m[1] =1; m[1]=1;}
+#if !defined(__APPLE__)
+#ifndef WIN32
+ //\TODO Fix mac & windows compiling
+ hsize_t & operator[](size_t i) { return m[i] ; }
+ hsize_t operator[](size_t i) const { return m[i] ; }
+ size_t get_size(const H5::DataSpace& space){
+ std::vector<hsize_t> d( space.getSimpleExtentNdims() );
+ space.getSimpleExtentDims(&(d[0]));
+ size_t size = d[0];
+ for (size_t i = 1; i < d.size(); ++i ) size *= d[i] ;
+ return size;
+ }
+#endif
+#endif
+ operator const hsize_t * () const { return m ; }
+ operator hsize_t * () { return m ; }
+ };
+
+ // Type information needed by HDF5 functions collected here to allow
+ // extensive polymorphism.
+ struct h5_type {
+ const std::type_info & info;
+ const H5::PredType & predicate;
+ const H5::DataType type;
+ const std::string name;
+ };
+
+ const h5_type& h5_type_find(const std::type_info & type);
+
+ const h5_type& h5_type_find(const H5::DataType& t );
+
+ const std::string& h5_type_find_name(const H5::DataType& t );
+
+ void dump_string(H5::CommonFG &g, const std::string &name, const std::string &s);
+
+ /// Easily create a data space for either scalars or arrays
+ H5::DataSpace create_dataspace(const h2s& h);
+
+ herr_t hard_link(const H5::CommonFG& f, const std::string & orig, H5::Group& g, const std::string& target);
+
+ /// Dump an array with knowledge of the datatype. This is the only
+ /// function containing details of the datalayout and controls the
+ /// possible compression of the data on disk.
+ H5::DataSet dump_array_raw( H5::CommonFG & g, const h5_type &t,
+ const std::string& name,
+ const void *a, size_t m=1, size_t n=1);
+
+ //// This function takes care of the type identification and is therefore
+ /// templated. It resolves to the raw write function.
+ template <typename R>
+ H5::DataSet dump_array( H5::CommonFG & g, const std::string& name, const R *a, const size_t m=1, const size_t n=1){
+ try {
+ const h5_type & t = h5_type_find(typeid(a[0]));
+ if ( typeid(a[0]) == t.info ) {
+ return dump_array_raw(g, t, name, a, m, n);
+ }
+ }catch ( const H5::DataSetIException& exception ) {
+ exception.printError();
+ }
+ return H5::DataSet();
+ }
+
+ /// this assumes that the datatype, such as valarray or vector, has an []
+ //// operator as well as a size() method
+ template <typename R>
+ H5::DataSet dump_vector( H5::CommonFG & g, const std::string& name, const R& v, size_t n=1){
+ return dump_array(g, name, &(v[0]), v.size()/n, n);
+ }
+
+ template <typename R>
+ void dump_scalar(H5::CommonFG & g, const std::string &name, const R a){
+ dump_array<R>(g, name, &a);
+ }
+
+ template <typename R>
+ void set_scalar_attribute (H5::H5Object &g, const std::string &name, const R& val){
+ try {
+ const h5_type & t = h5_type_find(typeid(val));
+ if ( typeid(val) == t.info ) {
+ H5::Attribute attr = g.createAttribute(name, t.type, H5::DataSpace(H5S_SCALAR));
+ attr.write(t.type, &val);
+ } else {
+ }
+ }
+ catch ( const H5::AttributeIException& exception ) {
+ exception.printError();
+ return;
+ }
+ catch ( const H5::DataTypeIException& exception ) {
+ exception.printError();
+ return;
+ }
+ }
+
+ template<>
+ void set_scalar_attribute(H5::H5Object& g, const std::string &name, const std::string& val);
+
+ void print_attributes( H5::H5Object& loc, const H5std_string attr_name, void *operator_data);
+
+ /**
+ Read the value of the attribute with the given name from the given group.
+ \return True if a write was made to val, false if there is no such attribute,
+ or if the attribute was of the wrong type. */
+ template < typename T >
+ bool get_scalar_attribute(H5::H5Object & g, const std::string& name, T& result){
+ try {
+ const h5_type & t = h5_type_find(typeid(result));
+ H5::Attribute attribute;
+ try{
+ attribute = g.openAttribute(name);
+ } catch ( const H5::AttributeIException & exception ) {
+ return false;
+ }
+ try {
+ if ( attribute.getDataType() == t.predicate ) {
+ attribute.read( attribute.getDataType(), &result );
+ return true;
+ }
+ } catch ( const H5::DataTypeIException& exception ) {
+ return false;
+ }
+ } catch ( const H5::DataTypeIException& exception ) {
+ return false;
+ }
+ return false;
+ }
+
+ /**
+ Read the value of the attribute with the given name from the given group.
+ \return True if a write was made to val, false if there is no such attribute,
+ or if the attribute was of the wrong type. */
+ template <>
+ bool get_scalar_attribute<std::string>(H5::H5Object & g, const std::string& name, std::string & result);
+
+
+ /** Read a scalar in a data set with the given name from the given group. */
+ template <typename R>
+ bool get_scalar(H5::CommonFG &g, const std::string& name, R& result)
+ {
+ const h5_type & t = h5_type_find(typeid(result));
+ try {
+ H5::DataSet dataset = g.openDataSet(name);
+ if ( dataset.getSpace().getSimpleExtentType() == H5S_SCALAR ) {
+ if ( dataset.getDataType() == t.predicate ) { // need error message or exception if this is false
+ try {
+ dataset.read(&result, dataset.getDataType() );
+ return true;
+ } catch ( const H5::DataSetIException& exception ) {
+ return false;
+ }
+ } else {
+ }
+ } else {
+ return false;
+ }
+ } catch ( const H5::DataSetIException& exception ) {
+ return false;
+ }
+ return false;
+ }
+
+ /**
+ Open and existing file to append a problem set, or create a
+ new one. This does not overwrite existing data.
+
+ */
+
+ H5::H5File * append_or_create(const std::string & filename);
+
+ /**
+ * Create a new group to write a dataset. No utility is provided here
+ * to delete problems.
+ */
+ H5::Group append_problem(H5::H5File *file, const std::string & name = "Problem");
+
+ /**
+ \return The index of the child with the given name. Returns parent.getNumObjs() if no such child exists.
+ */
+ hsize_t getIndexOf( const H5::CommonFG& parent, const H5std_string& childName );
+
+ /**
+ Find a child with given name.
+ */
+ bool hasChildNamed( const H5::CommonFG& parent, const H5std_string& childName );
+
+
+ /**
+ Return the child group with the given name. Will be created if it doesn't exist.
+
+ This is the same principle as append_or_create. This library is
+ intended only for writing datasets, not to manipulate them.
+ */
+ H5::Group getOrCreateGroup( H5::CommonFG& parent, const H5std_string& childName );
+
+
+
+ /** A utility that just checks for the existence of an attribute with given name */
+ bool check_attr(const H5::CommonFG &g, const std::string & name);
+
+ /** Read the string dataset of given name into the std::string buff.
+ Return 0 if not found, 1 for success. */
+ int get_string(const H5::CommonFG& g, const std::string &name, std::string & buff );
+
+
+ /**
+ Read an array and put content in object v which must be a class with a method
+ resize(size_t n) and operator * which returns the data buffer.
+ */
+ template<typename R>
+ H5::DataType get_array(const H5::CommonFG& g, const std::string &name, R& v )
+ {
+ try {
+ H5::Exception::dontPrint();
+ H5::DataSet t = g.openDataSet(name);
+ try {
+ H5::Exception::dontPrint();
+ H5::DataSpace ds = t.getSpace();
+ // this is to make sure that we have enough room
+ const h5_type & type = h5::h5_type_find(typeid(v[0]));
+ size_t N = h2s().get_size(ds);
+ // make sure we have enough space
+ size_t s1 = sizeof(v[0]);
+ size_t s2 = t.getDataType().getSize();
+ size_t alloc = N;
+ if ( s2 > s1 ) alloc *= s2/s1;
+ R w(alloc);
+ t.read(&(w[0]), t.getDataType() );
+ // at this point, we can't be certain that we have the correct
+ // datatype for the array that was given so we use the conversion
+ // if necessary.
+ t.getDataType().convert(type.type, N , &(w[0]), NULL, H5::PropList());
+ t.close();
+ v.resize(N);
+ memcpy(&(v[0]), &(w[0]), N * sizeof(v[0]));
+ return type.type;
+ } catch ( H5::DataSetIException& error ) {
+ }
+ }
+ catch (H5::GroupIException& error ) {
+ }
+ return H5::DataType();
+ }
+
+//
+// This version is intendent for the case where we don't know the datatype
+// at all and are only interested in working on the raw storage as char*
+// The template remains since we might have valarray<char*> or
+// vector<char*> for instance. However, not knowing the data type, we
+// cannot use the resize operator using the information from the
+// dataspace. The purpose is to be able to apply filters without first
+// making a determination of the datatype and then instantiation of arrays
+// with the correct version.
+//
+// "Raw" here means that we have just bytes to manipulate.
+//
+template<typename R>
+H5::DataType get_array_raw(const H5::CommonFG& g, const std::string &name, R& v)
+{
+ H5::DataType ret;
+ try {
+ H5::Exception::dontPrint();
+ if ( g.getNumObjs()) {
+ H5::DataSet t = g.openDataSet(name);
+ H5::DataSpace ds = t.getSpace();
+ h5::h2s dims;
+ ds.getSimpleExtentDims(dims);
+ v.resize( t.getStorageSize() );
+ if ( v.size() ) {
+ t.read(&(v[0]), t.getDataType());
+ }
+ ret = t.getDataType();
+ t.close();
+ return ret;
+ }
+ }
+ catch ( H5::DataSetIException& error ) {
+ }
+ catch (H5::GroupIException& error ) {
+ }
+ return H5::DataType();
+}
+
+
+///////////////////////////////////////////////////////
+///
+/// Utilities to locate datasets which have nown names but unknown location
+/// in the file.
+///
+///////////////////////////////////////////////////////
+
+
+///
+/// This allows operating on the idx' child of g with an iterator
+///
+struct TraverserFn {
+ virtual bool operator()(H5::Group& parent, hsize_t idx) = 0;
+};
+
+/// A traverser function which matches a name during a depth first
+/// traversal from a given group.
+struct MatchName : public TraverserFn {
+ H5std_string target_name;
+ H5::Group parent;
+ hsize_t child_index;
+ bool operator()(H5::Group& g, hsize_t idx){
+ std::string n = g.getObjnameByIdx(idx);
+ if ( g.getObjnameByIdx(idx) == target_name ) {
+ parent = g;
+ child_index = idx;
+ return true;
+ }
+ return false;
+ }
+
+ MatchName(const std::string& s) : target_name(s), parent(), child_index(-1) {};
+
+ void set_name(const std::string& s) {
+ target_name = s;
+ parent = H5::Group();
+ return;
+ }
+
+ ~MatchName() { }
+};
+
+////
+/// This is a depth first search operation which starts from a group.
+///
+/// This could be designed to start from a file but this causes problems to
+/// return the correct object type in the MatchName struct for instance.
+/// One can always open the "/" group in a file and start from there.
+///
+bool find_first(H5::Group g, TraverserFn& fn);
+}
+
+#endif
+
+#endif // HDF5_INSTRUMENT
diff --git a/deps/opende/src/joints/hinge.cpp b/deps/opende/src/joints/hinge.cpp
index 768b336..ab809ec 100644
--- a/deps/opende/src/joints/hinge.cpp
+++ b/deps/opende/src/joints/hinge.cpp
@@ -75,9 +75,6 @@ dxJointHinge::getInfo1( dxJoint::Info1 *info )
if ( limot.testRotationalLimit( cumulative_angle ) )
info->m = 6;
}
- // joint damping
- if ( use_damping )
- info->m = 6;
}
@@ -96,7 +93,6 @@ dxJointHinge::getInfo2( dxJoint::Info2 *info )
info->cfm[2] = cfm;
info->cfm[3] = cfm;
info->cfm[4] = cfm;
- info->cfm[5] = cfm;
}
// set the three ball-and-socket rows
@@ -181,22 +177,6 @@ dxJointHinge::getInfo2( dxJoint::Info2 *info )
// if the hinge is powered, or has joint limits, add in the stuff
limot.addLimot( this, info, 5, ax1, 1 );
-
- // joint damping
- if (this->use_damping)
- {
- // added J1ad and J2ad for damping, only 1 row
- info->J1ad[0] = ax1[0];
- info->J1ad[1] = ax1[1];
- info->J1ad[2] = ax1[2];
- if ( this->node[1].body )
- {
- info->J2ad[0] = -ax1[0];
- info->J2ad[1] = -ax1[1];
- info->J2ad[2] = -ax1[2];
- }
- // there's no rhs for damping setup, all we want to use is the jacobian information above
- }
}
diff --git a/deps/opende/src/joints/hinge2.cpp b/deps/opende/src/joints/hinge2.cpp
index e3ac1bb..c1866cd 100644
--- a/deps/opende/src/joints/hinge2.cpp
+++ b/deps/opende/src/joints/hinge2.cpp
@@ -138,8 +138,6 @@ dxJointHinge2::getInfo2( dxJoint::Info2 *info )
info->cfm[1] = cfm;
info->cfm[2] = cfm;
info->cfm[3] = cfm;
- info->cfm[4] = cfm;
- info->cfm[5] = cfm;
}
// get information we need to set the hinge row
diff --git a/deps/opende/src/joints/joint.cpp b/deps/opende/src/joints/joint.cpp
index 6f97454..2965b2b 100644
--- a/deps/opende/src/joints/joint.cpp
+++ b/deps/opende/src/joints/joint.cpp
@@ -59,10 +59,6 @@ dxJoint::dxJoint( dxWorld *w ) :
w->nj++;
feedback = 0;
- // joint damping
- use_damping = false;
- damping_coefficient = 0.0;
-
// Moved here by OSRF
// Default to negative value, which means the current global value
// will be used. If set non-negative, then this joint-specific
diff --git a/deps/opende/src/joints/joint.h b/deps/opende/src/joints/joint.h
index d1bfacc..479046a 100644
--- a/deps/opende/src/joints/joint.h
+++ b/deps/opende/src/joints/joint.h
@@ -94,10 +94,6 @@ struct dxJoint : public dObject
// J2xx pointers may be 0.
dReal *J1l, *J1a, *J2l, *J2a;
- // jacobians for joint damping
- // angular ones are implemented first
- dReal *J1ld, *J1ad, *J2ld, *J2ad;
-
// elements to jump from one row to the next in J's
int rowskip;
@@ -162,11 +158,6 @@ struct dxJoint : public dObject
// (has the enabled flag set, and is attached to at least one dynamic body)
bool isEnabled() const;
- // to use joint damping set use_damping to true
- // and set damping_coefficient to the desired value
- bool use_damping;
- dReal damping_coefficient;
-
// Moved from ball.h and fixed.h by OSRF
dReal erp; // error reduction
dReal cfm; // constraint force mix in
diff --git a/deps/opende/src/joints/piston.cpp b/deps/opende/src/joints/piston.cpp
index 0e5eb5e..d83f478 100644
--- a/deps/opende/src/joints/piston.cpp
+++ b/deps/opende/src/joints/piston.cpp
@@ -231,8 +231,6 @@ dxJointPiston::getInfo2 ( dxJoint::Info2 *info )
info->cfm[1] = cfm;
info->cfm[2] = cfm;
info->cfm[3] = cfm;
- info->cfm[4] = cfm;
- info->cfm[5] = cfm;
}
const int s0 = 0;
diff --git a/deps/opende/src/joints/screw.cpp b/deps/opende/src/joints/screw.cpp
index 5ed7786..2a116a3 100644
--- a/deps/opende/src/joints/screw.cpp
+++ b/deps/opende/src/joints/screw.cpp
@@ -137,7 +137,6 @@ dxJointScrew::getInfo2( dxJoint::Info2 *info )
info->cfm[2] = cfm;
info->cfm[3] = cfm;
info->cfm[4] = cfm;
- info->cfm[5] = cfm;
}
// constraint rows 1 to 3
@@ -331,9 +330,8 @@ dxJointScrew::getInfo2( dxJoint::Info2 *info )
// info->J1a[s1+0] = -1;
// info->J1a[s1+1] = 0;
// info->J1a[s1+2] = 0;
- // printf("screw err lin[%f], ang[%f], diff[%f] [%d] tp[%f]\n",
- // thread_pitch*lin_disp, cumulative_angle, lin_err,
- // (int)this->use_damping, thread_pitch);
+ // printf("screw err lin[%f], ang[%f], diff[%f] tp[%f]\n",
+ // thread_pitch*lin_disp, cumulative_angle, lin_err, thread_pitch);
// compute last two elements of right hand side. we want to align the offset
// point (in body 2's frame) with the center of body 1.
@@ -387,25 +385,6 @@ dxJointScrew::getInfo2( dxJoint::Info2 *info )
// uncommnet to enforce slider joint limit
// limot.addLimot ( this, info, 5, ax1, 0 );
-
- /* comment out linear damping, use only rotation damping
- // linear joint damping
- if (this->use_damping)
- {
- // added J1ld and J2ld for damping, only 1 row
- info->J1ld[0] = ax1[0];
- info->J1ld[1] = ax1[1];
- info->J1ld[2] = ax1[2];
- if ( this->node[1].body )
- {
- info->J2ld[0] = -ax1[0];
- info->J2ld[1] = -ax1[1];
- info->J2ld[2] = -ax1[2];
- }
- // there's no rhs for damping setup, all we want to use is the
- // jacobian information above
- }
- */
}
// constraint rows 4 and 5
@@ -477,23 +456,6 @@ dxJointScrew::getInfo2( dxJoint::Info2 *info )
// enforcing rotation joint limit
limot.addLimot( this, info, 5, ax1, 1 );
-
- // rotational joint damping
- if (this->use_damping)
- {
- // added J1ad and J2ad for damping, only 1 row
- info->J1ad[0] = ax1[0];
- info->J1ad[1] = ax1[1];
- info->J1ad[2] = ax1[2];
- if ( this->node[1].body )
- {
- info->J2ad[0] = -ax1[0];
- info->J2ad[1] = -ax1[1];
- info->J2ad[2] = -ax1[2];
- }
- // there's no rhs for damping setup, all we want to use is
- // the jacobian information above
- }
}
}
diff --git a/deps/opende/src/joints/slider.cpp b/deps/opende/src/joints/slider.cpp
index c602a0f..4aea52a 100644
--- a/deps/opende/src/joints/slider.cpp
+++ b/deps/opende/src/joints/slider.cpp
@@ -145,10 +145,6 @@ dxJointSlider::getInfo1 ( dxJoint::Info1 *info )
info->m = 6;
}
}
-
- // joint damping
- if ( use_damping )
- info->m = 6;
}
@@ -167,7 +163,6 @@ dxJointSlider::getInfo2 ( dxJoint::Info2 *info )
info->cfm[2] = cfm;
info->cfm[3] = cfm;
info->cfm[4] = cfm;
- info->cfm[5] = cfm;
}
int i, s = info->rowskip;
@@ -247,22 +242,6 @@ dxJointSlider::getInfo2 ( dxJoint::Info2 *info )
// if the slider is powered, or has joint limits, add in the extra row
limot.addLimot ( this, info, 5, ax1, 0 );
-
- // joint damping
- if (this->use_damping)
- {
- // added J1ld and J2ld for damping, only 1 row
- info->J1ld[0] = ax1[0];
- info->J1ld[1] = ax1[1];
- info->J1ld[2] = ax1[2];
- if ( this->node[1].body )
- {
- info->J2ld[0] = -ax1[0];
- info->J2ld[1] = -ax1[1];
- info->J2ld[2] = -ax1[2];
- }
- // there's no rhs for damping setup, all we want to use is the jacobian information above
- }
}
diff --git a/deps/opende/src/joints/universal.cpp b/deps/opende/src/joints/universal.cpp
index 7fc65e2..3813804 100644
--- a/deps/opende/src/joints/universal.cpp
+++ b/deps/opende/src/joints/universal.cpp
@@ -321,8 +321,6 @@ dxJointUniversal::getInfo2( dxJoint::Info2 *info )
info->cfm[1] = cfm;
info->cfm[2] = cfm;
info->cfm[3] = cfm;
- info->cfm[4] = cfm;
- info->cfm[5] = cfm;
}
// set the three ball-and-socket rows
diff --git a/deps/opende/src/lcp.cpp b/deps/opende/src/lcp.cpp
index 90d9d2f..5f03f38 100644
--- a/deps/opende/src/lcp.cpp
+++ b/deps/opende/src/lcp.cpp
@@ -534,10 +534,17 @@ void dLCP::transfer_i_to_C (int i)
for (int j=0; j<nC; ++j) Ltgt[j] = ell[j];
}
const int nC = m_nC;
- m_d[nC] = dRecip (AROW(i)[i] - dDot(m_ell,m_Dell,nC));
+ dReal Aii_dDot = AROW(i)[i] - dDot(m_ell, m_Dell, nC);
+ if(dFabs(Aii_dDot) < 1e-16) {
+ Aii_dDot += 1e-6;
+ }
+ m_d[nC] = dRecip (Aii_dDot);
}
else {
- m_d[0] = dRecip (AROW(i)[i]);
+ if(dFabs(AROW(i)[i]) < 1e-16) {
+ AROW(i)[i] += 1e-6;
+ }
+ m_d[0] = dRecip (AROW(i)[i]);
}
swapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);
@@ -582,10 +589,17 @@ void dLCP::transfer_i_from_N_to_C (int i)
for (int j=0; j<nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j];
}
const int nC = m_nC;
- m_d[nC] = dRecip (AROW(i)[i] - dDot(m_ell,m_Dell,nC));
+ dReal Aii_dDot = AROW(i)[i] - dDot(m_ell, m_Dell, nC);
+ if(dFabs(Aii_dDot) < 1e-16) {
+ Aii_dDot += 1e-6;
+ }
+ m_d[nC] = dRecip (Aii_dDot);
}
else {
- m_d[0] = dRecip (AROW(i)[i]);
+ if(dFabs(AROW(i)[i]) < 1e-16) {
+ AROW(i)[i] += 1e-6;
+ }
+ m_d[0] = dRecip (AROW(i)[i]);
}
swapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);
diff --git a/deps/opende/src/objects.h b/deps/opende/src/objects.h
index 674114c..dfdf1e4 100644
--- a/deps/opende/src/objects.h
+++ b/deps/opende/src/objects.h
@@ -30,6 +30,7 @@
#include <ode/common.h>
#include <ode/memory.h>
#include <ode/mass.h>
+#include <ode/objects.h>
#include "array.h"
#include <boost/threadpool.hpp>
@@ -49,7 +50,6 @@ enum {
dxBodyGyroscopic = 256// use gyroscopic term
};
-
// base class that does correct object allocation / deallocation
struct dBase {
@@ -64,12 +64,12 @@ struct dBase {
// base class for bodies and joints
struct dObject : public dBase {
- dxWorld *world; // world this object is in
- dObject *next; // next object of this type in list
- dObject **tome; // pointer to previous object's next ptr
- int tag; // used by dynamics algorithms
- int island_tag; // used by island algorithms for grouping
- void *userdata; // user settable data
+ dxWorld *world; // world this object is in
+ dObject *next; // next object of this type in list
+ dObject **tome; // pointer to previous object's next ptr
+ int tag; // used by dynamics algorithms
+ int island_tag; // used by island algorithms for grouping
+ void *userdata; // user settable data
dObject(dxWorld *w);
virtual ~dObject() { }
};
@@ -77,8 +77,8 @@ struct dObject : public dBase {
// auto disable parameters
struct dxAutoDisable {
- dReal idle_time; // time the body needs to be idle to auto-disable it
- int idle_steps; // steps the body needs to be idle to auto-disable it
+ dReal idle_time; // time the body needs to be idle to auto-disable it
+ int idle_steps; // steps the body needs to be idle to auto-disable it
dReal linear_average_threshold; // linear (squared) average velocity threshold
dReal angular_average_threshold; // angular (squared) average velocity threshold
unsigned int average_samples; // size of the average_lvel and average_avel buffers
@@ -96,13 +96,13 @@ struct dxDampingParameters {
// quick-step parameters
struct dxQuickStepParameters {
- int precon_iterations; // number of preconditioned SOR iterations to perform (without error correction)
- int num_iterations; // number of SOR iterations to perform
- dReal w; // the SOR over-relaxation parameter
- int num_chunks; // divide rows to these many chunks
- int num_overlap; // divide rows but over lap this many rows
- // stop iteration if total rms_constraint_residual falls below this value
- dReal sor_lcp_tolerance;
+ int precon_iterations; // number of preconditioned PGS iterations to perform (without error correction)
+ int num_iterations; // number of PGS iterations to perform
+ dReal w; // the PGS over-relaxation parameter
+ int num_chunks; // divide rows to these many chunks
+ int num_overlap; // divide rows but over lap this many rows
+ // stop iteration if total rms_constraint_residual falls below this value
+ dReal pgs_lcp_tolerance;
// rms_dlambda for this time step
// rms_dlambda[0]: bilateral constraints (findex = -1)
// rms_dlambda[1]: contact normal constraints (findex = -2)
@@ -118,16 +118,19 @@ struct dxQuickStepParameters {
int num_contacts; // for monitoring number of contacts
bool dynamic_inertia_reduction; // turn on/off quickstep inertia reduction.
dReal smooth_contacts; // control quickstep smoothing for contact solution.
+ bool thread_position_correction; // threaded position correction computations
bool row_reorder1; // control quickstep row reordering
dReal warm_start; // warm start factor, 0: no warm start, 1: full warm start
int friction_iterations; // extra quickstep iterations friction.
+ Friction_Model friction_model; // friction model, enum type Friction_Model
+ World_Solver_Type world_solver_type; // world step solver, enum type World_Solver_Type.
};
// robust-step parameters
struct dxRobustStepParameters {
- dxRobustStepParameters()
- {
- eps_feas = eps = std::numeric_limits<dReal>::epsilon();
+ dxRobustStepParameters()
+ {
+ eps_feas = eps = std::numeric_limits<dReal>::epsilon();
max_iterations = 100;
}
dReal eps_feas; // feasibility tolerance
@@ -137,8 +140,8 @@ struct dxRobustStepParameters {
// contact generation parameters
struct dxContactParameters {
- dReal max_vel; // maximum correcting velocity
- dReal min_depth; // thickness of 'surface layer'
+ dReal max_vel; // maximum correcting velocity
+ dReal min_depth; // thickness of 'surface layer'
};
// position vector and rotation matrix for geometry objects that are not
@@ -149,22 +152,22 @@ struct dxPosR {
};
struct dxBody : public dObject {
- dxJointNode *firstjoint; // list of attached joints
- unsigned flags; // some dxBodyFlagXXX flags
- dGeomID geom; // first collision geom associated with body
- dMass mass; // mass parameters about POR
- dMatrix3 invI; // inverse of mass.I
- dReal invMass; // 1 / mass.mass
- dxPosR posr; // position and orientation of point of reference
- dQuaternion q; // orientation quaternion
- dVector3 lvel,avel; // linear and angular velocity of POR
- dVector3 facc,tacc; // force and torque accumulators
- dVector3 finite_rot_axis; // finite rotation axis, unit length or 0=none
+ dxJointNode *firstjoint; // list of attached joints
+ unsigned flags; // some dxBodyFlagXXX flags
+ dGeomID geom; // first collision geom associated with body
+ dMass mass; // mass parameters about POR
+ dMatrix3 invI; // inverse of mass.I
+ dReal invMass; // 1 / mass.mass
+ dxPosR posr; // position and orientation of point of reference
+ dQuaternion q; // orientation quaternion
+ dVector3 lvel,avel; // linear and angular velocity of POR
+ dVector3 facc,tacc; // force and torque accumulators
+ dVector3 finite_rot_axis; // finite rotation axis, unit length or 0=none
// auto-disable information
- dxAutoDisable adis; // auto-disable parameters
- dReal adis_timeleft; // time left to be idle
- int adis_stepsleft; // steps left to be idle
+ dxAutoDisable adis; // auto-disable parameters
+ dReal adis_timeleft; // time left to be idle
+ int adis_stepsleft; // steps left to be idle
dVector3* average_lvel_buffer; // buffer for the linear average velocity calculation
dVector3* average_avel_buffer; // buffer for the angular average velocity calculation
unsigned int average_counter; // counter/index to fill the average-buffers
@@ -182,16 +185,16 @@ struct dxBody : public dObject {
struct dxWorld : public dBase {
- dxBody *firstbody; // body linked list
- dxJoint *firstjoint; // joint linked list
- int nb,nj; // number of bodies and joints in lists
- dVector3 gravity; // gravity vector (m/s/s)
- dReal global_erp; // global error reduction parameter
- dReal global_cfm; // global constraint force mixing parameter
- dxAutoDisable adis; // auto-disable parameters
+ dxBody *firstbody; // body linked list
+ dxJoint *firstjoint; // joint linked list
+ int nb,nj; // number of bodies and joints in lists
+ dVector3 gravity; // gravity vector (m/s/s)
+ dReal global_erp; // global error reduction parameter
+ dReal global_cfm; // global constraint force mixing parameter
+ dxAutoDisable adis; // auto-disable parameters
int body_flags; // flags for new bodies
dxStepWorkingMemory *wmem; // Working memory object for dWorldStep/dWorldQuickStep
- dxStepWorkingMemory *island_wmems[1000]; // Working memory object for dWorldStep/dWorldQuickStep
+ std::vector<dxStepWorkingMemory *> island_wmems; // Working memory object for dWorldStep/dWorldQuickStep
dxQuickStepParameters qs;
dxRobustStepParameters rs;
diff --git a/deps/opende/src/ode.cpp b/deps/opende/src/ode.cpp
index 877617f..dfad70e 100644
--- a/deps/opende/src/ode.cpp
+++ b/deps/opende/src/ode.cpp
@@ -91,13 +91,13 @@ static void removeJointReferencesFromAttachedBodies (dxJoint *j)
dxJointNode *n = body->firstjoint;
dxJointNode *last = 0;
while (n) {
- if (n->joint == j) {
- if (last) last->next = n->next;
- else body->firstjoint = n->next;
- break;
- }
- last = n;
- n = n->next;
+ if (n->joint == j) {
+ if (last) last->next = n->next;
+ else body->firstjoint = n->next;
+ break;
+ }
+ last = n;
+ n = n->next;
}
}
}
@@ -133,8 +133,8 @@ static int g_world_check_tag_generator = 0;
static inline int generateWorldCheckTag()
{
- // Atomicity is not necessary here
- return ++g_world_check_tag_generator;
+ // Atomicity is not necessary here
+ return ++g_world_check_tag_generator;
}
static void checkWorld (dxWorld *w)
@@ -180,7 +180,7 @@ static void checkWorld (dxWorld *w)
for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
if (j->node[0].body || j->node[1].body) {
if (!(j->node[0].body && j->node[1].body))
- dDebug (0,"half connected joint found");
+ dDebug (0,"half connected joint found");
}
}
*/
@@ -229,7 +229,7 @@ static void checkWorld (dxWorld *w)
if (j->node[0].body && (j->node[0].body == j->node[1].body))
dDebug (0,"non-distinct body pointers in joint");
if ((j->node[0].body && j->node[0].body->tag != count) ||
- (j->node[1].body && j->node[1].body->tag != count))
+ (j->node[1].body && j->node[1].body->tag != count))
dDebug (0,"bad body pointer in joint");
}
}
@@ -285,7 +285,7 @@ dxBody *dBodyCreate (dxWorld *w)
// set auto-disable parameters
b->average_avel_buffer = b->average_lvel_buffer = 0; // no buffer at beginning
- dBodySetAutoDisableDefaults (b); // must do this after adding to world
+ dBodySetAutoDisableDefaults (b); // must do this after adding to world
b->adis_stepsleft = b->adis.idle_steps;
b->adis_timeleft = b->adis.idle_time;
b->average_counter = 0;
@@ -294,7 +294,7 @@ dxBody *dBodyCreate (dxWorld *w)
b->moved_callback = 0;
- dBodySetDampingDefaults(b); // must do this after adding to world
+ dBodySetDampingDefaults(b); // must do this after adding to world
b->flags |= w->body_flags & dxBodyMaxAngularSpeed;
b->max_angular_speed = w->max_angular_speed;
@@ -338,13 +338,13 @@ void dBodyDestroy (dxBody *b)
// delete the average buffers
if(b->average_lvel_buffer)
{
- delete[] (b->average_lvel_buffer);
- b->average_lvel_buffer = 0;
+ delete[] (b->average_lvel_buffer);
+ b->average_lvel_buffer = 0;
}
if(b->average_avel_buffer)
{
- delete[] (b->average_avel_buffer);
- b->average_avel_buffer = 0;
+ delete[] (b->average_avel_buffer);
+ b->average_avel_buffer = 0;
}
if (b->contactp) delete b->contactp;
@@ -454,11 +454,11 @@ const dReal * dBodyGetPosition (dBodyID b)
void dBodyCopyPosition (dBodyID b, dVector3 pos)
{
- dAASSERT (b);
- dReal* src = b->posr.pos;
- pos[0] = src[0];
- pos[1] = src[1];
- pos[2] = src[2];
+ dAASSERT (b);
+ dReal* src = b->posr.pos;
+ pos[0] = src[0];
+ pos[1] = src[1];
+ pos[2] = src[2];
}
@@ -471,20 +471,20 @@ const dReal * dBodyGetRotation (dBodyID b)
void dBodyCopyRotation (dBodyID b, dMatrix3 R)
{
- dAASSERT (b);
- const dReal* src = b->posr.R;
- R[0] = src[0];
- R[1] = src[1];
- R[2] = src[2];
- R[3] = src[3];
- R[4] = src[4];
- R[5] = src[5];
- R[6] = src[6];
- R[7] = src[7];
- R[8] = src[8];
- R[9] = src[9];
- R[10] = src[10];
- R[11] = src[11];
+ dAASSERT (b);
+ const dReal* src = b->posr.R;
+ R[0] = src[0];
+ R[1] = src[1];
+ R[2] = src[2];
+ R[3] = src[3];
+ R[4] = src[4];
+ R[5] = src[5];
+ R[6] = src[6];
+ R[7] = src[7];
+ R[8] = src[8];
+ R[9] = src[9];
+ R[10] = src[10];
+ R[11] = src[11];
}
@@ -497,12 +497,12 @@ const dReal * dBodyGetQuaternion (dBodyID b)
void dBodyCopyQuaternion (dBodyID b, dQuaternion quat)
{
- dAASSERT (b);
- dReal* src = b->q;
- quat[0] = src[0];
- quat[1] = src[1];
- quat[2] = src[2];
- quat[3] = src[3];
+ dAASSERT (b);
+ dReal* src = b->q;
+ quat[0] = src[0];
+ quat[1] = src[1];
+ quat[2] = src[2];
+ quat[3] = src[3];
}
@@ -528,8 +528,8 @@ void dBodySetMass (dBodyID b, const dMass *mass)
// The centre of mass must be at the origin.
// Use dMassTranslate( mass, -mass->c[0], -mass->c[1], -mass->c[2] ) to correct it.
dUASSERT( fabs( mass->c[0] ) <= dEpsilon &&
- fabs( mass->c[1] ) <= dEpsilon &&
- fabs( mass->c[2] ) <= dEpsilon, "The centre of mass must be at the origin." )
+ fabs( mass->c[1] ) <= dEpsilon &&
+ fabs( mass->c[2] ) <= dEpsilon, "The centre of mass must be at the origin." )
memcpy (&b->mass,mass,sizeof(dMass));
if (dInvertPDMatrix (b->mass.I,b->invI,3,NULL)==0) {
@@ -596,7 +596,7 @@ void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
- dReal px, dReal py, dReal pz)
+ dReal px, dReal py, dReal pz)
{
dAASSERT (b);
b->facc[0] += fx;
@@ -614,7 +614,7 @@ void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
- dReal px, dReal py, dReal pz)
+ dReal px, dReal py, dReal pz)
{
dAASSERT (b);
dVector3 prel,f,p;
@@ -635,7 +635,7 @@ void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
- dReal px, dReal py, dReal pz)
+ dReal px, dReal py, dReal pz)
{
dAASSERT (b);
dVector3 frel,f;
@@ -656,7 +656,7 @@ void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
- dReal px, dReal py, dReal pz)
+ dReal px, dReal py, dReal pz)
{
dAASSERT (b);
dVector3 frel,prel,f,p;
@@ -710,7 +710,7 @@ void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z)
void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz,
- dVector3 result)
+ dVector3 result)
{
dAASSERT (b);
dVector3 prel,p;
@@ -726,7 +726,7 @@ void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz,
void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz,
- dVector3 result)
+ dVector3 result)
{
dAASSERT (b);
dVector3 prel,p;
@@ -743,7 +743,7 @@ void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz,
void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz,
- dVector3 result)
+ dVector3 result)
{
dAASSERT (b);
dVector3 p;
@@ -759,7 +759,7 @@ void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz,
void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz,
- dVector3 result)
+ dVector3 result)
{
dAASSERT (b);
dVector3 prel;
@@ -772,7 +772,7 @@ void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz,
void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz,
- dVector3 result)
+ dVector3 result)
{
dAASSERT (b);
dVector3 p;
@@ -785,7 +785,7 @@ void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz,
void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz,
- dVector3 result)
+ dVector3 result)
{
dAASSERT (b);
dVector3 p;
@@ -929,132 +929,132 @@ int dBodyGetGravityMode (dBodyID b)
dReal dBodyGetAutoDisableLinearThreshold (dBodyID b)
{
- dAASSERT(b);
- return dSqrt (b->adis.linear_average_threshold);
+ dAASSERT(b);
+ return dSqrt (b->adis.linear_average_threshold);
}
void dBodySetAutoDisableLinearThreshold (dBodyID b, dReal linear_average_threshold)
{
- dAASSERT(b);
- b->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold;
+ dAASSERT(b);
+ b->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold;
}
dReal dBodyGetAutoDisableAngularThreshold (dBodyID b)
{
- dAASSERT(b);
- return dSqrt (b->adis.angular_average_threshold);
+ dAASSERT(b);
+ return dSqrt (b->adis.angular_average_threshold);
}
void dBodySetAutoDisableAngularThreshold (dBodyID b, dReal angular_average_threshold)
{
- dAASSERT(b);
- b->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold;
+ dAASSERT(b);
+ b->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold;
}
int dBodyGetAutoDisableAverageSamplesCount (dBodyID b)
{
- dAASSERT(b);
- return b->adis.average_samples;
+ dAASSERT(b);
+ return b->adis.average_samples;
}
void dBodySetAutoDisableAverageSamplesCount (dBodyID b, unsigned int average_samples_count)
{
- dAASSERT(b);
- b->adis.average_samples = average_samples_count;
- // update the average buffers
- if(b->average_lvel_buffer)
- {
- delete[] b->average_lvel_buffer;
- b->average_lvel_buffer = 0;
- }
- if(b->average_avel_buffer)
- {
- delete[] b->average_avel_buffer;
- b->average_avel_buffer = 0;
- }
- if(b->adis.average_samples > 0)
- {
- b->average_lvel_buffer = new dVector3[b->adis.average_samples];
- b->average_avel_buffer = new dVector3[b->adis.average_samples];
- }
- else
- {
- b->average_lvel_buffer = 0;
- b->average_avel_buffer = 0;
- }
- // new buffer is empty
- b->average_counter = 0;
- b->average_ready = 0;
+ dAASSERT(b);
+ b->adis.average_samples = average_samples_count;
+ // update the average buffers
+ if(b->average_lvel_buffer)
+ {
+ delete[] b->average_lvel_buffer;
+ b->average_lvel_buffer = 0;
+ }
+ if(b->average_avel_buffer)
+ {
+ delete[] b->average_avel_buffer;
+ b->average_avel_buffer = 0;
+ }
+ if(b->adis.average_samples > 0)
+ {
+ b->average_lvel_buffer = new dVector3[b->adis.average_samples];
+ b->average_avel_buffer = new dVector3[b->adis.average_samples];
+ }
+ else
+ {
+ b->average_lvel_buffer = 0;
+ b->average_avel_buffer = 0;
+ }
+ // new buffer is empty
+ b->average_counter = 0;
+ b->average_ready = 0;
}
int dBodyGetAutoDisableSteps (dBodyID b)
{
- dAASSERT(b);
- return b->adis.idle_steps;
+ dAASSERT(b);
+ return b->adis.idle_steps;
}
void dBodySetAutoDisableSteps (dBodyID b, int steps)
{
- dAASSERT(b);
- b->adis.idle_steps = steps;
+ dAASSERT(b);
+ b->adis.idle_steps = steps;
}
dReal dBodyGetAutoDisableTime (dBodyID b)
{
- dAASSERT(b);
- return b->adis.idle_time;
+ dAASSERT(b);
+ return b->adis.idle_time;
}
void dBodySetAutoDisableTime (dBodyID b, dReal time)
{
- dAASSERT(b);
- b->adis.idle_time = time;
+ dAASSERT(b);
+ b->adis.idle_time = time;
}
int dBodyGetAutoDisableFlag (dBodyID b)
{
- dAASSERT(b);
- return ((b->flags & dxBodyAutoDisable) != 0);
+ dAASSERT(b);
+ return ((b->flags & dxBodyAutoDisable) != 0);
}
void dBodySetAutoDisableFlag (dBodyID b, int do_auto_disable)
{
- dAASSERT(b);
- if (!do_auto_disable)
- {
- b->flags &= ~dxBodyAutoDisable;
- // (mg) we should also reset the IsDisabled state to correspond to the DoDisabling flag
- b->flags &= ~dxBodyDisabled;
- b->adis.idle_steps = dWorldGetAutoDisableSteps(b->world);
- b->adis.idle_time = dWorldGetAutoDisableTime(b->world);
- // resetting the average calculations too
- dBodySetAutoDisableAverageSamplesCount(b, dWorldGetAutoDisableAverageSamplesCount(b->world) );
- }
- else
- {
- b->flags |= dxBodyAutoDisable;
- }
+ dAASSERT(b);
+ if (!do_auto_disable)
+ {
+ b->flags &= ~dxBodyAutoDisable;
+ // (mg) we should also reset the IsDisabled state to correspond to the DoDisabling flag
+ b->flags &= ~dxBodyDisabled;
+ b->adis.idle_steps = dWorldGetAutoDisableSteps(b->world);
+ b->adis.idle_time = dWorldGetAutoDisableTime(b->world);
+ // resetting the average calculations too
+ dBodySetAutoDisableAverageSamplesCount(b, dWorldGetAutoDisableAverageSamplesCount(b->world) );
+ }
+ else
+ {
+ b->flags |= dxBodyAutoDisable;
+ }
}
void dBodySetAutoDisableDefaults (dBodyID b)
{
- dAASSERT(b);
- dWorldID w = b->world;
- dAASSERT(w);
- b->adis = w->adis;
- dBodySetAutoDisableFlag (b, w->body_flags & dxBodyAutoDisable);
+ dAASSERT(b);
+ dWorldID w = b->world;
+ dAASSERT(w);
+ b->adis = w->adis;
+ dBodySetAutoDisableFlag (b, w->body_flags & dxBodyAutoDisable);
}
@@ -1250,7 +1250,7 @@ dxJoint * dJointCreateScrew (dWorldID w, dJointGroupID group)
dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group,
- const dContact *c)
+ const dContact *c)
{
dAASSERT (w && c);
dxJointContact *j = (dxJointContact *)
@@ -1421,13 +1421,13 @@ void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2)
dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2");
//dxWorld *world = joint->world;
dUASSERT ( (!body1 || body1->world == world) &&
- (!body2 || body2->world == world),
- "joint and bodies must be in same world");
+ (!body2 || body2->world == world),
+ "joint and bodies must be in same world");
// check if the joint can not be attached to just one body
dUASSERT (!((joint->flags & dJOINT_TWOBODIES) &&
- ((body1 != 0) ^ (body2 != 0))),
- "joint can not be attached to just one body");
+ ((body1 != 0) ^ (body2 != 0))),
+ "joint can not be attached to just one body");
// remove any existing body attachments
if (joint->node[0].body || joint->node[1].body) {
@@ -1536,25 +1536,6 @@ void dJointSetScrewThreadPitch (dxJoint *joint, dReal thread_pitch)
}
}
-void dJointSetDamping (dxJoint *joint, dReal damping)
-{
- dAASSERT (joint);
-
- if (joint->type() == dJointTypeHinge || joint->type() == dJointTypeSlider ||
- joint->type() == dJointTypeScrew)
- {
- if (!_dequal(damping, 0.0))
- {
- if (damping < 0.0) printf("bad to have negative viscous joint damping, make sure you know what's going on.\n");
- // set use_damping to true
- joint->use_damping = true;
- // damping coefficient is in jicurr->info.damping_coefficient);
- joint->damping_coefficient = damping;
- // FIXME: only hinge, slider, screw(rotational) are implemented at this time, extend?
- }
- }
-}
-
dJointFeedback *dJointGetFeedback (dxJoint *joint)
{
dAASSERT (joint);
@@ -1567,16 +1548,16 @@ dJointID dConnectingJoint (dBodyID in_b1, dBodyID in_b2)
{
dAASSERT (in_b1 || in_b2);
- dBodyID b1, b2;
+ dBodyID b1, b2;
- if (in_b1 == 0) {
- b1 = in_b2;
- b2 = in_b1;
- }
- else {
- b1 = in_b1;
- b2 = in_b2;
- }
+ if (in_b1 == 0) {
+ b1 = in_b2;
+ b2 = in_b1;
+ }
+ else {
+ b1 = in_b1;
+ b2 = in_b2;
+ }
// look through b1's neighbour list for b2
for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
@@ -1593,16 +1574,16 @@ int dConnectingJointList (dBodyID in_b1, dBodyID in_b2, dJointID* out_list)
dAASSERT (in_b1 || in_b2);
- dBodyID b1, b2;
+ dBodyID b1, b2;
- if (in_b1 == 0) {
- b1 = in_b2;
- b2 = in_b1;
- }
- else {
- b1 = in_b1;
- b2 = in_b2;
- }
+ if (in_b1 == 0) {
+ b1 = in_b2;
+ b2 = in_b1;
+ }
+ else {
+ b1 = in_b1;
+ b2 = in_b2;
+ }
// look through b1's neighbour list for b2
int numConnectingJoints = 0;
@@ -1659,23 +1640,19 @@ dxWorld * dWorldCreate()
w->body_flags = 0; // everything disabled
w->wmem = 0;
- for (int jj=0; jj < 1000; jj++)
- {
- w->island_wmems[jj] = 0;
- }
w->adis.idle_steps = 10;
w->adis.idle_time = 0;
- w->adis.average_samples = 1; // Default is 1 sample => Instantaneous velocity
- w->adis.angular_average_threshold = REAL(0.01)*REAL(0.01); // (magnitude squared)
- w->adis.linear_average_threshold = REAL(0.01)*REAL(0.01); // (magnitude squared)
+ w->adis.average_samples = 1; // Default is 1 sample => Instantaneous velocity
+ w->adis.angular_average_threshold = REAL(0.01)*REAL(0.01); // (magnitude squared)
+ w->adis.linear_average_threshold = REAL(0.01)*REAL(0.01); // (magnitude squared)
w->qs.num_iterations = 20;
w->qs.precon_iterations = 0;
w->qs.w = REAL(1.3);
w->qs.num_chunks = 1;
w->qs.num_overlap = 0;
- w->qs.sor_lcp_tolerance = -1;
+ w->qs.pgs_lcp_tolerance = -1;
w->qs.rms_dlambda[0] = 0;
w->qs.rms_dlambda[1] = 0;
w->qs.rms_dlambda[2] = 0;
@@ -1687,9 +1664,12 @@ dxWorld * dWorldCreate()
w->qs.num_contacts = 0;
w->qs.dynamic_inertia_reduction = true;
w->qs.smooth_contacts = 0.01;
+ w->qs.thread_position_correction = false;
w->qs.row_reorder1 = true;
w->qs.warm_start = 0.5;
w->qs.friction_iterations = 10;
+ w->qs.friction_model = pyramid_friction;
+ w->qs.world_solver_type = ODE_DEFAULT;
w->contactp.max_vel = dInfinity;
w->contactp.min_depth = 0;
@@ -2019,8 +1999,8 @@ int dWorldRobustStep(dWorldID w, dReal stepsize)
}
void dWorldImpulseToForce (dWorldID /*w*/, dReal stepsize,
- dReal ix, dReal iy, dReal iz,
- dVector3 force)
+ dReal ix, dReal iy, dReal iz,
+ dVector3 force)
{
//dAASSERT (w);
stepsize = dRecip(stepsize);
@@ -2035,88 +2015,88 @@ void dWorldImpulseToForce (dWorldID /*w*/, dReal stepsize,
dReal dWorldGetAutoDisableLinearThreshold (dWorldID w)
{
- dAASSERT(w);
- return dSqrt (w->adis.linear_average_threshold);
+ dAASSERT(w);
+ return dSqrt (w->adis.linear_average_threshold);
}
void dWorldSetAutoDisableLinearThreshold (dWorldID w, dReal linear_average_threshold)
{
- dAASSERT(w);
- w->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold;
+ dAASSERT(w);
+ w->adis.linear_average_threshold = linear_average_threshold * linear_average_threshold;
}
dReal dWorldGetAutoDisableAngularThreshold (dWorldID w)
{
- dAASSERT(w);
- return dSqrt (w->adis.angular_average_threshold);
+ dAASSERT(w);
+ return dSqrt (w->adis.angular_average_threshold);
}
void dWorldSetAutoDisableAngularThreshold (dWorldID w, dReal angular_average_threshold)
{
- dAASSERT(w);
- w->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold;
+ dAASSERT(w);
+ w->adis.angular_average_threshold = angular_average_threshold * angular_average_threshold;
}
int dWorldGetAutoDisableAverageSamplesCount (dWorldID w)
{
- dAASSERT(w);
- return w->adis.average_samples;
+ dAASSERT(w);
+ return w->adis.average_samples;
}
void dWorldSetAutoDisableAverageSamplesCount (dWorldID w, unsigned int average_samples_count)
{
- dAASSERT(w);
- w->adis.average_samples = average_samples_count;
+ dAASSERT(w);
+ w->adis.average_samples = average_samples_count;
}
int dWorldGetAutoDisableSteps (dWorldID w)
{
- dAASSERT(w);
- return w->adis.idle_steps;
+ dAASSERT(w);
+ return w->adis.idle_steps;
}
void dWorldSetAutoDisableSteps (dWorldID w, int steps)
{
- dAASSERT(w);
- w->adis.idle_steps = steps;
+ dAASSERT(w);
+ w->adis.idle_steps = steps;
}
dReal dWorldGetAutoDisableTime (dWorldID w)
{
- dAASSERT(w);
- return w->adis.idle_time;
+ dAASSERT(w);
+ return w->adis.idle_time;
}
void dWorldSetAutoDisableTime (dWorldID w, dReal time)
{
- dAASSERT(w);
- w->adis.idle_time = time;
+ dAASSERT(w);
+ w->adis.idle_time = time;
}
int dWorldGetAutoDisableFlag (dWorldID w)
{
- dAASSERT(w);
- return w->body_flags & dxBodyAutoDisable;
+ dAASSERT(w);
+ return w->body_flags & dxBodyAutoDisable;
}
void dWorldSetAutoDisableFlag (dWorldID w, int do_auto_disable)
{
- dAASSERT(w);
- if (do_auto_disable)
- w->body_flags |= dxBodyAutoDisable;
- else
- w->body_flags &= ~dxBodyAutoDisable;
+ dAASSERT(w);
+ if (do_auto_disable)
+ w->body_flags |= dxBodyAutoDisable;
+ else
+ w->body_flags &= ~dxBodyAutoDisable;
}
@@ -2203,185 +2183,220 @@ void dWorldSetMaxAngularSpeed(dWorldID w, dReal max_speed)
double dWorldGetQuickStepTolerance (dWorldID w)
{
- dAASSERT(w);
- return w->qs.sor_lcp_tolerance;
+ dAASSERT(w);
+ return w->qs.pgs_lcp_tolerance;
}
void dWorldSetQuickStepTolerance (dWorldID w, dReal tol)
{
- dAASSERT(w);
- w->qs.sor_lcp_tolerance = tol;
+ dAASSERT(w);
+ w->qs.pgs_lcp_tolerance = tol;
}
void dWorldSetQuickStepNumChunks (dWorldID w, int num)
{
- dAASSERT(w);
- w->qs.num_chunks = num;
+ dAASSERT(w);
+ w->qs.num_chunks = num;
}
void dWorldSetQuickStepNumOverlap (dWorldID w, int num)
{
- dAASSERT(w);
- w->qs.num_overlap = num;
+ dAASSERT(w);
+ w->qs.num_overlap = num;
}
void dWorldSetQuickStepNumIterations (dWorldID w, int num)
{
- dAASSERT(w);
- w->qs.num_iterations = num;
+ dAASSERT(w);
+ w->qs.num_iterations = num;
}
void dWorldSetQuickStepPreconIterations (dWorldID w, int num)
{
- dAASSERT(w);
- w->qs.precon_iterations = num;
+ dAASSERT(w);
+ w->qs.precon_iterations = num;
}
int dWorldGetQuickStepPreconIterations (dWorldID w)
{
- dAASSERT(w);
- return w->qs.precon_iterations;
+ dAASSERT(w);
+ return w->qs.precon_iterations;
}
int dWorldGetQuickStepNumIterations (dWorldID w)
{
- dAASSERT(w);
- return w->qs.num_iterations;
+ dAASSERT(w);
+ return w->qs.num_iterations;
}
void dWorldSetRobustStepMaxIterations (dWorldID w, int num)
{
- dAASSERT(w);
- w->rs.max_iterations = num;
+ dAASSERT(w);
+ w->rs.max_iterations = num;
}
int dWorldGetRobustStepMaxIterations (dWorldID w)
{
- dAASSERT(w);
- return w->rs.max_iterations;
+ dAASSERT(w);
+ return w->rs.max_iterations;
}
void dWorldSetQuickStepW (dWorldID w, dReal param)
{
- dAASSERT(w);
- w->qs.w = param;
+ dAASSERT(w);
+ w->qs.w = param;
}
dReal dWorldGetQuickStepW (dWorldID w)
{
- dAASSERT(w);
- return w->qs.w;
+ dAASSERT(w);
+ return w->qs.w;
}
dReal *dWorldGetQuickStepRMSDeltaLambda (dWorldID w)
{
- dAASSERT(w);
- return w->qs.rms_dlambda;
+ dAASSERT(w);
+ return w->qs.rms_dlambda;
}
dReal* dWorldGetQuickStepRMSConstraintResidual (dWorldID w)
{
- dAASSERT(w);
- return w->qs.rms_constraint_residual;
+ dAASSERT(w);
+ return w->qs.rms_constraint_residual;
}
int dWorldGetQuickStepNumContacts (dWorldID w)
{
- dAASSERT(w);
- return w->qs.num_contacts;
+ dAASSERT(w);
+ return w->qs.num_contacts;
}
/* experimental PGS */
bool dWorldGetQuickStepInertiaRatioReduction (dWorldID w)
{
- dAASSERT(w);
+ dAASSERT(w);
return w->qs.dynamic_inertia_reduction;
}
dReal dWorldGetQuickStepContactResidualSmoothing (dWorldID w)
{
- dAASSERT(w);
+ dAASSERT(w);
return w->qs.smooth_contacts;
}
+bool dWorldGetQuickStepThreadPositionCorrection (dWorldID w)
+{
+ dAASSERT(w);
+ return w->qs.thread_position_correction;
+}
+
bool dWorldGetQuickStepExperimentalRowReordering (dWorldID w)
{
- dAASSERT(w);
+ dAASSERT(w);
return w->qs.row_reorder1;
}
dReal dWorldGetQuickStepWarmStartFactor (dWorldID w)
{
- dAASSERT(w);
+ dAASSERT(w);
return w->qs.warm_start;
}
int dWorldGetQuickStepExtraFrictionIterations (dWorldID w)
{
- dAASSERT(w);
+ dAASSERT(w);
return w->qs.friction_iterations;
}
+Friction_Model dWorldGetQuickStepFrictionModel (dWorldID w)
+{
+ dAASSERT(w);
+ return w->qs.friction_model;
+}
+
+World_Solver_Type dWorldGetWorldStepSolverType (dWorldID w)
+{
+ dAASSERT(w);
+ return w->qs.world_solver_type;
+}
+
void dWorldSetQuickStepInertiaRatioReduction (dWorldID w, bool irr)
{
- dAASSERT(w);
+ dAASSERT(w);
w->qs.dynamic_inertia_reduction = irr;
}
void dWorldSetQuickStepContactResidualSmoothing (dWorldID w, dReal smoo)
{
- dAASSERT(w);
+ dAASSERT(w);
w->qs.smooth_contacts = smoo;
}
+void dWorldSetQuickStepThreadPositionCorrection (dWorldID w, bool thread)
+{
+ dAASSERT(w);
+ w->qs.thread_position_correction = thread;
+}
+
void dWorldSetQuickStepExperimentalRowReordering (dWorldID w, bool order)
{
- dAASSERT(w);
+ dAASSERT(w);
w->qs.row_reorder1 = order;
}
void dWorldSetQuickStepWarmStartFactor (dWorldID w, dReal warm)
{
- dAASSERT(w);
+ dAASSERT(w);
w->qs.warm_start = warm;
}
void dWorldSetQuickStepExtraFrictionIterations (dWorldID w, int iters)
{
- dAASSERT(w);
+ dAASSERT(w);
w->qs.friction_iterations = iters;
}
+void dWorldSetQuickStepFrictionModel (dWorldID w, Friction_Model fricmodel)
+{
+ dAASSERT(w);
+ w->qs.friction_model = fricmodel;
+}
+void dWorldSetWorldStepSolverType(dWorldID w, World_Solver_Type solvertype)
+{
+ dAASSERT(w);
+ w->qs.world_solver_type= solvertype;
+}
+
void dWorldSetContactMaxCorrectingVel (dWorldID w, dReal vel)
{
- dAASSERT(w);
- w->contactp.max_vel = vel;
+ dAASSERT(w);
+ w->contactp.max_vel = vel;
}
dReal dWorldGetContactMaxCorrectingVel (dWorldID w)
{
- dAASSERT(w);
- return w->contactp.max_vel;
+ dAASSERT(w);
+ return w->contactp.max_vel;
}
void dWorldSetContactSurfaceLayer (dWorldID w, dReal depth)
{
- dAASSERT(w);
- w->contactp.min_depth = depth;
+ dAASSERT(w);
+ w->contactp.min_depth = depth;
}
dReal dWorldGetContactSurfaceLayer (dWorldID w)
{
- dAASSERT(w);
- return w->contactp.min_depth;
+ dAASSERT(w);
+ return w->contactp.min_depth;
}
//****************************************************************************
@@ -2422,25 +2437,25 @@ extern "C" void dTestDataStructures()
dBodyID b1 = body [dRand() % nb];
dBodyID b2 = body [dRand() % nb];
if (b1 != b2) {
- DO(printf ("creating joint, attaching to %p,%p\n",b1,b2));
- joint[nj] = dJointCreateBall (w,0);
- DO(printf ("\t-->%p\n",joint[nj]));
- checkWorld (w);
- dJointAttach (joint[nj],b1,b2);
- nj++;
- checkWorld (w);
- DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
+ DO(printf ("creating joint, attaching to %p,%p\n",b1,b2));
+ joint[nj] = dJointCreateBall (w,0);
+ DO(printf ("\t-->%p\n",joint[nj]));
+ checkWorld (w);
+ dJointAttach (joint[nj],b1,b2);
+ nj++;
+ checkWorld (w);
+ DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
}
}
if (nj > 0 && nb > 2 && dRandReal() > 0.5) {
dBodyID b1 = body [dRand() % nb];
dBodyID b2 = body [dRand() % nb];
if (b1 != b2) {
- int k = dRand() % nj;
- DO(printf ("reattaching joint %p\n",joint[k]));
- dJointAttach (joint[k],b1,b2);
- checkWorld (w);
- DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
+ int k = dRand() % nj;
+ DO(printf ("reattaching joint %p\n",joint[k]));
+ dJointAttach (joint[k],b1,b2);
+ checkWorld (w);
+ DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
}
}
if (nb > 0 && dRandReal() > 0.5) {
@@ -2512,26 +2527,26 @@ REGISTER_EXTENSION( ODE_EXT_no_debug )
#if dTRIMESH_ENABLED
REGISTER_EXTENSION( ODE_EXT_trimesh )
- // tri-mesh extensions
- #if dTRIMESH_OPCODE
- REGISTER_EXTENSION( ODE_EXT_opcode )
+ // tri-mesh extensions
+ #if dTRIMESH_OPCODE
+ REGISTER_EXTENSION( ODE_EXT_opcode )
- // opcode extensions
- #if dTRIMESH_16BIT_INDICES
- REGISTER_EXTENSION( ODE_OPC_16bit_indices )
- #endif
+ // opcode extensions
+ #if dTRIMESH_16BIT_INDICES
+ REGISTER_EXTENSION( ODE_OPC_16bit_indices )
+ #endif
- #if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
- REGISTER_EXTENSION( ODE_OPC_new_collider )
- #endif
+ #if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
+ REGISTER_EXTENSION( ODE_OPC_new_collider )
+ #endif
- #endif // dTRIMESH_OPCODE
+ #endif // dTRIMESH_OPCODE
- #if dTRIMESH_GIMPACT
- REGISTER_EXTENSION( ODE_EXT_gimpact )
+ #if dTRIMESH_GIMPACT
+ REGISTER_EXTENSION( ODE_EXT_gimpact )
- // gimpact extensions
- #endif
+ // gimpact extensions
+ #endif
#endif // dTRIMESH_ENABLED
@@ -2553,45 +2568,45 @@ REGISTER_EXTENSION( ODE_EXT_mt_collisions )
const char* dGetConfiguration (void)
{
- return ode_configuration;
+ return ode_configuration;
}
// Helper to check for a feature of ODE
int dCheckConfiguration( const char* extension )
{
- const char *start;
- char *where, *terminator;
+ const char *start;
+ char *where, *terminator;
- /* Feature names should not have spaces. */
- where = (char*)strchr(extension, ' ');
- if ( where || *extension == '\0')
- return 1;
+ /* Feature names should not have spaces. */
+ where = (char*)strchr(extension, ' ');
+ if ( where || *extension == '\0')
+ return 1;
- const char* config = dGetConfiguration();
+ const char* config = dGetConfiguration();
- const size_t ext_length = strlen(extension);
+ const size_t ext_length = strlen(extension);
- /* It takes a bit of care to be fool-proof. Don't be fooled by sub-strings, etc. */
- start = config;
- for ( ; ; )
- {
- where = (char*)strstr((const char *) start, extension);
- if (!where)
- break;
+ /* It takes a bit of care to be fool-proof. Don't be fooled by sub-strings, etc. */
+ start = config;
+ for ( ; ; )
+ {
+ where = (char*)strstr((const char *) start, extension);
+ if (!where)
+ break;
- terminator = where + ext_length;
+ terminator = where + ext_length;
- if ( (where == start || *(where - 1) == ' ') &&
- (*terminator == ' ' || *terminator == '\0') )
- {
- return 1;
- }
+ if ( (where == start || *(where - 1) == ' ') &&
+ (*terminator == ' ' || *terminator == '\0') )
+ {
+ return 1;
+ }
- start = terminator;
- }
+ start = terminator;
+ }
- return 0;
+ return 0;
}
diff --git a/deps/opende/src/odeou.cpp b/deps/opende/src/odeou.cpp
index dd10d5f..cf29f57 100644
--- a/deps/opende/src/odeou.cpp
+++ b/deps/opende/src/odeou.cpp
@@ -43,55 +43,55 @@ ODE interface to OU library implementation.
BEGIN_NAMESPACE_OU()
template<>
-const char *const CEnumUnsortedElementArray<EASSERTIONFAILURESEVERITY, AFS__MAX, const char *>::m_aetElementArray[] =
+ODE_API const char *const CEnumUnsortedElementArray<EASSERTIONFAILURESEVERITY, AFS__MAX, const char *>::m_aetElementArray[] =
{
- "assert", // AFS_ASSERT,
- "check", // AFS_CHECK,
+ "assert", // AFS_ASSERT,
+ "check", // AFS_CHECK,
};
static const CEnumUnsortedElementArray<EASSERTIONFAILURESEVERITY, AFS__MAX, const char *> g_aszAssertionFailureSeverityNames;
END_NAMESPACE_OU()
-static void _OU_CONVENTION_CALLBACK ForwardOUAssertionFailure(EASSERTIONFAILURESEVERITY fsFailureSeverity,
- const char *szAssertionExpression, const char *szAssertionFileName, unsigned int uiAssertionSourceLine)
+static void _OU_CONVENTION_CALLBACK ForwardOUAssertionFailure(EASSERTIONFAILURESEVERITY fsFailureSeverity,
+ const char *szAssertionExpression, const char *szAssertionFileName, unsigned int uiAssertionSourceLine)
{
- dDebug(d_ERR_IASSERT, "Assertion failure in OU Library. Kind: %s, expression: \"%s\", file: \"%s\", line: %u",
- g_aszAssertionFailureSeverityNames.Encode(fsFailureSeverity),
- szAssertionExpression, szAssertionFileName, uiAssertionSourceLine);
+ dDebug(d_ERR_IASSERT, "Assertion failure in OU Library. Kind: %s, expression: \"%s\", file: \"%s\", line: %u",
+ g_aszAssertionFailureSeverityNames.Encode(fsFailureSeverity),
+ szAssertionExpression, szAssertionFileName, uiAssertionSourceLine);
}
static void *_OU_CONVENTION_CALLBACK ForwardOUMemoryAlloc(size_t nBlockSize)
{
- return dAlloc(nBlockSize);
+ return dAlloc(nBlockSize);
}
static void *_OU_CONVENTION_CALLBACK ForwardOUMemoryRealloc(void *pv_ExistingBlock, size_t nBlockNewSize)
{
- return dRealloc(pv_ExistingBlock, 0, nBlockNewSize);
+ return dRealloc(pv_ExistingBlock, 0, nBlockNewSize);
}
static void _OU_CONVENTION_CALLBACK ForwardOUMemoryFree(void *pv_ExistingBlock)
{
- return dFree(pv_ExistingBlock, 0);
+ return dFree(pv_ExistingBlock, 0);
}
bool COdeOu::DoOUCustomizations()
{
- CMemoryManagerCustomization::CustomizeMemoryManager(&ForwardOUMemoryAlloc,
- &ForwardOUMemoryRealloc, &ForwardOUMemoryFree);
+ CMemoryManagerCustomization::CustomizeMemoryManager(&ForwardOUMemoryAlloc,
+ &ForwardOUMemoryRealloc, &ForwardOUMemoryFree);
- CAssertionCheckCustomization::CustomizeAssertionChecks(&ForwardOUAssertionFailure);
+ CAssertionCheckCustomization::CustomizeAssertionChecks(&ForwardOUAssertionFailure);
- return true;
+ return true;
}
void COdeOu::UndoOUCustomizations()
{
- CAssertionCheckCustomization::CustomizeAssertionChecks(NULL);
+ CAssertionCheckCustomization::CustomizeAssertionChecks(NULL);
- CMemoryManagerCustomization::CustomizeMemoryManager(NULL, NULL, NULL);
+ CMemoryManagerCustomization::CustomizeMemoryManager(NULL, NULL, NULL);
}
diff --git a/deps/opende/src/quickstep.cpp b/deps/opende/src/quickstep.cpp
index 5cdfb68..31a5f7e 100644
--- a/deps/opende/src/quickstep.cpp
+++ b/deps/opende/src/quickstep.cpp
@@ -33,372 +33,18 @@
#include "lcp.h"
#include "util.h"
+#ifndef _WIN32
#include <sys/time.h>
-
-#ifdef SSE
-#include <xmmintrin.h>
-#define Kf(x) _mm_set_pd((x),(x))
-#endif
-
-
-#undef REPORT_THREAD_TIMING
-#define USE_TPROW
-#undef TIMING
-#undef DEBUG_CONVERGENCE_TOLERANCE
-#undef SHOW_CONVERGENCE
-#define SMOOTH_LAMBDA
-#undef USE_1NORM
-#undef DEBUG_INERTIA_PROPAGATION
-//#define LOCAL_STEPPING // not yet implemented
-//#define PENETRATION_JVERROR_CORRECTION
-//#define POST_UPDATE_CONSTRAINT_VIOLATION_CORRECTION
-
-#undef CHECK_VELOCITY_OBEYS_CONSTRAINT
-
-
-#ifdef USE_TPROW
-// added for threading per constraint rows
-#include <boost/thread/recursive_mutex.hpp>
-#include <boost/bind.hpp>
-#include "ode/odeinit.h"
-#endif
-
-typedef const dReal *dRealPtr;
-typedef dReal *dRealMutablePtr;
-
-//***************************************************************************
-// configuration
-
-// for the SOR and CG methods:
-// warm starting:
-// this definitely help for motor-driven joints.
-// unfortunately it appears to hurt with high-friction contacts
-// using the SOR method. use with care
-
-// for the SOR method:
-// uncomment the following line to determine a new constraint-solving
-// order for each iteration. however, the qsort per iteration is expensive,
-// and the optimal order is somewhat problem dependent.
-// @@@ try the leaf->root ordering.
-
-// #define REORDER_CONSTRAINTS 1
-
-// for the SOR method:
-// uncomment the following line to randomly reorder constraint rows
-// during the solution. depending on the situation, this can help a lot
-// or hardly at all, but it doesn't seem to hurt.
-
-// #define RANDOMLY_REORDER_CONSTRAINTS 1
-#undef LOCK_WHILE_RANDOMLY_REORDER_CONSTRAINTS
-
-/// scale SOR for contact to reduce overshoot in solution for contacts
-/// \TODO: make this a parameter
-#define CONTACT_SOR_SCALE 0.25
-
-// structure for passing variable pointers in SOR_LCP
-struct dxSORLCPParameters {
- dxQuickStepParameters *qs;
- int nStart; // 0
- int nChunkSize;
- int m; // m
- int nb;
- dReal stepsize;
- int* jb;
- const int* findex;
- dRealPtr hi;
- dRealPtr lo;
- dRealPtr invMOI;
- dRealPtr MOI;
- dRealPtr Ad;
- dRealPtr Adcfm;
- dRealPtr Adcfm_precon;
- dRealMutablePtr rhs;
- dRealMutablePtr rhs_erp;
- dRealMutablePtr J;
- dRealMutablePtr caccel;
- dRealMutablePtr caccel_erp;
- dRealMutablePtr lambda;
- dRealMutablePtr lambda_erp;
- dRealMutablePtr iMJ;
- dRealMutablePtr rhs_precon ;
- dRealMutablePtr J_precon ;
- dRealMutablePtr J_orig ;
- dRealMutablePtr cforce ;
- dRealMutablePtr vnew ;
-#ifdef REORDER_CONSTRAINTS
- dRealMutablePtr last_lambda ;
- dRealMutablePtr last_lambda_erp ;
-#endif
-};
-
-
-//****************************************************************************
-// special matrix multipliers
-
-// multiply block of B matrix (q x 6) with 12 dReal per row with C vektor (q)
-static void Multiply1_12q1 (dReal *A, const dReal *B, const dReal *C, int q)
-{
- dIASSERT (q>0 && A && B && C);
-
- dReal a = 0;
- dReal b = 0;
- dReal c = 0;
- dReal d = 0;
- dReal e = 0;
- dReal f = 0;
- dReal s;
-
- for(int i=0, k = 0; i<q; k += 12, i++)
- {
- s = C[i]; //C[i] and B[n+k] cannot overlap because its value has been read into a temporary.
-
- //For the rest of the loop, the only memory dependency (array) is from B[]
- a += B[ k] * s;
- b += B[1+k] * s;
- c += B[2+k] * s;
- d += B[3+k] * s;
- e += B[4+k] * s;
- f += B[5+k] * s;
- }
-
- A[0] = a;
- A[1] = b;
- A[2] = c;
- A[3] = d;
- A[4] = e;
- A[5] = f;
-}
-
-//***************************************************************************
-// testing stuff
-
-#ifdef TIMING
-#define IFTIMING(x) x
-#else
-#define IFTIMING(x) ((void)0)
-#endif
-
-//***************************************************************************
-// various common computations involving the matrix J
-
-// compute iMJ = inv(M)*J'
-
-static void compute_invM_JT (int m, dRealPtr J, dRealMutablePtr iMJ, int *jb,
- dxBody * const *body, dRealPtr invMOI)
-{
- dRealMutablePtr iMJ_ptr = iMJ;
- dRealPtr J_ptr = J;
- for (int i=0; i<m; J_ptr += 12, iMJ_ptr += 12, i++) {
- int b1 = jb[i*2];
- int b2 = jb[i*2+1];
- dReal k1 = body[b1]->invMass;
- for (int j=0; j<3; j++) iMJ_ptr[j] = k1*J_ptr[j];
- const dReal *invMOIrow1 = invMOI + 12*b1;
- dMultiply0_331 (iMJ_ptr + 3, invMOIrow1, J_ptr + 3);
- if (b2 >= 0) {
- dReal k2 = body[b2]->invMass;
- for (int j=0; j<3; j++) iMJ_ptr[j+6] = k2*J_ptr[j+6];
- const dReal *invMOIrow2 = invMOI + 12*b2;
- dMultiply0_331 (iMJ_ptr + 9, invMOIrow2, J_ptr + 9);
- }
- }
-}
-
-// warm starting
-// compute out = inv(M)*J'*in.
-static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
- dRealPtr in, dRealMutablePtr out)
-{
- dSetZero (out,6*nb);
- dRealPtr iMJ_ptr = iMJ;
- for (int i=0; i<m; i++) {
- int b1 = jb[i*2];
- int b2 = jb[i*2+1];
- const dReal in_i = in[i];
- dRealMutablePtr out_ptr = out + b1*6;
- for (int j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in_i;
- iMJ_ptr += 6;
- if (b2 >= 0) {
- out_ptr = out + b2*6;
- for (int j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in_i;
- }
- iMJ_ptr += 6;
- }
-}
-
-// compute out = J*in.
-
-static void multiply_J (int m, dRealPtr J, int *jb,
- dRealPtr in, dRealMutablePtr out)
-{
- dRealPtr J_ptr = J;
- for (int i=0; i<m; i++) {
- int b1 = jb[i*2];
- int b2 = jb[i*2+1];
- dReal sum = 0;
- dRealPtr in_ptr = in + b1*6;
- for (int j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
- J_ptr += 6;
- if (b2 >= 0) {
- in_ptr = in + b2*6;
- for (int j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
- }
- J_ptr += 6;
- out[i] = sum;
- }
-}
-
-
-// compute out = (J*inv(M)*J' + cfm)*in.
-// use z as an nb*6 temporary.
-/* not used
-// warm starting
-static void multiply_J_invM_JT (int m, int nb, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
- dRealPtr cfm, dRealMutablePtr z, dRealMutablePtr in, dRealMutablePtr out)
-{
- multiply_invM_JT (m,nb,iMJ,jb,in,z);
- multiply_J (m,J,jb,z,out);
-
- // add cfm
- for (int i=0; i<m; i++) out[i] += cfm[i] * in[i];
-}
-*/
-
-//***************************************************************************
-// conjugate gradient method with jacobi preconditioner
-// THIS IS EXPERIMENTAL CODE that doesn't work too well, so it is ifdefed out.
-//
-// adding CFM seems to be critically important to this method.
-
-#ifdef USE_CG_LCP
-
-static inline dReal dot (int n, dRealPtr x, dRealPtr y)
-{
- dReal sum=0;
- for (int i=0; i<n; i++) sum += x[i]*y[i];
- return sum;
-}
-
-
-// x = y + z*alpha
-
-static inline void add (int n, dRealMutablePtr x, dRealPtr y, dRealPtr z, dReal alpha)
-{
- for (int i=0; i<n; i++) x[i] = y[i] + z[i]*alpha;
-}
-
-static void CG_LCP (dxWorldProcessContext *context,
- int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
- dRealPtr invMOI, dRealMutablePtr lambda, dRealMutablePtr cforce, dRealMutablePtr rhs,
- dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
- dxQuickStepParameters *qs)
-{
- const int num_iterations = qs->num_iterations;
-
- // precompute iMJ = inv(M)*J'
- dReal *iMJ = context->AllocateArray<dReal> (m*12);
- compute_invM_JT (m,J,iMJ,jb,body,invMOI);
-
- dReal last_rho = 0;
- dReal *r = context->AllocateArray<dReal> (m);
- dReal *z = context->AllocateArray<dReal> (m);
- dReal *p = context->AllocateArray<dReal> (m);
- dReal *q = context->AllocateArray<dReal> (m);
-
- // precompute 1 / diagonals of A
- dReal *Ad = context->AllocateArray<dReal> (m);
- dRealPtr iMJ_ptr = iMJ;
- dRealPtr J_ptr = J;
- for (int i=0; i<m; i++) {
- dReal sum = 0;
- for (int j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
- if (jb[i*2+1] >= 0) {
- for (int j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
- }
- iMJ_ptr += 12;
- J_ptr += 12;
- Ad[i] = REAL(1.0) / (sum + cfm[i]);
- }
-
- if (qs->warm_start > 0)
- {
- // warm start
- // compute residual r = rhs - A*lambda
- multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,cforce,lambda,r);
- for (int k=0; k<m; k++) r[k] = rhs[k] - r[k];
- }
- else
- {
- dSetZero (lambda,m);
- memcpy (r,rhs,m*sizeof(dReal)); // residual r = rhs - A*lambda
- }
-
- for (int iteration=0; iteration < num_iterations; iteration++) {
- for (int i=0; i<m; i++) z[i] = r[i]*Ad[i]; // z = inv(M)*r
- dReal rho = dot (m,r,z); // rho = r'*z
-
- // @@@
- // we must check for convergence, otherwise rho will go to 0 if
- // we get an exact solution, which will introduce NaNs into the equations.
- if (rho < 1e-10) {
- printf ("CG returned at iteration %d\n",iteration);
- break;
- }
-
- if (iteration==0) {
- memcpy (p,z,m*sizeof(dReal)); // p = z
- }
- else {
- add (m,p,z,p,rho/last_rho); // p = z + (rho/last_rho)*p
- }
-
- // compute q = (J*inv(M)*J')*p
- multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,cforce,p,q);
-
- dReal alpha = rho/dot (m,p,q); // alpha = rho/(p'*q)
- add (m,lambda,lambda,p,alpha); // lambda = lambda + alpha*p
- add (m,r,r,q,-alpha); // r = r - alpha*q
- last_rho = rho;
- }
-
- // compute cforce = inv(M)*J'*lambda
- multiply_invM_JT (m,nb,iMJ,jb,lambda,cforce);
-
-#if 0
- // measure solution error
- multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,cforce,lambda,r);
- dReal error = 0;
- for (int i=0; i<m; i++) error += dFabs(r[i] - rhs[i]);
- printf ("lambda error = %10.6e\n",error);
-#endif
-}
-
-#endif
-
-
-struct IndexError {
-#ifdef REORDER_CONSTRAINTS
- dReal error; // error to sort on
- int findex;
+#include <sys/types.h>
+#include <unistd.h>
#endif
- int index; // row index
-};
+#include "quickstep_util.h"
+#include "quickstep_cg_lcp.h"
+#include "quickstep_pgs_lcp.h"
+#include "quickstep_update_bodies.h"
-
-#ifdef REORDER_CONSTRAINTS
-
-static int compare_index_error (const void *a, const void *b)
-{
- const IndexError *i1 = (IndexError*) a;
- const IndexError *i2 = (IndexError*) b;
- if (i1->findex < 0 && i2->findex >= 0) return -1;
- if (i1->findex >= 0 && i2->findex < 0) return 1;
- if (i1->error < i2->error) return -1;
- if (i1->error > i2->error) return 1;
- return 0;
-}
-
-#endif
+using namespace ode;
+using namespace quickstep;
void computeRHSPrecon(dxWorldProcessContext *context, const int m, const int nb,
dRealPtr MOI, dxBody * const *body,
@@ -454,1494 +100,6 @@ void computeRHSPrecon(dxWorldProcessContext *context, const int m, const int nb,
} END_STATE_SAVE(context, tmp2state);
}
-static inline dReal dot6(dRealPtr a, dRealPtr b)
-{
-#ifdef SSE
- __m128d d = _mm_load_pd(a+0) * _mm_load_pd(b+0) + _mm_load_pd(a+2) * _mm_load_pd(b+2) + _mm_load_pd(a+4) * _mm_load_pd(b+4);
- double r[2];
- _mm_store_pd(r, d);
- return r[0] + r[1];
-#else
- return a[0] * b[0] +
- a[1] * b[1] +
- a[2] * b[2] +
- a[3] * b[3] +
- a[4] * b[4] +
- a[5] * b[5];
-#endif
-}
-
-static inline void sum6(dRealMutablePtr a, dReal delta, dRealPtr b)
-{
-#ifdef SSE
- __m128d __delta = Kf(delta);
- _mm_store_pd(a + 0, _mm_load_pd(a + 0) + __delta * _mm_load_pd(b + 0));
- _mm_store_pd(a + 2, _mm_load_pd(a + 2) + __delta * _mm_load_pd(b + 2));
- _mm_store_pd(a + 4, _mm_load_pd(a + 4) + __delta * _mm_load_pd(b + 4));
-#else
- a[0] += delta * b[0];
- a[1] += delta * b[1];
- a[2] += delta * b[2];
- a[3] += delta * b[3];
- a[4] += delta * b[4];
- a[5] += delta * b[5];
-#endif
-}
-
-static void ComputeRows(
-#ifdef SHOW_CONVERGENCE
- int thread_id,
-#else
- int /*thread_id*/,
-#endif
- IndexError* order,
- dxBody* const * /*body*/,
- dxSORLCPParameters params,
- boost::recursive_mutex* /*mutex*/)
-{
-
- #ifdef REPORT_THREAD_TIMING
- struct timeval tv;
- double cur_time;
- gettimeofday(&tv,NULL);
- cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
- //printf("thread %d started at time %f\n",thread_id,cur_time);
- #endif
-
- //boost::recursive_mutex::scoped_lock lock(*mutex); // put in caccel read/writes?
- dxQuickStepParameters *qs = params.qs;
- int startRow = params.nStart; // 0
- int nRows = params.nChunkSize; // m
-#ifdef USE_1NORM
- int m = params.m; // m used for rms error computation
-#endif
- // int nb = params.nb;
-#ifdef PENETRATION_JVERROR_CORRECTION
- dReal stepsize = params.stepsize;
- dRealMutablePtr vnew = params.vnew;
-#endif
- int* jb = params.jb;
- const int* findex = params.findex;
- dRealPtr hi = params.hi;
- dRealPtr lo = params.lo;
- dRealPtr Ad = params.Ad;
- dRealPtr Adcfm = params.Adcfm;
- dRealPtr Adcfm_precon = params.Adcfm_precon;
- dRealMutablePtr rhs = params.rhs;
- dRealMutablePtr rhs_erp = params.rhs_erp;
- dRealMutablePtr J = params.J;
- dRealMutablePtr caccel = params.caccel;
- dRealMutablePtr caccel_erp = params.caccel_erp;
- dRealMutablePtr lambda = params.lambda;
- dRealMutablePtr lambda_erp = params.lambda_erp;
- dRealMutablePtr iMJ = params.iMJ;
- dRealMutablePtr rhs_precon = params.rhs_precon;
- dRealMutablePtr J_precon = params.J_precon;
- dRealMutablePtr J_orig = params.J_orig;
- dRealMutablePtr cforce = params.cforce;
-#ifdef REORDER_CONSTRAINTS
- dRealMutablePtr last_lambda = params.last_lambda;
- dRealMutablePtr last_lambda_erp = params.last_lambda_erp;
-#endif
-
- //printf("iiiiiiiii %d %d %d\n",thread_id,jb[0],jb[1]);
- //for (int i=startRow; i<startRow+nRows; i++) // swap within boundary of our own segment
- // printf("wwwwwwwwwwwww>id %d start %d n %d order[%d].index=%d\n",thread_id,startRow,nRows,i,order[i].index);
-
-
-
- /* DEBUG PRINTOUTS
- // print J_orig
- printf("J_orig\n");
- for (int i=startRow; i<startRow+nRows; i++) {
- for (int j=0; j < 12 ; j++) {
- printf(" %12.6f",J_orig[i*12+j]);
- }
- printf("\n");
- }
- printf("\n");
-
- // print J, J_precon (already premultiplied by inverse of diagonal of LHS) and rhs_precon and rhs
- printf("J_precon\n");
- for (int i=startRow; i<startRow+nRows; i++) {
- for (int j=0; j < 12 ; j++) {
- printf(" %12.6f",J_precon[i*12+j]);
- }
- printf("\n");
- }
- printf("\n");
-
- printf("J\n");
- for (int i=startRow; i<startRow+nRows; i++) {
- for (int j=0; j < 12 ; j++) {
- printf(" %12.6f",J[i*12+j]);
- }
- printf("\n");
- }
- printf("\n");
-
- printf("rhs_precon\n");
- for (int i=startRow; i<startRow+nRows; i++)
- printf(" %12.6f",rhs_precon[i]);
- printf("\n");
-
- printf("rhs\n");
- for (int i=startRow; i<startRow+nRows; i++)
- printf(" %12.6f",rhs[i]);
- printf("\n");
- */
-
- // m_rms_dlambda[3] keeps track of number of constraint
- // rows per type of constraint.
- // m_rms_dlambda[0]: bilateral constraints (findex = -1)
- // m_rms_dlambda[1]: contact normal constraints (findex = -2)
- // rm_ms_dlambda[2]: friction constraints (findex >= 0)
- int m_rms_dlambda[3];
- m_rms_dlambda[0] = 0;
- m_rms_dlambda[1] = 0;
- m_rms_dlambda[2] = 0;
-
- // rms of dlambda
- dReal rms_dlambda[4];
- dSetZero(rms_dlambda, 4);
- // rms of b_i - A_ij \lambda_j as we sweep through rows
- dReal rms_error[4];
- dSetZero(rms_error, 4);
-
- int num_iterations = qs->num_iterations;
- int precon_iterations = qs->precon_iterations;
- dReal sor_lcp_tolerance = qs->sor_lcp_tolerance;
- int friction_iterations = qs->friction_iterations;
- dReal smooth_contacts = qs->smooth_contacts;
-
-#ifdef SHOW_CONVERGENCE
- // show starting lambda
- printf("lambda start: [");
- for (int i=startRow; i<startRow+nRows; i++)
- printf("%f, ", lambda[i]);
- printf("]\n");
-#endif
-
-#ifdef PENETRATION_JVERROR_CORRECTION
- dReal Jvnew_final = 0;
-#endif
- dRealMutablePtr caccel_ptr1;
- dRealMutablePtr caccel_ptr2;
- dRealMutablePtr caccel_erp_ptr1;
- dRealMutablePtr caccel_erp_ptr2;
- dRealMutablePtr cforce_ptr1;
- dRealMutablePtr cforce_ptr2;
- int total_iterations = precon_iterations + num_iterations +
- friction_iterations;
- for (int iteration = 0; iteration < total_iterations; ++iteration)
- {
- // reset rms_dlambda at beginning of iteration
- rms_dlambda[2] = 0;
- // reset rms_error at beginning of iteration
- rms_error[2] = 0;
- m_rms_dlambda[2] = 0;
- if (iteration < num_iterations + precon_iterations)
- {
- // skip resetting rms_dlambda and rms_error for bilateral constraints
- // and contact normals during extra friction iterations.
- rms_dlambda[0] = 0;
- rms_dlambda[1] = 0;
- rms_error[0] = 0;
- rms_error[1] = 0;
- m_rms_dlambda[0] = 0;
- m_rms_dlambda[1] = 0;
- }
-
-#ifdef REORDER_CONSTRAINTS //FIXME: do it for lambda_erp and last_lambda_erp
- // constraints with findex < 0 always come first.
- if (iteration < 2) {
- // for the first two iterations, solve the constraints in
- // the given order
- IndexError *ordercurr = order+startRow;
- for (int i = startRow; i != startRow+nRows; ordercurr++, i++) {
- ordercurr->error = i;
- ordercurr->findex = findex[i];
- ordercurr->index = i;
- }
- }
- else {
- // sort the constraints so that the ones converging slowest
- // get solved last. use the absolute (not relative) error.
- for (int i=startRow; i<startRow+nRows; i++) {
- dReal v1 = dFabs (lambda[i]);
- dReal v2 = dFabs (last_lambda[i]);
- dReal max = (v1 > v2) ? v1 : v2;
- if (max > 0) {
- //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
- order[i].error = dFabs(lambda[i]-last_lambda[i]);
- }
- else {
- order[i].error = dInfinity;
- }
- order[i].findex = findex[i];
- order[i].index = i;
- }
- }
-
- //if (thread_id == 0) for (int i=startRow;i<startRow+nRows;i++) printf("=====> %d %d %d %f %d\n",thread_id,iteration,i,order[i].error,order[i].index);
-
- qsort (order+startRow,nRows,sizeof(IndexError),&compare_index_error);
-
- //@@@ potential optimization: swap lambda and last_lambda pointers rather
- // than copying the data. we must make sure lambda is properly
- // returned to the caller
- memcpy (last_lambda+startRow,lambda+startRow,nRows*sizeof(dReal));
-
- //if (thread_id == 0) for (int i=startRow;i<startRow+nRows;i++) printf("-----> %d %d %d %f %d\n",thread_id,iteration,i,order[i].error,order[i].index);
-
-#endif
-#ifdef RANDOMLY_REORDER_CONSTRAINTS
- if ((iteration & 7) == 0) {
- #ifdef LOCK_WHILE_RANDOMLY_REORDER_CONSTRAINTS
- boost::recursive_mutex::scoped_lock lock(*mutex); // lock for every swap
- #endif
- // int swapi = dRandInt(i+1); // swap across engire matrix
- for (int i=startRow+1; i<startRow+nRows; i++) { // swap within boundary of our own segment
- int swapi = dRandInt(i+1-startRow)+startRow; // swap within boundary of our own segment
- //printf("xxxxxxxx>id %d swaping order[%d].index=%d order[%d].index=%d\n",thread_id,i,order[i].index,swapi,order[swapi].index);
- IndexError tmp = order[i];
- order[i] = order[swapi];
- order[swapi] = tmp;
- }
-
- // {
- // // verify
- // boost::recursive_mutex::scoped_lock lock(*mutex); // lock for every row
- // printf(" random id %d iter %d\n",thread_id,iteration);
- // for (int i=startRow+1; i<startRow+nRows; i++)
- // printf(" %5d,",i);
- // printf("\n");
- // for (int i=startRow+1; i<startRow+nRows; i++)
- // printf(" %5d;",(int)order[i].index);
- // printf("\n");
- // }
- }
-#endif
-
-#ifdef PENETRATION_JVERROR_CORRECTION
- dRealMutablePtr vnew_ptr1;
- dRealMutablePtr vnew_ptr2;
- const dReal stepsize1 = dRecip(stepsize);
- dReal Jvnew = 0;
-#endif
- for (int i=startRow; i<startRow+nRows; i++) {
- //boost::recursive_mutex::scoped_lock lock(*mutex); // lock for every row
-
- // @@@ potential optimization: we could pre-sort J and iMJ, thereby
- // linearizing access to those arrays. hmmm, this does not seem
- // like a win, but we should think carefully about our memory
- // access pattern.
-
- int index = order[i].index;
- int constraint_index = findex[index]; // cache for efficiency
-
- // check if we are doing extra friction_iterations, if so, only solve
- // friction force constraints and nothing else.
- // i.e. skip bilateral and contact normal constraints.
- if (iteration >= (num_iterations + precon_iterations) &&
- constraint_index < 0)
- continue;
-
- dReal delta,delta_erp;
- dReal delta_precon;
-
- {
- int b1 = jb[index*2];
- int b2 = jb[index*2+1];
- caccel_ptr1 = caccel + 6*b1;
- caccel_erp_ptr1 = caccel_erp + 6*b1;
- cforce_ptr1 = cforce + 6*b1;
- if (b2 >= 0)
- {
- caccel_ptr2 = caccel + 6*b2;
- caccel_erp_ptr2 = caccel_erp + 6*b2;
- cforce_ptr2 = cforce + 6*b2;
- }
- else
- {
- caccel_ptr2 = NULL;
- caccel_erp_ptr2 = NULL;
- cforce_ptr2 = NULL;
- }
-#ifdef PENETRATION_JVERROR_CORRECTION
- vnew_ptr1 = vnew + 6*b1;
- vnew_ptr2 = (b2 >= 0) ? vnew + 6*b2 : NULL;
-#endif
- }
-
- dReal old_lambda = lambda[index];
- dReal old_lambda_erp = lambda_erp[index];
-
- //
- // caccel is the constraint accel in the non-precon case
- // cforce is the constraint force in the precon case
- // J_precon and J differs essentially in Ad and Ad_precon,
- // Ad is derived from diagonal of J inv(M) J'
- // Ad_precon is derived from diagonal of J J'
- //
- // caccel_erp is from the non-precon case with erp turned on
- if (iteration < precon_iterations)
- {
- // preconditioning
-
- // update delta_precon
- delta_precon = rhs_precon[index] - old_lambda*Adcfm_precon[index];
-
- dRealPtr J_ptr = J_precon + index*12;
-
- // for preconditioned case, update delta using cforce, not caccel
-
- delta_precon -= dot6(cforce_ptr1, J_ptr);
- if (cforce_ptr2)
- delta_precon -= dot6(cforce_ptr2, J_ptr + 6);
-
- // set the limits for this constraint.
- // this is the place where the QuickStep method differs from the
- // direct LCP solving method, since that method only performs this
- // limit adjustment once per time step, whereas this method performs
- // once per iteration per constraint row.
- // the constraints are ordered so that all lambda[] values needed have
- // already been computed.
- dReal hi_act, lo_act;
- if (constraint_index >= 0) {
- hi_act = dFabs (hi[index] * lambda[constraint_index]);
- lo_act = -hi_act;
- } else {
- hi_act = hi[index];
- lo_act = lo[index];
- }
-
- // compute lambda and clamp it to [lo,hi].
- // @@@ SSE not a win here
-#if 1
- lambda[index] = old_lambda+ delta_precon;
- if (lambda[index] < lo_act) {
- delta_precon = lo_act-old_lambda;
- lambda[index] = lo_act;
- }
- else if (lambda[index] > hi_act) {
- delta_precon = hi_act-old_lambda;
- lambda[index] = hi_act;
- }
-#else
- dReal nl = old_lambda+ delta_precon;
- _mm_store_sd(&nl, _mm_max_sd(_mm_min_sd(_mm_load_sd(&nl),
- _mm_load_sd(&hi_act)), _mm_load_sd(&lo_act)));
- lambda[index] = nl;
- delta_precon = nl - old_lambda;
-#endif
-
- // update cforce (this is strictly for the precon case)
- {
- // for preconditioning case, compute cforce
- // FIXME: need un-altered unscaled J, not J_precon!!
- J_ptr = J_orig + index*12;
-
- // update cforce.
- sum6(cforce_ptr1, delta_precon, J_ptr);
- if (cforce_ptr2)
- sum6(cforce_ptr2, delta_precon, J_ptr + 6);
- }
-
- // record residual (error) (for the non-erp version)
- // given
- // dlambda = sor * (b_i - A_ij * lambda_j)/(A_ii + cfm)
- // define scalar Ad:
- // Ad = sor / (A_ii + cfm)
- // then
- // dlambda = Ad * (b_i - A_ij * lambda_j)
- // thus, to get residual from dlambda,
- // residual = dlambda / Ad
- // or
- // residual = sqrt(sum( Ad2 * dlambda_i * dlambda_i))
- // where Ad2 = 1/(Ad * Ad)
- dReal Ad2 = 0.0;
- if (!_dequal(Ad[index], 0.0))
- {
- // Ad[i] = sor_w / (sum + cfm[i]);
- Ad2 = 1.0 / (Ad[index] * Ad[index]);
- }
- else
- {
- // TODO: Usually, this means qs->w (SOR param) is zero.
- // Residual calculation is wrong when SOR (w) is zero
- // Given SOR is rarely 0, we'll set residual as 0 for now.
- // To do this properly, we should compute dlambda without sor
- // then use the Ad without SOR to back out residual.
- }
-
- dReal delta_precon2 = delta_precon*delta_precon;
- if (constraint_index == -1) // bilateral
- {
- rms_dlambda[0] += delta_precon2;
- rms_error[0] += delta_precon2*Ad2;
- m_rms_dlambda[0]++;
- }
- else if (constraint_index == -2) // contact normal
- {
- rms_dlambda[1] += delta_precon2;
- rms_error[1] += delta_precon2*Ad2;
- m_rms_dlambda[1]++;
- }
- else // friction forces
- {
- rms_dlambda[2] += delta_precon2;
- rms_error[2] += delta_precon2*Ad2;
- m_rms_dlambda[2]++;
- }
-
- old_lambda_erp = old_lambda;
- lambda_erp[index] = lambda[index];
- }
- else
- {
- // NOTE:
- // for this update, we need not throw away J*v(n+1)/h term from rhs
- // ...so adding it back, but remember rhs has already been
- // scaled by Ad_i, so we need to do the same to J*v(n+1)/h
- // but given that J is already scaled by Ad_i, we don't have
- // to do it explicitly here
-
- // delta: erp throttled by info.c_v_max or info.c
- delta =
-#ifdef PENETRATION_JVERROR_CORRECTION
- Jvnew_final +
-#endif
- rhs[index] - old_lambda*Adcfm[index];
- dRealPtr J_ptr = J + index*12;
- delta -= dot6(caccel_ptr1, J_ptr);
- if (caccel_ptr2)
- delta -= dot6(caccel_ptr2, J_ptr + 6);
-
- // delta_erp: unthrottled version compute for rhs with custom erp
- // for rhs_erp note: Adcfm does not have erp because it is on the lhs
- delta_erp = rhs_erp[index] - old_lambda_erp*Adcfm[index];
- delta_erp -= dot6(caccel_erp_ptr1, J_ptr);
- if (caccel_erp_ptr2)
- delta_erp -= dot6(caccel_erp_ptr2, J_ptr + 6);
-
- // set the limits for this constraint.
- // this is the place where the QuickStep method differs from the
- // direct LCP solving method, since that method only performs this
- // limit adjustment once per time step, whereas this method performs
- // once per iteration per constraint row.
- // the constraints are ordered so that all lambda[] values needed have
- // already been computed.
- dReal hi_act, lo_act;
- dReal hi_act_erp, lo_act_erp;
- if (constraint_index >= 0) {
- // FOR erp throttled by info.c_v_max or info.c
- hi_act = dFabs (hi[index] * lambda[constraint_index]);
- lo_act = -hi_act;
- // for the unthrottled _erp version
- hi_act_erp = dFabs (hi[index] * lambda_erp[constraint_index]);
- lo_act_erp = -hi_act_erp;
- } else {
- // FOR erp throttled by info.c_v_max or info.c
- hi_act = hi[index];
- lo_act = lo[index];
- // for the unthrottled _erp version
- hi_act_erp = hi[index];
- lo_act_erp = lo[index];
- }
-
- // compute lambda and clamp it to [lo,hi].
- // @@@ SSE not a win here
-#if 1
- // FOR erp throttled by info.c_v_max or info.c
- lambda[index] = old_lambda + delta;
- if (lambda[index] < lo_act) {
- delta = lo_act-old_lambda;
- lambda[index] = lo_act;
- }
- else if (lambda[index] > hi_act) {
- delta = hi_act-old_lambda;
- lambda[index] = hi_act;
- }
-
- // for the unthrottled _erp version
- lambda_erp[index] = old_lambda_erp + delta_erp;
- if (lambda_erp[index] < lo_act_erp) {
- delta_erp = lo_act_erp-old_lambda_erp;
- lambda_erp[index] = lo_act_erp;
- }
- else if (lambda_erp[index] > hi_act_erp) {
- delta_erp = hi_act_erp-old_lambda_erp;
- lambda_erp[index] = hi_act_erp;
- }
-#else
- // FOR erp throttled by info.c_v_max or info.c
- dReal nl = old_lambda + delta;
- _mm_store_sd(&nl, _mm_max_sd(_mm_min_sd(_mm_load_sd(&nl), _mm_load_sd(&hi_act)), _mm_load_sd(&lo_act)));
- lambda[index] = nl;
- delta = nl - old_lambda;
-
- // for the unthrottled _erp version
- dReal nl = old_lambda_erp + delta_erp;
- _mm_store_sd(&nl, _mm_max_sd(_mm_min_sd(_mm_load_sd(&nl), _mm_load_sd(&hi_act)), _mm_load_sd(&lo_act)));
- lambda_erp[index] = nl;
- delta_erp = nl - old_lambda_erp;
-#endif
-
- // option to smooth lambda
-#ifdef SMOOTH_LAMBDA
- {
- // smooth delta lambda
- // equivalent to first order artificial dissipation on lambda update.
-
- // debug smoothing
- // if (i == 0)
- // printf("rhs[%f] adcfm[%f]: ",rhs[index], Adcfm[index]);
- // if (i == 0)
- // printf("dlambda iter[%d]: ",iteration);
- // printf(" %f ", lambda[index]-old_lambda);
- // if (i == startRow + nRows - 1)
- // printf("\n");
-
- // extra residual smoothing for contact constraints
- // was smoothing both contact normal and friction constraints for VRC
- // if (constraint_index != -1)
- // smooth only lambda for friction directions fails friction_demo.world
- if (constraint_index != -1)
- {
- lambda[index] = (1.0 - smooth_contacts)*lambda[index]
- + smooth_contacts*old_lambda;
- // is filtering lambda_erp necessary?
- // lambda_erp[index] = (1.0 - smooth_contacts)*lambda_erp[index]
- // + smooth_contacts*old_lambda_erp;
- }
- }
-#endif
-
- // update caccel
- {
- // FOR erp throttled by info.c_v_max or info.c
- dRealPtr iMJ_ptr = iMJ + index*12;
-
- // update caccel.
- sum6(caccel_ptr1, delta, iMJ_ptr);
- if (caccel_ptr2)
- sum6(caccel_ptr2, delta, iMJ_ptr + 6);
-
- // update caccel_erp.
- sum6(caccel_erp_ptr1, delta_erp, iMJ_ptr);
- if (caccel_erp_ptr2)
- sum6(caccel_erp_ptr2, delta_erp, iMJ_ptr + 6);
-
-#ifdef PENETRATION_JVERROR_CORRECTION
- // update vnew incrementally
- // add stepsize * delta_caccel to the body velocity
- // vnew = vnew + dt * delta_caccel
- sum6(vnew_ptr1, stepsize*delta, iMJ_ptr);;
- if (caccel_ptr2)
- sum6(vnew_ptr2, stepsize*delta, iMJ_ptr + 6);
-
- // COMPUTE Jvnew = J*vnew/h*Ad
- // but J is already scaled by Ad, and we multiply by h later
- // so it's just Jvnew = J*vnew here
- if (iteration >= num_iterations-7) {
- // check for non-contact bilateral constraints only
- // I've set findex to -2 for contact normal constraint
- if (constraint_index == -1) {
- dRealPtr J_ptr = J + index*12;
- Jvnew = dot6(vnew_ptr1,J_ptr);
- if (caccel_ptr2)
- Jvnew += dot6(vnew_ptr2,J_ptr+6);
- // printf("iter [%d] findex [%d] Jvnew [%f] lo [%f] hi [%f]\n",
- // iteration, constraint_index, Jvnew, lo[index], hi[index]);
- }
- }
- //printf("iter [%d] vnew [%f,%f,%f,%f,%f,%f] Jvnew [%f]\n",
- // iteration,
- // vnew_ptr1[0], vnew_ptr1[1], vnew_ptr1[2],
- // vnew_ptr1[3], vnew_ptr1[4], vnew_ptr1[5],Jvnew);
-#endif
- }
-
- // record residual (error) (for the non-erp version)
- // given
- // dlambda = sor * (b_i - A_ij * lambda_j)/(A_ii + cfm)
- // define scalar Ad:
- // Ad = sor / (A_ii + cfm)
- // then
- // dlambda = Ad * (b_i - A_ij * lambda_j)
- // thus, to get residual from dlambda,
- // residual = dlambda / Ad
- // or
- // residual = sqrt(sum( Ad2 * dlambda_i * dlambda_i))
- // where Ad2 = 1/(Ad * Ad)
- dReal Ad2 = 0.0;
- if (!_dequal(Ad[index], 0.0))
- {
- // Ad[i] = sor_w / (sum + cfm[i]);
- Ad2 = 1.0 / (Ad[index] * Ad[index]);
- }
- else
- {
- // TODO: Usually, this means qs->w (SOR param) is zero.
- // Residual calculation is wrong when SOR (w) is zero
- // Given SOR is rarely 0, we'll set residual as 0 for now.
- // To do this properly, we should compute dlambda without sor
- // then use the Ad without SOR to back out residual.
- }
-
- dReal delta2 = delta*delta;
- if (constraint_index == -1) // bilateral
- {
- rms_dlambda[0] += delta2;
- rms_error[0] += delta2*Ad2;
- m_rms_dlambda[0]++;
- }
- else if (constraint_index == -2) // contact normal
- {
- rms_dlambda[1] += delta2;
- rms_error[1] += delta2*Ad2;
- m_rms_dlambda[1]++;
- }
- else // friction forces
- {
- rms_dlambda[2] += delta2;
- rms_error[2] += delta2*Ad2;
- m_rms_dlambda[2]++;
- }
- }
-
- //@@@ a trick that may or may not help
- //dReal ramp = (1-((dReal)(iteration+1)/(dReal)iterations));
- //delta *= ramp;
-
- } // end of for loop on m
-
-#ifdef PENETRATION_JVERROR_CORRECTION
- Jvnew_final = Jvnew*stepsize1;
- Jvnew_final = Jvnew_final > 1.0 ? 1.0 : ( Jvnew_final < -1.0 ? -1.0 : Jvnew_final );
-#endif
-
- // DO WE NEED TO COMPUTE NORM ACROSS ENTIRE SOLUTION SPACE (0,m)?
- // since local convergence might produce errors in other nodes?
- dReal dlambda_bilateral_mean = rms_dlambda[0]/(dReal)m_rms_dlambda[0];
- dReal dlambda_contact_normal_mean = rms_dlambda[1]/(dReal)m_rms_dlambda[1];
- dReal dlambda_contact_friction_mean = rms_dlambda[2]/(dReal)m_rms_dlambda[2];
- dReal dlambda_total_mean = (rms_dlambda[0] + rms_dlambda[1] + rms_dlambda[2])/
- ((dReal)(m_rms_dlambda[0] + m_rms_dlambda[1] + m_rms_dlambda[2]));
-
- qs->rms_dlambda[0] = sqrt(dlambda_bilateral_mean);
- qs->rms_dlambda[1] = sqrt(dlambda_contact_normal_mean);
- qs->rms_dlambda[2] = sqrt(dlambda_contact_friction_mean);
- qs->rms_dlambda[3] = sqrt(dlambda_total_mean);
-
- dReal residual_bilateral_mean = rms_error[0]/(dReal)m_rms_dlambda[0];
- dReal residual_contact_normal_mean = rms_error[1]/(dReal)m_rms_dlambda[1];
- dReal residual_contact_friction_mean = rms_error[2]/(dReal)m_rms_dlambda[2];
- dReal residual_total_mean = (rms_error[0] + rms_error[1] + rms_error[2])/
- ((dReal)(m_rms_dlambda[0] + m_rms_dlambda[1] + m_rms_dlambda[2]));
-
- qs->rms_constraint_residual[0] = sqrt(residual_bilateral_mean);
- qs->rms_constraint_residual[1] = sqrt(residual_contact_normal_mean);
- qs->rms_constraint_residual[2] = sqrt(residual_contact_friction_mean);
- qs->rms_constraint_residual[3] = sqrt(residual_total_mean);
- qs->num_contacts = m_rms_dlambda[1];
-
- // debugging mutex locking
- //{
- // // verify
- // boost::recursive_mutex::scoped_lock lock(*mutex); // lock for every row
- // printf(" random id %d iter %d\n",thread_id,iteration);
- // for (int i=startRow+1; i<startRow+nRows; i++)
- // printf(" %10d,",i);
- // printf("\n");
- // for (int i=startRow+1; i<startRow+nRows; i++)
- // printf(" %10d;",order[i].index);
- // printf("\n%f %f %f\n",
- // qs->rms_dlambda[0],qs->rms_dlambda[1],qs->rms_dlambda[2]);
- //}
-
-#ifdef SHOW_CONVERGENCE
- /* uncomment for convergence information per row sweep (LOTS OF DATA!)
- printf("MONITOR: thread(%d) iter(%d) rms(%20.18f %20.18f %20.18)f\n",
- thread_id, iteration,
- qs->rms_dlambda[0], qs->rms_dlambda[1], qs->rms_dlambda[2]);
-
- // print lambda
- for (int i=startRow; i<startRow+nRows; i++)
- printf("%f, ", lambda[i]);
- printf("\n");
- */
-#endif
-
- // option to stop when tolerance has been met
- if (iteration >= precon_iterations &&
- qs->rms_constraint_residual[3] < sor_lcp_tolerance)
- {
- #ifdef DEBUG_CONVERGENCE_TOLERANCE
- printf("CONVERGED: id: %d steps: %d,"
- " rms(%20.18f + %20.18f + %20.18f) < tol(%20.18f)\n",
- thread_id, iteration,
- qs->rms_constraint_residual[0], qs->rms_constraint_residual[1],
- qs->rms_constraint_residual[2],
- sor_lcp_tolerance);
- #endif
- // tolerance satisfied, stop iterating
- break;
- }
- else if (iteration >= total_iterations - 1)
- {
- #ifdef DEBUG_CONVERGENCE_TOLERANCE
- printf("WARNING: id: %d did not converge in %d steps,"
- " rms(%20.18f + %20.18f + %20.18f) > tol(%20.18f)\n",
- thread_id, num_iterations,
- qs->rms_constraint_residual[0], qs->rms_constraint_residual[1],
- qs->rms_constraint_residual[2],
- sor_lcp_tolerance);
- #endif
- }
- } // end of for loop on iterations
-
-#ifdef SHOW_CONVERGENCE
- // show starting lambda
- printf("final lambdas: [");
- for (int i=startRow; i<startRow+nRows; i++)
- printf("%f, ", lambda[i]);
- printf("]\n");
- printf("MONITOR: id: %d steps: %d,"
- " dlambda(%20.18f + %20.18f + %20.18f),"
- " rms(%20.18f + %20.18f + %20.18f) < tol(%20.18f)\n",
- thread_id, total_iterations,
- qs->rms_dlambda[0], qs->rms_dlambda[1], qs->rms_dlambda[2],
- qs->rms_constraint_residual[0], qs->rms_constraint_residual[1],
- qs->rms_constraint_residual[2],
- sor_lcp_tolerance);
-#endif
- //printf("vnew: ");
- //for (int i=0; i<6*nb; i++) printf(" %f ",vnew[i]);
- //printf("\n");
-
- #ifdef REPORT_THREAD_TIMING
- gettimeofday(&tv,NULL);
- double end_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
- printf(" quickstep row thread %d start time %f ended time %f duration %f\n",thread_id,cur_time,end_time,end_time - cur_time);
- #endif
-}
-
-//***************************************************************************
-// SOR_LCP method
-//
-// nb is the number of bodies in the body array.
-// J is an m*12 matrix of constraint rows
-// jb is an array of first and second body numbers for each constraint row
-// invMOI is the global frame inverse inertia for each body (stacked 3x3 matrices)
-//
-// this returns lambda and cforce (the constraint force).
-// note: cforce is returned as inv(M)*J'*lambda,
-// the constraint force is actually J'*lambda
-//
-// rhs, lo and hi are modified on exit
-//
-static void SOR_LCP (dxWorldProcessContext *context,
- const int m, const int nb, dRealMutablePtr J, dRealMutablePtr J_precon, dRealMutablePtr J_orig, dRealMutablePtr vnew, int *jb, dxBody * const *body,
- dRealPtr invMOI, dRealPtr MOI, dRealMutablePtr lambda, dRealMutablePtr lambda_erp,
- dRealMutablePtr caccel, dRealMutablePtr caccel_erp, dRealMutablePtr cforce,
- dRealMutablePtr rhs, dRealMutablePtr rhs_erp, dRealMutablePtr rhs_precon,
- dRealPtr lo, dRealPtr hi, dRealPtr cfm, const int *findex,
- dxQuickStepParameters *qs,
-#ifdef USE_TPROW
- boost::threadpool::pool* row_threadpool,
-#endif
- const dReal stepsize)
-{
-
- // precompute iMJ = inv(M)*J'
- dReal *iMJ = context->AllocateArray<dReal> (m*12);
- compute_invM_JT (m,J,iMJ,jb,body,invMOI);
-
- if (qs->warm_start > 0)
- {
- // warm starting
- // compute cforce=(inv(M)*J')*lambda
- if (qs->precon_iterations > 0)
- multiply_invM_JT (m,nb,J,jb,lambda,cforce);
-
- // re-compute caccel=(inv(M)*J')*lambda with new iMJ
- // seems much better than using stored caccel's
- multiply_invM_JT (m,nb,iMJ,jb,lambda,caccel);
- multiply_invM_JT (m,nb,iMJ,jb,lambda_erp,caccel_erp);
- }
- else
- {
- // no warm starting
- if (qs->precon_iterations > 0)
- dSetZero (cforce,nb*6);
- dSetZero (caccel,nb*6);
- dSetZero (caccel_erp,nb*6);
- }
-
- dReal *Ad = context->AllocateArray<dReal> (m);
-
- {
- const dReal sor_w = qs->w; // SOR over-relaxation parameter
- // precompute 1 / diagonals of A
- dRealPtr iMJ_ptr = iMJ;
- dRealPtr J_ptr = J;
- for (int i=0; i<m; J_ptr += 12, iMJ_ptr += 12, i++) {
- dReal sum = 0;
- sum += dot6(iMJ_ptr, J_ptr);
- if (jb[i*2+1] >= 0) {
- sum += dot6(iMJ_ptr+6, J_ptr+6);
- }
- if (findex[i] < 0)
- Ad[i] = sor_w / (sum + cfm[i]);
- else
- Ad[i] = CONTACT_SOR_SCALE * sor_w / (sum + cfm[i]);
- }
- }
-
- // recompute Ad for preconditioned case, Ad_precon is similar to Ad but
- // whereas Ad is 1 over diagonals of J inv(M) J'
- // Ad_precon is 1 over diagonals of J J'
- dReal *Adcfm_precon = NULL;
- if (qs->precon_iterations > 0)
- {
- dReal *Ad_precon = context->AllocateArray<dReal> (m);
-
- {
- const dReal sor_w = qs->w; // SOR over-relaxation parameter
- // precompute 1 / diagonals of A
- // preconditioned version uses J instead of iMJ
- dRealPtr J_ptr = J;
- for (int i=0; i<m; J_ptr += 12, i++) {
- dReal sum = 0;
- sum += dot6(J_ptr, J_ptr);
- if (jb[i*2+1] >= 0) {
- sum += dot6(J_ptr+6, J_ptr+6);
- }
- if (findex[i] < 0)
- Ad_precon[i] = sor_w / (sum + cfm[i]);
- else
- Ad_precon[i] = CONTACT_SOR_SCALE * sor_w / (sum + cfm[i]);
- }
- }
-
- /********************************/
- /* allocate for Adcfm */
- /* which is a mX1 column vector */
- /********************************/
- // compute Adcfm_precon for the preconditioned case
- // do this first before J gets altered (J's diagonals gets premultiplied by Ad)
- // and save a copy of J into J_orig
- // as J becomes J * Ad, J_precon becomes J * Ad_precon
- Adcfm_precon = context->AllocateArray<dReal> (m);
- {
-
- // NOTE: This may seem unnecessary but it's indeed an optimization
- // to move multiplication by Ad[i] and cfm[i] out of iteration loop.
-
- // scale J_precon and rhs_precon by Ad
- // copy J_orig
- dRealMutablePtr J_ptr = J;
- dRealMutablePtr J_orig_ptr = J_orig;
- dRealMutablePtr J_precon_ptr = J_precon;
- for (int i=0; i<m; J_ptr += 12, J_precon_ptr += 12, J_orig_ptr += 12, i++) {
- dReal Ad_precon_i = Ad_precon[i];
- for (int j=0; j<12; j++) {
- J_precon_ptr[j] = J_ptr[j] * Ad_precon_i;
- J_orig_ptr[j] = J_ptr[j]; //copy J
- }
- rhs_precon[i] *= Ad_precon_i;
- // scale Ad by CFM. N.B. this should be done last since it is used above
- Adcfm_precon[i] = Ad_precon_i * cfm[i];
- }
- }
- }
-
- dReal *Adcfm = context->AllocateArray<dReal> (m);
-
-
- {
- // NOTE: This may seem unnecessary but it's indeed an optimization
- // to move multiplication by Ad[i] and cfm[i] out of iteration loop.
-
- // scale J and rhs by Ad
- dRealMutablePtr J_ptr = J;
- for (int i=0; i<m; J_ptr += 12, i++) {
- dReal Ad_i = Ad[i];
- for (int j=0; j<12; j++) {
- J_ptr[j] *= Ad_i;
- }
- rhs[i] *= Ad_i;
- rhs_erp[i] *= Ad_i;
- // scale Ad by CFM. N.B. this should be done last since it is used above
- Adcfm[i] = Ad_i * cfm[i];
- }
- }
-
-
- // order to solve constraint rows in
- IndexError *order = context->AllocateArray<IndexError> (m);
- int *tmpOrder = context->AllocateArray<int> (m);
-
-#ifndef REORDER_CONSTRAINTS
- if (qs->row_reorder1)
- {
- // -1 in front, followed by -2, lastly all the >0
- // Fill the array from both ends
- // where -1 is bilateral, and -2 is friction normal,
- // might be followed by 2 positive tangential indices
- // if friction is not zero.
- // first pass puts -1 in the back
- int front = 0;
- int back = m-1;
- for (int i=0; i<m; ++i) {
- if (findex[i] == -1) {
- tmpOrder[front] = i; // Place them at the front
- ++front;
- } else {
- tmpOrder[back] = i; // Place them at the end
- --back;
- }
- }
- dIASSERT (back - front==1);
- // second pass, put all negatives in the front,
- // should preserver -1 goes before -2,
- // and group all >0 at the end
- front = 0;
- back = m-1;
- for (int i=0; i<m; ++i) {
- if (findex[tmpOrder[i]] < 0) {
- order[front].index = tmpOrder[i]; // Place them at the front
- ++front;
- } else {
- order[back].index = tmpOrder[i]; // Place them at the end
- --back;
- }
- }
- dIASSERT (back - front==1);
- }
- else
- {
- // make sure constraints with findex < 0 come first.
- IndexError *orderhead = order, *ordertail = order + (m - 1);
-
- // Fill the array from both ends
- for (int i=0; i<m; i++) {
- if (findex[i] < 0) {
- orderhead->index = i; // Place them at the front
- ++orderhead;
- } else {
- ordertail->index = i; // Place them at the end
- --ordertail;
- }
- }
- dIASSERT (orderhead-ordertail==1);
- }
-#endif
-
-#ifdef SHOW_CONVERGENCE
- if (0)
- {
- printf("-------------- saved labmdas -------------\n");
- // print current lambdas
- for (int i = 0; i < m; ++i)
- {
- printf("%f, ", lambda[order[i].index]);
- }
- printf("\n");
- for (int i = 0; i < m; ++i)
- {
- printf("%d, ", findex[order[i].index]);
- }
- printf("\n");
- for (int i = 0; i < m; ++i)
- {
- printf("%d, ", order[i].index);
- }
- printf("\n-------------- end of saved labmdas -------------\n");
- }
-#endif
-
-#ifdef REORDER_CONSTRAINTS
- // the lambda computed at the previous iteration.
- // this is used to measure error for when we are reordering the indexes.
- dReal *last_lambda = context->AllocateArray<dReal> (m);
- dReal *last_lambda_erp = context->AllocateArray<dReal> (m);
-#endif
-
- boost::recursive_mutex* mutex = new boost::recursive_mutex();
-
- // number of chunks must be at least 1
- // (single iteration, through all the constraints)
- int num_chunks = qs->num_chunks > 0 ? qs->num_chunks : 1; // min is 1
-
- // prepare pointers for threads
- dxSORLCPParameters *params = new dxSORLCPParameters [num_chunks];
-
- // divide into chunks sequentially
- int chunk = m / num_chunks+1;
- chunk = chunk > 0 ? chunk : 1;
- int thread_id = 0;
-
-
- #ifdef REPORT_THREAD_TIMING
- // timing
- struct timeval tv;
- double cur_time;
- gettimeofday(&tv,NULL);
- cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
- //printf(" quickstep start threads at time %f\n",cur_time);
- #endif
-
-
- IFTIMING (dTimerNow ("start pgs rows"));
- for (int i=0; i<m; i+= chunk,thread_id++)
- {
- //for (int ijk=0;ijk<m;ijk++) printf("thread_id> id:%d jb[%d]=%d\n",thread_id,ijk,jb[ijk]);
-
- int nStart = i - qs->num_overlap < 0 ? 0 : i - qs->num_overlap;
- int nEnd = i + chunk + qs->num_overlap;
- if (nEnd > m) nEnd = m;
- // if every one reorders constraints, this might just work
- // comment out below if using defaults (0 and m) so every thread runs through all joints
- params[thread_id].qs = qs ;
- params[thread_id].nStart = nStart; // 0
- params[thread_id].nChunkSize = nEnd - nStart; // m
- params[thread_id].m = m; // m
- params[thread_id].nb = nb;
- params[thread_id].stepsize = stepsize;
- params[thread_id].jb = jb;
- params[thread_id].findex = findex;
- params[thread_id].hi = hi;
- params[thread_id].lo = lo;
- params[thread_id].invMOI = invMOI;
- params[thread_id].MOI= MOI;
- params[thread_id].Ad = Ad;
- params[thread_id].Adcfm = Adcfm;
- params[thread_id].Adcfm_precon = Adcfm_precon;
- params[thread_id].rhs = rhs;
- params[thread_id].rhs_erp = rhs_erp;
- params[thread_id].J = J;
- params[thread_id].caccel = caccel;
- params[thread_id].caccel_erp = caccel_erp;
- params[thread_id].lambda = lambda;
- params[thread_id].lambda_erp = lambda_erp;
- params[thread_id].iMJ = iMJ;
- params[thread_id].rhs_precon = rhs_precon ;
- params[thread_id].J_precon = J_precon ;
- params[thread_id].J_orig = J_orig ;
- params[thread_id].cforce = cforce ;
- params[thread_id].vnew = vnew ;
-#ifdef REORDER_CONSTRAINTS
- params[thread_id].last_lambda = last_lambda ;
- params[thread_id].last_lambda_erp = last_lambda_erp ;
-#endif
-
-#ifdef DEBUG_CONVERGENCE_TOLERANCE
- printf("thread summary: id %d i %d m %d chunk %d start %d end %d \n",
- thread_id,i,m,chunk,nStart,nEnd);
-#endif
-#ifdef USE_TPROW
- if (row_threadpool && row_threadpool->size() > 0)
- row_threadpool->schedule(boost::bind(ComputeRows,thread_id,order, body, params[thread_id], mutex));
- else //automatically skip threadpool if only 1 thread allocated
- ComputeRows(thread_id,order, body, params[thread_id], mutex);
-#else
- ComputeRows(thread_id,order, body, params[thread_id], mutex);
-#endif
- }
-
-
- // check time for scheduling, this is usually very quick
- //gettimeofday(&tv,NULL);
- //double wait_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
- //printf(" quickstep done scheduling start time %f stopped time %f duration %f\n",cur_time,wait_time,wait_time - cur_time);
-
-#ifdef USE_TPROW
- IFTIMING (dTimerNow ("wait for threads"));
- if (row_threadpool && row_threadpool->size() > 0)
- row_threadpool->wait();
- IFTIMING (dTimerNow ("threads done"));
-#endif
-
-
-
- #ifdef REPORT_THREAD_TIMING
- gettimeofday(&tv,NULL);
- double end_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
- printf(" quickstep threads start time %f stopped time %f duration %f\n",cur_time,end_time,end_time - cur_time);
- #endif
-
- delete [] params;
- delete mutex;
-}
-
-struct dJointWithInfo1
-{
- dxJoint *joint;
- dxJoint::Info1 info;
-};
-
-//***************************************************************************
-// Modifying inertia along constrained axes without modifying dynamics.
-static void DYNAMIC_INERTIA(const int infom, const dxJoint::Info2 &Jinfo, const int b1, const int b2,
- const dJointWithInfo1 *jicurr,
- dRealMutablePtr invMOI, dRealMutablePtr MOI)
-{
- /// INERTIA PROPAGATION ACROSS CONSTRAINED JOINTS
- for (int j=0; j<infom; j++) {
-#ifdef DEBUG_INERTIA_PROPAGATION
- printf("--------JAC---------------\n");
- printf("jacobian [%d] J1l [%f %f %f] J2l [%f %f %f] J1a [%f %f %f]"
- " J2a [%f %f %f]\n", j,
- Jinfo.J1l[0+j*Jinfo.rowskip],
- Jinfo.J1l[1+j*Jinfo.rowskip],
- Jinfo.J1l[2+j*Jinfo.rowskip],
- Jinfo.J2l[0+j*Jinfo.rowskip],
- Jinfo.J2l[1+j*Jinfo.rowskip],
- Jinfo.J2l[2+j*Jinfo.rowskip],
- Jinfo.J1a[0+j*Jinfo.rowskip],
- Jinfo.J1a[1+j*Jinfo.rowskip],
- Jinfo.J1a[2+j*Jinfo.rowskip],
- Jinfo.J2a[0+j*Jinfo.rowskip],
- Jinfo.J2a[1+j*Jinfo.rowskip],
- Jinfo.J2a[2+j*Jinfo.rowskip]);
-#endif
- /// \FIXME: For now, implement only for the two non-free axial rotation
- /// constraints for hinge joints.
- /// this only makes sense if joint connects two dynamic bodies (b2 >= 0)
- /// Skip this entire block if we don't want to modify inertia for stability.
- if (b2 >= 0 && jicurr->joint->type() == dJointTypeHinge &&
- (j == 3 || j == 4))
- {
- /// In hinge joint, pure rotational constraint,
- /// J1l and J2l should be zeros, and J1a and J2a should be equal
- /// and opposite to each other.
- /// J1a or J2a indicates the constrained axis direction.
- /// For this implementation, determine constrained axis(s)
- /// direction from J1a for hinge joints.
-
-
- /// get the moment of inertia (MOI) for parent and child bodies constrained by J1a and J2a.
- /// MOI and invMOI are already in inertial frame (previously rotated by body.posr.R)
- /// get pointers to our invMOI/MOI matrices
- dReal *invMOI_ptr1 = invMOI + b1 * 12;
- dReal *invMOI_ptr2 = invMOI + b2 * 12;
- dReal *MOI_ptr1 = MOI + b1 * 12;
- dReal *MOI_ptr2 = MOI + b2 * 12;
-
- // S: unit vector in the constrained axis direction
- // S is the line along which we want to compute MOI
- // FIXME: check that directions of J1a == J2a
- dVector3 S =
- {Jinfo.J1a[0+j*Jinfo.rowskip],
- Jinfo.J1a[1+j*Jinfo.rowskip],
- Jinfo.J1a[2+j*Jinfo.rowskip] };
- dNormalize3(S);
-
- // temporary vector used for matrix/vector math
- dVector3 tmp31;
-
- // compute scalar MOI in line with S for each body
- // m1 = S' * MOI1 * S
- dMultiply0_133(tmp31, S, MOI_ptr1);
- dReal m1 = dCalcVectorDot3(tmp31, S);
-
- // m2 = S' * MOI2 * S
- dMultiply0_133(tmp31, S, MOI_ptr2);
- dReal m2 = dCalcVectorDot3(tmp31, S);
-
- // identify body with larger inertia
- dReal m_large = m1;
- dReal m_small = m2;
- dReal *MOI_large = MOI_ptr1;
- dReal *MOI_small = MOI_ptr2;
- if (m2 > m1)
- {
- m_large = m2;
- m_small = m1;
- MOI_large = MOI_ptr2;
- MOI_small = MOI_ptr1;
- }
-
- /// get full axis MOI tensor representing the scalar axis MOI.
- // full MOI tensor for S needs matrix outer product of S:
- // SS = [ S * S' ]
- dMatrix3 SS = {
- S[0]*S[0], S[0]*S[1], S[0]*S[2], 0,
- S[1]*S[0], S[1]*S[1], S[1]*S[2], 0,
- S[2]*S[0], S[2]*S[1], S[2]*S[2], 0};
-
-#ifdef DEBUG_INERTIA_PROPAGATION
- printf("--------old MOI-----------\n");
- printf("MOI1[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n", b1,
- MOI_ptr1[0*4+0],MOI_ptr1[0*4+1],MOI_ptr1[0*4+2],MOI_ptr1[0*4+3],
- MOI_ptr1[1*4+0],MOI_ptr1[1*4+1],MOI_ptr1[1*4+2],MOI_ptr1[1*4+3],
- MOI_ptr1[2*4+0],MOI_ptr1[2*4+1],MOI_ptr1[2*4+2],MOI_ptr1[2*4+3]);
- printf("MOI2[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n", b2,
- MOI_ptr2[0*4+0],MOI_ptr2[0*4+1],MOI_ptr2[0*4+2],MOI_ptr2[0*4+3],
- MOI_ptr2[1*4+0],MOI_ptr2[1*4+1],MOI_ptr2[1*4+2],MOI_ptr2[1*4+3],
- MOI_ptr2[2*4+0],MOI_ptr2[2*4+1],MOI_ptr2[2*4+2],MOI_ptr2[2*4+3]);
- printf("--------S VECTORS-----------\n");
- printf("MOI1 b1[%d] S[%f %f %f] = %g\n",b1, S[0], S[1], S[2], m1);
- printf("MOI2 b2[%d] S[%f %f %f] = %g\n",b2, S[0], S[1], S[2], m2);
- printf("--------SS----------------\n");
- printf("SS [%d]\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n", b1,
- SS[0*4+0],SS[0*4+1],SS[0*4+2],SS[0*4+3],
- SS[1*4+0],SS[1*4+1],SS[1*4+2],SS[1*4+3],
- SS[2*4+0],SS[2*4+1],SS[2*4+2],SS[2*4+3]);
-#endif
-
- // define maximum ratio of moment of inertia for adjacent bodies
- // ie. m_large / m_small <= moi_ratio_max
- /// \todo make moi_ratio_max adjustable
- /// \todo automatically adjust moi_ratio_max such that
- /// abs sum of off-diagonals remains smaller than the diagonal
- /// for all rows (see comments below about Gauss-Seidel stability).
- // increase moi_ratio_max to skip checks and increase performance
- const dReal moi_ratio_max = 200.0;
- if (m_large > moi_ratio_max * m_small)
- {
- // Large inertia ratio detected, try reducing it.
- // Increase m_small by dm
- // Reduce m_large by dm
- // such that (m_large - dm) = moi_ratio_max * (m_small + dm)
- // Intermediate math step:
- // m_large - moi_ratio_max * m_small = dm * (1 + moi_ratio_max)
- // Then:
- dReal dm = (m_large - moi_ratio_max * m_small) / (1 + moi_ratio_max);
-
- // This will then be applied to the bodies by multiplying by [S*S']
- // MOI_large -= dm * SS
- // MOI_small += dm * SS
- // But first it should be verified that the change will not
- // destabilize the Gauss-Seidel solver.
- // To verify this, the Generalized Line Criterion for Gauss-Seidel
- // is used, which is stated below:
- //
- // The Gauss-Seidel method will converge if for each row of the matrix:
- // the sum of absolute values of off-diagonal elements
- // is less than absolute value of the diagonal element.
- //
- // @article{Garcia2003,
- // author = {Garcia, M.V.P. and {Humes Jr.}, C. and Stern, J.M.},
- // doi = {10.1590/S0101-82052003000100006},
- // issn = {0101-8205},
- // journal = {Computational \& Applied Mathematics},
- // number = {1},
- // pages = {91--97},
- // title = {{Generalized line criterion for Gauss-Seidel method}},
- // url = {http://www.ime.usp.br/~jstern/papers/papersJS/ghs03.pdf},
- // volume = {22},
- // year = {2003}
- // }
-
- // For 3x3 Gauss-Seidel matrix M, this is equivalent to:
- // abs(M[0][0]) > abs(M[0][1]) + abs(M[0][2])
- // abs(M[1][1]) > abs(M[1][0]) + abs(M[1][2])
- // abs(M[2][2]) > abs(M[2][0]) + abs(M[2][1])
- //
- // Since M is a mass matrix, it is positive definite, which implies
- // that the diagonal elements are strictly positive:
- // M[0][0] > abs(M[0][1]) + abs(M[0][2])
- // M[1][1] > abs(M[1][0]) + abs(M[1][2])
- // M[2][2] > abs(M[2][0]) + abs(M[2][1])
- //
- // For extra safety factor define a parameter gamma >= 1 such that:
- // M[0][0] > gamma * (abs(M[0][1]) + abs(M[0][2]))
- // M[1][1] > gamma * (abs(M[1][0]) + abs(M[1][2]))
- // M[2][2] > gamma * (abs(M[2][0]) + abs(M[2][1]))
- const dReal gamma = 1.0;
-
- int problem = 0;
- for (int row = 0; row < 3; ++row)
- {
- // off-diagonal columns
- int col1 = (row + 1) % 3;
- int col2 = (row + 2) % 3;
-
- // diagonal index
- int id = row*4 + row;
-
- // off-diagonal indices
- int iod1 = row*4 + col1;
- int iod2 = row*4 + col2;
-
- // diagonal element of adjusted M_large
- dReal Md_large = MOI_large[id] - dm*SS[id];
-
- // sum of absolute values of off-diagonal elements of adjusted M_large
- dReal Mod_large = fabs(MOI_large[iod1] - dm*SS[iod1])
- + fabs(MOI_large[iod2] - dm*SS[iod2]);
-
- // diagonal element of adjusted M_small
- dReal Md_small = MOI_small[id] + dm*SS[id];
-
- // sum of absolute values of off-diagonal elements of adjusted M_small
- dReal Mod_small = fabs(MOI_small[iod1] + dm*SS[iod1])
- + fabs(MOI_small[iod2] + dm*SS[iod2]);
-
- if (Md_large <= gamma*Mod_large || Md_small <= gamma*Mod_small)
- {
- problem = 1;
- }
- }
-
- if (problem == 0)
- {
- // Everything looks good, update the inertia matrices
- for (int i = 0; i < 12; ++i)
- {
- int col = i%4;
- if (col == 3)
- {
- // set unused terms to zero
- MOI_large[i] = 0;
- MOI_small[i] = 0;
- }
- else
- {
- MOI_large[i] -= dm * SS[i];
- MOI_small[i] += dm * SS[i];
- }
- }
- }
-
- // Update invMOI by inverting analytically (may not be efficient).
- // try 1981 Ken Miller (http://www.jstor.org/stable/2690437) or
- // (http://math.stackexchange.com/questions/17776)
- // try taking advantage of symmetry of MOI
- dReal det1 = MOI_ptr1[0*4+0]*(MOI_ptr1[2*4+2]*MOI_ptr1[1*4+1]
- -MOI_ptr1[2*4+1]*MOI_ptr1[1*4+2])
- -MOI_ptr1[1*4+0]*(MOI_ptr1[2*4+2]*MOI_ptr1[0*4+1]
- -MOI_ptr1[2*4+1]*MOI_ptr1[0*4+2])
- +MOI_ptr1[2*4+0]*(MOI_ptr1[1*4+2]*MOI_ptr1[0*4+1]
- -MOI_ptr1[1*4+1]*MOI_ptr1[0*4+2]);
- invMOI_ptr1[0*4+0] = (MOI_ptr1[2*4+2]*MOI_ptr1[1*4+1]
- -MOI_ptr1[2*4+1]*MOI_ptr1[1*4+2])/det1;
- invMOI_ptr1[0*4+1] = -(MOI_ptr1[2*4+2]*MOI_ptr1[0*4+1]
- -MOI_ptr1[2*4+1]*MOI_ptr1[0*4+2])/det1;
- invMOI_ptr1[0*4+2] = (MOI_ptr1[1*4+2]*MOI_ptr1[0*4+1]
- -MOI_ptr1[1*4+1]*MOI_ptr1[0*4+2])/det1;
- // invMOI_ptr1[0*4+3] = 0.0;
- invMOI_ptr1[1*4+0] = invMOI_ptr1[0*4+1];
- invMOI_ptr1[1*4+1] = (MOI_ptr1[2*4+2]*MOI_ptr1[0*4+0]
- -MOI_ptr1[2*4+0]*MOI_ptr1[0*4+2])/det1;
- invMOI_ptr1[1*4+2] = -(MOI_ptr1[1*4+2]*MOI_ptr1[0*4+0]
- -MOI_ptr1[1*4+0]*MOI_ptr1[0*4+2])/det1;
- // invMOI_ptr1[1*4+3] = 0.0;
- invMOI_ptr1[2*4+0] = invMOI_ptr1[0*4+2];
- invMOI_ptr1[2*4+1] = invMOI_ptr1[1*4+2];
- invMOI_ptr1[2*4+2] = (MOI_ptr1[1*4+1]*MOI_ptr1[0*4+0]
- -MOI_ptr1[1*4+0]*MOI_ptr1[0*4+1])/det1;
- // invMOI_ptr1[2*4+3] = 0.0;
-
- dReal det2 = MOI_ptr2[0*4+0]*(MOI_ptr2[2*4+2]*MOI_ptr2[1*4+1]
- -MOI_ptr2[2*4+1]*MOI_ptr2[1*4+2])
- -MOI_ptr2[1*4+0]*(MOI_ptr2[2*4+2]*MOI_ptr2[0*4+1]
- -MOI_ptr2[2*4+1]*MOI_ptr2[0*4+2])
- +MOI_ptr2[2*4+0]*(MOI_ptr2[1*4+2]*MOI_ptr2[0*4+1]
- -MOI_ptr2[1*4+1]*MOI_ptr2[0*4+2]);
- invMOI_ptr2[0*4+0] = (MOI_ptr2[2*4+2]*MOI_ptr2[1*4+1]
- -MOI_ptr2[2*4+1]*MOI_ptr2[1*4+2])/det2;
- invMOI_ptr2[0*4+1] = -(MOI_ptr2[2*4+2]*MOI_ptr2[0*4+1]
- -MOI_ptr2[2*4+1]*MOI_ptr2[0*4+2])/det2;
- invMOI_ptr2[0*4+2] = (MOI_ptr2[1*4+2]*MOI_ptr2[0*4+1]
- -MOI_ptr2[1*4+1]*MOI_ptr2[0*4+2])/det2;
- // invMOI_ptr2[0*4+3] = 0.0;
- invMOI_ptr2[1*4+0] = invMOI_ptr2[0*4+1];
- invMOI_ptr2[1*4+1] = (MOI_ptr2[2*4+2]*MOI_ptr2[0*4+0]
- -MOI_ptr2[2*4+0]*MOI_ptr2[0*4+2])/det2;
- invMOI_ptr2[1*4+2] = -(MOI_ptr2[1*4+2]*MOI_ptr2[0*4+0]
- -MOI_ptr2[1*4+0]*MOI_ptr2[0*4+2])/det2;
- // invMOI_ptr2[1*4+3] = 0.0;
- invMOI_ptr2[2*4+0] = invMOI_ptr2[0*4+2];
- invMOI_ptr2[2*4+1] = invMOI_ptr2[1*4+2];
- invMOI_ptr2[2*4+2] = (MOI_ptr2[1*4+1]*MOI_ptr2[0*4+0]
- -MOI_ptr2[1*4+0]*MOI_ptr2[0*4+1])/det2;
- // invMOI_ptr2[2*4+3] = 0.0;
-
- #ifdef DEBUG_INERTIA_PROPAGATION
- printf("---------S Scalars--------\n");
- printf(" original S1 [%g] S2 [%g]\n", m1, m2);
- printf(" distributed S1 [%g] S2 [%g]\n", m1_new, m2_new);
- printf("----------new MOI---------\n");
-
- printf("new MOI1[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n",
- b1,
- MOI_ptr1[0*4+0],MOI_ptr1[0*4+1],MOI_ptr1[0*4+2],MOI_ptr1[0*4+3],
- MOI_ptr1[1*4+0],MOI_ptr1[1*4+1],MOI_ptr1[1*4+2],MOI_ptr1[1*4+3],
- MOI_ptr1[2*4+0],MOI_ptr1[2*4+1],MOI_ptr1[2*4+2],MOI_ptr1[2*4+3]);
-
- // Modify MOI_ptr2
- printf("new MOI2[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n",
- b2,
- MOI_ptr2[0*4+0],MOI_ptr2[0*4+1],MOI_ptr2[0*4+2],MOI_ptr2[0*4+3],
- MOI_ptr2[1*4+0],MOI_ptr2[1*4+1],MOI_ptr2[1*4+2],MOI_ptr2[1*4+3],
- MOI_ptr2[2*4+0],MOI_ptr2[2*4+1],MOI_ptr2[2*4+2],MOI_ptr2[2*4+3]);
-
- // double check resulting MOI along s
- dMultiply0_133(tmp31, S, MOI_ptr1);
- // scalar body 1 MOI component along vector S
- m1 = dCalcVectorDot3(tmp31, S);
- printf("new MOI1 along S [%f]\n", m1);
- dMultiply0_133(tmp31, S, MOI_ptr2);
- // scalar body 2 MOI component along vector S
- m2 = dCalcVectorDot3(tmp31, S);
- printf("new MOI2 along S [%f]\n", m2);
-
- /// \todo double check resulting MOI along joint axis and
- /// see that it's the same
-
- printf("----------new inv---------\n");
- printf("new invMOI1[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n"
- "[%f %f %f %f]\n", b1,
- invMOI_ptr1[0*4+0], invMOI_ptr1[0*4+1],
- invMOI_ptr1[0*4+2], invMOI_ptr1[0*4+3],
- invMOI_ptr1[1*4+0], invMOI_ptr1[1*4+1],
- invMOI_ptr1[1*4+2], invMOI_ptr1[1*4+3],
- invMOI_ptr1[2*4+0], invMOI_ptr1[2*4+1],
- invMOI_ptr1[2*4+2], invMOI_ptr1[2*4+3]);
-
- // Modify invMOI_ptr2
- printf("new invMOI2[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n"
- "[%f %f %f %f]\n", b2,
- invMOI_ptr2[0*4+0], invMOI_ptr2[0*4+1],
- invMOI_ptr2[0*4+2], invMOI_ptr2[0*4+3],
- invMOI_ptr2[1*4+0], invMOI_ptr2[1*4+1],
- invMOI_ptr2[1*4+2], invMOI_ptr2[1*4+3],
- invMOI_ptr2[2*4+0], invMOI_ptr2[2*4+1],
- invMOI_ptr2[2*4+2], invMOI_ptr2[2*4+3]);
- #endif
-
- #ifdef DEBUG_INERTIA_PROPAGATION
- // check if diagonally-dominant
- if (MOI_ptr1[0*4+0] < dFabs(MOI_ptr1[0*4+1])+dFabs(MOI_ptr1[0*4+2]))
- printf(" * new MOI1 row 1 d[%f] < o[%f, %f]\n",
- MOI_ptr1[0*4+0],MOI_ptr1[0*4+1], MOI_ptr1[0*4+2]);
- if (MOI_ptr1[1*4+1] < dFabs(MOI_ptr1[1*4+0])+dFabs(MOI_ptr1[1*4+2]))
- printf(" * new MOI1 row 2 d[%f] < o[%f, %f]\n",
- MOI_ptr1[1*4+1],MOI_ptr1[1*4+0], MOI_ptr1[1*4+2]);
- if (MOI_ptr1[2*4+2] < dFabs(MOI_ptr1[2*4+0])+dFabs(MOI_ptr1[2*4+1]))
- printf(" * new MOI1 row 3 d[%f] < o[%f, %f]\n",
- MOI_ptr1[2*4+2],MOI_ptr1[2*4+0], MOI_ptr1[2*4+1]);
-
- if (MOI_ptr2[0*4+0] < dFabs(MOI_ptr2[0*4+1])+dFabs(MOI_ptr2[0*4+2]))
- printf(" * new MOI2 row 1 d[%f] < o[%f, %f]\n",
- MOI_ptr2[0*4+0],MOI_ptr2[0*4+1], MOI_ptr2[0*4+2]);
- if (MOI_ptr2[1*4+1] < dFabs(MOI_ptr2[1*4+0])+dFabs(MOI_ptr2[1*4+2]))
- printf(" * new MOI2 row 2 d[%f] < o[%f, %f]\n",
- MOI_ptr2[1*4+1],MOI_ptr2[1*4+0], MOI_ptr2[1*4+2]);
- if (MOI_ptr2[2*4+2] < dFabs(MOI_ptr2[2*4+0])+dFabs(MOI_ptr2[2*4+1]))
- printf(" * new MOI2 row 3 d[%f] < o[%f, %f]\n",
- MOI_ptr2[2*4+2],MOI_ptr2[2*4+0], MOI_ptr2[2*4+1]);
- #endif
- }
- }
- }
-}
-
void dxQuickStepper (dxWorldProcessContext *context,
dxWorld *world, dxBody * const *body, int nb,
dxJoint * const *_joint, int _nj, dReal stepsize)
@@ -2079,12 +237,15 @@ void dxQuickStepper (dxWorldProcessContext *context,
// if there are constraints, compute the constraint force
dReal *J = NULL;
+ dReal *Jcopy = NULL;
dReal *J_precon = NULL;
dReal *J_orig = NULL;
int *jb = NULL;
int *findex;
+#ifdef PENETRATION_JVERROR_CORRECTION
dReal *vnew = NULL; // used by PENETRATION_JVERROR_CORRECTION
+#endif
dReal *cforce = context->AllocateArray<dReal> (nb*6);
dReal *caccel = context->AllocateArray<dReal> (nb*6);
@@ -2093,9 +254,19 @@ void dxQuickStepper (dxWorldProcessContext *context,
dReal *caccel_corr = context->AllocateArray<dReal> (nb*6);
#endif
+ // load lambda from the value saved on the previous iteration,
+ // need access to lambda in dxUpdateBodies()
+ dReal *lambda = context->AllocateArray<dReal> (m);
+ dReal *lambda_erp = context->AllocateArray<dReal> (m);
+
+ dSetZero(caccel, nb*6);
+ dSetZero(caccel_erp, nb*6);
+ dSetZero(lambda, m);
+ dSetZero(lambda_erp, m);
+
// Get Joint Information, setup Jacobians by calling getInfo2.
if (m > 0) {
- dReal *cfm, *lo, *hi, *rhs, *rhs_erp, *rhs_precon, *Jcopy;
+ dReal *cfm, *lo, *hi, *rhs, *rhs_erp, *rhs_precon;
dReal *c_v_max;
{
@@ -2185,7 +356,7 @@ void dxQuickStepper (dxWorldProcessContext *context,
const int infom = jicurr->info.m;
// we need a copy of Jacobian for joint feedbacks
- // because it gets destroyed by SOR solver
+ // because it gets destroyed by PGS solver
// instead of saving all Jacobian, we can save just rows
// for joints, that requested feedback (which is normally much less)
if (joint->feedback) {
@@ -2221,7 +392,13 @@ void dxQuickStepper (dxWorldProcessContext *context,
// double check jb_ptr length
dIASSERT (jb_ptr == jb+2*m);
}
+ //assign J to J_orig
+ memcpy(J_orig, J, 12*m*sizeof(dReal));
}
+#ifdef HDF5_INSTRUMENT
+ IFTIMING (dTimerNow ("Dump data"));
+ IFDUMP(h5dump_world(DATA_FILE, world, stepsize));
+#endif
BEGIN_STATE_SAVE(context, tmp1state) {
IFTIMING (dTimerNow ("compute rhs"));
@@ -2292,10 +469,6 @@ void dxQuickStepper (dxWorldProcessContext *context,
}
#endif
- // load lambda from the value saved on the previous iteration
- dReal *lambda = context->AllocateArray<dReal> (m);
- dReal *lambda_erp = context->AllocateArray<dReal> (m);
-
// initialize lambda and lambda_erp
if (world->qs.warm_start > 0)
{
@@ -2329,16 +502,21 @@ void dxQuickStepper (dxWorldProcessContext *context,
BEGIN_STATE_SAVE(context, lcpstate) {
IFTIMING (dTimerNow ("solving LCP problem"));
// solve the LCP problem and get lambda and invM*constraint_force
- SOR_LCP (context,m,nb,J,J_precon,J_orig,vnew,jb,body,
+ PGS_LCP (context,m,nb,J,J_precon,J_orig,
+#ifdef PENETRATION_JVERROR_CORRECTION
+ vnew,
+ stepsize,
+#endif
+ jb,body,
invMOI,MOI,lambda,lambda_erp,
caccel,caccel_erp,cforce,
rhs,rhs_erp,rhs_precon,
lo,hi,cfm,findex,
- &world->qs,
+ &world->qs
#ifdef USE_TPROW
- world->row_threadpool,
+ , world->row_threadpool
#endif
- stepsize);
+ );
} END_STATE_SAVE(context, lcpstate);
@@ -2362,335 +540,15 @@ void dxQuickStepper (dxWorldProcessContext *context,
}
}
- // note that the SOR method overwrites rhs and J at this point, so
- // they should not be used again.
- {
- IFTIMING (dTimerNow ("velocity update due to constraint forces"));
- //
- // update new velocity
- // add stepsize * caccel_erp to the body velocity
- //
- const dReal *caccelcurr = caccel_erp;
- dxBody *const *const bodyend = body + nb;
- for (dxBody *const *bodycurr = body; bodycurr != bodyend; caccelcurr+=6, bodycurr++) {
- dxBody *b_ptr = *bodycurr;
- for (int j=0; j<3; j++) {
- b_ptr->lvel[j] += stepsize * caccelcurr[j];
- b_ptr->avel[j] += stepsize * caccelcurr[3+j];
- }
- // printf("caccel [%f %f %f] [%f %f %f]\n"
- // ,caccelcurr[0] ,caccelcurr[1] ,caccelcurr[2]
- // ,caccelcurr[3] ,caccelcurr[4] ,caccelcurr[5]);
- // printf(" vel [%f %f %f] [%f %f %f]\n"
- // ,b_ptr->lvel[0] ,b_ptr->lvel[1] ,b_ptr->lvel[2]
- // ,b_ptr->avel[0] ,b_ptr->avel[1] ,b_ptr->avel[2]);
- }
- }
-
- if (mfb > 0) {
- // force feedback without erp is better
- // straightforward computation of joint constraint forces:
- // multiply related lambdas with respective J' block for joints
- // where feedback was requested
- dReal data[6];
- const dReal *lambdacurr = lambda;
- const dReal *Jcopyrow = Jcopy;
- const dJointWithInfo1 *jicurr = jointiinfos;
- const dJointWithInfo1 *const jiend = jicurr + nj;
- for (; jicurr != jiend; jicurr++) {
- dxJoint *joint = jicurr->joint;
- const int infom = jicurr->info.m;
-
- if (joint->feedback) {
- dJointFeedback *fb = joint->feedback;
- Multiply1_12q1 (data, Jcopyrow, lambdacurr, infom);
- fb->f1[0] = data[0];
- fb->f1[1] = data[1];
- fb->f1[2] = data[2];
- fb->t1[0] = data[3];
- fb->t1[1] = data[4];
- fb->t1[2] = data[5];
-
- if (joint->node[1].body)
- {
- Multiply1_12q1 (data, Jcopyrow+6, lambdacurr, infom);
- fb->f2[0] = data[0];
- fb->f2[1] = data[1];
- fb->f2[2] = data[2];
- fb->t2[0] = data[3];
- fb->t2[1] = data[4];
- fb->t2[2] = data[5];
- }
-
- Jcopyrow += infom * 12;
- }
-
- lambdacurr += infom;
- }
- }
- }
-
- {
- IFTIMING (dTimerNow ("compute velocity update"));
- // compute the velocity update:
- // add stepsize * invM * fe to the body velocity
- const dReal *invMOIrow = invMOI;
- dxBody *const *const bodyend = body + nb;
- for (dxBody *const *bodycurr = body; bodycurr != bodyend; invMOIrow += 12, bodycurr++) {
- dxBody *b_ptr = *bodycurr;
- dReal body_invMass_mul_stepsize = stepsize * b_ptr->invMass;
- for (int j=0; j<3; j++) {
- b_ptr->lvel[j] += body_invMass_mul_stepsize * b_ptr->facc[j];
- b_ptr->tacc[j] *= stepsize;
- }
- dMultiplyAdd0_331 (b_ptr->avel, invMOIrow, b_ptr->tacc);
- // printf("fe [%f %f %f] [%f %f %f]\n"
- // ,b_ptr->facc[0] ,b_ptr->facc[1] ,b_ptr->facc[2]
- // ,b_ptr->tacc[0] ,b_ptr->tacc[1] ,b_ptr->tacc[2]);
- /* DEBUG PRINTOUTS
- printf("uncorrect vel [%f %f %f] [%f %f %f]\n"
- ,b_ptr->lvel[0] ,b_ptr->lvel[1] ,b_ptr->lvel[2]
- ,b_ptr->avel[0] ,b_ptr->avel[1] ,b_ptr->avel[2]);
- */
- }
- }
+ } // if (m>0)
+ dxUpdateBodies(
#ifdef CHECK_VELOCITY_OBEYS_CONSTRAINT
- if (m > 0) {
- BEGIN_STATE_SAVE(context, rmsstate) {
- dReal *vel = context->AllocateArray<dReal>(nb*6);
-
- // CHECK THAT THE UPDATED VELOCITY OBEYS THE CONSTRAINT
- // (this check needs unmodified J)
- // put residual into tmp
- dRealMutablePtr velcurr = vel;
- dxBody* const* bodyend = body + nb;
- for (dxBody* const* bodycurr = body; bodycurr != bodyend; velcurr += 6, bodycurr++) {
- dxBody *b_ptr = *bodycurr;
- for (int j=0; j<3; j++) {
- velcurr[j] = b_ptr->lvel[j];
- velcurr[3+j] = b_ptr->avel[j];
- }
- }
- dReal *tmp = context->AllocateArray<dReal> (m);
- multiply_J (m,J,jb,vel,tmp);
-
- int m_Jv_bilateral = 0;
- int m_Jv_contact = 0;
- int m_Jv_friction = 0;
- dReal Jv_bilateral = 0;
- dReal Jv_contact = 0;
- dReal Jv_friction = 0;
- for (int i=0; i<m; i++)
- {
- if (findex[i] == -1)
- {
- m_Jv_bilateral++;
- Jv_bilateral += dFabs(tmp[i])*dFabs(tmp[i]);
- }
- else if (findex[i] == -2)
- {
- // contact error includes joint limits
- Jv_contact += dFabs(tmp[i])*dFabs(tmp[i]);
- m_Jv_contact++;
- }
- else if (findex[i] >= 0)
- {
- m_Jv_friction++;
- Jv_friction += dFabs(tmp[i])*dFabs(tmp[i]);
- }
-
- // Note: This is not a good measure of constraint error
- // for soft contact, as Jv is not necessarily zero here.
- // Better measure is compute the residual. \\\ TODO
- }
- // printf ("error = %10.6e %10.6e %10.6e\n",
- // error, Jv_bilateral, Jv_contact);
- // world->qs.rms_constraint_residual[0] = sqrt(error/(dReal)m);
-
- dReal residual_bilateral_mean = Jv_bilateral/(dReal)m_Jv_bilateral;
- dReal residual_contact_normal_mean = Jv_contact/(dReal)m_Jv_contact;
- dReal residual_contact_friction_mean = Jv_friction/(dReal)m_Jv_friction;
- dReal residual_total_mean = (Jv_bilateral + Jv_contact + Jv_friction)/
- ((dReal)(m_Jv_bilateral + m_Jv_contact + m_Jv_friction));
-
- world->qs.rms_constraint_residual[0] = sqrt(residual_bilateral_mean);
- world->qs.rms_constraint_residual[1] = sqrt(residual_contact_normal_mean);
- world->qs.rms_constraint_residual[2] = sqrt(residual_contact_friction_mean);
- world->qs.rms_constraint_residual[3] = sqrt(residual_total_mean);
- world->qs.num_contacts = m_Jv_contact;
- } END_STATE_SAVE(context, rmsstate);
- }
+ context, findex, cforce, &world->qs,
#endif
- {
- // update the position and orientation from the new linear/angular velocity
- // (over the given timestep)
- IFTIMING (dTimerNow ("update position"));
- const dReal *caccelcurr = caccel;
- dxBody *const *const bodyend = body + nb;
- for (dxBody *const *bodycurr = body; bodycurr != bodyend;
- caccelcurr += 6, ++bodycurr)
- {
- dxBody *b_ptr = *bodycurr;
- {
- // sum all forces (external and constraint) into facc and tacc
- // so dBodyGetForce and dBodyGetTorque returns total force and torque
- // on the body
- dReal cf[6];
- cf[0] = b_ptr->mass.mass * caccelcurr[0];
- cf[1] = b_ptr->mass.mass * caccelcurr[1];
- cf[2] = b_ptr->mass.mass * caccelcurr[2];
- dMultiply0_331 (cf+3, b_ptr->mass.I, caccelcurr+3);
- for (unsigned int j = 0; j < 3; ++j)
- {
- b_ptr->facc[j] += cf[j];
- b_ptr->tacc[j] += cf[3+j];
- }
- }
- dxStepBody (b_ptr,stepsize);
- }
- }
+ m, mfb, body, nb, jointiinfos, nj, stepsize,
+ lambda, caccel, caccel_erp, Jcopy, invMOI);
- // revert lvel and avel with the non-erp version of caccel
- if (m > 0) {
- dReal erp_removal = 1.00;
- IFTIMING (dTimerNow ("velocity update due to constraint forces"));
- // remove caccel_erp
- const dReal *caccel_erp_curr = caccel_erp;
- const dReal *caccel_curr = caccel;
- dxBody *const *const bodyend = body + nb;
- int debug_count = 0;
- for (dxBody *const *bodycurr = body; bodycurr != bodyend;
- caccel_curr+=6, caccel_erp_curr+=6, bodycurr++, debug_count++) {
- dxBody *b_ptr = *bodycurr;
- for (int j=0; j<3; j++) {
- // dReal v0 = b_ptr->lvel[j];
- // dReal a0 = b_ptr->avel[j];
- dReal dv = erp_removal * stepsize *
- (caccel_curr[j] - caccel_erp_curr[j]);
- dReal da = erp_removal * stepsize *
- (caccel_curr[3+j] - caccel_erp_curr[3+j]);
-
- /* default v removal
- */
- b_ptr->lvel[j] += dv;
- b_ptr->avel[j] += da;
- /* think about minimize J*v somehow without SORLCP...
- */
- /* minimize final velocity test 1,
- if (v0 * dv < 0) {
- if (fabs(v0) < fabs(dv))
- b_ptr->lvel[j] = 0.0;
- else
- b_ptr->lvel[j] += dv;
- }
- if (a0 * da < 0) {
- if (fabs(a0) < fabs(da))
- b_ptr->avel[j] = 0.0;
- else
- b_ptr->avel[j] += da;
- }
- */
-
- /* DEBUG PRINTOUTS, total forces/accel on a body
- printf("nb[%d] m[%d] b[%d] i[%d] v[%f] dv[%f] vf[%f] a[%f] da[%f] af[%f] debug[%f - %f][%f - %f]\n"
- ,nb, m, debug_count, j, v0, dv, b_ptr->lvel[j]
- , a0, da, b_ptr->avel[j]
- ,caccel_curr[j], caccel_erp_curr[j]
- ,caccel_curr[3+j], caccel_erp_curr[3+j]);
- */
- }
- /* DEBUG PRINTOUTS
- printf("corrected vel [%f %f %f] [%f %f %f]\n",
- b_ptr->lvel[0], b_ptr->lvel[1], b_ptr->lvel[2],
- b_ptr->avel[0], b_ptr->avel[1], b_ptr->avel[2]);
- */
- }
-
-#ifdef POST_UPDATE_CONSTRAINT_VIOLATION_CORRECTION
- // ADD CACCEL CORRECTION FROM VELOCITY CONSTRAINT VIOLATION
- BEGIN_STATE_SAVE(context, velstate) {
- dReal *vel = context->AllocateArray<dReal>(nb*6);
-
- // CHECK THAT THE UPDATED VELOCITY OBEYS THE CONSTRAINT
- // (this check needs unmodified J)
- // put residual into tmp
- dRealMutablePtr velcurr = vel;
- //dxBody* const* bodyend = body + nb;
- for (dxBody* const* bodycurr = body; bodycurr != bodyend; velcurr += 6, bodycurr++) {
- dxBody *b_ptr = *bodycurr;
- for (int j=0; j<3; j++) {
- velcurr[j] = b_ptr->lvel[j];
- velcurr[3+j] = b_ptr->avel[j];
- }
- }
- dReal *tmp = context->AllocateArray<dReal> (m);
- multiply_J (m,J,jb,vel,tmp);
-
- // DIRECTLY ADD THE CONSTRAINT VIOLATION TERM (TMP) TO VELOCITY UPDATE
- // add correction term dlambda = J*v(n+1)/dt
- // and caccel += dt*invM*JT*dlambda (dt's cancel out)
- dReal *iMJ = context->AllocateArray<dReal> (m*12);
- compute_invM_JT (m,J,iMJ,jb,body,invMOI);
- // compute caccel_corr=(inv(M)*J')*dlambda, correction term
- // as we change lambda.
- multiply_invM_JT (m,nb,iMJ,jb,tmp,caccel_corr);
-
- } END_STATE_SAVE(context, velstate);
-
- // ADD CACCEL CORRECTION FROM VELOCITY CONSTRAINT VIOLATION
- caccelcurr = caccel;
- const dReal* caccel_corrcurr = caccel_corr;
- for (dxBody *const *bodycurr = body; bodycurr != bodyend; caccel_corrcurr+=6, bodycurr++) {
- dxBody *b_ptr = *bodycurr;
- for (int j=0; j<3; j++) {
- b_ptr->lvel[j] += erp_removal * stepsize * caccel_corrcurr[j];
- b_ptr->avel[j] += erp_removal * stepsize * caccel_corrcurr[3+j];
- }
- }
-#endif
-
-
- }
-
- {
- IFTIMING (dTimerNow ("tidy up"));
- // zero all force accumulators
- dxBody *const *const bodyend = body + nb;
- for (dxBody *const *bodycurr = body; bodycurr != bodyend; bodycurr++) {
- dxBody *b_ptr = *bodycurr;
- dSetZero (b_ptr->facc,3);
- dSetZero (b_ptr->tacc,3);
- }
- }
-
- IFTIMING (dTimerEnd());
- IFTIMING (if (m > 0) dTimerReport (stdout,1));
-}
-
-#ifdef USE_CG_LCP
-static size_t EstimateGR_LCPMemoryRequirements(int m)
-{
- size_t res = dEFFICIENT_SIZE(sizeof(dReal) * 12 * m); // for iMJ
- res += 5 * dEFFICIENT_SIZE(sizeof(dReal) * m); // for r, z, p, q, Ad
- return res;
-}
-#endif
-
-static size_t EstimateSOR_LCPMemoryRequirements(int m,int /*nb*/)
-{
- size_t res = dEFFICIENT_SIZE(sizeof(dReal) * 12 * m); // for iMJ
- res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for Ad
- res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for Adcfm
- res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for Ad_precon
- res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for Adcfm_precon
- res += dEFFICIENT_SIZE(sizeof(IndexError) * m); // for order
- res += dEFFICIENT_SIZE(sizeof(int) * m); // for tmpOrder
-#ifdef REORDER_CONSTRAINTS
- res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for last_lambda
- res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for last_lambda_erp
-#endif
- return res;
}
size_t dxEstimateQuickStepMemoryRequirements (
@@ -2736,7 +594,9 @@ size_t dxEstimateQuickStepMemoryRequirements (
sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * 12 * m); // for J
sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * 12 * m); // for J_precon
sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * 12 * m); // for J_orig
+#ifdef PENETRATION_JVERROR_CORRECTION
sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * 6 * nb); // for vnew
+#endif
sub1_res2 += 3 * dEFFICIENT_SIZE(sizeof(dReal) * m); // for cfm, lo, hi
sub1_res2 += 2 * dEFFICIENT_SIZE(sizeof(dReal) * m); // for rhs, rhs_erp
sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * m); // for rhs_precon
@@ -2766,17 +626,14 @@ size_t dxEstimateQuickStepMemoryRequirements (
sub2_res2 += dEFFICIENT_SIZE(sizeof(dReal) * 12 * m); // for iMJ
#endif
{
- // for SOR_LCP
- size_t sub3_res1 = EstimateSOR_LCPMemoryRequirements(m,nb);
+ // for PGS_LCP
+ size_t sub3_res1 = EstimatePGS_LCPMemoryRequirements(m,nb);
size_t sub3_res2 = 0;
#ifdef CHECK_VELOCITY_OBEYS_CONSTRAINT
{
- // for vel
- size_t sub4_res1 = dEFFICIENT_SIZE(sizeof(dReal) * 6 * nb);
- sub4_res1 += dEFFICIENT_SIZE(sizeof(dReal) * m); // for tmp
- sub4_res1 += dEFFICIENT_SIZE(sizeof(dReal) * 12 * m); // for iMJ
-
+ // for Update Body velocity and position
+ size_t sub4_res1 = dxUpdateBodiesMemoryRequirements(m, nb);
size_t sub4_res2 = 0;
sub3_res2 += (sub4_res1 >= sub4_res2) ? sub4_res1 : sub4_res2;
@@ -2794,5 +651,3 @@ size_t dxEstimateQuickStepMemoryRequirements (
return res;
}
-
-
diff --git a/deps/opende/src/quickstep_cg_lcp.cpp b/deps/opende/src/quickstep_cg_lcp.cpp
new file mode 100644
index 0000000..f035621
--- /dev/null
+++ b/deps/opende/src/quickstep_cg_lcp.cpp
@@ -0,0 +1,142 @@
+/*************************************************************************
+* *
+* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
+* All rights reserved. Email: russ at q12.org Web: www.q12.org *
+* *
+* This library is free software; you can redistribute it and/or *
+* modify it under the terms of EITHER: *
+* (1) The GNU Lesser General Public License as published by the Free *
+* Software Foundation; either version 2.1 of the License, or (at *
+* your option) any later version. The text of the GNU Lesser *
+* General Public License is included with this library in the *
+* file LICENSE.TXT. *
+* (2) The BSD-style license that is included with this library in *
+* the file LICENSE-BSD.TXT. *
+* *
+* This library is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+* *
+*************************************************************************/
+#include <ode/common.h>
+#include <ode/odemath.h>
+#include <ode/rotation.h>
+#include <ode/timer.h>
+#include <ode/error.h>
+#include <ode/matrix.h>
+#include <ode/misc.h>
+#include "config.h"
+#include "objects.h"
+#include "joints/joint.h"
+#include "util.h"
+
+#ifndef _WIN32
+ #include <sys/time.h>
+#endif
+
+#include "quickstep_util.h"
+#include "quickstep_cg_lcp.h"
+using namespace ode;
+
+//***************************************************************************
+// conjugate gradient method with jacobi preconditioner
+// THIS IS EXPERIMENTAL CODE that doesn't work too well, so it is ifdefed out.
+//
+// adding CFM seems to be critically important to this method.
+
+#ifdef USE_CG_LCP
+
+void quickstep::CG_LCP (dxWorldProcessContext *context,
+ int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
+ dRealPtr invMOI, dRealMutablePtr lambda, dRealMutablePtr cforce, dRealMutablePtr rhs,
+ dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
+ dxQuickStepParameters *qs)
+{
+ const int num_iterations = qs->num_iterations;
+
+ // precompute iMJ = inv(M)*J'
+ dReal *iMJ = context->AllocateArray<dReal> (m*12);
+ compute_invM_JT (m,J,iMJ,jb,body,invMOI);
+
+ dReal last_rho = 0;
+ dReal *r = context->AllocateArray<dReal> (m);
+ dReal *z = context->AllocateArray<dReal> (m);
+ dReal *p = context->AllocateArray<dReal> (m);
+ dReal *q = context->AllocateArray<dReal> (m);
+
+ // precompute 1 / diagonals of A
+ dReal *Ad = context->AllocateArray<dReal> (m);
+ dRealPtr iMJ_ptr = iMJ;
+ dRealPtr J_ptr = J;
+ for (int i=0; i<m; i++) {
+ dReal sum = 0;
+ for (int j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
+ if (jb[i*2+1] >= 0) {
+ for (int j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
+ }
+ iMJ_ptr += 12;
+ J_ptr += 12;
+ Ad[i] = REAL(1.0) / (sum + cfm[i]);
+ }
+
+ if (qs->warm_start > 0)
+ {
+ // warm start
+ // compute residual r = rhs - A*lambda
+ multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,cforce,lambda,r);
+ for (int k=0; k<m; k++) r[k] = rhs[k] - r[k];
+ }
+ else
+ {
+ dSetZero (lambda,m);
+ memcpy (r,rhs,m*sizeof(dReal)); // residual r = rhs - A*lambda
+ }
+
+ for (int iteration=0; iteration < num_iterations; iteration++) {
+ for (int i=0; i<m; i++) z[i] = r[i]*Ad[i]; // z = inv(M)*r
+ dReal rho = quickstep::dot_n (m,r,z); // rho = r'*z
+
+ // @@@
+ // we must check for convergence, otherwise rho will go to 0 if
+ // we get an exact solution, which will introduce NaNs into the equations.
+ if (rho < 1e-10) {
+ printf ("CG returned at iteration %d\n",iteration);
+ break;
+ }
+
+ if (iteration==0) {
+ memcpy (p,z,m*sizeof(dReal)); // p = z
+ }
+ else {
+ quickstep::scaled_add (m,p,z,p,rho/last_rho); // p = z + (rho/last_rho)*p
+ }
+
+ // compute q = (J*inv(M)*J')*p
+ multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,cforce,p,q);
+
+ dReal alpha = rho/quickstep::dot_n (m,p,q); // alpha = rho/(p'*q)
+ quickstep::scaled_add (m,lambda,lambda,p,alpha); // lambda = lambda + alpha*p
+ quickstep::scaled_add (m,r,r,q,-alpha); // r = r - alpha*q
+ last_rho = rho;
+ }
+
+ // compute cforce = inv(M)*J'*lambda
+ multiply_invM_JT (m,nb,iMJ,jb,lambda,cforce);
+
+#if 0
+ // measure solution error
+ multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,cforce,lambda,r);
+ dReal error = 0;
+ for (int i=0; i<m; i++) error += dFabs(r[i] - rhs[i]);
+ printf ("lambda error = %10.6e\n",error);
+#endif
+}
+
+size_t quickstep::EstimateGR_LCPMemoryRequirements(int m)
+{
+ size_t res = dEFFICIENT_SIZE(sizeof(dReal) * 12 * m); // for iMJ
+ res += 5 * dEFFICIENT_SIZE(sizeof(dReal) * m); // for r, z, p, q, Ad
+ return res;
+}
+#endif
diff --git a/deps/opende/src/quickstep_cg_lcp.h b/deps/opende/src/quickstep_cg_lcp.h
new file mode 100644
index 0000000..a105ca7
--- /dev/null
+++ b/deps/opende/src/quickstep_cg_lcp.h
@@ -0,0 +1,42 @@
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ at q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#ifndef _ODE_QUICK_STEP_CG_LCP_H_
+#define _ODE_QUICK_STEP_CG_LCP_H_
+
+#include <ode/common.h>
+#include "quickstep_util.h"
+
+namespace ode {
+ namespace quickstep{
+#ifdef USE_CG_LCP
+ void CG_LCP (dxWorldProcessContext *context,
+ int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
+ dRealPtr invMOI, dRealMutablePtr lambda, dRealMutablePtr cforce, dRealMutablePtr rhs,
+ dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
+ dxQuickStepParameters *qs);
+
+ size_t quickstep::EstimateGR_LCPMemoryRequirements(int m);
+#endif
+ } // namespace quickstep
+} // namespace ode
+#endif
\ No newline at end of file
diff --git a/deps/opende/src/quickstep_pgs_lcp.cpp b/deps/opende/src/quickstep_pgs_lcp.cpp
new file mode 100644
index 0000000..f441dee
--- /dev/null
+++ b/deps/opende/src/quickstep_pgs_lcp.cpp
@@ -0,0 +1,1563 @@
+/*************************************************************************
+* *
+* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
+* All rights reserved. Email: russ at q12.org Web: www.q12.org *
+* *
+* This library is free software; you can redistribute it and/or *
+* modify it under the terms of EITHER: *
+* (1) The GNU Lesser General Public License as published by the Free *
+* Software Foundation; either version 2.1 of the License, or (at *
+* your option) any later version. The text of the GNU Lesser *
+* General Public License is included with this library in the *
+* file LICENSE.TXT. *
+* (2) The BSD-style license that is included with this library in *
+* the file LICENSE-BSD.TXT. *
+* *
+* This library is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+* *
+*************************************************************************/
+#include <thread>
+
+#include <ode/common.h>
+#include <ode/odemath.h>
+#include <ode/rotation.h>
+#include <ode/objects.h>
+#include <ode/timer.h>
+#include <ode/error.h>
+#include <ode/matrix.h>
+#include <ode/misc.h>
+#include "config.h"
+#include "objects.h"
+#include "joints/joint.h"
+#include "util.h"
+
+#ifndef _WIN32
+ #include <sys/time.h>
+#endif
+
+#include "quickstep_util.h"
+#include "quickstep_pgs_lcp.h"
+#ifndef TIMING
+#ifdef HDF5_INSTRUMENT
+#define DUMP
+std::vector<dReal> errors;
+#endif //HDF5_INSTRUMENT
+#endif //timing
+
+using namespace ode;
+
+static void* ComputeRows(void *p)
+{
+ dxPGSLCPParameters *params = (dxPGSLCPParameters *)p;
+
+ #ifdef REPORT_THREAD_TIMING
+ int thread_id = params->thread_id;
+ struct timeval tv;
+ double cur_time;
+ gettimeofday(&tv,NULL);
+ cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
+ // printf("thread %d started at time %f\n",thread_id,cur_time);
+ #endif
+
+ IndexError* order = params->order;
+ dxBody* const* body = params->body;
+#ifdef RANDOMLY_REORDER_CONSTRAINTS
+#ifdef LOCK_WHILE_RANDOMLY_REORDER_CONSTRAINTS
+ boost::recursive_mutex* mutex = params->mutex;
+#endif
+#endif
+ bool inline_position_correction = params->inline_position_correction;
+ bool position_correction_thread = params->position_correction_thread;
+
+ dxQuickStepParameters *qs = params->qs;
+ int startRow = params->nStart; // 0
+ int nRows = params->nChunkSize; // m
+#ifdef USE_1NORM
+ int m = params->m; // m used for rms error computation
+#endif
+ int nb = params->nb;
+#ifdef PENETRATION_JVERROR_CORRECTION
+ dReal stepsize = params->stepsize;
+ dRealMutablePtr vnew = params->vnew;
+#endif
+ int* jb = params->jb;
+ const int* findex = params->findex;
+ bool skip_friction = params->skip_friction;
+ dRealPtr hi = params->hi;
+ dRealPtr lo = params->lo;
+ dRealPtr Ad = params->Ad;
+ dRealPtr Adcfm = params->Adcfm;
+ dRealPtr Adcfm_precon = params->Adcfm_precon;
+ dRealPtr J = params->J;
+ dRealPtr iMJ = params->iMJ;
+ dRealPtr rhs_precon = params->rhs_precon;
+ dRealPtr J_precon = params->J_precon;
+ dRealPtr J_orig = params->J_orig;
+ dRealMutablePtr cforce = params->cforce;
+
+ dRealPtr rhs = params->rhs;
+ dRealMutablePtr caccel = params->caccel;
+ dRealMutablePtr lambda = params->lambda;
+
+ /// THREAD_POSITION_CORRECTION
+ dRealPtr rhs_erp = params->rhs_erp;
+ dRealMutablePtr caccel_erp = params->caccel_erp;
+ dRealMutablePtr lambda_erp = params->lambda_erp;
+
+#ifdef REORDER_CONSTRAINTS
+ dRealMutablePtr last_lambda = params->last_lambda;
+#endif
+
+ //printf("iiiiiiiii %d %d %d\n",thread_id,jb[0],jb[1]);
+ //for (int i=startRow; i<startRow+nRows; i++) // swap within boundary of our own segment
+ // printf("wwwwwwwwwwwww>id %d start %d n %d order[%d].index=%d\n",thread_id,startRow,nRows,i,order[i].index);
+
+ /* DEBUG PRINTOUTS
+ // print J_orig
+ printf("J_orig\n");
+ for (int i=startRow; i<startRow+nRows; i++) {
+ for (int j=0; j < 12 ; j++) {
+ printf(" %12.6f",J_orig[i*12+j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+
+ // print J, J_precon (already premultiplied by inverse of diagonal of LHS) and rhs_precon and rhs
+ printf("J_precon\n");
+ for (int i=startRow; i<startRow+nRows; i++) {
+ for (int j=0; j < 12 ; j++) {
+ printf(" %12.6f",J_precon[i*12+j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+
+ printf("J\n");
+ for (int i=startRow; i<startRow+nRows; i++) {
+ for (int j=0; j < 12 ; j++) {
+ printf(" %12.6f",J[i*12+j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+
+ printf("rhs_precon\n");
+ for (int i=startRow; i<startRow+nRows; i++)
+ printf(" %12.6f",rhs_precon[i]);
+ printf("\n");
+
+ printf("rhs\n");
+ for (int i=startRow; i<startRow+nRows; i++)
+ printf(" %12.6f",rhs[i]);
+ printf("\n");
+ */
+
+ // m_rms_dlambda[3] keeps track of number of constraint
+ // rows per type of constraint.
+ // m_rms_dlambda[0]: bilateral constraints (findex = -1)
+ // m_rms_dlambda[1]: contact normal constraints (findex = -2)
+ // rm_ms_dlambda[2]: friction constraints (findex >= 0)
+ int m_rms_dlambda[3];
+ m_rms_dlambda[0] = 0;
+ m_rms_dlambda[1] = 0;
+ m_rms_dlambda[2] = 0;
+
+ // rms of dlambda
+ dReal rms_dlambda[4];
+ dSetZero(rms_dlambda, 4);
+ // rms of b_i - A_ij \lambda_j as we sweep through rows
+ dReal rms_error[4];
+ dSetZero(rms_error, 4);
+
+ int num_iterations = qs->num_iterations;
+ int precon_iterations = qs->precon_iterations;
+ dReal pgs_lcp_tolerance = qs->pgs_lcp_tolerance;
+ int friction_iterations = qs->friction_iterations;
+ Friction_Model friction_model = qs->friction_model;
+ dReal smooth_contacts = qs->smooth_contacts;
+
+#ifdef SHOW_CONVERGENCE
+ // show starting lambda
+ printf("lambda start: [");
+ for (int i=startRow; i<startRow+nRows; i++)
+ printf("%f, ", lambda[i]);
+ printf("]\n");
+#endif
+
+#ifdef PENETRATION_JVERROR_CORRECTION
+ dReal Jvnew_final = 0;
+#endif
+#ifdef HDF5_INSTRUMENT
+ errors.resize(num_iterations + precon_iterations + friction_iterations);
+#endif
+ dRealMutablePtr caccel_ptr1;
+ dRealMutablePtr caccel_ptr2;
+
+ /// THREAD_POSITION_CORRECTION
+ dRealMutablePtr caccel_erp_ptr1;
+ dRealMutablePtr caccel_erp_ptr2;
+
+ dRealMutablePtr cforce_ptr1;
+ dRealMutablePtr cforce_ptr2;
+ int total_iterations = precon_iterations + num_iterations +
+ friction_iterations;
+ for (int iteration = 0; iteration < total_iterations; ++iteration)
+ {
+ // reset rms_dlambda at beginning of iteration
+ rms_dlambda[2] = 0;
+ // reset rms_error at beginning of iteration
+ rms_error[2] = 0;
+ m_rms_dlambda[2] = 0;
+ if (iteration < num_iterations + precon_iterations)
+ {
+ // skip resetting rms_dlambda and rms_error for bilateral constraints
+ // and contact normals during extra friction iterations.
+ rms_dlambda[0] = 0;
+ rms_dlambda[1] = 0;
+ rms_error[0] = 0;
+ rms_error[1] = 0;
+ m_rms_dlambda[0] = 0;
+ m_rms_dlambda[1] = 0;
+ }
+
+#ifdef REORDER_CONSTRAINTS //FIXME: do it for lambda_erp and last_lambda_erp
+ // constraints with findex < 0 always come first.
+ if (iteration < 2) {
+ // for the first two iterations, solve the constraints in
+ // the given order
+ IndexError *ordercurr = order+startRow;
+ for (int i = startRow; i != startRow+nRows; ordercurr++, i++) {
+ ordercurr->error = i;
+ ordercurr->findex = findex[i];
+ ordercurr->index = i;
+ }
+ }
+ else {
+ // sort the constraints so that the ones converging slowest
+ // get solved last. use the absolute (not relative) error.
+ for (int i=startRow; i<startRow+nRows; i++) {
+ dReal v1 = dFabs (lambda[i]);
+ dReal v2 = dFabs (last_lambda[i]);
+ dReal max = (v1 > v2) ? v1 : v2;
+ if (max > 0) {
+ //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
+ order[i].error = dFabs(lambda[i]-last_lambda[i]);
+ }
+ else {
+ order[i].error = dInfinity;
+ }
+ order[i].findex = findex[i];
+ order[i].index = i;
+ }
+ }
+
+ //if (thread_id == 0) for (int i=startRow;i<startRow+nRows;i++) printf("=====> %d %d %d %f %d\n",thread_id,iteration,i,order[i].error,order[i].index);
+
+ qsort (order+startRow,nRows,sizeof(IndexError),&compare_index_error);
+
+ //@@@ potential optimization: swap lambda and last_lambda pointers rather
+ // than copying the data. we must make sure lambda is properly
+ // returned to the caller
+ memcpy (last_lambda+startRow,lambda+startRow,nRows*sizeof(dReal));
+
+ //if (thread_id == 0) for (int i=startRow;i<startRow+nRows;i++) printf("-----> %d %d %d %f %d\n",thread_id,iteration,i,order[i].error,order[i].index);
+
+#endif
+#ifdef RANDOMLY_REORDER_CONSTRAINTS
+ if ((iteration & 7) == 0) {
+ #ifdef LOCK_WHILE_RANDOMLY_REORDER_CONSTRAINTS
+ boost::recursive_mutex::scoped_lock lock(*mutex); // lock for every swap
+ #endif
+ // int swapi = dRandInt(i+1); // swap across engire matrix
+ for (int i=startRow+1; i<startRow+nRows; i++) { // swap within boundary of our own segment
+ int swapi = dRandInt(i+1-startRow)+startRow; // swap within boundary of our own segment
+ //printf("xxxxxxxx>id %d swaping order[%d].index=%d order[%d].index=%d\n",thread_id,i,order[i].index,swapi,order[swapi].index);
+ IndexError tmp = order[i];
+ order[i] = order[swapi];
+ order[swapi] = tmp;
+ }
+
+ // {
+ // // verify
+ // boost::recursive_mutex::scoped_lock lock(*mutex); // lock for every row
+ // printf(" random id %d iter %d\n",thread_id,iteration);
+ // for (int i=startRow+1; i<startRow+nRows; i++)
+ // printf(" %5d,",i);
+ // printf("\n");
+ // for (int i=startRow+1; i<startRow+nRows; i++)
+ // printf(" %5d;",(int)order[i].index);
+ // printf("\n");
+ // }
+ }
+#endif
+
+#ifdef PENETRATION_JVERROR_CORRECTION
+ dRealMutablePtr vnew_ptr1;
+ dRealMutablePtr vnew_ptr2;
+ const dReal stepsize1 = dRecip(stepsize);
+ dReal Jvnew = 0;
+#endif
+ for (int i=startRow; i<startRow+nRows; i++) {
+ //boost::recursive_mutex::scoped_lock lock(*mutex); // lock for every row
+
+ // @@@ potential optimization: we could pre-sort J and iMJ, thereby
+ // linearizing access to those arrays. hmmm, this does not seem
+ // like a win, but we should think carefully about our memory
+ // access pattern.
+
+ int index = order[i].index;
+ int constraint_index = findex[index]; // cache for efficiency
+
+ // check if we are doing extra friction_iterations, if so, only solve
+ // friction force constraints and nothing else.
+ // i.e. skip bilateral and contact normal constraints.
+ if (iteration >= (num_iterations + precon_iterations) &&
+ constraint_index < 0)
+ continue;
+
+
+ dReal delta = 0;
+ dReal delta_precon = 0;
+
+ // THREAD_POSITION_CORRECTION
+ dReal delta_erp = 0;
+ // precon does not support split position correction right now.
+ // dReal delta_precon_erp = 0;
+
+ // setup pointers
+ int b1 = jb[index*2];
+ int b2 = jb[index*2+1];
+
+ // for precon
+ {
+ cforce_ptr1 = cforce + 6*b1;
+ if (b2 >= 0)
+ {
+ cforce_ptr2 = cforce + 6*b2;
+ }
+ else
+ {
+ cforce_ptr2 = NULL;
+ }
+ }
+
+ // for non-precon
+ {
+ caccel_ptr1 = caccel + 6*b1;
+ if (b2 >= 0)
+ {
+ caccel_ptr2 = caccel + 6*b2;
+ }
+ else
+ {
+ caccel_ptr2 = NULL;
+ }
+
+ if (inline_position_correction)
+ {
+ caccel_erp_ptr1 = caccel_erp + 6*b1;
+ if (b2 >= 0)
+ {
+ caccel_erp_ptr2 = caccel_erp + 6*b2;
+ }
+ else
+ {
+ caccel_erp_ptr2 = NULL;
+ }
+ }
+ }
+ dReal old_lambda = lambda[index];
+
+ /// THREAD_POSITION_CORRECTION
+ dReal old_lambda_erp;
+ if (inline_position_correction)
+ old_lambda_erp = lambda_erp[index];
+
+#ifdef PENETRATION_JVERROR_CORRECTION
+ // 4/4 optional pointers for jverror correction
+ vnew_ptr1 = vnew + 6*b1;
+ vnew_ptr2 = (b2 >= 0) ? vnew + 6*b2 : NULL;
+#endif
+
+
+ //
+ // caccel is the constraint accel in the non-precon case
+ // cforce is the constraint force in the precon case
+ // J_precon and J differs essentially in Ad and Ad_precon,
+ // Ad is derived from diagonal of J inv(M) J'
+ // Ad_precon is derived from diagonal of J J'
+ //
+ if (iteration < precon_iterations)
+ {
+ // preconditioning
+
+ // update delta_precon
+ delta_precon = rhs_precon[index] - old_lambda*Adcfm_precon[index];
+
+ dRealPtr J_ptr = J_precon + index*12;
+
+ // for preconditioned case, update delta using cforce, not caccel
+
+ delta_precon -= quickstep::dot6(cforce_ptr1, J_ptr);
+ if (cforce_ptr2)
+ delta_precon -= quickstep::dot6(cforce_ptr2, J_ptr + 6);
+
+ // set the limits for this constraint.
+ // this is the place where the QuickStep method differs from the
+ // direct LCP solving method, since that method only performs this
+ // limit adjustment once per time step, whereas this method performs
+ // once per iteration per constraint row.
+ // the constraints are ordered so that all lambda[] values needed have
+ // already been computed.
+ dReal hi_act, lo_act;
+ if (constraint_index >= 0) {
+ hi_act = dFabs (hi[index] * lambda[constraint_index]);
+ lo_act = -hi_act;
+ } else {
+ hi_act = hi[index];
+ lo_act = lo[index];
+ }
+
+ // compute lambda and clamp it to [lo,hi].
+ // @@@ SSE is used to speed up vector math
+ // operations with gcc compiler when defined
+ // but SSE is not a win here, #undef for now
+#undef SSE_CLAMP
+#ifndef SSE_CLAMP
+ lambda[index] = old_lambda+ delta_precon;
+ if (lambda[index] < lo_act) {
+ delta_precon = lo_act-old_lambda;
+ lambda[index] = lo_act;
+ }
+ else if (lambda[index] > hi_act) {
+ delta_precon = hi_act-old_lambda;
+ lambda[index] = hi_act;
+ }
+#else
+ dReal nl = old_lambda+ delta_precon;
+ _mm_store_sd(&nl, _mm_max_sd(_mm_min_sd(_mm_load_sd(&nl),
+ _mm_load_sd(&hi_act)), _mm_load_sd(&lo_act)));
+ lambda[index] = nl;
+ delta_precon = nl - old_lambda;
+#endif
+
+ // update cforce (this is strictly for the precon case)
+ {
+ // for preconditioning case, compute cforce
+ // FIXME: need un-altered unscaled J, not J_precon!!
+ J_ptr = J_orig + index*12;
+
+ // update cforce.
+ quickstep::sum6(cforce_ptr1, delta_precon, J_ptr);
+ if (cforce_ptr2)
+ quickstep::sum6(cforce_ptr2, delta_precon, J_ptr + 6);
+ }
+
+ // record residual (error) (for the non-erp version)
+ // given
+ // dlambda = sor * (b_i - A_ij * lambda_j)/(A_ii + cfm)
+ // define scalar Ad:
+ // Ad = sor / (A_ii + cfm)
+ // then
+ // dlambda = Ad * (b_i - A_ij * lambda_j)
+ // thus, to get residual from dlambda,
+ // residual = dlambda / Ad
+ // or
+ // residual = sqrt(sum( Ad2 * dlambda_i * dlambda_i))
+ // where Ad2 = 1/(Ad * Ad)
+ dReal Ad2 = 0.0;
+ if (!_dequal(Ad[index], 0.0))
+ {
+ // Ad[i] = sor_w / (sum + cfm[i]);
+ Ad2 = 1.0 / (Ad[index] * Ad[index]);
+ }
+ else
+ {
+ // TODO: Usually, this means qs->w (SOR param) is zero.
+ // Residual calculation is wrong when SOR (w) is zero
+ // Given SOR is rarely 0, we'll set residual as 0 for now.
+ // To do this properly, we should compute dlambda without sor
+ // then use the Ad without SOR to back out residual.
+ }
+
+ dReal delta_precon2 = delta_precon*delta_precon;
+ if (constraint_index == -1) // bilateral
+ {
+ rms_dlambda[0] += delta_precon2;
+ rms_error[0] += delta_precon2*Ad2;
+ m_rms_dlambda[0]++;
+ }
+ else if (constraint_index == -2) // contact normal
+ {
+ rms_dlambda[1] += delta_precon2;
+ rms_error[1] += delta_precon2*Ad2;
+ m_rms_dlambda[1]++;
+ }
+ else // friction forces
+ {
+ rms_dlambda[2] += delta_precon2;
+ rms_error[2] += delta_precon2*Ad2;
+ m_rms_dlambda[2]++;
+ }
+
+ // initialize position correction terms (_erp) with precon results
+ if (inline_position_correction)
+ {
+ old_lambda_erp = old_lambda;
+ lambda_erp[index] = lambda[index];
+ }
+ }
+ else
+ {
+ if (!skip_friction || constraint_index < 0)
+ {
+ // NOTE:
+ // for this update, we need not throw away J*v(n+1)/h term from rhs
+ // ...so adding it back, but remember rhs has already been
+ // scaled by Ad_i, so we need to do the same to J*v(n+1)/h
+ // but given that J is already scaled by Ad_i, we don't have
+ // to do it explicitly here
+
+ // delta: erp throttled by info.c_v_max or info.c
+ delta =
+#ifdef PENETRATION_JVERROR_CORRECTION
+ Jvnew_final +
+#endif
+ rhs[index] - old_lambda*Adcfm[index];
+ dRealPtr J_ptr = J + index*12;
+ delta -= quickstep::dot6(caccel_ptr1, J_ptr);
+ if (caccel_ptr2)
+ delta -= quickstep::dot6(caccel_ptr2, J_ptr + 6);
+
+ if (inline_position_correction)
+ {
+ delta_erp = rhs_erp[index] - old_lambda_erp*Adcfm[index];
+ delta_erp -= quickstep::dot6(caccel_erp_ptr1, J_ptr);
+ if (caccel_ptr2)
+ delta_erp -= quickstep::dot6(caccel_erp_ptr2, J_ptr + 6);
+ }
+
+ // set the limits for this constraint.
+ // this is the place where the QuickStep method differs from the
+ // direct LCP solving method, since that method only performs this
+ // limit adjustment once per time step, whereas this method performs
+ // once per iteration per constraint row.
+ // the constraints are ordered so that all lambda[] values needed have
+ // already been computed.
+ dReal hi_act, lo_act;
+ /// THREAD_POSITION_CORRECTION
+ dReal hi_act_erp, lo_act_erp;
+ if (constraint_index >= 0)
+ {
+ if (friction_model == pyramid_friction)
+ {
+ // FOR erp throttled by info.c_v_max or info.c
+ hi_act = dFabs (hi[index] * lambda[constraint_index]);
+ lo_act = -hi_act;
+ if (inline_position_correction)
+ {
+ hi_act_erp = dFabs (hi[index] * lambda_erp[constraint_index]);
+ lo_act_erp = -hi_act_erp;
+ }
+ }
+ else if (friction_model == cone_friction)
+ {
+ quickstep::dxConeFrictionModel(lo_act, hi_act, lo_act_erp, hi_act_erp, jb, J_orig, index,
+ constraint_index, startRow, nRows, nb, body, i, order, findex, NULL, hi, lambda, lambda_erp);
+ }
+ else if(friction_model == box_friction)
+ {
+ hi_act = hi[index];
+ lo_act = -hi_act;
+ hi_act_erp = hi[index];
+ lo_act_erp = -hi_act_erp;
+ }
+ else
+ {
+ // initialize the hi and lo to get rid of warnings
+ hi_act = dInfinity;
+ lo_act = -dInfinity;
+ hi_act_erp = dInfinity;
+ lo_act_erp = -dInfinity;
+ dMessage (d_ERR_UASSERT, "internal error, undefined friction model");
+ }
+ } else {
+ // FOR erp throttled by info.c_v_max or info.c
+ hi_act = hi[index];
+ lo_act = lo[index];
+ if (inline_position_correction)
+ {
+ hi_act_erp = hi[index];
+ lo_act_erp = lo[index];
+ }
+ }
+ // compute lambda and clamp it to [lo,hi].
+ // @@@ SSE not a win here
+#undef SSE_CLAMP
+#ifndef SSE_CLAMP
+ // FOR erp throttled by info.c_v_max or info.c
+ lambda[index] = old_lambda + delta;
+ if (lambda[index] < lo_act) {
+ delta = lo_act-old_lambda;
+ lambda[index] = lo_act;
+ }
+ else if (lambda[index] > hi_act) {
+ delta = hi_act-old_lambda;
+ lambda[index] = hi_act;
+ }
+
+ if (inline_position_correction)
+ {
+ lambda_erp[index] = old_lambda_erp + delta_erp;
+ if (lambda_erp[index] < lo_act_erp) {
+ delta_erp = lo_act_erp-old_lambda_erp;
+ lambda_erp[index] = lo_act_erp;
+ }
+ else if (lambda_erp[index] > hi_act_erp) {
+ delta_erp = hi_act_erp-old_lambda_erp;
+ lambda_erp[index] = hi_act_erp;
+ }
+ }
+#else
+ // FOR erp throttled by info.c_v_max or info.c
+ dReal nl = old_lambda + delta;
+ _mm_store_sd(&nl,
+ _mm_max_sd(_mm_min_sd(_mm_load_sd(&nl),
+ _mm_load_sd(&hi_act)),
+ _mm_load_sd(&lo_act)));
+ lambda[index] = nl;
+ delta = nl - old_lambda;
+
+ if (inline_position_correction)
+ {
+ dReal nl_erp = old_lambda_erp + delta_erp;
+ _mm_store_sd(&nl_erp,
+ _mm_max_sd(_mm_min_sd(_mm_load_sd(&nl_erp),
+ _mm_load_sd(&hi_act_erp)),
+ _mm_load_sd(&lo_act_erp)));
+ lambda_erp[index] = nl_erp;
+ delta_erp = nl_erp - old_lambda_erp;
+ }
+#endif
+
+ // option to smooth lambda
+#ifdef SMOOTH_LAMBDA
+ // skip smoothing for the position correction thread
+ if (!position_correction_thread)
+ {
+ // smooth delta lambda
+ // equivalent to first order artificial dissipation on lambda update.
+
+ // debug smoothing
+ // if (i == 0)
+ // printf("rhs[%f] adcfm[%f]: ",rhs[index], Adcfm[index]);
+ // if (i == 0)
+ // printf("dlambda iter[%d]: ",iteration);
+ // printf(" %f ", lambda[index]-old_lambda);
+ // if (i == startRow + nRows - 1)
+ // printf("\n");
+
+ // extra residual smoothing for contact constraints
+ // was smoothing both contact normal and friction constraints for VRC
+ // if (constraint_index != -1)
+ // smooth only lambda for friction directions fails friction_demo.world
+ if (constraint_index != -1)
+ {
+ lambda[index] = (1.0 - smooth_contacts)*lambda[index]
+ + smooth_contacts*old_lambda;
+ }
+
+ // if (inline_position_correction)
+ // {
+ // /// not smoothing lambda_erp
+ // }
+ }
+#endif
+
+ // update caccel
+ {
+ // FOR erp throttled by info.c_v_max or info.c
+ dRealPtr iMJ_ptr = iMJ + index*12;
+
+ // update caccel.
+ quickstep::sum6(caccel_ptr1, delta, iMJ_ptr);
+ if (caccel_ptr2)
+ quickstep::sum6(caccel_ptr2, delta, iMJ_ptr + 6);
+
+ if (inline_position_correction)
+ {
+ quickstep::sum6(caccel_erp_ptr1, delta_erp, iMJ_ptr);
+ if (caccel_erp_ptr2)
+ quickstep::sum6(caccel_erp_ptr2, delta_erp, iMJ_ptr + 6);
+ }
+ }
+ } // end of skip friction check
+
+#ifdef PENETRATION_JVERROR_CORRECTION
+ {
+ // FOR erp throttled by info.c_v_max or info.c
+ dRealPtr iMJ_ptr = iMJ + index*12;
+ // update vnew incrementally
+ // add stepsize * delta_caccel to the body velocity
+ // vnew = vnew + dt * delta_caccel
+ quickstep::sum6(vnew_ptr1, stepsize*delta, iMJ_ptr);
+ if (caccel_ptr2)
+ quickstep::sum6(vnew_ptr2, stepsize*delta, iMJ_ptr + 6);
+
+ // COMPUTE Jvnew = J*vnew/h*Ad
+ // but J is already scaled by Ad, and we multiply by h later
+ // so it's just Jvnew = J*vnew here
+ if (iteration >= num_iterations-7) {
+ // check for non-contact bilateral constraints only
+ // I've set findex to -2 for contact normal constraint
+ if (constraint_index == -1) {
+ dRealPtr J_ptr = J + index*12;
+ Jvnew = quickstep::dot6(vnew_ptr1,J_ptr);
+ if (caccel_ptr2)
+ Jvnew += quickstep::dot6(vnew_ptr2,J_ptr+6);
+ // printf("iter [%d] findex [%d] Jvnew [%f] lo [%f] hi [%f]\n",
+ // iteration, constraint_index, Jvnew, lo[index], hi[index]);
+ }
+ }
+ //printf("iter [%d] vnew [%f,%f,%f,%f,%f,%f] Jvnew [%f]\n",
+ // iteration,
+ // vnew_ptr1[0], vnew_ptr1[1], vnew_ptr1[2],
+ // vnew_ptr1[3], vnew_ptr1[4], vnew_ptr1[5],Jvnew);
+ }
+#endif
+
+
+ //////////////////////////////////////////////////////
+ // record residual (error) (for the non-erp version)
+ //////////////////////////////////////////////////////
+ // given
+ // dlambda = sor * (b_i - A_ij * lambda_j)/(A_ii + cfm)
+ // define scalar Ad:
+ // Ad = sor / (A_ii + cfm)
+ // then
+ // dlambda = Ad * (b_i - A_ij * lambda_j)
+ // thus, to get residual from dlambda,
+ // residual = dlambda / Ad
+ // or
+ // residual = sqrt(sum( Ad2 * dlambda_i * dlambda_i))
+ // where Ad2 = 1/(Ad * Ad)
+ dReal Ad2 = 0.0;
+ if (!_dequal(Ad[index], 0.0))
+ {
+ // Ad[i] = sor_w / (sum + cfm[i]);
+ Ad2 = 1.0 / (Ad[index] * Ad[index]);
+ }
+ else
+ {
+ // TODO: Usually, this means qs->w (SOR param) is zero.
+ // Residual calculation is wrong when SOR (w) is zero
+ // Given SOR is rarely 0, we'll set residual as 0 for now.
+ // To do this properly, we should compute dlambda without sor
+ // then use the Ad without SOR to back out residual.
+ }
+
+ dReal delta2 = delta*delta;
+ if (constraint_index == -1) // bilateral
+ {
+ rms_dlambda[0] += delta2;
+ rms_error[0] += delta2*Ad2;
+ m_rms_dlambda[0]++;
+ }
+ else if (constraint_index == -2) // contact normal
+ {
+ rms_dlambda[1] += delta2;
+ rms_error[1] += delta2*Ad2;
+ m_rms_dlambda[1]++;
+ }
+ else // friction forces
+ {
+ rms_dlambda[2] += delta2;
+ rms_error[2] += delta2*Ad2;
+ m_rms_dlambda[2]++;
+ }
+ } // end of non-precon
+
+ //@@@ a trick that may or may not help
+ //dReal ramp = (1-((dReal)(iteration+1)/(dReal)iterations));
+ //delta *= ramp;
+
+ } // end of for loop on m
+
+#ifdef PENETRATION_JVERROR_CORRECTION
+ Jvnew_final = Jvnew*stepsize1;
+ Jvnew_final = Jvnew_final > 1.0 ? 1.0 : ( Jvnew_final < -1.0 ? -1.0 : Jvnew_final );
+#endif
+
+ // DO WE NEED TO COMPUTE NORM ACROSS ENTIRE SOLUTION SPACE (0,m)?
+ // since local convergence might produce errors in other nodes?
+ dReal dlambda_bilateral_mean = 0.0;
+ dReal dlambda_contact_normal_mean = 0.0;
+ dReal dlambda_contact_friction_mean = 0.0;
+ dReal dlambda_total_mean = 0.0;
+
+ if (m_rms_dlambda[0] > 0)
+ dlambda_bilateral_mean = rms_dlambda[0]/(dReal)m_rms_dlambda[0];
+ if (m_rms_dlambda[1] > 0)
+ dlambda_contact_normal_mean = rms_dlambda[1]/(dReal)m_rms_dlambda[1];
+ if (m_rms_dlambda[2] > 0)
+ dlambda_contact_friction_mean = rms_dlambda[2]/(dReal)m_rms_dlambda[2];
+ if (rms_dlambda[0] + rms_dlambda[1] + rms_dlambda[2] > 0)
+ dlambda_total_mean = (rms_dlambda[0] + rms_dlambda[1] + rms_dlambda[2])/
+ ((dReal)(m_rms_dlambda[0] + m_rms_dlambda[1] + m_rms_dlambda[2]));
+
+ qs->rms_dlambda[0] = sqrt(dlambda_bilateral_mean);
+ qs->rms_dlambda[1] = sqrt(dlambda_contact_normal_mean);
+ qs->rms_dlambda[2] = sqrt(dlambda_contact_friction_mean);
+ qs->rms_dlambda[3] = sqrt(dlambda_total_mean);
+
+ dReal residual_bilateral_mean = 0.0;
+ dReal residual_contact_normal_mean = 0.0;
+ dReal residual_contact_friction_mean = 0.0;
+ dReal residual_total_mean = 0.0;
+
+ if (m_rms_dlambda[0] > 0)
+ residual_bilateral_mean = rms_error[0]/(dReal)m_rms_dlambda[0];
+ if (m_rms_dlambda[1] > 0)
+ residual_contact_normal_mean = rms_error[1]/(dReal)m_rms_dlambda[1];
+ if (m_rms_dlambda[2] > 0)
+ residual_contact_friction_mean = rms_error[2]/(dReal)m_rms_dlambda[2];
+ if (rms_error[0] + rms_error[1] + rms_error[2] > 0)
+ residual_total_mean = (rms_error[0] + rms_error[1] + rms_error[2])/
+ ((dReal)(m_rms_dlambda[0] + m_rms_dlambda[1] + m_rms_dlambda[2]));
+
+ qs->rms_constraint_residual[0] = sqrt(residual_bilateral_mean);
+ qs->rms_constraint_residual[1] = sqrt(residual_contact_normal_mean);
+ qs->rms_constraint_residual[2] = sqrt(residual_contact_friction_mean);
+ qs->rms_constraint_residual[3] = sqrt(residual_total_mean);
+ qs->num_contacts = m_rms_dlambda[1];
+
+#ifdef HDF5_INSTRUMENT
+ errors[iteration] = residual_total_mean;
+#endif
+ // debugging mutex locking
+ //{
+ // // verify
+ // boost::recursive_mutex::scoped_lock lock(*mutex); // lock for every row
+ // printf(" random id %d iter %d\n",thread_id,iteration);
+ // for (int i=startRow+1; i<startRow+nRows; i++)
+ // printf(" %10d,",i);
+ // printf("\n");
+ // for (int i=startRow+1; i<startRow+nRows; i++)
+ // printf(" %10d;",order[i].index);
+ // printf("\n%f %f %f\n",
+ // qs->rms_dlambda[0],qs->rms_dlambda[1],qs->rms_dlambda[2]);
+ //}
+
+#ifdef SHOW_CONVERGENCE
+ /* uncomment for convergence information per row sweep (LOTS OF DATA!)
+ printf("MONITOR: thread(%d) iter(%d) rms(%20.18f %20.18f %20.18)f\n",
+ thread_id, iteration,
+ qs->rms_dlambda[0], qs->rms_dlambda[1], qs->rms_dlambda[2]);
+
+ // print lambda
+ for (int i=startRow; i<startRow+nRows; i++)
+ printf("%f, ", lambda[i]);
+ printf("\n");
+ */
+#endif
+
+ // option to stop when tolerance has been met
+ if (iteration >= precon_iterations &&
+ qs->rms_constraint_residual[3] < pgs_lcp_tolerance)
+ {
+ #ifdef DEBUG_CONVERGENCE_TOLERANCE
+ printf("CONVERGED: id: %d steps: %d,"
+ " rms(%20.18f + %20.18f + %20.18f) < tol(%20.18f)\n",
+ thread_id, iteration,
+ qs->rms_constraint_residual[0], qs->rms_constraint_residual[1],
+ qs->rms_constraint_residual[2],
+ pgs_lcp_tolerance);
+ #endif
+ // tolerance satisfied, stop iterating
+ break;
+ }
+ else if (iteration >= total_iterations - 1)
+ {
+ #ifdef DEBUG_CONVERGENCE_TOLERANCE
+ printf("WARNING: id: %d did not converge in %d steps,"
+ " rms(%20.18f + %20.18f + %20.18f) > tol(%20.18f)\n",
+ thread_id, num_iterations,
+ qs->rms_constraint_residual[0], qs->rms_constraint_residual[1],
+ qs->rms_constraint_residual[2],
+ pgs_lcp_tolerance);
+ #endif
+ }
+ } // end of for loop on iterations
+
+#ifdef SHOW_CONVERGENCE
+ // show starting lambda
+ printf("final lambdas: [");
+ for (int i=startRow; i<startRow+nRows; i++)
+ printf("%f, ", lambda[i]);
+ printf("]\n");
+ printf("MONITOR: id: %d steps: %d,"
+ " dlambda(%20.18f + %20.18f + %20.18f),"
+ " rms(%20.18f + %20.18f + %20.18f) < tol(%20.18f)\n",
+ thread_id, total_iterations,
+ qs->rms_dlambda[0], qs->rms_dlambda[1], qs->rms_dlambda[2],
+ qs->rms_constraint_residual[0], qs->rms_constraint_residual[1],
+ qs->rms_constraint_residual[2],
+ pgs_lcp_tolerance);
+#endif
+ //printf("vnew: ");
+ //for (int i=0; i<6*nb; i++) printf(" %f ",vnew[i]);
+ //printf("\n");
+
+ #ifdef REPORT_THREAD_TIMING
+ gettimeofday(&tv,NULL);
+ double end_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
+ printf(" quickstep row thread %d start time %f ended time %f duration %f\n",thread_id,cur_time,end_time,end_time - cur_time);
+ #endif
+
+ if (position_correction_thread)
+ IFTIMING (dTimerNow ("ComputeRows_erp ends"));
+ else
+ IFTIMING (dTimerNow ("ComputeRows ends"));
+
+ // doing below seems to slow things down
+ // if (position_correction_thread)
+ // pthread_exit(NULL);
+
+#ifdef HDF5_INSTRUMENT
+ IFDUMP(h5_write_errors(DATA_FILE, errors.data(), errors.size()));
+#endif
+
+ return NULL;
+}
+
+//***************************************************************************
+// PGS_LCP method was previously SOR_LCP
+//
+// this returns lambda and cforce (the constraint force).
+// note: cforce is returned as inv(M)*J'*lambda,
+// the constraint force is actually J'*lambda
+//
+// rhs, lo and hi are modified on exit
+//
+void quickstep::PGS_LCP (dxWorldProcessContext *context,
+ const int m, const int nb, dRealMutablePtr J, dRealMutablePtr J_precon,
+ dRealMutablePtr J_orig,
+#ifdef PENETRATION_JVERROR_CORRECTION
+ dRealMutablePtr vnew,
+ const dReal stepsize,
+#endif
+ int *jb, dxBody * const *body,
+ dRealPtr invMOI, dRealPtr MOI, dRealMutablePtr lambda,
+ dRealMutablePtr lambda_erp,
+ dRealMutablePtr caccel, dRealMutablePtr caccel_erp, dRealMutablePtr cforce,
+ dRealMutablePtr rhs, dRealMutablePtr rhs_erp, dRealMutablePtr rhs_precon,
+ dRealPtr lo, dRealPtr hi, dRealPtr cfm, const int *findex,
+ dxQuickStepParameters *qs
+#ifdef USE_TPROW
+ , boost::threadpool::pool* row_threadpool
+#endif
+ )
+{
+
+ // precompute iMJ = inv(M)*J'
+ dReal *iMJ = context->AllocateArray<dReal> (m*12);
+ compute_invM_JT (m,J,iMJ,jb,body,invMOI);
+
+ if (qs->warm_start > 0)
+ {
+ // warm starting
+ // compute cforce=(inv(M)*J')*lambda
+ if (qs->precon_iterations > 0)
+ multiply_invM_JT (m,nb,J,jb,lambda,cforce);
+
+ // re-compute caccel=(inv(M)*J')*lambda with new iMJ
+ // seems much better than using stored caccel's
+ multiply_invM_JT (m,nb,iMJ,jb,lambda,caccel);
+ multiply_invM_JT (m,nb,iMJ,jb,lambda_erp,caccel_erp);
+ }
+ else
+ {
+ // no warm starting
+ if (qs->precon_iterations > 0)
+ dSetZero (cforce,nb*6);
+ dSetZero (caccel,nb*6);
+ dSetZero (caccel_erp,nb*6);
+ }
+
+ dReal *Ad = context->AllocateArray<dReal> (m);
+
+ {
+ const dReal sor_w = qs->w; // SOR over-relaxation parameter
+ // precompute 1 / diagonals of A
+ dRealPtr iMJ_ptr = iMJ;
+ dRealPtr J_ptr = J;
+ for (int i=0; i<m; J_ptr += 12, iMJ_ptr += 12, i++) {
+ dReal sum = 0;
+ sum += quickstep::dot6(iMJ_ptr, J_ptr);
+ if (jb[i*2+1] >= 0) {
+ sum += quickstep::dot6(iMJ_ptr+6, J_ptr+6);
+ }
+ if (findex[i] < 0)
+ Ad[i] = sor_w / (sum + cfm[i]);
+ else
+ Ad[i] = CONTACT_SOR_SCALE * sor_w / (sum + cfm[i]);
+ }
+ }
+
+ // recompute Ad for preconditioned case, Ad_precon is similar to Ad but
+ // whereas Ad is 1 over diagonals of J inv(M) J'
+ // Ad_precon is 1 over diagonals of J J'
+ dReal *Adcfm_precon = NULL;
+ if (qs->precon_iterations > 0)
+ {
+ dReal *Ad_precon = context->AllocateArray<dReal> (m);
+
+ {
+ const dReal sor_w = qs->w; // SOR over-relaxation parameter
+ // precompute 1 / diagonals of A
+ // preconditioned version uses J instead of iMJ
+ dRealPtr J_ptr = J;
+ for (int i=0; i<m; J_ptr += 12, i++) {
+ dReal sum = 0;
+ sum += quickstep::dot6(J_ptr, J_ptr);
+ if (jb[i*2+1] >= 0) {
+ sum += quickstep::dot6(J_ptr+6, J_ptr+6);
+ }
+ if (findex[i] < 0)
+ Ad_precon[i] = sor_w / (sum + cfm[i]);
+ else
+ Ad_precon[i] = CONTACT_SOR_SCALE * sor_w / (sum + cfm[i]);
+ }
+ }
+
+ /********************************/
+ /* allocate for Adcfm */
+ /* which is a mX1 column vector */
+ /********************************/
+ // compute Adcfm_precon for the preconditioned case
+ // do this first before J gets altered (J's diagonals gets premultiplied by Ad)
+ // and save a copy of J into J_orig
+ // as J becomes J * Ad, J_precon becomes J * Ad_precon
+ Adcfm_precon = context->AllocateArray<dReal> (m);
+ {
+
+ // NOTE: This may seem unnecessary but it's indeed an optimization
+ // to move multiplication by Ad[i] and cfm[i] out of iteration loop.
+
+ // scale J_precon and rhs_precon by Ad
+ // copy J_orig
+ dRealMutablePtr J_ptr = J;
+ dRealMutablePtr J_orig_ptr = J_orig;
+ dRealMutablePtr J_precon_ptr = J_precon;
+ for (int i=0; i<m; J_ptr += 12, J_precon_ptr += 12, J_orig_ptr += 12, i++) {
+ dReal Ad_precon_i = Ad_precon[i];
+ for (int j=0; j<12; j++) {
+ J_precon_ptr[j] = J_ptr[j] * Ad_precon_i;
+ J_orig_ptr[j] = J_ptr[j]; //copy J
+ }
+ rhs_precon[i] *= Ad_precon_i;
+ // scale Ad by CFM. N.B. this should be done last since it is used above
+ Adcfm_precon[i] = Ad_precon_i * cfm[i];
+ }
+ }
+ }
+
+ dReal *Adcfm = context->AllocateArray<dReal> (m);
+
+
+ {
+ // NOTE: This may seem unnecessary but it's indeed an optimization
+ // to move multiplication by Ad[i] and cfm[i] out of iteration loop.
+
+ // scale J and rhs by Ad
+ dRealMutablePtr J_ptr = J;
+ for (int i=0; i<m; J_ptr += 12, i++) {
+ dReal Ad_i = Ad[i];
+ for (int j=0; j<12; j++) {
+ J_ptr[j] *= Ad_i;
+ }
+ rhs[i] *= Ad_i;
+ rhs_erp[i] *= Ad_i;
+ // scale Ad by CFM. N.B. this should be done last since it is used above
+ Adcfm[i] = Ad_i * cfm[i];
+ }
+ }
+
+
+ // order to solve constraint rows in
+ IndexError *order = context->AllocateArray<IndexError> (m);
+ int *tmpOrder = context->AllocateArray<int> (m);
+
+#ifndef REORDER_CONSTRAINTS
+ if (qs->row_reorder1)
+ {
+ // -1 in front, followed by -2, lastly all the >0
+ // Fill the array from both ends
+ // where -1 is bilateral, and -2 is friction normal,
+ // might be followed by 2 positive tangential indices
+ // if friction is not zero.
+ // first pass puts -1 in the back
+ int front = 0;
+ int back = m-1;
+ for (int i=0; i<m; ++i) {
+ if (findex[i] == -1) {
+ tmpOrder[front] = i; // Place them at the front
+ ++front;
+ } else {
+ tmpOrder[back] = i; // Place them at the end
+ --back;
+ }
+ }
+ dIASSERT (back - front==1);
+ // second pass, put all negatives in the front,
+ // should preserver -1 goes before -2,
+ // and group all >0 at the end
+ front = 0;
+ back = m-1;
+ for (int i=0; i<m; ++i) {
+ if (findex[tmpOrder[i]] < 0) {
+ order[front].index = tmpOrder[i]; // Place them at the front
+ ++front;
+ } else {
+ order[back].index = tmpOrder[i]; // Place them at the end
+ --back;
+ }
+ }
+ dIASSERT (back - front==1);
+ }
+ else
+ {
+ // make sure constraints with findex < 0 come first.
+ IndexError *orderhead = order, *ordertail = order + (m - 1);
+
+ // Fill the array from both ends
+ for (int i=0; i<m; i++) {
+ if (findex[i] < 0) {
+ orderhead->index = i; // Place them at the front
+ ++orderhead;
+ } else {
+ ordertail->index = i; // Place them at the end
+ --ordertail;
+ }
+ }
+ dIASSERT (orderhead-ordertail==1);
+ }
+#endif
+
+#ifdef SHOW_CONVERGENCE
+ if (0)
+ {
+ printf("-------------- saved labmdas -------------\n");
+ // print current lambdas
+ for (int i = 0; i < m; ++i)
+ {
+ printf("%f, ", lambda[order[i].index]);
+ }
+ printf("\n");
+ for (int i = 0; i < m; ++i)
+ {
+ printf("%d, ", findex[order[i].index]);
+ }
+ printf("\n");
+ for (int i = 0; i < m; ++i)
+ {
+ printf("%d, ", order[i].index);
+ }
+ printf("\n-------------- end of saved labmdas -------------\n");
+ }
+#endif
+
+#ifdef REORDER_CONSTRAINTS
+ // the lambda computed at the previous iteration.
+ // this is used to measure error for when we are reordering the indexes.
+ dReal *last_lambda = context->AllocateArray<dReal> (m);
+ dReal *last_lambda_erp = context->AllocateArray<dReal> (m);
+#endif
+
+ boost::recursive_mutex* mutex =
+ context->AllocateArray<boost::recursive_mutex>(1);
+
+ // number of chunks must be at least 1
+ // (single iteration, through all the constraints)
+ int num_chunks = qs->num_chunks > 0 ? qs->num_chunks : 1; // min is 1
+
+ // divide into chunks sequentially
+ int chunk = m / num_chunks+1;
+ chunk = chunk > 0 ? chunk : 1;
+ int thread_id = 0;
+
+ // prepare pointers for threads
+ // params for solution with correction (_erp) term
+ dxPGSLCPParameters *params_erp = NULL;
+ if (qs->thread_position_correction)
+ params_erp = context->AllocateArray<dxPGSLCPParameters>(num_chunks);
+
+ // params for solution without correction (_erp) term
+ dxPGSLCPParameters *params =
+ context->AllocateArray<dxPGSLCPParameters>(num_chunks);
+
+#ifdef REPORT_THREAD_TIMING
+ // timing
+ struct timeval tv;
+ double cur_time;
+ gettimeofday(&tv,NULL);
+ cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
+ //printf(" quickstep start threads at time %f\n",cur_time);
+#endif
+
+ // this is the main computational loop for PGS
+ // here we iterate over each row and make progressive updates
+
+ // to do split impulse / position projection correction,
+ // - solve lambda and caccel using rhs
+ // - solve lambda_erp and caccel_erp using rhs_erp
+ // these two solves can be performed simultaneously.
+
+ IFTIMING (dTimerNow ("start pgs rows"));
+ for (int i=0; i<m; i+= chunk,thread_id++)
+ {
+ // for (int ijk=0;ijk<m;ijk++)
+ // printf("thread_id> id:%d jb[%d]=%d\n",thread_id,ijk,jb[ijk]);
+
+ int nStart = i - qs->num_overlap < 0 ? 0 : i - qs->num_overlap;
+ int nEnd = i + chunk + qs->num_overlap;
+ if (nEnd > m) nEnd = m;
+
+ std::thread params_erp_thread;
+
+ if (qs->thread_position_correction && params_erp != NULL)
+ {
+ // setup params for ComputeRows
+ IFTIMING (dTimerNow ("start pgs_erp rows"));
+ //////////////////////////////////////////////////////
+ /// repeat for position projection
+ /// setup params_erp for ComputeRows
+ //////////////////////////////////////////////////////
+ params_erp[thread_id].thread_id = thread_id;
+ params_erp[thread_id].order = order;
+ params_erp[thread_id].body = body;
+ params_erp[thread_id].mutex = mutex;
+ params_erp[thread_id].inline_position_correction = false;
+ params_erp[thread_id].position_correction_thread = true;
+#ifdef PENETRATION_JVERROR_CORRECTION
+ params_erp[thread_id].stepsize = stepsize;
+ params_erp[thread_id].vnew = vnew_erp; /// \TODO need to allocate vnew_erp
+#endif
+ params_erp[thread_id].qs = qs;
+ // if every one reorders constraints, this might just work
+ // comment out below if using defaults (0 and m) so every
+ // thread runs through all joints
+ params_erp[thread_id].nStart = nStart; // 0
+ params_erp[thread_id].nChunkSize = nEnd - nStart; // m
+ params_erp[thread_id].m = m; // m
+ params_erp[thread_id].nb = nb;
+ params_erp[thread_id].jb = jb;
+ params_erp[thread_id].findex = findex;
+ params_erp[thread_id].skip_friction = false; // might be a save, but need to test more
+ params_erp[thread_id].hi = hi;
+ params_erp[thread_id].lo = lo;
+ params_erp[thread_id].invMOI = invMOI;
+ params_erp[thread_id].MOI= MOI;
+ params_erp[thread_id].Ad = Ad;
+ params_erp[thread_id].Adcfm = Adcfm;
+ params_erp[thread_id].Adcfm_precon = Adcfm_precon;
+ params_erp[thread_id].J = J;
+ params_erp[thread_id].iMJ = iMJ;
+ params_erp[thread_id].rhs_precon = rhs_precon;
+ params_erp[thread_id].J_precon = J_precon;
+ params_erp[thread_id].J_orig = J_orig;
+ params_erp[thread_id].cforce = cforce;
+
+ params_erp[thread_id].rhs = rhs_erp;
+ params_erp[thread_id].caccel = caccel_erp;
+ params_erp[thread_id].lambda = lambda_erp;
+
+#ifdef REORDER_CONSTRAINTS
+ params_erp[thread_id].last_lambda = last_lambda_erp;
+#endif
+
+#ifdef DEBUG_CONVERGENCE_TOLERANCE
+ printf("thread summary: id %d i %d m %d chunk %d start %d end %d \n",
+ thread_id,i,m,chunk,nStart,nEnd);
+#endif
+
+#ifdef USE_TPROW
+ if (row_threadpool && row_threadpool->size() > 1)
+ {
+ // skip threadpool if less than 2 threads allocated
+ // printf("threading out for params_erp\n");
+ row_threadpool->schedule(boost::bind(*ComputeRows, (void*)(¶ms_erp[thread_id])));
+ }
+ else
+ {
+ params_erp_thread = std::thread(*ComputeRows,
+ (void*)(&(params_erp[thread_id])));
+ }
+#else
+ params_erp_thread = std::thread(*ComputeRows,
+ (void*)(&(params_erp[thread_id])));
+#endif
+ }
+
+
+ // setup params for ComputeRows non_erp
+ params[thread_id].thread_id = thread_id;
+ params[thread_id].order = order;
+ params[thread_id].body = body;
+ params[thread_id].mutex = mutex;
+ params[thread_id].inline_position_correction = !qs->thread_position_correction;
+ params[thread_id].position_correction_thread = false;
+#ifdef PENETRATION_JVERROR_CORRECTION
+ params[thread_id].stepsize = stepsize;
+ params[thread_id].vnew = vnew;
+#endif
+ params[thread_id].qs = qs;
+ // if every one reorders constraints, this might just work
+ // comment out below if using defaults (0 and m) so every
+ // thread runs through all joints
+ params[thread_id].nStart = nStart; // 0
+ params[thread_id].nChunkSize = nEnd - nStart; // m
+ params[thread_id].m = m; // m
+ params[thread_id].nb = nb;
+ params[thread_id].jb = jb;
+ params[thread_id].findex = findex;
+ params[thread_id].skip_friction = false;
+ params[thread_id].hi = hi;
+ params[thread_id].lo = lo;
+ params[thread_id].invMOI = invMOI;
+ params[thread_id].MOI= MOI;
+ params[thread_id].Ad = Ad;
+ params[thread_id].Adcfm = Adcfm;
+ params[thread_id].Adcfm_precon = Adcfm_precon;
+ params[thread_id].J = J;
+ params[thread_id].iMJ = iMJ;
+ params[thread_id].rhs_precon = rhs_precon;
+ params[thread_id].J_precon = J_precon;
+ params[thread_id].J_orig = J_orig;
+ params[thread_id].cforce = cforce;
+
+ params[thread_id].rhs = rhs;
+ params[thread_id].caccel = caccel;
+ params[thread_id].lambda = lambda;
+
+ if (!qs->thread_position_correction)
+ {
+ /// if running without thread_position_correction, compute both in
+ /// the same loop
+ params[thread_id].rhs_erp = rhs_erp;
+ params[thread_id].caccel_erp = caccel_erp;
+ params[thread_id].lambda_erp = lambda_erp;
+ }
+
+#ifdef REORDER_CONSTRAINTS
+ params[thread_id].last_lambda = last_lambda;
+#endif
+
+#ifdef DEBUG_CONVERGENCE_TOLERANCE
+ printf("thread summary: id %d i %d m %d chunk %d start %d end %d \n",
+ thread_id,i,m,chunk,nStart,nEnd);
+#endif
+#ifdef USE_TPROW
+ if (row_threadpool && row_threadpool->size() > 0)
+ {
+ // skip threadpool if less than 2 threads allocated
+ // printf("threading out for params\n");
+ row_threadpool->schedule(boost::bind(*ComputeRows, (void*)(&(params[thread_id]))));
+ }
+ else
+ ComputeRows((void*)(&(params[thread_id])));
+#else
+ ComputeRows((void*)(&(params[thread_id])));
+#endif
+
+ if (qs->thread_position_correction && params_erp_thread.joinable())
+ {
+ IFTIMING (dTimerNow ("wait for params_erp threads"));
+ params_erp_thread.join();
+ IFTIMING (dTimerNow ("params_erp threads done"));
+ }
+ }
+
+
+ // check time for scheduling, this is usually very quick
+ //gettimeofday(&tv,NULL);
+ //double wait_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
+ //printf(" quickstep done scheduling start time %f stopped time %f duration %f\n",
+ // cur_time,wait_time,wait_time - cur_time);
+
+#ifdef USE_TPROW
+ IFTIMING (dTimerNow ("wait for threads"));
+ if (row_threadpool && row_threadpool->size() > 0)
+ row_threadpool->wait();
+ IFTIMING (dTimerNow ("threads done"));
+#endif
+
+
+ #ifdef REPORT_THREAD_TIMING
+ gettimeofday(&tv,NULL);
+ double end_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
+ printf(" quickstep threads start time %f stopped time %f duration %f\n",
+ cur_time,end_time,end_time - cur_time);
+ #endif
+}
+
+void quickstep::dxConeFrictionModel(dReal& lo_act, dReal& hi_act, dReal& lo_act_erp, dReal& hi_act_erp,
+ int *jb, dRealPtr J_orig, int index, int constraint_index, int startRow, int nRows,
+ const int nb, dxBody * const *body, int i, const IndexError *order, const int *findex,
+ dRealPtr /*lo*/, dRealPtr hi, dRealMutablePtr lambda, dRealMutablePtr lambda_erp)
+{
+ // This computes the corresponding hi_act and lo_act for friction constraints.
+ // For each contact, we have lambda_n, lambda_f1, and lambda_f2.
+ // Now couple the two friction and to satisfy the cone Coulomb friction model
+ // tangential velocity at the contact frame:
+ // v_f1 = J_f1 * v
+ // v_f2 = J_f2 * v
+ // v_f1 = J(0:5)*v_b1_global + J(6:11)*v_b2_global,
+ // v_f2 = J2(0:5) * v_b1_global + J2(6:11)*v_b2_global
+ // v = sqrt(v_f1^2 + v_f2^2);
+ //
+ // if (v < eps)
+ // lo_act_f1 = 0;
+ // hi_act_f1 = 0;
+ // lo_act_f2 = 0;
+ // hi_act_f2 = 0;
+ // else
+ // hi_act_f1 = abs(v_f1) / v * (mu * lambda_n);
+ // lo_act_f1 = - lo_act_f1;
+ // hi_act_f2 = abs(v_f2) / v * (mu * lambda_n);
+ // lo_act_f2 = - lo_act_f2;
+ // end
+ //
+ dReal v_f1, v_f2, v;
+ v_f1 = 0.0; v_f2 = 0.0, v = 0.0;
+ int b1 = jb[index*2];
+ int b2 = jb[index*2+1];
+ int bodycounter = 0;
+ int ivel=0;
+
+ // J_ptr has been scaled! use J_orig_ptr here!
+ dRealPtr J_orig_ptr = J_orig + index*12;
+ dReal body1_vel[6];
+ dReal body2_vel[6];
+ dSetZero(body1_vel, 6);
+ dSetZero(body2_vel, 6);
+ dxBody *const *const bodyend = body + nb;
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; bodycurr++, bodycounter++)
+ {
+ dxBody *b_ptr = *bodycurr;
+ // first direction
+ if (b1 >= 0)
+ {
+ if (bodycounter == b1)
+ {
+ for (ivel = 0; ivel < 3; ivel++)
+ {
+ body1_vel[ivel] = b_ptr->lvel[ivel];
+ body1_vel[ivel+3] = b_ptr->avel[ivel];
+ }
+ }
+ }
+ if (b2 >= 0)
+ {
+ if (bodycounter == b2)
+ {
+ for (ivel = 0; ivel < 3; ivel++)
+ {
+ body2_vel[ivel] = b_ptr->lvel[ivel];
+ body2_vel[ivel+3] = b_ptr->avel[ivel];
+ }
+ }
+ }
+ }
+ //startRow, nRows;
+ int previndex, nextindex, prev_constraint_index, next_constraint_index;
+ if (i == startRow)
+ {
+ prev_constraint_index = -100;
+ next_constraint_index = constraint_index;
+ }
+ else if (i == startRow+nRows-1)
+ {
+ prev_constraint_index = constraint_index;
+ next_constraint_index = -100;
+ }
+ else
+ {
+ previndex = order[i-1].index;
+ nextindex = order[i+1].index;
+ prev_constraint_index = findex[previndex];
+ next_constraint_index = findex[nextindex];
+ }
+
+ if (constraint_index == next_constraint_index)
+ {
+ // body1 was always the 1st body in the body pair
+ dRealPtr J_next_ptr = J_orig + index*12 + 12;
+ v_f1 = quickstep::dot6(J_orig_ptr, body1_vel) + quickstep::dot6(J_orig_ptr+6, body2_vel);
+ v_f2 = quickstep::dot6(J_next_ptr, body1_vel) + quickstep::dot6(J_next_ptr+6, body2_vel);
+ }
+ else if (constraint_index == prev_constraint_index)
+ {
+ dRealPtr J_prev_ptr = J_orig + index*12 - 12;
+ v_f1 = quickstep::dot6(J_prev_ptr, body1_vel) + quickstep::dot6(J_prev_ptr, body2_vel);
+ v_f2 = quickstep::dot6(J_orig_ptr, body1_vel) + quickstep::dot6(J_orig_ptr, body2_vel);
+ }
+ else
+ {
+ dMessage (d_ERR_LCP, "constraint index error");
+ }
+
+
+ v = sqrt(v_f1*v_f1 + v_f2*v_f2);
+ if (dFabs(v) < 1e-18)
+ {
+ hi_act = 0.0; lo_act = 0.0;
+ }
+ else
+ {
+ if (constraint_index == next_constraint_index)
+ {
+ // first direction ---> corresponds to the primary friction direction
+ hi_act = (dFabs(v_f1) / v) * dFabs (hi[index] * lambda[constraint_index]);
+ lo_act = -hi_act;
+ hi_act_erp = (dFabs(v_f1) / v) * dFabs (hi[index] * lambda_erp[constraint_index]);
+ lo_act_erp = -hi_act;
+ }
+ else if (constraint_index == prev_constraint_index)
+ {
+ // second-direction ---> corresponds to the secondary friction direction
+ hi_act = (dFabs(v_f2) / v) * dFabs (hi[index] * lambda[constraint_index]);
+ lo_act = -hi_act;
+ hi_act_erp = (dFabs(v_f2) / v) * dFabs (hi[index] * lambda_erp[constraint_index]);
+ lo_act_erp = -hi_act;
+ }
+ else
+ {
+ dMessage (d_ERR_LCP, "constraint index error");
+ }
+ } // if-else (abs(v)< eps)
+}
+
+size_t quickstep::EstimatePGS_LCPMemoryRequirements(int m,int /*nb*/)
+{
+ size_t res = dEFFICIENT_SIZE(sizeof(dReal) * 12 * m); // for iMJ
+ res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for Ad
+ res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for Adcfm
+ res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for Ad_precon
+ res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for Adcfm_precon
+ res += dEFFICIENT_SIZE(sizeof(IndexError) * m); // for order
+ res += dEFFICIENT_SIZE(sizeof(int) * m); // for tmpOrder
+#ifdef REORDER_CONSTRAINTS
+ res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for last_lambda
+ res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for last_lambda_erp
+#endif
+ res += dEFFICIENT_SIZE(sizeof(dxPGSLCPParameters) * m); // for params_erp
+ res += dEFFICIENT_SIZE(sizeof(dxPGSLCPParameters) * m); // for params
+ res += dEFFICIENT_SIZE(sizeof(boost::recursive_mutex)); // for mutex
+ return res;
+}
+
diff --git a/deps/opende/src/quickstep_pgs_lcp.h b/deps/opende/src/quickstep_pgs_lcp.h
new file mode 100644
index 0000000..a6c650a
--- /dev/null
+++ b/deps/opende/src/quickstep_pgs_lcp.h
@@ -0,0 +1,81 @@
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ at q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#ifndef _ODE_QUICK_STEP_PGS_LCP_H_
+#define _ODE_QUICK_STEP_PGS_LCP_H_
+
+#include <ode/common.h>
+#include "quickstep_util.h"
+
+namespace ode {
+ namespace quickstep{
+
+// PGS_LCP was previously named SOR_LCP
+void PGS_LCP (dxWorldProcessContext *context,
+ const int m, const int nb, dRealMutablePtr J, dRealMutablePtr J_precon,
+ dRealMutablePtr J_orig,
+#ifdef PENETRATION_JVERROR_CORRECTION
+ dRealMutablePtr vnew,
+ const dReal stepsize,
+#endif
+ int *jb, dxBody * const *body,
+ dRealPtr invMOI, dRealPtr MOI, dRealMutablePtr lambda,
+ dRealMutablePtr lambda_erp,
+ dRealMutablePtr caccel, dRealMutablePtr caccel_erp, dRealMutablePtr cforce,
+ dRealMutablePtr rhs, dRealMutablePtr rhs_erp, dRealMutablePtr rhs_precon,
+ dRealPtr lo, dRealPtr hi, dRealPtr cfm, const int *findex,
+ dxQuickStepParameters *qs
+#ifdef USE_TPROW
+ , boost::threadpool::pool* row_threadpool
+#endif
+ );
+
+/// \brief Compute the hi and lo bound for cone friction model to project onto
+/// \param[in] lo_act The low bound for cone friction model to project onto
+/// \param[in] hi_act The high bound for cone friction model to project onto
+/// \param[in] lo_act_erp The erp-version low bound for cone friction model to project onto
+/// \param[in] hi_act_erp The erp-version high bound for cone friction model to project onto
+/// \param[in] jb The list of consecutive body pair ids that form constraints
+/// \param[in] J_orig Unscaled Jacobian matrix J
+/// \param[in] index Index definition of the constraint type: -1 bilateral, -2 normal, >=0 friction
+/// \param[in] startRow Start row index
+/// \param[in] nRows Number of rows
+/// \param[in] nb Number of bodies
+/// \param[in] body Body array with kinematic information
+/// \param[in] i Current row index
+/// \param[in] order Constraint reorder
+/// \param[in] findex Constraint index, after reordering
+/// \param[in] lo Low bound for pyramid friction model to project onto
+/// \param[in] hi High bound for pyramid friction model to project onto
+/// \param[in] lambda Constraint force
+/// \param[in] lambda_erp The erp-version of constraint force
+///
+void dxConeFrictionModel(dReal& lo_act, dReal& hi_act, dReal& lo_act_erp, dReal& hi_act_erp,
+ int *jb, dRealPtr J_orig, int index, int constraint_index, int startRow,
+ int nRows, const int nb, dxBody * const *body, int i, const IndexError *order,
+ const int *findex, dRealPtr lo, dRealPtr hi, dRealMutablePtr lambda, dRealMutablePtr lambda_erp);
+
+size_t EstimatePGS_LCPMemoryRequirements(int m,int /*nb*/);
+
+ } // namespace quickstep
+} // namespace ode
+#endif
diff --git a/deps/opende/src/quickstep_update_bodies.cpp b/deps/opende/src/quickstep_update_bodies.cpp
new file mode 100644
index 0000000..5026603
--- /dev/null
+++ b/deps/opende/src/quickstep_update_bodies.cpp
@@ -0,0 +1,377 @@
+/*************************************************************************
+* *
+* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
+* All rights reserved. Email: russ at q12.org Web: www.q12.org *
+* *
+* This library is free software; you can redistribute it and/or *
+* modify it under the terms of EITHER: *
+* (1) The GNU Lesser General Public License as published by the Free *
+* Software Foundation; either version 2.1 of the License, or (at *
+* your option) any later version. The text of the GNU Lesser *
+* General Public License is included with this library in the *
+* file LICENSE.TXT. *
+* (2) The BSD-style license that is included with this library in *
+* the file LICENSE-BSD.TXT. *
+* *
+* This library is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+* *
+*************************************************************************/
+#include <ode/common.h>
+#include <ode/odemath.h>
+#include <ode/rotation.h>
+#include <ode/timer.h>
+#include <ode/error.h>
+#include <ode/matrix.h>
+#include <ode/misc.h>
+#include "config.h"
+#include "objects.h"
+#include "joints/joint.h"
+#include "util.h"
+#if !defined(WIN32)
+#include <sys/time.h>
+#endif
+#include "quickstep_util.h"
+#include "quickstep_update_bodies.h"
+using namespace ode;
+
+// Update the velocity, position of bodies
+void quickstep::dxUpdateBodies(
+#ifdef CHECK_VELOCITY_OBEYS_CONSTRAINT
+ dxWorldProcessContext *context,
+ const int *findex,
+ dRealMutablePtr const cforce,
+ dxQuickStepParameters *qs,
+#endif
+ const int m, const int mfb, dxBody *const *body, int nb,
+ dJointWithInfo1 *const jointiinfos, int nj, const dReal stepsize,
+ dRealMutablePtr const lambda, dRealMutablePtr const caccel,
+ dRealMutablePtr const caccel_erp, dRealPtr Jcopy, dRealPtr invMOI)
+{
+ // note that the PGS method overwrites rhs and J at this point, so
+ // they should not be used again.
+ if(m>0){
+ {
+ IFTIMING (dTimerNow ("velocity update due to constraint forces"));
+ //
+ // update new velocity
+ // add stepsize * caccel_erp to the body velocity
+ //
+ const dReal *caccelcurr = caccel_erp;
+ dxBody *const *const bodyend = body + nb;
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; caccelcurr+=6, bodycurr++) {
+ dxBody *b_ptr = *bodycurr;
+ for (int j=0; j<3; j++) {
+ b_ptr->lvel[j] += stepsize * caccelcurr[j];
+ b_ptr->avel[j] += stepsize * caccelcurr[3+j];
+ }
+ // printf("caccel [%f %f %f] [%f %f %f]\n"
+ // ,caccelcurr[0] ,caccelcurr[1] ,caccelcurr[2]
+ // ,caccelcurr[3] ,caccelcurr[4] ,caccelcurr[5]);
+ // printf(" vel [%f %f %f] [%f %f %f]\n"
+ // ,b_ptr->lvel[0] ,b_ptr->lvel[1] ,b_ptr->lvel[2]
+ // ,b_ptr->avel[0] ,b_ptr->avel[1] ,b_ptr->avel[2]);
+ }
+ }
+
+ if (mfb > 0) {
+ // force feedback without erp is better
+ // straightforward computation of joint constraint forces:
+ // multiply related lambdas with respective J' block for joints
+ // where feedback was requested
+ dReal data[6];
+ const dReal *lambdacurr = lambda;
+ const dReal *Jcopyrow = Jcopy;
+ const dJointWithInfo1 *jicurr = jointiinfos;
+ const dJointWithInfo1 *const jiend = jicurr + nj;
+ for (; jicurr != jiend; jicurr++) {
+ dxJoint *joint = jicurr->joint;
+ const int infom = jicurr->info.m;
+
+ if (joint->feedback) {
+ dJointFeedback *fb = joint->feedback;
+ Multiply1_12q1 (data, Jcopyrow, lambdacurr, infom);
+ fb->f1[0] = data[0];
+ fb->f1[1] = data[1];
+ fb->f1[2] = data[2];
+ fb->t1[0] = data[3];
+ fb->t1[1] = data[4];
+ fb->t1[2] = data[5];
+
+ if (joint->node[1].body)
+ {
+ Multiply1_12q1 (data, Jcopyrow+6, lambdacurr, infom);
+ fb->f2[0] = data[0];
+ fb->f2[1] = data[1];
+ fb->f2[2] = data[2];
+ fb->t2[0] = data[3];
+ fb->t2[1] = data[4];
+ fb->t2[2] = data[5];
+ }
+
+ Jcopyrow += infom * 12;
+ }
+
+ lambdacurr += infom;
+ }
+ }
+ }
+
+ {
+ IFTIMING (dTimerNow ("compute velocity update"));
+ // compute the velocity update:
+ // add stepsize * invM * fe to the body velocity
+ const dReal *invMOIrow = invMOI;
+ dxBody *const *const bodyend = body + nb;
+ dReal tmp_tacc[3];
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; invMOIrow += 12, bodycurr++) {
+ dxBody *b_ptr = *bodycurr;
+ dReal body_invMass_mul_stepsize = stepsize * b_ptr->invMass;
+ for (int j=0; j<3; j++) {
+ b_ptr->lvel[j] += body_invMass_mul_stepsize * b_ptr->facc[j];
+ tmp_tacc[j] = b_ptr->tacc[j] * stepsize;
+ }
+ dMultiplyAdd0_331 (b_ptr->avel, invMOIrow, tmp_tacc);
+ // printf("fe [%f %f %f] [%f %f %f]\n"
+ // ,b_ptr->facc[0] ,b_ptr->facc[1] ,b_ptr->facc[2]
+ // ,b_ptr->tacc[0] ,b_ptr->tacc[1] ,b_ptr->tacc[2]);
+ /* DEBUG PRINTOUTS
+ printf("uncorrect vel [%f %f %f] [%f %f %f]\n"
+ ,b_ptr->lvel[0] ,b_ptr->lvel[1] ,b_ptr->lvel[2]
+ ,b_ptr->avel[0] ,b_ptr->avel[1] ,b_ptr->avel[2]);
+ */
+ }
+ }
+
+#ifdef CHECK_VELOCITY_OBEYS_CONSTRAINT
+ if (m > 0) {
+ BEGIN_STATE_SAVE(context, rmsstate) {
+ dReal *vel = context->AllocateArray<dReal>(nb*6);
+
+ // CHECK THAT THE UPDATED VELOCITY OBEYS THE CONSTRAINT
+ // (this check needs unmodified J)
+ // put residual into tmp
+ dRealMutablePtr velcurr = vel;
+ dxBody* const* bodyend = body + nb;
+ for (dxBody* const* bodycurr = body; bodycurr != bodyend; velcurr += 6, bodycurr++) {
+ dxBody *b_ptr = *bodycurr;
+ for (int j=0; j<3; j++) {
+ velcurr[j] = b_ptr->lvel[j];
+ velcurr[3+j] = b_ptr->avel[j];
+ }
+ }
+ dReal *tmp = context->AllocateArray<dReal> (m);
+ multiply_J (m,J,jb,vel,tmp);
+
+ int m_Jv_bilateral = 0;
+ int m_Jv_contact = 0;
+ int m_Jv_friction = 0;
+ dReal Jv_bilateral = 0;
+ dReal Jv_contact = 0;
+ dReal Jv_friction = 0;
+ for (int i=0; i<m; i++)
+ {
+ if (findex[i] == -1)
+ {
+ m_Jv_bilateral++;
+ Jv_bilateral += dFabs(tmp[i])*dFabs(tmp[i]);
+ }
+ else if (findex[i] == -2)
+ {
+ // contact error includes joint limits
+ Jv_contact += dFabs(tmp[i])*dFabs(tmp[i]);
+ m_Jv_contact++;
+ }
+ else if (findex[i] >= 0)
+ {
+ m_Jv_friction++;
+ Jv_friction += dFabs(tmp[i])*dFabs(tmp[i]);
+ }
+
+ // Note: This is not a good measure of constraint error
+ // for soft contact, as Jv is not necessarily zero here.
+ // Better measure is compute the residual. \\\ TODO
+ }
+ // printf ("error = %10.6e %10.6e %10.6e\n",
+ // error, Jv_bilateral, Jv_contact);
+ // world->qs.rms_constraint_residual[0] = sqrt(error/(dReal)m);
+
+ dReal residual_bilateral_mean = 0.0;
+ dReal residual_contact_normal_mean = 0.0;
+ dReal residual_contact_friction_mean = 0.0;
+ dReal residual_total_mean = 0.0;
+
+ if (m_Jv_bilateral > 0)
+ residual_bilateral_mean = Jv_bilateral/(dReal)m_Jv_bilateral;
+ if (m_Jv_contact > 0)
+ residual_contact_normal_mean = Jv_contact/(dReal)m_Jv_contact;
+ if (m_Jv_friction > 0)
+ residual_contact_friction_mean = Jv_friction/(dReal)m_Jv_friction;
+ if (m_Jv_bilateral + m_Jv_contact + m_Jv_friction > 0)
+ residual_total_mean = (Jv_bilateral + Jv_contact + Jv_friction)/
+ ((dReal)(m_Jv_bilateral + m_Jv_contact + m_Jv_friction));
+
+ world->qs.rms_constraint_residual[0] = sqrt(residual_bilateral_mean);
+ world->qs.rms_constraint_residual[1] = sqrt(residual_contact_normal_mean);
+ world->qs.rms_constraint_residual[2] =
+ sqrt(residual_contact_friction_mean);
+ world->qs.rms_constraint_residual[3] = sqrt(residual_total_mean);
+ world->qs.num_contacts = m_Jv_contact;
+ } END_STATE_SAVE(context, rmsstate);
+ }
+#endif
+ {
+ // update the position and orientation from the new linear/angular velocity
+ // (over the given timestep)
+ IFTIMING (dTimerNow ("update position"));
+ const dReal *caccelcurr = caccel;
+ dxBody *const *const bodyend = body + nb;
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend;
+ caccelcurr += 6, ++bodycurr)
+ {
+ dxBody *b_ptr = *bodycurr;
+ {
+ // sum all forces (external and constraint) into facc and tacc
+ // so dBodyGetForce and dBodyGetTorque returns total force and torque
+ // on the body
+ dReal cf[6];
+ cf[0] = b_ptr->mass.mass * caccelcurr[0];
+ cf[1] = b_ptr->mass.mass * caccelcurr[1];
+ cf[2] = b_ptr->mass.mass * caccelcurr[2];
+ dMultiply0_331 (cf+3, b_ptr->mass.I, caccelcurr+3);
+ for (unsigned int j = 0; j < 3; ++j)
+ {
+ b_ptr->facc[j] += cf[j];
+ b_ptr->tacc[j] += cf[3+j];
+ }
+ }
+ dxStepBody (b_ptr,stepsize);
+ }
+ }
+
+ // revert lvel and avel with the non-erp version of caccel
+ if (m > 0) {
+ dReal erp_removal = 1.00;
+ IFTIMING (dTimerNow ("velocity update due to constraint forces"));
+ // remove caccel_erp
+ const dReal *caccel_erp_curr = caccel_erp;
+ const dReal *caccel_curr = caccel;
+ dxBody *const *const bodyend = body + nb;
+ int debug_count = 0;
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend;
+ caccel_curr+=6, caccel_erp_curr+=6, bodycurr++, debug_count++) {
+ dxBody *b_ptr = *bodycurr;
+ for (int j=0; j<3; j++) {
+ // dReal v0 = b_ptr->lvel[j];
+ // dReal a0 = b_ptr->avel[j];
+ dReal dv = erp_removal * stepsize *
+ (caccel_curr[j] - caccel_erp_curr[j]);
+ dReal da = erp_removal * stepsize *
+ (caccel_curr[3+j] - caccel_erp_curr[3+j]);
+
+ /* default v removal
+ */
+ b_ptr->lvel[j] += dv;
+ b_ptr->avel[j] += da;
+ /* think about minimize J*v somehow without PGSLCP...
+ */
+ /* minimize final velocity test 1,
+ if (v0 * dv < 0) {
+ if (fabs(v0) < fabs(dv))
+ b_ptr->lvel[j] = 0.0;
+ else
+ b_ptr->lvel[j] += dv;
+ }
+ if (a0 * da < 0) {
+ if (fabs(a0) < fabs(da))
+ b_ptr->avel[j] = 0.0;
+ else
+ b_ptr->avel[j] += da;
+ }
+ */
+
+ /* DEBUG PRINTOUTS, total forces/accel on a body
+ printf("nb[%d] m[%d] b[%d] i[%d] v[%f] dv[%f] vf[%f] a[%f] da[%f] af[%f] debug[%f - %f][%f - %f]\n"
+ ,nb, m, debug_count, j, v0, dv, b_ptr->lvel[j]
+ , a0, da, b_ptr->avel[j]
+ ,caccel_curr[j], caccel_erp_curr[j]
+ ,caccel_curr[3+j], caccel_erp_curr[3+j]);
+ */
+ }
+ /* DEBUG PRINTOUTS
+ printf("corrected vel [%f %f %f] [%f %f %f]\n",
+ b_ptr->lvel[0], b_ptr->lvel[1], b_ptr->lvel[2],
+ b_ptr->avel[0], b_ptr->avel[1], b_ptr->avel[2]);
+ */
+ }
+
+#ifdef POST_UPDATE_CONSTRAINT_VIOLATION_CORRECTION
+ // ADD CACCEL CORRECTION FROM VELOCITY CONSTRAINT VIOLATION
+ BEGIN_STATE_SAVE(context, velstate) {
+ dReal *vel = context->AllocateArray<dReal>(nb*6);
+
+ // CHECK THAT THE UPDATED VELOCITY OBEYS THE CONSTRAINT
+ // (this check needs unmodified J)
+ // put residual into tmp
+ dRealMutablePtr velcurr = vel;
+ //dxBody* const* bodyend = body + nb;
+ for (dxBody* const* bodycurr = body; bodycurr != bodyend; velcurr += 6, bodycurr++) {
+ dxBody *b_ptr = *bodycurr;
+ for (int j=0; j<3; j++) {
+ velcurr[j] = b_ptr->lvel[j];
+ velcurr[3+j] = b_ptr->avel[j];
+ }
+ }
+ dReal *tmp = context->AllocateArray<dReal> (m);
+ multiply_J (m,J,jb,vel,tmp);
+
+ // DIRECTLY ADD THE CONSTRAINT VIOLATION TERM (TMP) TO VELOCITY UPDATE
+ // add correction term dlambda = J*v(n+1)/dt
+ // and caccel += dt*invM*JT*dlambda (dt's cancel out)
+ dReal *iMJ = context->AllocateArray<dReal> (m*12);
+ compute_invM_JT (m,J,iMJ,jb,body,invMOI);
+ // compute caccel_corr=(inv(M)*J')*dlambda, correction term
+ // as we change lambda.
+ multiply_invM_JT (m,nb,iMJ,jb,tmp,caccel_corr);
+
+ } END_STATE_SAVE(context, velstate);
+
+ // ADD CACCEL CORRECTION FROM VELOCITY CONSTRAINT VIOLATION
+ caccelcurr = caccel;
+ const dReal* caccel_corrcurr = caccel_corr;
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; caccel_corrcurr+=6, bodycurr++) {
+ dxBody *b_ptr = *bodycurr;
+ for (int j=0; j<3; j++) {
+ b_ptr->lvel[j] += erp_removal * stepsize * caccel_corrcurr[j];
+ b_ptr->avel[j] += erp_removal * stepsize * caccel_corrcurr[3+j];
+ }
+ }
+#endif
+
+ }
+
+ {
+ IFTIMING (dTimerNow ("tidy up"));
+ // zero all force accumulators
+ dxBody *const *const bodyend = body + nb;
+ for (dxBody *const *bodycurr = body; bodycurr != bodyend; bodycurr++) {
+ dxBody *b_ptr = *bodycurr;
+ dSetZero (b_ptr->facc,3);
+ dSetZero (b_ptr->tacc,3);
+ }
+ }
+
+ IFTIMING (dTimerEnd());
+ IFTIMING (if (m > 0) dTimerReport (stdout,1));
+}
+
+size_t quickstep::dxUpdateBodiesMemoryRequirements(int m, int nb)
+{
+ // for vel
+ size_t res = dEFFICIENT_SIZE(sizeof(dReal) * 6 * nb);
+ res += dEFFICIENT_SIZE(sizeof(dReal) * m); // for tmp
+ res += dEFFICIENT_SIZE(sizeof(dReal) * 12 * m); // for iMJ
+ return res;
+}
diff --git a/deps/opende/src/quickstep_update_bodies.h b/deps/opende/src/quickstep_update_bodies.h
new file mode 100644
index 0000000..b610d14
--- /dev/null
+++ b/deps/opende/src/quickstep_update_bodies.h
@@ -0,0 +1,45 @@
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ at q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#ifndef _ODE_QUICK_STEP_UPDATE_BODIES_H_
+#define _ODE_QUICK_STEP_UPDATE_BODIES_H_
+
+#include <ode/common.h>
+#include "quickstep_util.h"
+
+namespace ode {
+ namespace quickstep{
+ // update velocity and position of all bodies
+ void dxUpdateBodies(
+#ifdef CHECK_VELOCITY_OBEYS_CONSTRAINT
+ dxWorldProcessContext *context, const int *findex,
+ dRealMutablePtr const cforce, dxQuickStepParameters *qs,
+#endif
+ const int m, const int mfb, dxBody *const *body, int nb,
+ dJointWithInfo1 *const jointiinfos, int nj, const dReal stepsize,
+ dRealMutablePtr const lambda, dRealMutablePtr const caccel,
+ dRealMutablePtr const caccel_erp, dRealPtr Jcopy, dRealPtr invMOI);
+
+ size_t dxUpdateBodiesMemoryRequirements(int m, int nb);
+ } // namespace quickstep
+} // namespace ode
+#endif
diff --git a/deps/opende/src/quickstep_util.cpp b/deps/opende/src/quickstep_util.cpp
new file mode 100644
index 0000000..74510a9
--- /dev/null
+++ b/deps/opende/src/quickstep_util.cpp
@@ -0,0 +1,524 @@
+/*************************************************************************
+* *
+* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
+* All rights reserved. Email: russ at q12.org Web: www.q12.org *
+* *
+* This library is free software; you can redistribute it and/or *
+* modify it under the terms of EITHER: *
+* (1) The GNU Lesser General Public License as published by the Free *
+* Software Foundation; either version 2.1 of the License, or (at *
+* your option) any later version. The text of the GNU Lesser *
+* General Public License is included with this library in the *
+* file LICENSE.TXT. *
+* (2) The BSD-style license that is included with this library in *
+* the file LICENSE-BSD.TXT. *
+* *
+* This library is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+* *
+*************************************************************************/
+#include <ode/common.h>
+#include <ode/odemath.h>
+#include <ode/rotation.h>
+#include <ode/timer.h>
+#include <ode/error.h>
+#include <ode/matrix.h>
+#include <ode/misc.h>
+#include "config.h"
+#include "objects.h"
+#include "joints/joint.h"
+#include "util.h"
+
+#ifndef _WIN32
+ #include <sys/time.h>
+#endif
+
+#include "quickstep_util.h"
+
+using namespace ode;
+// multiply block of B matrix (q x 6) with 12 dReal per row with C vektor (q)
+void quickstep::Multiply1_12q1 (dReal *A, const dReal *B, const dReal *C, int q)
+{
+ dIASSERT (q>0 && A && B && C);
+
+ dReal a = 0;
+ dReal b = 0;
+ dReal c = 0;
+ dReal d = 0;
+ dReal e = 0;
+ dReal f = 0;
+ dReal s;
+
+ for(int i=0, k = 0; i<q; k += 12, i++)
+ {
+ s = C[i]; //C[i] and B[n+k] cannot overlap because its value has been read into a temporary.
+
+ //For the rest of the loop, the only memory dependency (array) is from B[]
+ a += B[ k] * s;
+ b += B[1+k] * s;
+ c += B[2+k] * s;
+ d += B[3+k] * s;
+ e += B[4+k] * s;
+ f += B[5+k] * s;
+ }
+
+ A[0] = a;
+ A[1] = b;
+ A[2] = c;
+ A[3] = d;
+ A[4] = e;
+ A[5] = f;
+}
+
+//***************************************************************************
+// various common computations involving the matrix J
+
+// compute iMJ = inv(M)*J'
+void quickstep::compute_invM_JT (int m, dRealPtr J, dRealMutablePtr iMJ, int *jb,
+ dxBody * const *body, dRealPtr invMOI)
+{
+ dRealMutablePtr iMJ_ptr = iMJ;
+ dRealPtr J_ptr = J;
+ for (int i=0; i<m; J_ptr += 12, iMJ_ptr += 12, i++) {
+ int b1 = jb[i*2];
+ int b2 = jb[i*2+1];
+ dReal k1 = body[b1]->invMass;
+ for (int j=0; j<3; j++) iMJ_ptr[j] = k1*J_ptr[j];
+ const dReal *invMOIrow1 = invMOI + 12*b1;
+ dMultiply0_331 (iMJ_ptr + 3, invMOIrow1, J_ptr + 3);
+ if (b2 >= 0) {
+ dReal k2 = body[b2]->invMass;
+ for (int j=0; j<3; j++) iMJ_ptr[j+6] = k2*J_ptr[j+6];
+ const dReal *invMOIrow2 = invMOI + 12*b2;
+ dMultiply0_331 (iMJ_ptr + 9, invMOIrow2, J_ptr + 9);
+ }
+ }
+}
+
+// compute out = inv(M)*J'*in.
+void quickstep::multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
+ dRealPtr in, dRealMutablePtr out)
+{
+ dSetZero (out,6*nb);
+ dRealPtr iMJ_ptr = iMJ;
+ for (int i=0; i<m; i++) {
+ int b1 = jb[i*2];
+ int b2 = jb[i*2+1];
+ const dReal in_i = in[i];
+ dRealMutablePtr out_ptr = out + b1*6;
+ for (int j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in_i;
+ iMJ_ptr += 6;
+ if (b2 >= 0) {
+ out_ptr = out + b2*6;
+ for (int j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in_i;
+ }
+ iMJ_ptr += 6;
+ }
+}
+
+// compute out = J*in.
+void quickstep::multiply_J (int m, dRealPtr J, int *jb,
+ dRealPtr in, dRealMutablePtr out)
+{
+ dRealPtr J_ptr = J;
+ for (int i=0; i<m; i++) {
+ int b1 = jb[i*2];
+ int b2 = jb[i*2+1];
+ dReal sum = 0;
+ dRealPtr in_ptr = in + b1*6;
+ for (int j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
+ J_ptr += 6;
+ if (b2 >= 0) {
+ in_ptr = in + b2*6;
+ for (int j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
+ }
+ J_ptr += 6;
+ out[i] = sum;
+ }
+}
+
+#ifdef USE_CG_LCP
+// compute out = (J*inv(M)*J' + cfm)*in.
+void quickstep::multiply_J_invM_JT (int m, int nb, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
+ dRealPtr cfm, dRealMutablePtr z, dRealMutablePtr in, dRealMutablePtr out)
+{
+ multiply_invM_JT (m,nb,iMJ,jb,in,z);
+ multiply_J (m,J,jb,z,out);
+
+ // add cfm
+ for (int i=0; i<m; i++) out[i] += cfm[i] * in[i];
+}
+#endif
+
+#ifdef REORDER_CONSTRAINTS
+int quickstep::compare_index_error (const void *a, const void *b)
+{
+ const IndexError *i1 = (IndexError*) a;
+ const IndexError *i2 = (IndexError*) b;
+ if (i1->findex < 0 && i2->findex >= 0) return -1;
+ if (i1->findex >= 0 && i2->findex < 0) return 1;
+ if (i1->error < i2->error) return -1;
+ if (i1->error > i2->error) return 1;
+ return 0;
+}
+#endif
+
+//***************************************************************************
+// Modifying inertia along constrained axes without modifying dynamics.
+void quickstep::DYNAMIC_INERTIA(const int infom, const dxJoint::Info2 &Jinfo, const int b1, const int b2,
+ const dJointWithInfo1 *jicurr,
+ dRealMutablePtr invMOI, dRealMutablePtr MOI)
+{
+ /// INERTIA PROPAGATION ACROSS CONSTRAINED JOINTS
+ for (int j=0; j<infom; j++) {
+#ifdef DEBUG_INERTIA_PROPAGATION
+ printf("--------JAC---------------\n");
+ printf("jacobian [%d] J1l [%f %f %f] J2l [%f %f %f] J1a [%f %f %f]"
+ " J2a [%f %f %f]\n", j,
+ Jinfo.J1l[0+j*Jinfo.rowskip],
+ Jinfo.J1l[1+j*Jinfo.rowskip],
+ Jinfo.J1l[2+j*Jinfo.rowskip],
+ Jinfo.J2l[0+j*Jinfo.rowskip],
+ Jinfo.J2l[1+j*Jinfo.rowskip],
+ Jinfo.J2l[2+j*Jinfo.rowskip],
+ Jinfo.J1a[0+j*Jinfo.rowskip],
+ Jinfo.J1a[1+j*Jinfo.rowskip],
+ Jinfo.J1a[2+j*Jinfo.rowskip],
+ Jinfo.J2a[0+j*Jinfo.rowskip],
+ Jinfo.J2a[1+j*Jinfo.rowskip],
+ Jinfo.J2a[2+j*Jinfo.rowskip]);
+#endif
+ /// \FIXME: For now, implement only for the two non-free axial rotation
+ /// constraints for hinge joints.
+ /// this only makes sense if joint connects two dynamic bodies (b2 >= 0)
+ /// Skip this entire block if we don't want to modify inertia for stability.
+ if (b2 >= 0 && jicurr->joint->type() == dJointTypeHinge &&
+ (j == 3 || j == 4))
+ {
+ /// In hinge joint, pure rotational constraint,
+ /// J1l and J2l should be zeros, and J1a and J2a should be equal
+ /// and opposite to each other.
+ /// J1a or J2a indicates the constrained axis direction.
+ /// For this implementation, determine constrained axis(s)
+ /// direction from J1a for hinge joints.
+
+
+ /// get the moment of inertia (MOI) for parent and child bodies constrained by J1a and J2a.
+ /// MOI and invMOI are already in inertial frame (previously rotated by body.posr.R)
+ /// get pointers to our invMOI/MOI matrices
+ dReal *invMOI_ptr1 = invMOI + b1 * 12;
+ dReal *invMOI_ptr2 = invMOI + b2 * 12;
+ dReal *MOI_ptr1 = MOI + b1 * 12;
+ dReal *MOI_ptr2 = MOI + b2 * 12;
+
+ // S: unit vector in the constrained axis direction
+ // S is the line along which we want to compute MOI
+ // FIXME: check that directions of J1a == J2a
+ dVector3 S =
+ {Jinfo.J1a[0+j*Jinfo.rowskip],
+ Jinfo.J1a[1+j*Jinfo.rowskip],
+ Jinfo.J1a[2+j*Jinfo.rowskip] };
+ dNormalize3(S);
+ // temporary vector used for matrix/vector math
+ dVector3 tmp31;
+
+ // compute scalar MOI in line with S for each body
+ // m1 = S' * MOI1 * S
+ dMultiply0_133(tmp31, S, MOI_ptr1);
+ dReal m1 = dCalcVectorDot3(tmp31, S);
+
+ // m2 = S' * MOI2 * S
+ dMultiply0_133(tmp31, S, MOI_ptr2);
+ dReal m2 = dCalcVectorDot3(tmp31, S);
+
+ // identify body with larger inertia
+ dReal m_large = m1;
+ dReal m_small = m2;
+ dReal *MOI_large = MOI_ptr1;
+ dReal *MOI_small = MOI_ptr2;
+ if (m2 > m1)
+ {
+ m_large = m2;
+ m_small = m1;
+ MOI_large = MOI_ptr2;
+ MOI_small = MOI_ptr1;
+ }
+
+ /// get full axis MOI tensor representing the scalar axis MOI.
+ // full MOI tensor for S needs matrix outer product of S:
+ // SS = [ S * S' ]
+ dMatrix3 SS = {
+ S[0]*S[0], S[0]*S[1], S[0]*S[2], 0,
+ S[1]*S[0], S[1]*S[1], S[1]*S[2], 0,
+ S[2]*S[0], S[2]*S[1], S[2]*S[2], 0};
+
+#ifdef DEBUG_INERTIA_PROPAGATION
+ printf("--------old MOI-----------\n");
+ printf("MOI1[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n", b1,
+ MOI_ptr1[0*4+0],MOI_ptr1[0*4+1],MOI_ptr1[0*4+2],MOI_ptr1[0*4+3],
+ MOI_ptr1[1*4+0],MOI_ptr1[1*4+1],MOI_ptr1[1*4+2],MOI_ptr1[1*4+3],
+ MOI_ptr1[2*4+0],MOI_ptr1[2*4+1],MOI_ptr1[2*4+2],MOI_ptr1[2*4+3]);
+ printf("MOI2[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n", b2,
+ MOI_ptr2[0*4+0],MOI_ptr2[0*4+1],MOI_ptr2[0*4+2],MOI_ptr2[0*4+3],
+ MOI_ptr2[1*4+0],MOI_ptr2[1*4+1],MOI_ptr2[1*4+2],MOI_ptr2[1*4+3],
+ MOI_ptr2[2*4+0],MOI_ptr2[2*4+1],MOI_ptr2[2*4+2],MOI_ptr2[2*4+3]);
+ printf("--------S VECTORS-----------\n");
+ printf("MOI1 b1[%d] S[%f %f %f] = %g\n",b1, S[0], S[1], S[2], m1);
+ printf("MOI2 b2[%d] S[%f %f %f] = %g\n",b2, S[0], S[1], S[2], m2);
+ printf("--------SS----------------\n");
+ printf("SS [%d]\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n", b1,
+ SS[0*4+0],SS[0*4+1],SS[0*4+2],SS[0*4+3],
+ SS[1*4+0],SS[1*4+1],SS[1*4+2],SS[1*4+3],
+ SS[2*4+0],SS[2*4+1],SS[2*4+2],SS[2*4+3]);
+#endif
+
+ // define maximum ratio of moment of inertia for adjacent bodies
+ // ie. m_large / m_small <= moi_ratio_max
+ /// \todo make moi_ratio_max adjustable
+ /// \todo automatically adjust moi_ratio_max such that
+ /// abs sum of off-diagonals remains smaller than the diagonal
+ /// for all rows (see comments below about Gauss-Seidel stability).
+ // increase moi_ratio_max to skip checks and increase performance
+ const dReal moi_ratio_max = 200.0;
+ if (m_large > moi_ratio_max * m_small)
+ {
+ // Large inertia ratio detected, try reducing it.
+ // Increase m_small by dm
+ // Reduce m_large by dm
+ // such that (m_large - dm) = moi_ratio_max * (m_small + dm)
+ // Intermediate math step:
+ // m_large - moi_ratio_max * m_small = dm * (1 + moi_ratio_max)
+ // Then:
+ dReal dm = (m_large - moi_ratio_max * m_small) / (1 + moi_ratio_max);
+
+ // This will then be applied to the bodies by multiplying by [S*S']
+ // MOI_large -= dm * SS
+ // MOI_small += dm * SS
+ // But first it should be verified that the change will not
+ // destabilize the Gauss-Seidel solver.
+ // To verify this, the Generalized Line Criterion for Gauss-Seidel
+ // is used, which is stated below:
+ //
+ // The Gauss-Seidel method will converge if for each row of the matrix:
+ // the sum of absolute values of off-diagonal elements
+ // is less than absolute value of the diagonal element.
+ //
+ // @article{Garcia2003,
+ // author = {Garcia, M.V.P. and {Humes Jr.}, C. and Stern, J.M.},
+ // doi = {10.1590/S0101-82052003000100006},
+ // issn = {0101-8205},
+ // journal = {Computational \& Applied Mathematics},
+ // number = {1},
+ // pages = {91--97},
+ // title = {{Generalized line criterion for Gauss-Seidel method}},
+ // url = {http://www.ime.usp.br/~jstern/papers/papersJS/ghs03.pdf},
+ // volume = {22},
+ // year = {2003}
+ // }
+
+ // For 3x3 Gauss-Seidel matrix M, this is equivalent to:
+ // abs(M[0][0]) > abs(M[0][1]) + abs(M[0][2])
+ // abs(M[1][1]) > abs(M[1][0]) + abs(M[1][2])
+ // abs(M[2][2]) > abs(M[2][0]) + abs(M[2][1])
+ //
+ // Since M is a mass matrix, it is positive definite, which implies
+ // that the diagonal elements are strictly positive:
+ // M[0][0] > abs(M[0][1]) + abs(M[0][2])
+ // M[1][1] > abs(M[1][0]) + abs(M[1][2])
+ // M[2][2] > abs(M[2][0]) + abs(M[2][1])
+ //
+ // For extra safety factor define a parameter gamma >= 1 such that:
+ // M[0][0] > gamma * (abs(M[0][1]) + abs(M[0][2]))
+ // M[1][1] > gamma * (abs(M[1][0]) + abs(M[1][2]))
+ // M[2][2] > gamma * (abs(M[2][0]) + abs(M[2][1]))
+ const dReal gamma = 1.0;
+
+ int problem = 0;
+ for (int row = 0; row < 3; ++row)
+ {
+ // off-diagonal columns
+ int col1 = (row + 1) % 3;
+ int col2 = (row + 2) % 3;
+
+ // diagonal index
+ int id = row*4 + row;
+
+ // off-diagonal indices
+ int iod1 = row*4 + col1;
+ int iod2 = row*4 + col2;
+
+ // diagonal element of adjusted M_large
+ dReal Md_large = MOI_large[id] - dm*SS[id];
+
+ // sum of absolute values of off-diagonal elements of adjusted M_large
+ dReal Mod_large = fabs(MOI_large[iod1] - dm*SS[iod1])
+ + fabs(MOI_large[iod2] - dm*SS[iod2]);
+
+ // diagonal element of adjusted M_small
+ dReal Md_small = MOI_small[id] + dm*SS[id];
+
+ // sum of absolute values of off-diagonal elements of adjusted M_small
+ dReal Mod_small = fabs(MOI_small[iod1] + dm*SS[iod1])
+ + fabs(MOI_small[iod2] + dm*SS[iod2]);
+
+ if (Md_large <= gamma*Mod_large || Md_small <= gamma*Mod_small)
+ {
+ problem = 1;
+ }
+ }
+
+ if (problem == 0)
+ {
+ // Everything looks good, update the inertia matrices
+ for (int i = 0; i < 12; ++i)
+ {
+ int col = i%4;
+ if (col == 3)
+ {
+ // set unused terms to zero
+ MOI_large[i] = 0;
+ MOI_small[i] = 0;
+ }
+ else
+ {
+ MOI_large[i] -= dm * SS[i];
+ MOI_small[i] += dm * SS[i];
+ }
+ }
+ }
+
+ // Update invMOI by inverting analytically (may not be efficient).
+ // try 1981 Ken Miller (http://www.jstor.org/stable/2690437) or
+ // (http://math.stackexchange.com/questions/17776)
+ // try taking advantage of symmetry of MOI
+ dReal det1 = MOI_ptr1[0*4+0]*(MOI_ptr1[2*4+2]*MOI_ptr1[1*4+1]
+ -MOI_ptr1[2*4+1]*MOI_ptr1[1*4+2])
+ -MOI_ptr1[1*4+0]*(MOI_ptr1[2*4+2]*MOI_ptr1[0*4+1]
+ -MOI_ptr1[2*4+1]*MOI_ptr1[0*4+2])
+ +MOI_ptr1[2*4+0]*(MOI_ptr1[1*4+2]*MOI_ptr1[0*4+1]
+ -MOI_ptr1[1*4+1]*MOI_ptr1[0*4+2]);
+ invMOI_ptr1[0*4+0] = (MOI_ptr1[2*4+2]*MOI_ptr1[1*4+1]
+ -MOI_ptr1[2*4+1]*MOI_ptr1[1*4+2])/det1;
+ invMOI_ptr1[0*4+1] = -(MOI_ptr1[2*4+2]*MOI_ptr1[0*4+1]
+ -MOI_ptr1[2*4+1]*MOI_ptr1[0*4+2])/det1;
+ invMOI_ptr1[0*4+2] = (MOI_ptr1[1*4+2]*MOI_ptr1[0*4+1]
+ -MOI_ptr1[1*4+1]*MOI_ptr1[0*4+2])/det1;
+ // invMOI_ptr1[0*4+3] = 0.0;
+ invMOI_ptr1[1*4+0] = invMOI_ptr1[0*4+1];
+ invMOI_ptr1[1*4+1] = (MOI_ptr1[2*4+2]*MOI_ptr1[0*4+0]
+ -MOI_ptr1[2*4+0]*MOI_ptr1[0*4+2])/det1;
+ invMOI_ptr1[1*4+2] = -(MOI_ptr1[1*4+2]*MOI_ptr1[0*4+0]
+ -MOI_ptr1[1*4+0]*MOI_ptr1[0*4+2])/det1;
+ // invMOI_ptr1[1*4+3] = 0.0;
+ invMOI_ptr1[2*4+0] = invMOI_ptr1[0*4+2];
+ invMOI_ptr1[2*4+1] = invMOI_ptr1[1*4+2];
+ invMOI_ptr1[2*4+2] = (MOI_ptr1[1*4+1]*MOI_ptr1[0*4+0]
+ -MOI_ptr1[1*4+0]*MOI_ptr1[0*4+1])/det1;
+ // invMOI_ptr1[2*4+3] = 0.0;
+
+ dReal det2 = MOI_ptr2[0*4+0]*(MOI_ptr2[2*4+2]*MOI_ptr2[1*4+1]
+ -MOI_ptr2[2*4+1]*MOI_ptr2[1*4+2])
+ -MOI_ptr2[1*4+0]*(MOI_ptr2[2*4+2]*MOI_ptr2[0*4+1]
+ -MOI_ptr2[2*4+1]*MOI_ptr2[0*4+2])
+ +MOI_ptr2[2*4+0]*(MOI_ptr2[1*4+2]*MOI_ptr2[0*4+1]
+ -MOI_ptr2[1*4+1]*MOI_ptr2[0*4+2]);
+ invMOI_ptr2[0*4+0] = (MOI_ptr2[2*4+2]*MOI_ptr2[1*4+1]
+ -MOI_ptr2[2*4+1]*MOI_ptr2[1*4+2])/det2;
+ invMOI_ptr2[0*4+1] = -(MOI_ptr2[2*4+2]*MOI_ptr2[0*4+1]
+ -MOI_ptr2[2*4+1]*MOI_ptr2[0*4+2])/det2;
+ invMOI_ptr2[0*4+2] = (MOI_ptr2[1*4+2]*MOI_ptr2[0*4+1]
+ -MOI_ptr2[1*4+1]*MOI_ptr2[0*4+2])/det2;
+ // invMOI_ptr2[0*4+3] = 0.0;
+ invMOI_ptr2[1*4+0] = invMOI_ptr2[0*4+1];
+ invMOI_ptr2[1*4+1] = (MOI_ptr2[2*4+2]*MOI_ptr2[0*4+0]
+ -MOI_ptr2[2*4+0]*MOI_ptr2[0*4+2])/det2;
+ invMOI_ptr2[1*4+2] = -(MOI_ptr2[1*4+2]*MOI_ptr2[0*4+0]
+ -MOI_ptr2[1*4+0]*MOI_ptr2[0*4+2])/det2;
+ // invMOI_ptr2[1*4+3] = 0.0;
+ invMOI_ptr2[2*4+0] = invMOI_ptr2[0*4+2];
+ invMOI_ptr2[2*4+1] = invMOI_ptr2[1*4+2];
+ invMOI_ptr2[2*4+2] = (MOI_ptr2[1*4+1]*MOI_ptr2[0*4+0]
+ -MOI_ptr2[1*4+0]*MOI_ptr2[0*4+1])/det2;
+ // invMOI_ptr2[2*4+3] = 0.0;
+
+ #ifdef DEBUG_INERTIA_PROPAGATION
+ printf("---------S Scalars--------\n");
+ printf(" original S1 [%g] S2 [%g]\n", m1, m2);
+ printf(" distributed S1 [%g] S2 [%g]\n", m1_new, m2_new);
+ printf("----------new MOI---------\n");
+
+ printf("new MOI1[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n",
+ b1,
+ MOI_ptr1[0*4+0],MOI_ptr1[0*4+1],MOI_ptr1[0*4+2],MOI_ptr1[0*4+3],
+ MOI_ptr1[1*4+0],MOI_ptr1[1*4+1],MOI_ptr1[1*4+2],MOI_ptr1[1*4+3],
+ MOI_ptr1[2*4+0],MOI_ptr1[2*4+1],MOI_ptr1[2*4+2],MOI_ptr1[2*4+3]);
+
+ // Modify MOI_ptr2
+ printf("new MOI2[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n",
+ b2,
+ MOI_ptr2[0*4+0],MOI_ptr2[0*4+1],MOI_ptr2[0*4+2],MOI_ptr2[0*4+3],
+ MOI_ptr2[1*4+0],MOI_ptr2[1*4+1],MOI_ptr2[1*4+2],MOI_ptr2[1*4+3],
+ MOI_ptr2[2*4+0],MOI_ptr2[2*4+1],MOI_ptr2[2*4+2],MOI_ptr2[2*4+3]);
+
+ // double check resulting MOI along s
+ dMultiply0_133(tmp31, S, MOI_ptr1);
+ // scalar body 1 MOI component along vector S
+ m1 = dCalcVectorDot3(tmp31, S);
+ printf("new MOI1 along S [%f]\n", m1);
+ dMultiply0_133(tmp31, S, MOI_ptr2);
+ // scalar body 2 MOI component along vector S
+ m2 = dCalcVectorDot3(tmp31, S);
+ printf("new MOI2 along S [%f]\n", m2);
+
+ /// \todo double check resulting MOI along joint axis and
+ /// see that it's the same
+
+ printf("----------new inv---------\n");
+ printf("new invMOI1[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n"
+ "[%f %f %f %f]\n", b1,
+ invMOI_ptr1[0*4+0], invMOI_ptr1[0*4+1],
+ invMOI_ptr1[0*4+2], invMOI_ptr1[0*4+3],
+ invMOI_ptr1[1*4+0], invMOI_ptr1[1*4+1],
+ invMOI_ptr1[1*4+2], invMOI_ptr1[1*4+3],
+ invMOI_ptr1[2*4+0], invMOI_ptr1[2*4+1],
+ invMOI_ptr1[2*4+2], invMOI_ptr1[2*4+3]);
+
+ // Modify invMOI_ptr2
+ printf("new invMOI2[%d]\n[%f %f %f %f]\n[%f %f %f %f]\n"
+ "[%f %f %f %f]\n", b2,
+ invMOI_ptr2[0*4+0], invMOI_ptr2[0*4+1],
+ invMOI_ptr2[0*4+2], invMOI_ptr2[0*4+3],
+ invMOI_ptr2[1*4+0], invMOI_ptr2[1*4+1],
+ invMOI_ptr2[1*4+2], invMOI_ptr2[1*4+3],
+ invMOI_ptr2[2*4+0], invMOI_ptr2[2*4+1],
+ invMOI_ptr2[2*4+2], invMOI_ptr2[2*4+3]);
+ #endif
+
+ #ifdef DEBUG_INERTIA_PROPAGATION
+ // check if diagonally-dominant
+ if (MOI_ptr1[0*4+0] < dFabs(MOI_ptr1[0*4+1])+dFabs(MOI_ptr1[0*4+2]))
+ printf(" * new MOI1 row 1 d[%f] < o[%f, %f]\n",
+ MOI_ptr1[0*4+0],MOI_ptr1[0*4+1], MOI_ptr1[0*4+2]);
+ if (MOI_ptr1[1*4+1] < dFabs(MOI_ptr1[1*4+0])+dFabs(MOI_ptr1[1*4+2]))
+ printf(" * new MOI1 row 2 d[%f] < o[%f, %f]\n",
+ MOI_ptr1[1*4+1],MOI_ptr1[1*4+0], MOI_ptr1[1*4+2]);
+ if (MOI_ptr1[2*4+2] < dFabs(MOI_ptr1[2*4+0])+dFabs(MOI_ptr1[2*4+1]))
+ printf(" * new MOI1 row 3 d[%f] < o[%f, %f]\n",
+ MOI_ptr1[2*4+2],MOI_ptr1[2*4+0], MOI_ptr1[2*4+1]);
+
+ if (MOI_ptr2[0*4+0] < dFabs(MOI_ptr2[0*4+1])+dFabs(MOI_ptr2[0*4+2]))
+ printf(" * new MOI2 row 1 d[%f] < o[%f, %f]\n",
+ MOI_ptr2[0*4+0],MOI_ptr2[0*4+1], MOI_ptr2[0*4+2]);
+ if (MOI_ptr2[1*4+1] < dFabs(MOI_ptr2[1*4+0])+dFabs(MOI_ptr2[1*4+2]))
+ printf(" * new MOI2 row 2 d[%f] < o[%f, %f]\n",
+ MOI_ptr2[1*4+1],MOI_ptr2[1*4+0], MOI_ptr2[1*4+2]);
+ if (MOI_ptr2[2*4+2] < dFabs(MOI_ptr2[2*4+0])+dFabs(MOI_ptr2[2*4+1]))
+ printf(" * new MOI2 row 3 d[%f] < o[%f, %f]\n",
+ MOI_ptr2[2*4+2],MOI_ptr2[2*4+0], MOI_ptr2[2*4+1]);
+ #endif
+ }
+ }
+ }
+}
diff --git a/deps/opende/src/quickstep_util.h b/deps/opende/src/quickstep_util.h
new file mode 100644
index 0000000..483aedf
--- /dev/null
+++ b/deps/opende/src/quickstep_util.h
@@ -0,0 +1,270 @@
+/*************************************************************************
+ * *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
+ * All rights reserved. Email: russ at q12.org Web: www.q12.org *
+ * *
+ * This library is free software; you can redistribute it and/or *
+ * modify it under the terms of EITHER: *
+ * (1) The GNU Lesser General Public License as published by the Free *
+ * Software Foundation; either version 2.1 of the License, or (at *
+ * your option) any later version. The text of the GNU Lesser *
+ * General Public License is included with this library in the *
+ * file LICENSE.TXT. *
+ * (2) The BSD-style license that is included with this library in *
+ * the file LICENSE-BSD.TXT. *
+ * *
+ * This library is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
+ * *
+ *************************************************************************/
+
+#ifndef _ODE_QUICK_STEP_UTIL_H_
+#define _ODE_QUICK_STEP_UTIL_H_
+
+#include <ode/common.h>
+#include "gazebo/gazebo_config.h"
+
+#ifdef ODE_SSE
+#include <xmmintrin.h>
+#define Kf(x) _mm_set_pd((x),(x))
+#endif
+
+
+#undef REPORT_THREAD_TIMING
+#undef USE_TPROW
+#undef TIMING
+#undef DEBUG_CONVERGENCE_TOLERANCE
+#undef SHOW_CONVERGENCE
+#define SMOOTH_LAMBDA
+#undef USE_1NORM
+#undef DEBUG_INERTIA_PROPAGATION
+//#define LOCAL_STEPPING // not yet implemented
+//#define PENETRATION_JVERROR_CORRECTION
+//#define POST_UPDATE_CONSTRAINT_VIOLATION_CORRECTION
+
+#undef CHECK_VELOCITY_OBEYS_CONSTRAINT
+
+#ifndef TIMING
+#ifdef HDF5_INSTRUMENT
+#define DUMP
+#include <ode/h5dump.h>
+#define DATA_FILE "ode_frames.hdf5"
+#endif // instrument
+#endif // timing
+
+#ifdef USE_TPROW
+// added for threading per constraint rows
+#include <boost/thread/recursive_mutex.hpp>
+#include <boost/bind.hpp>
+#include "ode/odeinit.h"
+#endif
+
+typedef const dReal *dRealPtr;
+typedef dReal *dRealMutablePtr;
+
+//***************************************************************************
+// configuration
+
+// for the PGS and CG methods:
+// warm starting:
+// this definitely help for motor-driven joints.
+// unfortunately it appears to hurt with high-friction contacts
+// using the PGS method. use with care
+
+// for the PGS method:
+// uncomment the following line to determine a new constraint-solving
+// order for each iteration. however, the qsort per iteration is expensive,
+// and the optimal order is somewhat problem dependent.
+// @@@ try the leaf->root ordering.
+
+// #define REORDER_CONSTRAINTS 1
+
+// for the PGS method:
+// uncomment the following line to randomly reorder constraint rows
+// during the solution. depending on the situation, this can help a lot
+// or hardly at all, but it doesn't seem to hurt.
+
+// #define RANDOMLY_REORDER_CONSTRAINTS 1
+#undef LOCK_WHILE_RANDOMLY_REORDER_CONSTRAINTS
+
+/// scale SOR for contact to reduce overshoot in solution for contacts
+/// \TODO: make this a parameter
+#define CONTACT_SOR_SCALE 0.25
+
+//***************************************************************************
+// testing stuff
+
+#ifdef TIMING
+#define IFTIMING(x) x
+#else
+#define IFTIMING(x) ((void)0)
+#endif
+
+#ifdef DUMP
+#define IFDUMP(x) x
+#else
+#define IFDUMP(x) ((void)0)
+#endif
+// ****************************************************************
+// ******************* Struct Definition **************************
+// ****************************************************************
+struct dJointWithInfo1
+{
+ dxJoint *joint;
+ dxJoint::Info1 info;
+};
+
+struct IndexError {
+#ifdef REORDER_CONSTRAINTS
+ dReal error; // error to sort on
+ int findex;
+#endif
+ int index; // row index
+};
+
+// structure for passing variable pointers in PGS_LCP
+struct dxPGSLCPParameters {
+ int thread_id;
+ IndexError* order;
+ dxBody* const* body;
+ boost::recursive_mutex* mutex;
+ bool inline_position_correction;
+ bool position_correction_thread;
+ dxQuickStepParameters *qs;
+ int nStart; // 0
+ int nChunkSize;
+ int m; // m
+ int nb;
+#ifdef PENETRATION_JVERROR_CORRECTION
+ dReal stepsize;
+ dRealMutablePtr vnew;
+#endif
+ int* jb;
+ const int* findex;
+ bool skip_friction;
+ dRealPtr hi;
+ dRealPtr lo;
+ dRealPtr invMOI;
+ dRealPtr MOI;
+ dRealPtr Ad;
+ dRealPtr Adcfm;
+ dRealPtr Adcfm_precon;
+ dRealPtr J;
+ dRealPtr iMJ;
+ dRealPtr rhs_precon ;
+ dRealPtr J_precon ;
+ dRealPtr J_orig ;
+ dRealMutablePtr cforce ;
+
+ dRealPtr rhs;
+ dRealMutablePtr caccel;
+ dRealMutablePtr lambda;
+
+ /// Only used if THREAD_POSITION_CORRECTION is not active,
+ /// in that case, compute both updates in the same
+ /// ComputeRows update.
+ dRealPtr rhs_erp;
+ dRealMutablePtr caccel_erp;
+ dRealMutablePtr lambda_erp;
+
+#ifdef REORDER_CONSTRAINTS
+ dRealMutablePtr last_lambda ;
+ dRealMutablePtr last_lambda_erp;
+#endif
+};
+// ****************************************************************
+// ******************* Util Functions *****************************
+// ****************************************************************
+
+//****************************************************************************
+// special matrix multipliers
+
+namespace ode {
+ namespace quickstep{
+
+// multiply block of B matrix (q x 6) with 12 dReal per row with C vektor (q)
+void Multiply1_12q1 (dReal *A, const dReal *B, const dReal *C, int q);
+
+// compute iMJ = inv(M)*J'
+void compute_invM_JT (int m, dRealPtr J, dRealMutablePtr iMJ, int *jb,
+ dxBody * const *body, dRealPtr invMOI);
+
+// compute out = inv(M)*J'*in.
+void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
+ dRealPtr in, dRealMutablePtr out);
+
+// compute out = J*in.
+void multiply_J (int m, dRealPtr J, int *jb,
+ dRealPtr in, dRealMutablePtr out);
+
+#ifdef USE_CG_LCP
+// compute out = (J*inv(M)*J' + cfm)*in.
+void multiply_J_invM_JT (int m, int nb, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
+ dRealPtr cfm, dRealMutablePtr z, dRealMutablePtr in, dRealMutablePtr out);
+#endif
+
+// dot product of two vector x, y with length of n
+inline dReal dot_n (int n, dRealPtr x, dRealPtr y)
+{
+ dReal sum=0;
+ for (int i=0; i<n; i++) sum += x[i]*y[i];
+ return sum;
+}
+
+// x = y + z*alpha
+inline void scaled_add (int n, dRealMutablePtr x, dRealPtr y, dRealPtr z, dReal alpha)
+{
+ for (int i=0; i<n; i++) x[i] = y[i] + z[i]*alpha;
+}
+
+// dot product of two vector a and b with length 6
+// define ODE_SSE to enable SSE, which is used to speed up
+// vector math operations with gcc compiler
+// macro SSE is renamed to ODE_SSE due to conflict with Eigen3 in DART
+inline dReal dot6(dRealPtr a, dRealPtr b)
+{
+#ifdef ODE_SSE
+ __m128d d = _mm_load_pd(a+0) * _mm_load_pd(b+0) + _mm_load_pd(a+2) * _mm_load_pd(b+2) + _mm_load_pd(a+4) * _mm_load_pd(b+4);
+ double r[2];
+ _mm_store_pd(r, d);
+ return r[0] + r[1];
+#else
+ return a[0] * b[0] +
+ a[1] * b[1] +
+ a[2] * b[2] +
+ a[3] * b[3] +
+ a[4] * b[4] +
+ a[5] * b[5];
+#endif
+}
+
+// a = a + delta * b, vector a and b with length 6
+inline void sum6(dRealMutablePtr a, dReal delta, dRealPtr b)
+{
+#ifdef ODE_SSE
+ __m128d __delta = Kf(delta);
+ _mm_store_pd(a + 0, _mm_load_pd(a + 0) + __delta * _mm_load_pd(b + 0));
+ _mm_store_pd(a + 2, _mm_load_pd(a + 2) + __delta * _mm_load_pd(b + 2));
+ _mm_store_pd(a + 4, _mm_load_pd(a + 4) + __delta * _mm_load_pd(b + 4));
+#else
+ a[0] += delta * b[0];
+ a[1] += delta * b[1];
+ a[2] += delta * b[2];
+ a[3] += delta * b[3];
+ a[4] += delta * b[4];
+ a[5] += delta * b[5];
+#endif
+}
+
+// compare the index error when REORDER_CONSTRAINTS is defined
+int compare_index_error (const void *a, const void *b);
+
+// Modifying inertia along constrained axes without modifying dynamics.
+void DYNAMIC_INERTIA(const int infom, const dxJoint::Info2 &Jinfo, const int b1, const int b2,
+ const dJointWithInfo1 *jicurr,
+ dRealMutablePtr invMOI, dRealMutablePtr MOI);
+ } // namespace quickstep
+} // namespace ode
+#endif
diff --git a/deps/opende/src/step.cpp b/deps/opende/src/step.cpp
index 42fca21..8bf379d 100644
--- a/deps/opende/src/step.cpp
+++ b/deps/opende/src/step.cpp
@@ -32,15 +32,21 @@
#include "lcp.h"
#include "util.h"
#include "joints/hinge.h"
+#include "gazebo/gazebo_config.h"
+
+#ifdef HAVE_DART
+#include "step_dart_pgs_wrapper.h"
+#endif
+
+#ifdef HDF5_INSTRUMENT
+#include <ode/h5dump.h>
+#endif
//****************************************************************************
// misc defines
-#define USE_JOINT_DAMPING
-
//#define TIMING
-
#ifdef TIMING
#define IFTIMING(x) x
#else
@@ -270,7 +276,9 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
}
}
}
-
+#ifdef HDF5_INSTRUMENT
+ h5dump_world("ode_frames.hdf5", world, stepsize);
+#endif
// get m = total constraint dimension, nub = number of unbounded variables.
// create constraint offset array and number-of-rows array for all joints.
// the constraints are re-ordered as follows: the purely unbounded
@@ -428,24 +436,6 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
m = mcurr;
}
-
-#ifdef USE_JOINT_DAMPING
- /************************************************************************/
- /* for joint damping, get the total number of rows for damping jacobian */
- /************************************************************************/
- int m_damp; // number of rows for damped joint jacobian
- {
- int mcurr = 0;
- const dJointWithInfo1 *jicurr = jointiinfos; // info1 stored in jointiinfos
- const dJointWithInfo1 *const jiend = jicurr + nj;
- for (; jicurr != jiend; jicurr++)
- if (jicurr->joint->use_damping)
- mcurr ++;
-
- m_damp = mcurr;
- }
-#endif
-
// this will be set to the force due to the constraints
dReal *cforce = context->AllocateArray<dReal> (nb*8);
dSetZero (cforce,nb*8);
@@ -456,6 +446,8 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
// force mixing vector `cfm', and LCP low and high bound vectors, and an
// 'findex' vector.
dReal *lo, *hi, *J, *A, *rhs;
+ dReal *c_v_max;
+ World_Solver_Type solver_type;
int *findex;
{
@@ -473,6 +465,11 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
findex = context->AllocateArray<int> (mlocal);
for (int i=0; i<mlocal; ++i) findex[i] = -1;
+
+ c_v_max = context->AllocateArray<dReal> (mlocal);
+ for(int i=0; i<mlocal; i++) c_v_max[i] = world->contactp.max_vel;
+ solver_type = world->qs.world_solver_type;
+
int mskip = dPAD(mlocal);
A = context->AllocateArray<dReal> (mlocal*mskip);
dSetZero (A,mlocal*mskip);
@@ -481,23 +478,6 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
dSetZero (rhs,mlocal);
}
-#ifdef USE_JOINT_DAMPING
- dReal *J_damp = NULL;
- dReal *coeff_damp = NULL;
- {
- int mlocal = m_damp;
-
- const unsigned jelements = mlocal*12;
- J_damp = context->AllocateArray<dReal> (jelements);
- dSetZero (J_damp,jelements);
-
- const unsigned coeffelements = mlocal;
- coeff_damp = context->AllocateArray<dReal> (coeffelements);
- dSetZero (coeff_damp,coeffelements);
- }
-#endif
-
-
// Put 'c' in the same memory as 'rhs' as they transit into each other
dReal *c = rhs; rhs = NULL; // erase rhs pointer for now as it is not to be used yet
@@ -534,9 +514,6 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
Jinfo.erp = world->global_erp;
unsigned ofsi = 0;
-#ifdef USE_JOINT_DAMPING
- unsigned ofsi_damp = 0;
-#endif
const dJointWithInfo1 *jicurr = jointiinfos;
const dJointWithInfo1 *const jiend = jicurr + nj;
for (; jicurr != jiend; ++jicurr) {
@@ -552,27 +529,8 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
Jinfo.lo = lo + ofsi;
Jinfo.hi = hi + ofsi;
Jinfo.findex = findex + ofsi;
-
-#ifdef USE_JOINT_DAMPING
- /*******************************************************/
- /* allocate space for damped joint Jacobians */
- /*******************************************************/
- if (jicurr->joint->use_damping)
- {
- // damping coefficient is in jicurr->info.damping_coefficient);
- coeff_damp[ofsi_damp] = jicurr->joint->damping_coefficient;
-
- // setup joint damping pointers so getinfo2 will fill in J_damp
- dReal *const Jrow_damp = J_damp + ofsi_damp * 12;
- Jinfo.J1ld = Jrow_damp;
- Jinfo.J1ad = Jrow_damp + 3;
- Jinfo.J2ld = Jrow_damp + 6;
- Jinfo.J2ad = Jrow_damp + 9;
- // one row of constraint per joint
- ofsi_damp ++;
- }
-#endif
-
+ Jinfo.c_v_max = c_v_max + ofsi;
+
dxJoint *joint = jicurr->joint;
joint->getInfo2 (&Jinfo);
@@ -773,10 +731,25 @@ void dInternalStepIsland_x2 (dxWorldProcessContext *context,
BEGIN_STATE_SAVE(context, lcpstate) {
IFTIMING(dTimerNow ("solving LCP problem"));
- // solve the LCP problem and get lambda.
- // this will destroy A but that's OK
- dSolveLCP (context, m, A, lambda, rhs, NULL, nub, lo, hi, findex);
-
+ if (solver_type == ODE_DEFAULT)
+ {
+ // solve the LCP problem and get lambda.
+ // this will destroy A but that's OK
+ dSolveLCP (context, m, A, lambda, rhs, NULL, nub, lo, hi, findex);
+ }
+ else if (solver_type == DART_PGS)
+ {
+#ifdef HAVE_DART
+ const int mskip = dPAD(m);
+ dSolveLCP_dart_pgs(m, mskip, A, lambda, rhs, nub, lo, hi, findex);
+#else
+ dMessage(d_ERR_LCP, "HAVE_DART is NOT defined");
+#endif
+ }
+ else
+ {
+ dMessage(d_ERR_LCP, "Unrecognized Solver Type");
+ }
} END_STATE_SAVE(context, lcpstate);
{
@@ -928,24 +901,6 @@ size_t dxEstimateStepMemoryRequirements (dxBody * const * /*body*/, int nb, dxJo
nj = njcurr; m = mcurr;
}
-#ifdef USE_JOINT_DAMPING
- int m_damp;
- {
- int m_dampcurr = 0;
- //dxJoint::SureMaxInfo info;
- dxJoint *const *const _jend = _joint + _nj;
- for (dxJoint *const *_jcurr = _joint; _jcurr != _jend; _jcurr++) {
- dxJoint *j = *_jcurr;
- /***************************/
- /* size for damping joints */
- /***************************/
- if (j->use_damping)
- m_dampcurr ++;
- }
- m_damp = m_dampcurr;
- }
-#endif
-
size_t res = 0;
res += dEFFICIENT_SIZE(sizeof(dReal) * 3 * 4 * nb); // for invI
@@ -962,10 +917,7 @@ size_t dxEstimateStepMemoryRequirements (dxBody * const * /*body*/, int nb, dxJo
sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * mskip * m); // for A
sub1_res2 += 3 * dEFFICIENT_SIZE(sizeof(dReal) * m); // for lo, hi, rhs
sub1_res2 += dEFFICIENT_SIZE(sizeof(int) * m); // for findex
-#ifdef USE_JOINT_DAMPING
- sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * 12 * m_damp); // for J_damp
- sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * m_damp); // for coeff_damp
-#endif
+ sub1_res2 += dEFFICIENT_SIZE(sizeof(dReal) * m); // for c_v_max
{
size_t sub2_res1 = dEFFICIENT_SIZE(sizeof(dReal) * m); // for cfm
sub2_res1 += dEFFICIENT_SIZE(sizeof(dReal) * 2 * 8 * m); // for JinvM
diff --git a/deps/opende/src/step_dart_pgs_wrapper.cpp b/deps/opende/src/step_dart_pgs_wrapper.cpp
new file mode 100644
index 0000000..5caa89e
--- /dev/null
+++ b/deps/opende/src/step_dart_pgs_wrapper.cpp
@@ -0,0 +1,46 @@
+/*
+ * 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"
+
+#ifdef HAVE_DART
+#include "dart/constraint/PGSLCPSolver.h"
+#include "step_dart_pgs_wrapper.h"
+
+void dSolveLCP_dart_pgs(int m, int mskip, dReal *A, dReal *x, dReal *b,
+ int nub, dReal *lo, dReal *hi, int *findex)
+{
+ // For the case m = 0, return true;
+ if(m>0)
+ {
+ dart::constraint::PGSOption option;
+ option.setDefault();
+ dart::constraint::solvePGS(m, mskip, nub, A, x, b, lo, hi, findex, &option);
+ }
+}
+#endif
diff --git a/deps/opende/src/step_dart_pgs_wrapper.h b/deps/opende/src/step_dart_pgs_wrapper.h
new file mode 100644
index 0000000..1922965
--- /dev/null
+++ b/deps/opende/src/step_dart_pgs_wrapper.h
@@ -0,0 +1,24 @@
+/*
+ * 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 _ODE_STEP_DART_PGS_WRAPPER_H_
+#define _ODE_STEP_DART_PGS_WRAPPER_H_
+
+#include <ode/common.h>
+void dSolveLCP_dart_pgs(int m, int mskip, dReal *A, dReal *x, dReal *b,
+ int nub, dReal *lo, dReal *hi, int *findex);
+
+#endif
diff --git a/deps/opende/src/symm.c b/deps/opende/src/symm.c
index bb656b8..48920c0 100644
--- a/deps/opende/src/symm.c
+++ b/deps/opende/src/symm.c
@@ -20,7 +20,7 @@ typedef short ftnlen;
typedef int logical;
typedef double doublereal;
-#define dabs(x) (doublereal)abs(x)
+#define dabs(x) (doublereal)fabs(x)
#define min(a,b) ((a) <= (b) ? (a) : (b))
#define max(a,b) ((a) >= (b) ? (a) : (b))
#define dmax(a,b) (doublereal)max(a,b)
diff --git a/deps/opende/src/util.cpp b/deps/opende/src/util.cpp
index 82d7186..38236af 100644
--- a/deps/opende/src/util.cpp
+++ b/deps/opende/src/util.cpp
@@ -115,140 +115,140 @@ void dxWorldProcessContext::FreePreallocationsContext()
void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize)
{
- dxBody *bb;
- for ( bb=world->firstbody; bb; bb=(dxBody*)bb->next )
- {
- // don't freeze objects mid-air (patch 1586738)
- if ( bb->firstjoint == NULL ) continue;
-
- // nothing to do unless this body is currently enabled and has
- // the auto-disable flag set
- if ( (bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable ) continue;
-
- // if sampling / threshold testing is disabled, we can never sleep.
- if ( bb->adis.average_samples == 0 ) continue;
-
- //
- // see if the body is idle
- //
-
+ dxBody *bb;
+ for ( bb=world->firstbody; bb; bb=(dxBody*)bb->next )
+ {
+ // don't freeze objects mid-air (patch 1586738)
+ if ( bb->firstjoint == NULL ) continue;
+
+ // nothing to do unless this body is currently enabled and has
+ // the auto-disable flag set
+ if ( (bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable ) continue;
+
+ // if sampling / threshold testing is disabled, we can never sleep.
+ if ( bb->adis.average_samples == 0 ) continue;
+
+ //
+ // see if the body is idle
+ //
+
#ifndef dNODEBUG
- // sanity check
- if ( bb->average_counter >= bb->adis.average_samples )
- {
- dUASSERT( bb->average_counter < bb->adis.average_samples, "buffer overflow" );
-
- // something is going wrong, reset the average-calculations
- bb->average_ready = 0; // not ready for average calculation
- bb->average_counter = 0; // reset the buffer index
- }
+ // sanity check
+ if ( bb->average_counter >= bb->adis.average_samples )
+ {
+ dUASSERT( bb->average_counter < bb->adis.average_samples, "buffer overflow" );
+
+ // something is going wrong, reset the average-calculations
+ bb->average_ready = 0; // not ready for average calculation
+ bb->average_counter = 0; // reset the buffer index
+ }
#endif // dNODEBUG
- // sample the linear and angular velocity
- bb->average_lvel_buffer[bb->average_counter][0] = bb->lvel[0];
- bb->average_lvel_buffer[bb->average_counter][1] = bb->lvel[1];
- bb->average_lvel_buffer[bb->average_counter][2] = bb->lvel[2];
- bb->average_avel_buffer[bb->average_counter][0] = bb->avel[0];
- bb->average_avel_buffer[bb->average_counter][1] = bb->avel[1];
- bb->average_avel_buffer[bb->average_counter][2] = bb->avel[2];
- bb->average_counter++;
-
- // buffer ready test
- if ( bb->average_counter >= bb->adis.average_samples )
- {
- bb->average_counter = 0; // fill the buffer from the beginning
- bb->average_ready = 1; // this body is ready now for average calculation
- }
-
- int idle = 0; // Assume it's in motion unless we have samples to disprove it.
-
- // enough samples?
- if ( bb->average_ready )
- {
- idle = 1; // Initial assumption: IDLE
-
- // the sample buffers are filled and ready for calculation
- dVector3 average_lvel, average_avel;
-
- // Store first velocity samples
- average_lvel[0] = bb->average_lvel_buffer[0][0];
- average_avel[0] = bb->average_avel_buffer[0][0];
- average_lvel[1] = bb->average_lvel_buffer[0][1];
- average_avel[1] = bb->average_avel_buffer[0][1];
- average_lvel[2] = bb->average_lvel_buffer[0][2];
- average_avel[2] = bb->average_avel_buffer[0][2];
-
- // If we're not in "instantaneous mode"
- if ( bb->adis.average_samples > 1 )
- {
- // add remaining velocities together
- for ( unsigned int i = 1; i < bb->adis.average_samples; ++i )
- {
- average_lvel[0] += bb->average_lvel_buffer[i][0];
- average_avel[0] += bb->average_avel_buffer[i][0];
- average_lvel[1] += bb->average_lvel_buffer[i][1];
- average_avel[1] += bb->average_avel_buffer[i][1];
- average_lvel[2] += bb->average_lvel_buffer[i][2];
- average_avel[2] += bb->average_avel_buffer[i][2];
- }
-
- // make average
- dReal r1 = dReal( 1.0 ) / dReal( bb->adis.average_samples );
-
- average_lvel[0] *= r1;
- average_avel[0] *= r1;
- average_lvel[1] *= r1;
- average_avel[1] *= r1;
- average_lvel[2] *= r1;
- average_avel[2] *= r1;
- }
-
- // threshold test
- dReal av_lspeed, av_aspeed;
- av_lspeed = dCalcVectorDot3( average_lvel, average_lvel );
- if ( av_lspeed > bb->adis.linear_average_threshold )
- {
- idle = 0; // average linear velocity is too high for idle
- }
- else
- {
- av_aspeed = dCalcVectorDot3( average_avel, average_avel );
- if ( av_aspeed > bb->adis.angular_average_threshold )
- {
- idle = 0; // average angular velocity is too high for idle
- }
- }
- }
-
- // if it's idle, accumulate steps and time.
- // these counters won't overflow because this code doesn't run for disabled bodies.
- if (idle) {
- bb->adis_stepsleft--;
- bb->adis_timeleft -= stepsize;
- }
- else {
- // Reset countdowns
- bb->adis_stepsleft = bb->adis.idle_steps;
- bb->adis_timeleft = bb->adis.idle_time;
- }
-
- // disable the body if it's idle for a long enough time
- if ( bb->adis_stepsleft <= 0 && bb->adis_timeleft <= 0 )
- {
- bb->flags |= dxBodyDisabled; // set the disable flag
+ // sample the linear and angular velocity
+ bb->average_lvel_buffer[bb->average_counter][0] = bb->lvel[0];
+ bb->average_lvel_buffer[bb->average_counter][1] = bb->lvel[1];
+ bb->average_lvel_buffer[bb->average_counter][2] = bb->lvel[2];
+ bb->average_avel_buffer[bb->average_counter][0] = bb->avel[0];
+ bb->average_avel_buffer[bb->average_counter][1] = bb->avel[1];
+ bb->average_avel_buffer[bb->average_counter][2] = bb->avel[2];
+ bb->average_counter++;
+
+ // buffer ready test
+ if ( bb->average_counter >= bb->adis.average_samples )
+ {
+ bb->average_counter = 0; // fill the buffer from the beginning
+ bb->average_ready = 1; // this body is ready now for average calculation
+ }
+
+ int idle = 0; // Assume it's in motion unless we have samples to disprove it.
+
+ // enough samples?
+ if ( bb->average_ready )
+ {
+ idle = 1; // Initial assumption: IDLE
+
+ // the sample buffers are filled and ready for calculation
+ dVector3 average_lvel, average_avel;
+
+ // Store first velocity samples
+ average_lvel[0] = bb->average_lvel_buffer[0][0];
+ average_avel[0] = bb->average_avel_buffer[0][0];
+ average_lvel[1] = bb->average_lvel_buffer[0][1];
+ average_avel[1] = bb->average_avel_buffer[0][1];
+ average_lvel[2] = bb->average_lvel_buffer[0][2];
+ average_avel[2] = bb->average_avel_buffer[0][2];
+
+ // If we're not in "instantaneous mode"
+ if ( bb->adis.average_samples > 1 )
+ {
+ // add remaining velocities together
+ for ( unsigned int i = 1; i < bb->adis.average_samples; ++i )
+ {
+ average_lvel[0] += bb->average_lvel_buffer[i][0];
+ average_avel[0] += bb->average_avel_buffer[i][0];
+ average_lvel[1] += bb->average_lvel_buffer[i][1];
+ average_avel[1] += bb->average_avel_buffer[i][1];
+ average_lvel[2] += bb->average_lvel_buffer[i][2];
+ average_avel[2] += bb->average_avel_buffer[i][2];
+ }
+
+ // make average
+ dReal r1 = dReal( 1.0 ) / dReal( bb->adis.average_samples );
+
+ average_lvel[0] *= r1;
+ average_avel[0] *= r1;
+ average_lvel[1] *= r1;
+ average_avel[1] *= r1;
+ average_lvel[2] *= r1;
+ average_avel[2] *= r1;
+ }
+
+ // threshold test
+ dReal av_lspeed, av_aspeed;
+ av_lspeed = dCalcVectorDot3( average_lvel, average_lvel );
+ if ( av_lspeed > bb->adis.linear_average_threshold )
+ {
+ idle = 0; // average linear velocity is too high for idle
+ }
+ else
+ {
+ av_aspeed = dCalcVectorDot3( average_avel, average_avel );
+ if ( av_aspeed > bb->adis.angular_average_threshold )
+ {
+ idle = 0; // average angular velocity is too high for idle
+ }
+ }
+ }
+
+ // if it's idle, accumulate steps and time.
+ // these counters won't overflow because this code doesn't run for disabled bodies.
+ if (idle) {
+ bb->adis_stepsleft--;
+ bb->adis_timeleft -= stepsize;
+ }
+ else {
+ // Reset countdowns
+ bb->adis_stepsleft = bb->adis.idle_steps;
+ bb->adis_timeleft = bb->adis.idle_time;
+ }
+
+ // disable the body if it's idle for a long enough time
+ if ( bb->adis_stepsleft <= 0 && bb->adis_timeleft <= 0 )
+ {
+ bb->flags |= dxBodyDisabled; // set the disable flag
if (bb->disabled_callback)
bb->disabled_callback(bb);
- // disabling bodies should also include resetting the velocity
- // should prevent jittering in big "islands"
- bb->lvel[0] = 0;
- bb->lvel[1] = 0;
- bb->lvel[2] = 0;
- bb->avel[0] = 0;
- bb->avel[1] = 0;
- bb->avel[2] = 0;
- }
- }
+ // disabling bodies should also include resetting the velocity
+ // should prevent jittering in big "islands"
+ bb->lvel[0] = 0;
+ bb->lvel[1] = 0;
+ bb->lvel[2] = 0;
+ bb->avel[0] = 0;
+ bb->avel[1] = 0;
+ bb->avel[2] = 0;
+ }
+ }
}
@@ -289,13 +289,13 @@ void dxStepBody (dxBody *b, dReal h)
for (int j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
- dVector3 irv; // infitesimal rotation vector
- dQuaternion q; // quaternion for finite rotation
+ dVector3 irv; // infitesimal rotation vector
+ dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
- dVector3 frv; // finite rotation vector
+ dVector3 frv; // finite rotation vector
dReal k = dCalcVectorDot3 (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
@@ -405,7 +405,7 @@ static size_t EstimateIslandsProcessingMemoryRequirements(dxWorld *world, size_t
// context->SavePreallocations(islandcount, islandsizes, body, joint,islandreqs);
// and put into context
//
-static size_t BuildIslandsAndEstimateStepperMemoryRequirements(dxWorldProcessContext *context,
+static size_t BuildIslandsAndEstimateStepperMemoryRequirements(dxWorldProcessContext *context,
dxWorld *world, dReal stepsize, dmemestimate_fn_t stepperestimate)
{
const int sizeelements = 2;
@@ -495,8 +495,8 @@ static size_t BuildIslandsAndEstimateStepperMemoryRequirements(dxWorldProcessCon
break;
}
- b = stack[--stacksize]; // pop body off stack
- *bodycurr++ = b; // put body on body list
+ b = stack[--stacksize]; // pop body off stack
+ *bodycurr++ = b; // put body on body list
}
int bcount = bodycurr - bodystart;
@@ -536,7 +536,7 @@ static size_t BuildIslandsAndEstimateStepperMemoryRequirements(dxWorldProcessCon
for (dxJoint *j=world->firstjoint; j; j=(dxJoint*)j->next) {
if ( (( j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0 ) ||
(j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0) )
- &&
+ &&
j->isEnabled() ) {
if (j->island_tag <= 0) dDebug (0,"attached enabled joint not tagged");
}
@@ -600,7 +600,7 @@ void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper)
dxStepWorkingMemory *wmem = world->wmem;
dIASSERT(wmem != NULL);
- dxWorldProcessContext *context = wmem->GetWorldProcessingContext();
+ dxWorldProcessContext *context = wmem->GetWorldProcessingContext();
int islandcount;
size_t const *islandreqs;
@@ -663,8 +663,10 @@ void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper)
printf("<<<<<<<<<<<< all island threads stopped at time %f with duration %f\n",end_time,end_time - cur_time);
#endif
- for (int jj=0; jj < islandcount; jj++)
- world->island_wmems[jj]->GetWorldProcessingContext()->CleanupContext();
+ for (auto &m : world->island_wmems)
+ {
+ m->GetWorldProcessingContext()->CleanupContext();
+ }
context->CleanupContext();
dIASSERT(context->IsStructureValid());
@@ -682,13 +684,13 @@ static size_t AdjustArenaSizeForReserveRequirements(size_t arenareq, float rsrvf
}
static dxWorldProcessContext *InternalReallocateWorldProcessContext (
- dxWorldProcessContext *oldcontext, size_t memreq,
+ dxWorldProcessContext *oldcontext, size_t memreq,
const dxWorldProcessMemoryManager *memmgr, float rsrvfactor, unsigned rsrvminimum)
{
dxWorldProcessContext *context = oldcontext;
bool allocsuccess = false;
- size_t oldarenasize;
+ size_t oldarenasize;
void *pOldArena;
do {
@@ -709,11 +711,11 @@ static dxWorldProcessContext *InternalReallocateWorldProcessContext (
if (oldcontext->m_pAllocCurrent != oldcontext->m_pAllocBegin) {
- // Save old efficient offset and meaningful data size for the case if
+ // Save old efficient offset and meaningful data size for the case if
// reallocation throws the block at different efficient offset
size_t oldcontextofs = (size_t)oldcontext - (size_t)pOldArena;
size_t datasize = (size_t)oldcontext->m_pAllocCurrent - (size_t)oldcontext;
-
+
// Extra EFFICIENT_ALIGNMENT bytes might be needed after re-allocation with different alignment
size_t shrunkarenasize = dEFFICIENT_SIZE(datasize + oldcontextofs) + EFFICIENT_ALIGNMENT;
if (shrunkarenasize < oldarenasize) {
@@ -761,7 +763,7 @@ static dxWorldProcessContext *InternalReallocateWorldProcessContext (
} else {
oldcontext->m_pArenaMemMgr->m_fnFree(pOldArena, oldarenasize);
oldcontext = NULL;
-
+
// Zero variables to avoid another freeing on exit
pOldArena = NULL;
oldarenasize = 0;
@@ -818,7 +820,7 @@ static void InternalFreeWorldProcessContext (dxWorldProcessContext *context)
}
-bool dxReallocateWorldProcessContext (dxWorld *world,
+bool dxReallocateWorldProcessContext (dxWorld *world,
dReal stepsize, dmemestimate_fn_t stepperestimate)
{
dxStepWorkingMemory *wmem = AllocateOnDemand(world->wmem); // this is starting a new instance of dxStepWorkingMemory
@@ -844,7 +846,7 @@ bool dxReallocateWorldProcessContext (dxWorld *world,
size_t stepperestimatereq = islandsreq + sesize;
context = InternalReallocateWorldProcessContext(context, stepperestimatereq, memmgr, 1.0f, reserveinfo->m_uiReserveMinimum);
-
+
//
// above context allocation of the island arrays is successful, then we proceed to allocate more spaces for the actual stepping work
//
@@ -863,10 +865,23 @@ bool dxReallocateWorldProcessContext (dxWorld *world,
dxJoint *const *joint;
context->RetrievePreallocations(islandcount, islandsizes, body, joint, islandreqs);
+ if (static_cast<size_t>(islandcount) > world->island_wmems.size())
+ world->island_wmems.resize(islandcount);
+
for (int jj = 0; jj < islandcount; jj++)
{
+ dxStepWorkingMemory *island_wmem = NULL;
+
// for individual islands
- dxStepWorkingMemory *island_wmem = AllocateOnDemand(world->island_wmems[jj]); // this is starting a new instance of dxStepWorkingMemory
+ // this is starting a new instance of dxStepWorkingMemory
+ if (!world->island_wmems[jj])
+ {
+ island_wmem = new dxStepWorkingMemory();
+ world->island_wmems[jj] = island_wmem;
+ }
+ else
+ island_wmem = world->island_wmems[jj];
+
if (!island_wmem) return false;
dxWorldProcessContext *island_oldcontext = island_wmem->GetWorldProcessingContext();
@@ -887,7 +902,7 @@ bool dxReallocateWorldProcessContext (dxWorld *world,
return context != NULL;
}
-dxWorldProcessContext *dxReallocateTemporayWorldProcessContext(dxWorldProcessContext *oldcontext,
+dxWorldProcessContext *dxReallocateTemporayWorldProcessContext(dxWorldProcessContext *oldcontext,
size_t memreq, const dxWorldProcessMemoryManager *memmgr/*=NULL*/, const dxWorldProcessMemoryReserveInfo *reserveinfo/*=NULL*/)
{
dxWorldProcessContext *context = oldcontext;
diff --git a/deps/win/tinyxml/VERSION_2.6.2 b/deps/win/tinyxml/VERSION_2.6.2
new file mode 100644
index 0000000..ca466dd
--- /dev/null
+++ b/deps/win/tinyxml/VERSION_2.6.2
@@ -0,0 +1,2 @@
+with the patch:
+https://raw.githubusercontent.com/robotology/yarp/master/extern/tinyxml/patches/entity-encoding.patch
diff --git a/deps/win/tinyxml/tinystr.cpp b/deps/win/tinyxml/tinystr.cpp
new file mode 100644
index 0000000..0665768
--- /dev/null
+++ b/deps/win/tinyxml/tinystr.cpp
@@ -0,0 +1,111 @@
+/*
+www.sourceforge.net/projects/tinyxml
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+
+#ifndef TIXML_USE_STL
+
+#include "tinystr.h"
+
+// Error value for find primitive
+const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1);
+
+
+// Null rep.
+TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } };
+
+
+void TiXmlString::reserve (size_type cap)
+{
+ if (cap > capacity())
+ {
+ TiXmlString tmp;
+ tmp.init(length(), cap);
+ memcpy(tmp.start(), data(), length());
+ swap(tmp);
+ }
+}
+
+
+TiXmlString& TiXmlString::assign(const char* str, size_type len)
+{
+ size_type cap = capacity();
+ if (len > cap || cap > 3*(len + 8))
+ {
+ TiXmlString tmp;
+ tmp.init(len);
+ memcpy(tmp.start(), str, len);
+ swap(tmp);
+ }
+ else
+ {
+ memmove(start(), str, len);
+ set_size(len);
+ }
+ return *this;
+}
+
+
+TiXmlString& TiXmlString::append(const char* str, size_type len)
+{
+ size_type newsize = length() + len;
+ if (newsize > capacity())
+ {
+ reserve (newsize + capacity());
+ }
+ memmove(finish(), str, len);
+ set_size(newsize);
+ return *this;
+}
+
+
+TiXmlString operator + (const TiXmlString & a, const TiXmlString & b)
+{
+ TiXmlString tmp;
+ tmp.reserve(a.length() + b.length());
+ tmp += a;
+ tmp += b;
+ return tmp;
+}
+
+TiXmlString operator + (const TiXmlString & a, const char* b)
+{
+ TiXmlString tmp;
+ TiXmlString::size_type b_len = static_cast<TiXmlString::size_type>( strlen(b) );
+ tmp.reserve(a.length() + b_len);
+ tmp += a;
+ tmp.append(b, b_len);
+ return tmp;
+}
+
+TiXmlString operator + (const char* a, const TiXmlString & b)
+{
+ TiXmlString tmp;
+ TiXmlString::size_type a_len = static_cast<TiXmlString::size_type>( strlen(a) );
+ tmp.reserve(a_len + b.length());
+ tmp.append(a, a_len);
+ tmp += b;
+ return tmp;
+}
+
+
+#endif // TIXML_USE_STL
diff --git a/deps/win/tinyxml/tinystr.h b/deps/win/tinyxml/tinystr.h
new file mode 100644
index 0000000..89cca33
--- /dev/null
+++ b/deps/win/tinyxml/tinystr.h
@@ -0,0 +1,305 @@
+/*
+www.sourceforge.net/projects/tinyxml
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+
+#ifndef TIXML_USE_STL
+
+#ifndef TIXML_STRING_INCLUDED
+#define TIXML_STRING_INCLUDED
+
+#include <assert.h>
+#include <string.h>
+
+/* The support for explicit isn't that universal, and it isn't really
+ required - it is used to check that the TiXmlString class isn't incorrectly
+ used. Be nice to old compilers and macro it here:
+*/
+#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
+ // Microsoft visual studio, version 6 and higher.
+ #define TIXML_EXPLICIT explicit
+#elif defined(__GNUC__) && (__GNUC__ >= 3 )
+ // GCC version 3 and higher.s
+ #define TIXML_EXPLICIT explicit
+#else
+ #define TIXML_EXPLICIT
+#endif
+
+
+/*
+ TiXmlString is an emulation of a subset of the std::string template.
+ Its purpose is to allow compiling TinyXML on compilers with no or poor STL support.
+ Only the member functions relevant to the TinyXML project have been implemented.
+ The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase
+ a string and there's no more room, we allocate a buffer twice as big as we need.
+*/
+class TiXmlString
+{
+ public :
+ // The size type used
+ typedef size_t size_type;
+
+ // Error value for find primitive
+ static const size_type npos; // = -1;
+
+
+ // TiXmlString empty constructor
+ TiXmlString () : rep_(&nullrep_)
+ {
+ }
+
+ // TiXmlString copy constructor
+ TiXmlString ( const TiXmlString & copy) : rep_(0)
+ {
+ init(copy.length());
+ memcpy(start(), copy.data(), length());
+ }
+
+ // TiXmlString constructor, based on a string
+ TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0)
+ {
+ init( static_cast<size_type>( strlen(copy) ));
+ memcpy(start(), copy, length());
+ }
+
+ // TiXmlString constructor, based on a string
+ TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0)
+ {
+ init(len);
+ memcpy(start(), str, len);
+ }
+
+ // TiXmlString destructor
+ ~TiXmlString ()
+ {
+ quit();
+ }
+
+ TiXmlString& operator = (const char * copy)
+ {
+ return assign( copy, (size_type)strlen(copy));
+ }
+
+ TiXmlString& operator = (const TiXmlString & copy)
+ {
+ return assign(copy.start(), copy.length());
+ }
+
+
+ // += operator. Maps to append
+ TiXmlString& operator += (const char * suffix)
+ {
+ return append(suffix, static_cast<size_type>( strlen(suffix) ));
+ }
+
+ // += operator. Maps to append
+ TiXmlString& operator += (char single)
+ {
+ return append(&single, 1);
+ }
+
+ // += operator. Maps to append
+ TiXmlString& operator += (const TiXmlString & suffix)
+ {
+ return append(suffix.data(), suffix.length());
+ }
+
+
+ // Convert a TiXmlString into a null-terminated char *
+ const char * c_str () const { return rep_->str; }
+
+ // Convert a TiXmlString into a char * (need not be null terminated).
+ const char * data () const { return rep_->str; }
+
+ // Return the length of a TiXmlString
+ size_type length () const { return rep_->size; }
+
+ // Alias for length()
+ size_type size () const { return rep_->size; }
+
+ // Checks if a TiXmlString is empty
+ bool empty () const { return rep_->size == 0; }
+
+ // Return capacity of string
+ size_type capacity () const { return rep_->capacity; }
+
+
+ // single char extraction
+ const char& at (size_type index) const
+ {
+ assert( index < length() );
+ return rep_->str[ index ];
+ }
+
+ // [] operator
+ char& operator [] (size_type index) const
+ {
+ assert( index < length() );
+ return rep_->str[ index ];
+ }
+
+ // find a char in a string. Return TiXmlString::npos if not found
+ size_type find (char lookup) const
+ {
+ return find(lookup, 0);
+ }
+
+ // find a char in a string from an offset. Return TiXmlString::npos if not found
+ size_type find (char tofind, size_type offset) const
+ {
+ if (offset >= length()) return npos;
+
+ for (const char* p = c_str() + offset; *p != '\0'; ++p)
+ {
+ if (*p == tofind) return static_cast< size_type >( p - c_str() );
+ }
+ return npos;
+ }
+
+ void clear ()
+ {
+ //Lee:
+ //The original was just too strange, though correct:
+ // TiXmlString().swap(*this);
+ //Instead use the quit & re-init:
+ quit();
+ init(0,0);
+ }
+
+ /* Function to reserve a big amount of data when we know we'll need it. Be aware that this
+ function DOES NOT clear the content of the TiXmlString if any exists.
+ */
+ void reserve (size_type cap);
+
+ TiXmlString& assign (const char* str, size_type len);
+
+ TiXmlString& append (const char* str, size_type len);
+
+ void swap (TiXmlString& other)
+ {
+ Rep* r = rep_;
+ rep_ = other.rep_;
+ other.rep_ = r;
+ }
+
+ private:
+
+ void init(size_type sz) { init(sz, sz); }
+ void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; }
+ char* start() const { return rep_->str; }
+ char* finish() const { return rep_->str + rep_->size; }
+
+ struct Rep
+ {
+ size_type size, capacity;
+ char str[1];
+ };
+
+ void init(size_type sz, size_type cap)
+ {
+ if (cap)
+ {
+ // Lee: the original form:
+ // rep_ = static_cast<Rep*>(operator new(sizeof(Rep) + cap));
+ // doesn't work in some cases of new being overloaded. Switching
+ // to the normal allocation, although use an 'int' for systems
+ // that are overly picky about structure alignment.
+ const size_type bytesNeeded = sizeof(Rep) + cap;
+ const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int );
+ rep_ = reinterpret_cast<Rep*>( new int[ intsNeeded ] );
+
+ rep_->str[ rep_->size = sz ] = '\0';
+ rep_->capacity = cap;
+ }
+ else
+ {
+ rep_ = &nullrep_;
+ }
+ }
+
+ void quit()
+ {
+ if (rep_ != &nullrep_)
+ {
+ // The rep_ is really an array of ints. (see the allocator, above).
+ // Cast it back before delete, so the compiler won't incorrectly call destructors.
+ delete [] ( reinterpret_cast<int*>( rep_ ) );
+ }
+ }
+
+ Rep * rep_;
+ static Rep nullrep_;
+
+} ;
+
+
+inline bool operator == (const TiXmlString & a, const TiXmlString & b)
+{
+ return ( a.length() == b.length() ) // optimization on some platforms
+ && ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare
+}
+inline bool operator < (const TiXmlString & a, const TiXmlString & b)
+{
+ return strcmp(a.c_str(), b.c_str()) < 0;
+}
+
+inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); }
+inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; }
+inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); }
+inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); }
+
+inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; }
+inline bool operator == (const char* a, const TiXmlString & b) { return b == a; }
+inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); }
+inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); }
+
+TiXmlString operator + (const TiXmlString & a, const TiXmlString & b);
+TiXmlString operator + (const TiXmlString & a, const char* b);
+TiXmlString operator + (const char* a, const TiXmlString & b);
+
+
+/*
+ TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString.
+ Only the operators that we need for TinyXML have been developped.
+*/
+class TiXmlOutStream : public TiXmlString
+{
+public :
+
+ // TiXmlOutStream << operator.
+ TiXmlOutStream & operator << (const TiXmlString & in)
+ {
+ *this += in;
+ return *this;
+ }
+
+ // TiXmlOutStream << operator.
+ TiXmlOutStream & operator << (const char * in)
+ {
+ *this += in;
+ return *this;
+ }
+
+} ;
+
+#endif // TIXML_STRING_INCLUDED
+#endif // TIXML_USE_STL
diff --git a/deps/win/tinyxml/tinyxml.cpp b/deps/win/tinyxml/tinyxml.cpp
new file mode 100644
index 0000000..f113706
--- /dev/null
+++ b/deps/win/tinyxml/tinyxml.cpp
@@ -0,0 +1,1863 @@
+/*
+www.sourceforge.net/projects/tinyxml
+Original code by Lee Thomason (www.grinninglizard.com)
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+#include <ctype.h>
+
+#ifdef TIXML_USE_STL
+#include <sstream>
+#include <iostream>
+#endif
+
+#include "tinyxml.h"
+
+FILE* TiXmlFOpen( const char* filename, const char* mode );
+
+bool TiXmlBase::condenseWhiteSpace = true;
+
+// Microsoft compiler security
+FILE* TiXmlFOpen( const char* filename, const char* mode )
+{
+ #if defined(_MSC_VER) && (_MSC_VER >= 1400 )
+ FILE* fp = 0;
+ errno_t err = fopen_s( &fp, filename, mode );
+ if ( !err && fp )
+ return fp;
+ return 0;
+ #else
+ return fopen( filename, mode );
+ #endif
+}
+
+void TiXmlBase::EncodeString( const TIXML_STRING& str, TIXML_STRING* outString )
+{
+ int i=0;
+
+ while( i<(int)str.length() )
+ {
+ unsigned char c = (unsigned char) str[i];
+
+ if ( c == '&' )
+ {
+ outString->append( entity[0].str, entity[0].strLength );
+ ++i;
+ }
+ else if ( c == '<' )
+ {
+ outString->append( entity[1].str, entity[1].strLength );
+ ++i;
+ }
+ else if ( c == '>' )
+ {
+ outString->append( entity[2].str, entity[2].strLength );
+ ++i;
+ }
+ else if ( c == '\"' )
+ {
+ outString->append( entity[3].str, entity[3].strLength );
+ ++i;
+ }
+ else if ( c == '\'' )
+ {
+ outString->append( entity[4].str, entity[4].strLength );
+ ++i;
+ }
+ else if ( c < 32 )
+ {
+ // Easy pass at non-alpha/numeric/symbol
+ // Below 32 is symbolic.
+ char buf[ 32 ];
+
+ #if defined(TIXML_SNPRINTF)
+ TIXML_SNPRINTF( buf, sizeof(buf), "&#x%02X;", (unsigned) ( c & 0xff ) );
+ #else
+ sprintf( buf, "&#x%02X;", (unsigned) ( c & 0xff ) );
+ #endif
+
+ //*ME: warning C4267: convert 'size_t' to 'int'
+ //*ME: Int-Cast to make compiler happy ...
+ outString->append( buf, (int)strlen( buf ) );
+ ++i;
+ }
+ else
+ {
+ //char realc = (char) c;
+ //outString->append( &realc, 1 );
+ *outString += (char) c; // somewhat more efficient function call.
+ ++i;
+ }
+ }
+}
+
+
+TiXmlNode::TiXmlNode( NodeType _type ) : TiXmlBase()
+{
+ parent = 0;
+ type = _type;
+ firstChild = 0;
+ lastChild = 0;
+ prev = 0;
+ next = 0;
+}
+
+
+TiXmlNode::~TiXmlNode()
+{
+ TiXmlNode* node = firstChild;
+ TiXmlNode* temp = 0;
+
+ while ( node )
+ {
+ temp = node;
+ node = node->next;
+ delete temp;
+ }
+}
+
+
+void TiXmlNode::CopyTo( TiXmlNode* target ) const
+{
+ target->SetValue (value.c_str() );
+ target->userData = userData;
+ target->location = location;
+}
+
+
+void TiXmlNode::Clear()
+{
+ TiXmlNode* node = firstChild;
+ TiXmlNode* temp = 0;
+
+ while ( node )
+ {
+ temp = node;
+ node = node->next;
+ delete temp;
+ }
+
+ firstChild = 0;
+ lastChild = 0;
+}
+
+
+TiXmlNode* TiXmlNode::LinkEndChild( TiXmlNode* node )
+{
+ assert( node->parent == 0 || node->parent == this );
+ assert( node->GetDocument() == 0 || node->GetDocument() == this->GetDocument() );
+
+ if ( node->Type() == TiXmlNode::TINYXML_DOCUMENT )
+ {
+ delete node;
+ if ( GetDocument() )
+ GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ node->parent = this;
+
+ node->prev = lastChild;
+ node->next = 0;
+
+ if ( lastChild )
+ lastChild->next = node;
+ else
+ firstChild = node; // it was an empty list.
+
+ lastChild = node;
+ return node;
+}
+
+
+TiXmlNode* TiXmlNode::InsertEndChild( const TiXmlNode& addThis )
+{
+ if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT )
+ {
+ if ( GetDocument() )
+ GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+ TiXmlNode* node = addThis.Clone();
+ if ( !node )
+ return 0;
+
+ return LinkEndChild( node );
+}
+
+
+TiXmlNode* TiXmlNode::InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis )
+{
+ if ( !beforeThis || beforeThis->parent != this ) {
+ return 0;
+ }
+ if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT )
+ {
+ if ( GetDocument() )
+ GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ TiXmlNode* node = addThis.Clone();
+ if ( !node )
+ return 0;
+ node->parent = this;
+
+ node->next = beforeThis;
+ node->prev = beforeThis->prev;
+ if ( beforeThis->prev )
+ {
+ beforeThis->prev->next = node;
+ }
+ else
+ {
+ assert( firstChild == beforeThis );
+ firstChild = node;
+ }
+ beforeThis->prev = node;
+ return node;
+}
+
+
+TiXmlNode* TiXmlNode::InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis )
+{
+ if ( !afterThis || afterThis->parent != this ) {
+ return 0;
+ }
+ if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT )
+ {
+ if ( GetDocument() )
+ GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ TiXmlNode* node = addThis.Clone();
+ if ( !node )
+ return 0;
+ node->parent = this;
+
+ node->prev = afterThis;
+ node->next = afterThis->next;
+ if ( afterThis->next )
+ {
+ afterThis->next->prev = node;
+ }
+ else
+ {
+ assert( lastChild == afterThis );
+ lastChild = node;
+ }
+ afterThis->next = node;
+ return node;
+}
+
+
+TiXmlNode* TiXmlNode::ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis )
+{
+ if ( !replaceThis )
+ return 0;
+
+ if ( replaceThis->parent != this )
+ return 0;
+
+ if ( withThis.ToDocument() ) {
+ // A document can never be a child. Thanks to Noam.
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ TiXmlNode* node = withThis.Clone();
+ if ( !node )
+ return 0;
+
+ node->next = replaceThis->next;
+ node->prev = replaceThis->prev;
+
+ if ( replaceThis->next )
+ replaceThis->next->prev = node;
+ else
+ lastChild = node;
+
+ if ( replaceThis->prev )
+ replaceThis->prev->next = node;
+ else
+ firstChild = node;
+
+ delete replaceThis;
+ node->parent = this;
+ return node;
+}
+
+
+bool TiXmlNode::RemoveChild( TiXmlNode* removeThis )
+{
+ if ( !removeThis ) {
+ return false;
+ }
+
+ if ( removeThis->parent != this )
+ {
+ assert( 0 );
+ return false;
+ }
+
+ if ( removeThis->next )
+ removeThis->next->prev = removeThis->prev;
+ else
+ lastChild = removeThis->prev;
+
+ if ( removeThis->prev )
+ removeThis->prev->next = removeThis->next;
+ else
+ firstChild = removeThis->next;
+
+ delete removeThis;
+ return true;
+}
+
+const TiXmlNode* TiXmlNode::FirstChild( const char * _value ) const
+{
+ const TiXmlNode* node;
+ for ( node = firstChild; node; node = node->next )
+ {
+ if ( strcmp( node->Value(), _value ) == 0 )
+ return node;
+ }
+ return 0;
+}
+
+
+const TiXmlNode* TiXmlNode::LastChild( const char * _value ) const
+{
+ const TiXmlNode* node;
+ for ( node = lastChild; node; node = node->prev )
+ {
+ if ( strcmp( node->Value(), _value ) == 0 )
+ return node;
+ }
+ return 0;
+}
+
+
+const TiXmlNode* TiXmlNode::IterateChildren( const TiXmlNode* previous ) const
+{
+ if ( !previous )
+ {
+ return FirstChild();
+ }
+ else
+ {
+ assert( previous->parent == this );
+ return previous->NextSibling();
+ }
+}
+
+
+const TiXmlNode* TiXmlNode::IterateChildren( const char * val, const TiXmlNode* previous ) const
+{
+ if ( !previous )
+ {
+ return FirstChild( val );
+ }
+ else
+ {
+ assert( previous->parent == this );
+ return previous->NextSibling( val );
+ }
+}
+
+
+const TiXmlNode* TiXmlNode::NextSibling( const char * _value ) const
+{
+ const TiXmlNode* node;
+ for ( node = next; node; node = node->next )
+ {
+ if ( strcmp( node->Value(), _value ) == 0 )
+ return node;
+ }
+ return 0;
+}
+
+
+const TiXmlNode* TiXmlNode::PreviousSibling( const char * _value ) const
+{
+ const TiXmlNode* node;
+ for ( node = prev; node; node = node->prev )
+ {
+ if ( strcmp( node->Value(), _value ) == 0 )
+ return node;
+ }
+ return 0;
+}
+
+
+void TiXmlElement::RemoveAttribute( const char * name )
+{
+ #ifdef TIXML_USE_STL
+ TIXML_STRING str( name );
+ TiXmlAttribute* node = attributeSet.Find( str );
+ #else
+ TiXmlAttribute* node = attributeSet.Find( name );
+ #endif
+ if ( node )
+ {
+ attributeSet.Remove( node );
+ delete node;
+ }
+}
+
+const TiXmlElement* TiXmlNode::FirstChildElement() const
+{
+ const TiXmlNode* node;
+
+ for ( node = FirstChild();
+ node;
+ node = node->NextSibling() )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+
+const TiXmlElement* TiXmlNode::FirstChildElement( const char * _value ) const
+{
+ const TiXmlNode* node;
+
+ for ( node = FirstChild( _value );
+ node;
+ node = node->NextSibling( _value ) )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+
+const TiXmlElement* TiXmlNode::NextSiblingElement() const
+{
+ const TiXmlNode* node;
+
+ for ( node = NextSibling();
+ node;
+ node = node->NextSibling() )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+
+const TiXmlElement* TiXmlNode::NextSiblingElement( const char * _value ) const
+{
+ const TiXmlNode* node;
+
+ for ( node = NextSibling( _value );
+ node;
+ node = node->NextSibling( _value ) )
+ {
+ if ( node->ToElement() )
+ return node->ToElement();
+ }
+ return 0;
+}
+
+
+const TiXmlDocument* TiXmlNode::GetDocument() const
+{
+ const TiXmlNode* node;
+
+ for( node = this; node; node = node->parent )
+ {
+ if ( node->ToDocument() )
+ return node->ToDocument();
+ }
+ return 0;
+}
+
+
+TiXmlElement::TiXmlElement (const char * _value)
+ : TiXmlNode( TiXmlNode::TINYXML_ELEMENT )
+{
+ firstChild = lastChild = 0;
+ value = _value;
+}
+
+
+#ifdef TIXML_USE_STL
+TiXmlElement::TiXmlElement( const std::string& _value )
+ : TiXmlNode( TiXmlNode::TINYXML_ELEMENT )
+{
+ firstChild = lastChild = 0;
+ value = _value;
+}
+#endif
+
+
+TiXmlElement::TiXmlElement( const TiXmlElement& copy)
+ : TiXmlNode( TiXmlNode::TINYXML_ELEMENT )
+{
+ firstChild = lastChild = 0;
+ copy.CopyTo( this );
+}
+
+
+TiXmlElement& TiXmlElement::operator=( const TiXmlElement& base )
+{
+ ClearThis();
+ base.CopyTo( this );
+ return *this;
+}
+
+
+TiXmlElement::~TiXmlElement()
+{
+ ClearThis();
+}
+
+
+void TiXmlElement::ClearThis()
+{
+ Clear();
+ while( attributeSet.First() )
+ {
+ TiXmlAttribute* node = attributeSet.First();
+ attributeSet.Remove( node );
+ delete node;
+ }
+}
+
+
+const char* TiXmlElement::Attribute( const char* name ) const
+{
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( node )
+ return node->Value();
+ return 0;
+}
+
+
+#ifdef TIXML_USE_STL
+const std::string* TiXmlElement::Attribute( const std::string& name ) const
+{
+ const TiXmlAttribute* attrib = attributeSet.Find( name );
+ if ( attrib )
+ return &attrib->ValueStr();
+ return 0;
+}
+#endif
+
+
+const char* TiXmlElement::Attribute( const char* name, int* i ) const
+{
+ const TiXmlAttribute* attrib = attributeSet.Find( name );
+ const char* result = 0;
+
+ if ( attrib ) {
+ result = attrib->Value();
+ if ( i ) {
+ attrib->QueryIntValue( i );
+ }
+ }
+ return result;
+}
+
+
+#ifdef TIXML_USE_STL
+const std::string* TiXmlElement::Attribute( const std::string& name, int* i ) const
+{
+ const TiXmlAttribute* attrib = attributeSet.Find( name );
+ const std::string* result = 0;
+
+ if ( attrib ) {
+ result = &attrib->ValueStr();
+ if ( i ) {
+ attrib->QueryIntValue( i );
+ }
+ }
+ return result;
+}
+#endif
+
+
+const char* TiXmlElement::Attribute( const char* name, double* d ) const
+{
+ const TiXmlAttribute* attrib = attributeSet.Find( name );
+ const char* result = 0;
+
+ if ( attrib ) {
+ result = attrib->Value();
+ if ( d ) {
+ attrib->QueryDoubleValue( d );
+ }
+ }
+ return result;
+}
+
+
+#ifdef TIXML_USE_STL
+const std::string* TiXmlElement::Attribute( const std::string& name, double* d ) const
+{
+ const TiXmlAttribute* attrib = attributeSet.Find( name );
+ const std::string* result = 0;
+
+ if ( attrib ) {
+ result = &attrib->ValueStr();
+ if ( d ) {
+ attrib->QueryDoubleValue( d );
+ }
+ }
+ return result;
+}
+#endif
+
+
+int TiXmlElement::QueryIntAttribute( const char* name, int* ival ) const
+{
+ const TiXmlAttribute* attrib = attributeSet.Find( name );
+ if ( !attrib )
+ return TIXML_NO_ATTRIBUTE;
+ return attrib->QueryIntValue( ival );
+}
+
+#pragma GCC diagnostic ignored "-Wshadow"
+int TiXmlElement::QueryUnsignedAttribute( const char* name, unsigned* value ) const
+{
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( !node )
+ return TIXML_NO_ATTRIBUTE;
+
+ int ival = 0;
+ int result = node->QueryIntValue( &ival );
+ *value = (unsigned)ival;
+ return result;
+}
+#pragma GCC diagnostic pop
+
+int TiXmlElement::QueryBoolAttribute( const char* name, bool* bval ) const
+{
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( !node )
+ return TIXML_NO_ATTRIBUTE;
+
+ int result = TIXML_WRONG_TYPE;
+ if ( StringEqual( node->Value(), "true", true, TIXML_ENCODING_UNKNOWN )
+ || StringEqual( node->Value(), "yes", true, TIXML_ENCODING_UNKNOWN )
+ || StringEqual( node->Value(), "1", true, TIXML_ENCODING_UNKNOWN ) )
+ {
+ *bval = true;
+ result = TIXML_SUCCESS;
+ }
+ else if ( StringEqual( node->Value(), "false", true, TIXML_ENCODING_UNKNOWN )
+ || StringEqual( node->Value(), "no", true, TIXML_ENCODING_UNKNOWN )
+ || StringEqual( node->Value(), "0", true, TIXML_ENCODING_UNKNOWN ) )
+ {
+ *bval = false;
+ result = TIXML_SUCCESS;
+ }
+ return result;
+}
+
+
+
+#ifdef TIXML_USE_STL
+int TiXmlElement::QueryIntAttribute( const std::string& name, int* ival ) const
+{
+ const TiXmlAttribute* attrib = attributeSet.Find( name );
+ if ( !attrib )
+ return TIXML_NO_ATTRIBUTE;
+ return attrib->QueryIntValue( ival );
+}
+#endif
+
+
+int TiXmlElement::QueryDoubleAttribute( const char* name, double* dval ) const
+{
+ const TiXmlAttribute* attrib = attributeSet.Find( name );
+ if ( !attrib )
+ return TIXML_NO_ATTRIBUTE;
+ return attrib->QueryDoubleValue( dval );
+}
+
+
+#ifdef TIXML_USE_STL
+int TiXmlElement::QueryDoubleAttribute( const std::string& name, double* dval ) const
+{
+ const TiXmlAttribute* attrib = attributeSet.Find( name );
+ if ( !attrib )
+ return TIXML_NO_ATTRIBUTE;
+ return attrib->QueryDoubleValue( dval );
+}
+#endif
+
+
+void TiXmlElement::SetAttribute( const char * name, int val )
+{
+ TiXmlAttribute* attrib = attributeSet.FindOrCreate( name );
+ if ( attrib ) {
+ attrib->SetIntValue( val );
+ }
+}
+
+
+#ifdef TIXML_USE_STL
+void TiXmlElement::SetAttribute( const std::string& name, int val )
+{
+ TiXmlAttribute* attrib = attributeSet.FindOrCreate( name );
+ if ( attrib ) {
+ attrib->SetIntValue( val );
+ }
+}
+#endif
+
+
+void TiXmlElement::SetDoubleAttribute( const char * name, double val )
+{
+ TiXmlAttribute* attrib = attributeSet.FindOrCreate( name );
+ if ( attrib ) {
+ attrib->SetDoubleValue( val );
+ }
+}
+
+
+#ifdef TIXML_USE_STL
+void TiXmlElement::SetDoubleAttribute( const std::string& name, double val )
+{
+ TiXmlAttribute* attrib = attributeSet.FindOrCreate( name );
+ if ( attrib ) {
+ attrib->SetDoubleValue( val );
+ }
+}
+#endif
+
+
+void TiXmlElement::SetAttribute( const char * cname, const char * cvalue )
+{
+ TiXmlAttribute* attrib = attributeSet.FindOrCreate( cname );
+ if ( attrib ) {
+ attrib->SetValue( cvalue );
+ }
+}
+
+
+#ifdef TIXML_USE_STL
+void TiXmlElement::SetAttribute( const std::string& _name, const std::string& _value )
+{
+ TiXmlAttribute* attrib = attributeSet.FindOrCreate( _name );
+ if ( attrib ) {
+ attrib->SetValue( _value );
+ }
+}
+#endif
+
+
+void TiXmlElement::Print( FILE* cfile, int depth ) const
+{
+ int i;
+ assert( cfile );
+ for ( i=0; i<depth; i++ ) {
+ fprintf( cfile, " " );
+ }
+
+ fprintf( cfile, "<%s", value.c_str() );
+
+ const TiXmlAttribute* attrib;
+ for ( attrib = attributeSet.First(); attrib; attrib = attrib->Next() )
+ {
+ fprintf( cfile, " " );
+ attrib->Print( cfile, depth );
+ }
+
+ // There are 3 different formatting approaches:
+ // 1) An element without children is printed as a <foo /> node
+ // 2) An element with only a text child is printed as <foo> text </foo>
+ // 3) An element with children is printed on multiple lines.
+ TiXmlNode* node;
+ if ( !firstChild )
+ {
+ fprintf( cfile, " />" );
+ }
+ else if ( firstChild == lastChild && firstChild->ToText() )
+ {
+ fprintf( cfile, ">" );
+ firstChild->Print( cfile, depth + 1 );
+ fprintf( cfile, "</%s>", value.c_str() );
+ }
+ else
+ {
+ fprintf( cfile, ">" );
+
+ for ( node = firstChild; node; node=node->NextSibling() )
+ {
+ if ( !node->ToText() )
+ {
+ fprintf( cfile, "\n" );
+ }
+ node->Print( cfile, depth+1 );
+ }
+ fprintf( cfile, "\n" );
+ for( i=0; i<depth; ++i ) {
+ fprintf( cfile, " " );
+ }
+ fprintf( cfile, "</%s>", value.c_str() );
+ }
+}
+
+
+void TiXmlElement::CopyTo( TiXmlElement* target ) const
+{
+ // superclass:
+ TiXmlNode::CopyTo( target );
+
+ // Element class:
+ // Clone the attributes, then clone the children.
+ const TiXmlAttribute* attribute = 0;
+ for( attribute = attributeSet.First();
+ attribute;
+ attribute = attribute->Next() )
+ {
+ target->SetAttribute( attribute->Name(), attribute->Value() );
+ }
+
+ TiXmlNode* node = 0;
+ for ( node = firstChild; node; node = node->NextSibling() )
+ {
+ target->LinkEndChild( node->Clone() );
+ }
+}
+
+bool TiXmlElement::Accept( TiXmlVisitor* visitor ) const
+{
+ if ( visitor->VisitEnter( *this, attributeSet.First() ) )
+ {
+ for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() )
+ {
+ if ( !node->Accept( visitor ) )
+ break;
+ }
+ }
+ return visitor->VisitExit( *this );
+}
+
+
+TiXmlNode* TiXmlElement::Clone() const
+{
+ TiXmlElement* clone = new TiXmlElement( Value() );
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+
+const char* TiXmlElement::GetText() const
+{
+ const TiXmlNode* child = this->FirstChild();
+ if ( child ) {
+ const TiXmlText* childText = child->ToText();
+ if ( childText ) {
+ return childText->Value();
+ }
+ }
+ return 0;
+}
+
+
+TiXmlDocument::TiXmlDocument() : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT )
+{
+ tabsize = 4;
+ useMicrosoftBOM = false;
+ ClearError();
+}
+
+TiXmlDocument::TiXmlDocument( const char * documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT )
+{
+ tabsize = 4;
+ useMicrosoftBOM = false;
+ value = documentName;
+ ClearError();
+}
+
+
+#ifdef TIXML_USE_STL
+TiXmlDocument::TiXmlDocument( const std::string& documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT )
+{
+ tabsize = 4;
+ useMicrosoftBOM = false;
+ value = documentName;
+ ClearError();
+}
+#endif
+
+
+TiXmlDocument::TiXmlDocument( const TiXmlDocument& copy ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT )
+{
+ copy.CopyTo( this );
+}
+
+
+TiXmlDocument& TiXmlDocument::operator=( const TiXmlDocument& copy )
+{
+ Clear();
+ copy.CopyTo( this );
+ return *this;
+}
+
+
+bool TiXmlDocument::LoadFile( TiXmlEncoding encoding )
+{
+ return LoadFile( Value(), encoding );
+}
+
+
+bool TiXmlDocument::SaveFile() const
+{
+ return SaveFile( Value() );
+}
+
+bool TiXmlDocument::LoadFile( const char* _filename, TiXmlEncoding encoding )
+{
+ TIXML_STRING filename( _filename );
+ value = filename;
+
+ // reading in binary mode so that tinyxml can normalize the EOL
+ FILE* file = TiXmlFOpen( value.c_str (), "rb" );
+
+ if ( file )
+ {
+ bool result = LoadFile( file, encoding );
+ fclose( file );
+ return result;
+ }
+ else
+ {
+ SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return false;
+ }
+}
+
+bool TiXmlDocument::LoadFile( FILE* file, TiXmlEncoding encoding )
+{
+ if ( !file )
+ {
+ SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return false;
+ }
+
+ // Delete the existing data:
+ Clear();
+ location.Clear();
+
+ // Get the file size, so we can pre-allocate the string. HUGE speed impact.
+ long length = 0;
+ fseek( file, 0, SEEK_END );
+ length = ftell( file );
+ fseek( file, 0, SEEK_SET );
+
+ // Strange case, but good to handle up front.
+ if ( length <= 0 )
+ {
+ SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return false;
+ }
+
+ // Subtle bug here. TinyXml did use fgets. But from the XML spec:
+ // 2.11 End-of-Line Handling
+ // <snip>
+ // <quote>
+ // ...the XML processor MUST behave as if it normalized all line breaks in external
+ // parsed entities (including the document entity) on input, before parsing, by translating
+ // both the two-character sequence #xD #xA and any #xD that is not followed by #xA to
+ // a single #xA character.
+ // </quote>
+ //
+ // It is not clear fgets does that, and certainly isn't clear it works cross platform.
+ // Generally, you expect fgets to translate from the convention of the OS to the c/unix
+ // convention, and not work generally.
+
+ /*
+ while( fgets( buf, sizeof(buf), file ) )
+ {
+ data += buf;
+ }
+ */
+
+ char* buf = new char[ length+1 ];
+ buf[0] = 0;
+
+ if ( fread( buf, length, 1, file ) != 1 ) {
+ delete [] buf;
+ SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return false;
+ }
+
+ // Process the buffer in place to normalize new lines. (See comment above.)
+ // Copies from the 'p' to 'q' pointer, where p can advance faster if
+ // a newline-carriage return is hit.
+ //
+ // Wikipedia:
+ // Systems based on ASCII or a compatible character set use either LF (Line feed, '\n', 0x0A, 10 in decimal) or
+ // CR (Carriage return, '\r', 0x0D, 13 in decimal) individually, or CR followed by LF (CR+LF, 0x0D 0x0A)...
+ // * LF: Multics, Unix and Unix-like systems (GNU/Linux, AIX, Xenix, Mac OS X, FreeBSD, etc.), BeOS, Amiga, RISC OS, and others
+ // * CR+LF: DEC RT-11 and most other early non-Unix, non-IBM OSes, CP/M, MP/M, DOS, OS/2, Microsoft Windows, Symbian OS
+ // * CR: Commodore 8-bit machines, Apple II family, Mac OS up to version 9 and OS-9
+
+ const char* p = buf; // the read head
+ char* q = buf; // the write head
+ const char CR = 0x0d;
+ const char LF = 0x0a;
+
+ buf[length] = 0;
+ while( *p ) {
+ assert( p < (buf+length) );
+ assert( q <= (buf+length) );
+ assert( q <= p );
+
+ if ( *p == CR ) {
+ *q++ = LF;
+ p++;
+ if ( *p == LF ) { // check for CR+LF (and skip LF)
+ p++;
+ }
+ }
+ else {
+ *q++ = *p++;
+ }
+ }
+ assert( q <= (buf+length) );
+ *q = 0;
+
+ Parse( buf, 0, encoding );
+
+ delete [] buf;
+ return !Error();
+}
+
+
+bool TiXmlDocument::SaveFile( const char * filename ) const
+{
+ // The old c stuff lives on...
+ FILE* fp = TiXmlFOpen( filename, "w" );
+ if ( fp )
+ {
+ bool result = SaveFile( fp );
+ fclose( fp );
+ return result;
+ }
+ return false;
+}
+
+
+bool TiXmlDocument::SaveFile( FILE* fp ) const
+{
+ if ( useMicrosoftBOM )
+ {
+ const unsigned char TIXML_UTF_LEAD_0 = 0xefU;
+ const unsigned char TIXML_UTF_LEAD_1 = 0xbbU;
+ const unsigned char TIXML_UTF_LEAD_2 = 0xbfU;
+
+ fputc( TIXML_UTF_LEAD_0, fp );
+ fputc( TIXML_UTF_LEAD_1, fp );
+ fputc( TIXML_UTF_LEAD_2, fp );
+ }
+ Print( fp, 0 );
+ return (ferror(fp) == 0);
+}
+
+
+void TiXmlDocument::CopyTo( TiXmlDocument* target ) const
+{
+ TiXmlNode::CopyTo( target );
+
+ target->error = error;
+ target->errorId = errorId;
+ target->errorDesc = errorDesc;
+ target->tabsize = tabsize;
+ target->errorLocation = errorLocation;
+ target->useMicrosoftBOM = useMicrosoftBOM;
+
+ TiXmlNode* node = 0;
+ for ( node = firstChild; node; node = node->NextSibling() )
+ {
+ target->LinkEndChild( node->Clone() );
+ }
+}
+
+
+TiXmlNode* TiXmlDocument::Clone() const
+{
+ TiXmlDocument* clone = new TiXmlDocument();
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+
+void TiXmlDocument::Print( FILE* cfile, int depth ) const
+{
+ assert( cfile );
+ for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() )
+ {
+ node->Print( cfile, depth );
+ fprintf( cfile, "\n" );
+ }
+}
+
+
+bool TiXmlDocument::Accept( TiXmlVisitor* visitor ) const
+{
+ if ( visitor->VisitEnter( *this ) )
+ {
+ for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() )
+ {
+ if ( !node->Accept( visitor ) )
+ break;
+ }
+ }
+ return visitor->VisitExit( *this );
+}
+
+
+const TiXmlAttribute* TiXmlAttribute::Next() const
+{
+ // We are using knowledge of the sentinel. The sentinel
+ // have a value or name.
+ if ( next->value.empty() && next->name.empty() )
+ return 0;
+ return next;
+}
+
+/*
+TiXmlAttribute* TiXmlAttribute::Next()
+{
+ // We are using knowledge of the sentinel. The sentinel
+ // have a value or name.
+ if ( next->value.empty() && next->name.empty() )
+ return 0;
+ return next;
+}
+*/
+
+const TiXmlAttribute* TiXmlAttribute::Previous() const
+{
+ // We are using knowledge of the sentinel. The sentinel
+ // have a value or name.
+ if ( prev->value.empty() && prev->name.empty() )
+ return 0;
+ return prev;
+}
+
+/*
+TiXmlAttribute* TiXmlAttribute::Previous()
+{
+ // We are using knowledge of the sentinel. The sentinel
+ // have a value or name.
+ if ( prev->value.empty() && prev->name.empty() )
+ return 0;
+ return prev;
+}
+*/
+
+void TiXmlAttribute::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const
+{
+ TIXML_STRING n, v;
+
+ EncodeString( name, &n );
+ EncodeString( value, &v );
+
+ if (value.find ('\"') == TIXML_STRING::npos) {
+ if ( cfile ) {
+ fprintf (cfile, "%s=\"%s\"", n.c_str(), v.c_str() );
+ }
+ if ( str ) {
+ (*str) += n; (*str) += "=\""; (*str) += v; (*str) += "\"";
+ }
+ }
+ else {
+ if ( cfile ) {
+ fprintf (cfile, "%s='%s'", n.c_str(), v.c_str() );
+ }
+ if ( str ) {
+ (*str) += n; (*str) += "='"; (*str) += v; (*str) += "'";
+ }
+ }
+}
+
+
+int TiXmlAttribute::QueryIntValue( int* ival ) const
+{
+ if ( TIXML_SSCANF( value.c_str(), "%d", ival ) == 1 )
+ return TIXML_SUCCESS;
+ return TIXML_WRONG_TYPE;
+}
+
+int TiXmlAttribute::QueryDoubleValue( double* dval ) const
+{
+ if ( TIXML_SSCANF( value.c_str(), "%lf", dval ) == 1 )
+ return TIXML_SUCCESS;
+ return TIXML_WRONG_TYPE;
+}
+
+void TiXmlAttribute::SetIntValue( int _value )
+{
+ char buf [64];
+ #if defined(TIXML_SNPRINTF)
+ TIXML_SNPRINTF(buf, sizeof(buf), "%d", _value);
+ #else
+ sprintf (buf, "%d", _value);
+ #endif
+ SetValue (buf);
+}
+
+void TiXmlAttribute::SetDoubleValue( double _value )
+{
+ char buf [256];
+ #if defined(TIXML_SNPRINTF)
+ TIXML_SNPRINTF( buf, sizeof(buf), "%g", _value);
+ #else
+ sprintf (buf, "%g", _value);
+ #endif
+ SetValue (buf);
+}
+
+int TiXmlAttribute::IntValue() const
+{
+ return atoi (value.c_str ());
+}
+
+double TiXmlAttribute::DoubleValue() const
+{
+ return atof (value.c_str ());
+}
+
+
+TiXmlComment::TiXmlComment( const TiXmlComment& copy ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT )
+{
+ copy.CopyTo( this );
+}
+
+
+TiXmlComment& TiXmlComment::operator=( const TiXmlComment& base )
+{
+ Clear();
+ base.CopyTo( this );
+ return *this;
+}
+
+
+void TiXmlComment::Print( FILE* cfile, int depth ) const
+{
+ assert( cfile );
+ for ( int i=0; i<depth; i++ )
+ {
+ fprintf( cfile, " " );
+ }
+ fprintf( cfile, "<!--%s-->", value.c_str() );
+}
+
+
+void TiXmlComment::CopyTo( TiXmlComment* target ) const
+{
+ TiXmlNode::CopyTo( target );
+}
+
+
+bool TiXmlComment::Accept( TiXmlVisitor* visitor ) const
+{
+ return visitor->Visit( *this );
+}
+
+
+TiXmlNode* TiXmlComment::Clone() const
+{
+ TiXmlComment* clone = new TiXmlComment();
+
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+
+void TiXmlText::Print( FILE* cfile, int depth ) const
+{
+ assert( cfile );
+ if ( cdata )
+ {
+ int i;
+ fprintf( cfile, "\n" );
+ for ( i=0; i<depth; i++ ) {
+ fprintf( cfile, " " );
+ }
+ fprintf( cfile, "<![CDATA[%s]]>\n", value.c_str() ); // unformatted output
+ }
+ else
+ {
+ TIXML_STRING buffer;
+ EncodeString( value, &buffer );
+ fprintf( cfile, "%s", buffer.c_str() );
+ }
+}
+
+
+void TiXmlText::CopyTo( TiXmlText* target ) const
+{
+ TiXmlNode::CopyTo( target );
+ target->cdata = cdata;
+}
+
+
+bool TiXmlText::Accept( TiXmlVisitor* visitor ) const
+{
+ return visitor->Visit( *this );
+}
+
+
+TiXmlNode* TiXmlText::Clone() const
+{
+ TiXmlText* clone = 0;
+ clone = new TiXmlText( "" );
+
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+
+TiXmlDeclaration::TiXmlDeclaration( const char * _version,
+ const char * _encoding,
+ const char * _standalone )
+ : TiXmlNode( TiXmlNode::TINYXML_DECLARATION )
+{
+ version = _version;
+ encoding = _encoding;
+ standalone = _standalone;
+}
+
+
+#ifdef TIXML_USE_STL
+TiXmlDeclaration::TiXmlDeclaration( const std::string& _version,
+ const std::string& _encoding,
+ const std::string& _standalone )
+ : TiXmlNode( TiXmlNode::TINYXML_DECLARATION )
+{
+ version = _version;
+ encoding = _encoding;
+ standalone = _standalone;
+}
+#endif
+
+
+TiXmlDeclaration::TiXmlDeclaration( const TiXmlDeclaration& copy )
+ : TiXmlNode( TiXmlNode::TINYXML_DECLARATION )
+{
+ copy.CopyTo( this );
+}
+
+
+TiXmlDeclaration& TiXmlDeclaration::operator=( const TiXmlDeclaration& copy )
+{
+ Clear();
+ copy.CopyTo( this );
+ return *this;
+}
+
+
+void TiXmlDeclaration::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const
+{
+ if ( cfile ) fprintf( cfile, "<?xml " );
+ if ( str ) (*str) += "<?xml ";
+
+ if ( !version.empty() ) {
+ if ( cfile ) fprintf (cfile, "version=\"%s\" ", version.c_str ());
+ if ( str ) { (*str) += "version=\""; (*str) += version; (*str) += "\" "; }
+ }
+ if ( !encoding.empty() ) {
+ if ( cfile ) fprintf (cfile, "encoding=\"%s\" ", encoding.c_str ());
+ if ( str ) { (*str) += "encoding=\""; (*str) += encoding; (*str) += "\" "; }
+ }
+ if ( !standalone.empty() ) {
+ if ( cfile ) fprintf (cfile, "standalone=\"%s\" ", standalone.c_str ());
+ if ( str ) { (*str) += "standalone=\""; (*str) += standalone; (*str) += "\" "; }
+ }
+ if ( cfile ) fprintf( cfile, "?>" );
+ if ( str ) (*str) += "?>";
+}
+
+
+void TiXmlDeclaration::CopyTo( TiXmlDeclaration* target ) const
+{
+ TiXmlNode::CopyTo( target );
+
+ target->version = version;
+ target->encoding = encoding;
+ target->standalone = standalone;
+}
+
+
+bool TiXmlDeclaration::Accept( TiXmlVisitor* visitor ) const
+{
+ return visitor->Visit( *this );
+}
+
+
+TiXmlNode* TiXmlDeclaration::Clone() const
+{
+ TiXmlDeclaration* clone = new TiXmlDeclaration();
+
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+
+void TiXmlUnknown::Print( FILE* cfile, int depth ) const
+{
+ for ( int i=0; i<depth; i++ )
+ fprintf( cfile, " " );
+ fprintf( cfile, "<%s>", value.c_str() );
+}
+
+
+void TiXmlUnknown::CopyTo( TiXmlUnknown* target ) const
+{
+ TiXmlNode::CopyTo( target );
+}
+
+
+bool TiXmlUnknown::Accept( TiXmlVisitor* visitor ) const
+{
+ return visitor->Visit( *this );
+}
+
+
+TiXmlNode* TiXmlUnknown::Clone() const
+{
+ TiXmlUnknown* clone = new TiXmlUnknown();
+
+ if ( !clone )
+ return 0;
+
+ CopyTo( clone );
+ return clone;
+}
+
+
+TiXmlAttributeSet::TiXmlAttributeSet()
+{
+ sentinel.next = &sentinel;
+ sentinel.prev = &sentinel;
+}
+
+
+TiXmlAttributeSet::~TiXmlAttributeSet()
+{
+ assert( sentinel.next == &sentinel );
+ assert( sentinel.prev == &sentinel );
+}
+
+
+void TiXmlAttributeSet::Add( TiXmlAttribute* addMe )
+{
+ #ifdef TIXML_USE_STL
+ assert( !Find( TIXML_STRING( addMe->Name() ) ) ); // Shouldn't be multiply adding to the set.
+ #else
+ assert( !Find( addMe->Name() ) ); // Shouldn't be multiply adding to the set.
+ #endif
+
+ addMe->next = &sentinel;
+ addMe->prev = sentinel.prev;
+
+ sentinel.prev->next = addMe;
+ sentinel.prev = addMe;
+}
+
+void TiXmlAttributeSet::Remove( TiXmlAttribute* removeMe )
+{
+ TiXmlAttribute* node;
+
+ for( node = sentinel.next; node != &sentinel; node = node->next )
+ {
+ if ( node == removeMe )
+ {
+ node->prev->next = node->next;
+ node->next->prev = node->prev;
+ node->next = 0;
+ node->prev = 0;
+ return;
+ }
+ }
+ assert( 0 ); // we tried to remove a non-linked attribute.
+}
+
+
+#ifdef TIXML_USE_STL
+TiXmlAttribute* TiXmlAttributeSet::Find( const std::string& name ) const
+{
+ for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next )
+ {
+ if ( node->name == name )
+ return node;
+ }
+ return 0;
+}
+
+TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const std::string& _name )
+{
+ TiXmlAttribute* attrib = Find( _name );
+ if ( !attrib ) {
+ attrib = new TiXmlAttribute();
+ Add( attrib );
+ attrib->SetName( _name );
+ }
+ return attrib;
+}
+#endif
+
+
+TiXmlAttribute* TiXmlAttributeSet::Find( const char* name ) const
+{
+ for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next )
+ {
+ if ( strcmp( node->name.c_str(), name ) == 0 )
+ return node;
+ }
+ return 0;
+}
+
+
+TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const char* _name )
+{
+ TiXmlAttribute* attrib = Find( _name );
+ if ( !attrib ) {
+ attrib = new TiXmlAttribute();
+ Add( attrib );
+ attrib->SetName( _name );
+ }
+ return attrib;
+}
+
+
+#ifdef TIXML_USE_STL
+std::istream& operator>> (std::istream & in, TiXmlNode & base)
+{
+ TIXML_STRING tag;
+ tag.reserve( 8 * 1000 );
+ base.StreamIn( &in, &tag );
+
+ base.Parse( tag.c_str(), 0, TIXML_DEFAULT_ENCODING );
+ return in;
+}
+#endif
+
+
+#ifdef TIXML_USE_STL
+std::ostream& operator<< (std::ostream & out, const TiXmlNode & base)
+{
+ TiXmlPrinter printer;
+ printer.SetStreamPrinting();
+ base.Accept( &printer );
+ out << printer.Str();
+
+ return out;
+}
+
+
+std::string& operator<< (std::string& out, const TiXmlNode& base )
+{
+ TiXmlPrinter printer;
+ printer.SetStreamPrinting();
+ base.Accept( &printer );
+ out.append( printer.Str() );
+
+ return out;
+}
+#endif
+
+
+TiXmlHandle TiXmlHandle::FirstChild() const
+{
+ if ( node )
+ {
+ TiXmlNode* child = node->FirstChild();
+ if ( child )
+ return TiXmlHandle( child );
+ }
+ return TiXmlHandle( 0 );
+}
+
+
+TiXmlHandle TiXmlHandle::FirstChild( const char * value ) const
+{
+ if ( node )
+ {
+ TiXmlNode* child = node->FirstChild( value );
+ if ( child )
+ return TiXmlHandle( child );
+ }
+ return TiXmlHandle( 0 );
+}
+
+
+TiXmlHandle TiXmlHandle::FirstChildElement() const
+{
+ if ( node )
+ {
+ TiXmlElement* child = node->FirstChildElement();
+ if ( child )
+ return TiXmlHandle( child );
+ }
+ return TiXmlHandle( 0 );
+}
+
+
+TiXmlHandle TiXmlHandle::FirstChildElement( const char * value ) const
+{
+ if ( node )
+ {
+ TiXmlElement* child = node->FirstChildElement( value );
+ if ( child )
+ return TiXmlHandle( child );
+ }
+ return TiXmlHandle( 0 );
+}
+
+
+TiXmlHandle TiXmlHandle::Child( int count ) const
+{
+ if ( node )
+ {
+ int i;
+ TiXmlNode* child = node->FirstChild();
+ for ( i=0;
+ child && i<count;
+ child = child->NextSibling(), ++i )
+ {
+ // nothing
+ }
+ if ( child )
+ return TiXmlHandle( child );
+ }
+ return TiXmlHandle( 0 );
+}
+
+
+TiXmlHandle TiXmlHandle::Child( const char* value, int count ) const
+{
+ if ( node )
+ {
+ int i;
+ TiXmlNode* child = node->FirstChild( value );
+ for ( i=0;
+ child && i<count;
+ child = child->NextSibling( value ), ++i )
+ {
+ // nothing
+ }
+ if ( child )
+ return TiXmlHandle( child );
+ }
+ return TiXmlHandle( 0 );
+}
+
+
+TiXmlHandle TiXmlHandle::ChildElement( int count ) const
+{
+ if ( node )
+ {
+ int i;
+ TiXmlElement* child = node->FirstChildElement();
+ for ( i=0;
+ child && i<count;
+ child = child->NextSiblingElement(), ++i )
+ {
+ // nothing
+ }
+ if ( child )
+ return TiXmlHandle( child );
+ }
+ return TiXmlHandle( 0 );
+}
+
+
+TiXmlHandle TiXmlHandle::ChildElement( const char* value, int count ) const
+{
+ if ( node )
+ {
+ int i;
+ TiXmlElement* child = node->FirstChildElement( value );
+ for ( i=0;
+ child && i<count;
+ child = child->NextSiblingElement( value ), ++i )
+ {
+ // nothing
+ }
+ if ( child )
+ return TiXmlHandle( child );
+ }
+ return TiXmlHandle( 0 );
+}
+
+
+bool TiXmlPrinter::VisitEnter( const TiXmlDocument& )
+{
+ return true;
+}
+
+bool TiXmlPrinter::VisitExit( const TiXmlDocument& )
+{
+ return true;
+}
+
+bool TiXmlPrinter::VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute )
+{
+ DoIndent();
+ buffer += "<";
+ buffer += element.Value();
+
+ for( const TiXmlAttribute* attrib = firstAttribute; attrib; attrib = attrib->Next() )
+ {
+ buffer += " ";
+ attrib->Print( 0, 0, &buffer );
+ }
+
+ if ( !element.FirstChild() )
+ {
+ buffer += " />";
+ DoLineBreak();
+ }
+ else
+ {
+ buffer += ">";
+ if ( element.FirstChild()->ToText()
+ && element.LastChild() == element.FirstChild()
+ && element.FirstChild()->ToText()->CDATA() == false )
+ {
+ simpleTextPrint = true;
+ // no DoLineBreak()!
+ }
+ else
+ {
+ DoLineBreak();
+ }
+ }
+ ++depth;
+ return true;
+}
+
+
+bool TiXmlPrinter::VisitExit( const TiXmlElement& element )
+{
+ --depth;
+ if ( !element.FirstChild() )
+ {
+ // nothing.
+ }
+ else
+ {
+ if ( simpleTextPrint )
+ {
+ simpleTextPrint = false;
+ }
+ else
+ {
+ DoIndent();
+ }
+ buffer += "</";
+ buffer += element.Value();
+ buffer += ">";
+ DoLineBreak();
+ }
+ return true;
+}
+
+
+bool TiXmlPrinter::Visit( const TiXmlText& text )
+{
+ if ( text.CDATA() )
+ {
+ DoIndent();
+ buffer += "<![CDATA[";
+ buffer += text.Value();
+ buffer += "]]>";
+ DoLineBreak();
+ }
+ else if ( simpleTextPrint )
+ {
+ TIXML_STRING str;
+ TiXmlBase::EncodeString( text.ValueTStr(), &str );
+ buffer += str;
+ }
+ else
+ {
+ DoIndent();
+ TIXML_STRING str;
+ TiXmlBase::EncodeString( text.ValueTStr(), &str );
+ buffer += str;
+ DoLineBreak();
+ }
+ return true;
+}
+
+
+bool TiXmlPrinter::Visit( const TiXmlDeclaration& declaration )
+{
+ DoIndent();
+ declaration.Print( 0, 0, &buffer );
+ DoLineBreak();
+ return true;
+}
+
+
+bool TiXmlPrinter::Visit( const TiXmlComment& comment )
+{
+ DoIndent();
+ buffer += "<!--";
+ buffer += comment.Value();
+ buffer += "-->";
+ DoLineBreak();
+ return true;
+}
+
+
+bool TiXmlPrinter::Visit( const TiXmlUnknown& unknown )
+{
+ DoIndent();
+ buffer += "<";
+ buffer += unknown.Value();
+ buffer += ">";
+ DoLineBreak();
+ return true;
+}
+
diff --git a/deps/win/tinyxml/tinyxml.h b/deps/win/tinyxml/tinyxml.h
new file mode 100644
index 0000000..a3589e5
--- /dev/null
+++ b/deps/win/tinyxml/tinyxml.h
@@ -0,0 +1,1805 @@
+/*
+www.sourceforge.net/projects/tinyxml
+Original code by Lee Thomason (www.grinninglizard.com)
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+
+#ifndef TINYXML_INCLUDED
+#define TINYXML_INCLUDED
+
+#ifdef _MSC_VER
+#pragma warning( push )
+#pragma warning( disable : 4530 )
+#pragma warning( disable : 4786 )
+#endif
+
+#include <ctype.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+
+// Help out windows:
+#if defined( _DEBUG ) && !defined( DEBUG )
+#define DEBUG
+#endif
+
+#ifdef TIXML_USE_STL
+ #include <string>
+ #include <iostream>
+ #include <sstream>
+ #define TIXML_STRING std::string
+#else
+ #include "tinystr.h"
+ #define TIXML_STRING TiXmlString
+#endif
+
+// Deprecated library function hell. Compilers want to use the
+// new safe versions. This probably doesn't fully address the problem,
+// but it gets closer. There are too many compilers for me to fully
+// test. If you get compilation troubles, undefine TIXML_SAFE
+#define TIXML_SAFE
+
+#ifdef TIXML_SAFE
+ #if defined(_MSC_VER) && (_MSC_VER >= 1400 )
+ // Microsoft visual studio, version 2005 and higher.
+ #define TIXML_SNPRINTF _snprintf_s
+ #define TIXML_SSCANF sscanf_s
+ #elif defined(_MSC_VER) && (_MSC_VER >= 1200 )
+ // Microsoft visual studio, version 6 and higher.
+ //#pragma message( "Using _sn* functions." )
+ #define TIXML_SNPRINTF _snprintf
+ #define TIXML_SSCANF sscanf
+ #elif defined(__GNUC__) && (__GNUC__ >= 3 )
+ // GCC version 3 and higher.s
+ //#warning( "Using sn* functions." )
+ #define TIXML_SNPRINTF snprintf
+ #define TIXML_SSCANF sscanf
+ #else
+ #define TIXML_SNPRINTF snprintf
+ #define TIXML_SSCANF sscanf
+ #endif
+#endif
+
+class TiXmlDocument;
+class TiXmlElement;
+class TiXmlComment;
+class TiXmlUnknown;
+class TiXmlAttribute;
+class TiXmlText;
+class TiXmlDeclaration;
+class TiXmlParsingData;
+
+const int TIXML_MAJOR_VERSION = 2;
+const int TIXML_MINOR_VERSION = 6;
+const int TIXML_PATCH_VERSION = 2;
+
+/* Internal structure for tracking location of items
+ in the XML file.
+*/
+struct TiXmlCursor
+{
+ TiXmlCursor() { Clear(); }
+ void Clear() { row = col = -1; }
+
+ int row; // 0 based.
+ int col; // 0 based.
+};
+
+
+/**
+ Implements the interface to the "Visitor pattern" (see the Accept() method.)
+ If you call the Accept() method, it requires being passed a TiXmlVisitor
+ class to handle callbacks. For nodes that contain other nodes (Document, Element)
+ you will get called with a VisitEnter/VisitExit pair. Nodes that are always leaves
+ are simply called with Visit().
+
+ If you return 'true' from a Visit method, recursive parsing will continue. If you return
+ false, <b>no children of this node or its sibilings</b> will be Visited.
+
+ All flavors of Visit methods have a default implementation that returns 'true' (continue
+ visiting). You need to only override methods that are interesting to you.
+
+ Generally Accept() is called on the TiXmlDocument, although all nodes suppert Visiting.
+
+ You should never change the document from a callback.
+
+ @sa TiXmlNode::Accept()
+*/
+class TiXmlVisitor
+{
+public:
+ virtual ~TiXmlVisitor() {}
+
+ /// Visit a document.
+ virtual bool VisitEnter( const TiXmlDocument& /*doc*/ ) { return true; }
+ /// Visit a document.
+ virtual bool VisitExit( const TiXmlDocument& /*doc*/ ) { return true; }
+
+ /// Visit an element.
+ virtual bool VisitEnter( const TiXmlElement& /*element*/, const TiXmlAttribute* /*firstAttribute*/ ) { return true; }
+ /// Visit an element.
+ virtual bool VisitExit( const TiXmlElement& /*element*/ ) { return true; }
+
+ /// Visit a declaration
+ virtual bool Visit( const TiXmlDeclaration& /*declaration*/ ) { return true; }
+ /// Visit a text node
+ virtual bool Visit( const TiXmlText& /*text*/ ) { return true; }
+ /// Visit a comment node
+ virtual bool Visit( const TiXmlComment& /*comment*/ ) { return true; }
+ /// Visit an unknown node
+ virtual bool Visit( const TiXmlUnknown& /*unknown*/ ) { return true; }
+};
+
+// Only used by Attribute::Query functions
+enum
+{
+ TIXML_SUCCESS,
+ TIXML_NO_ATTRIBUTE,
+ TIXML_WRONG_TYPE
+};
+
+
+// Used by the parsing routines.
+enum TiXmlEncoding
+{
+ TIXML_ENCODING_UNKNOWN,
+ TIXML_ENCODING_UTF8,
+ TIXML_ENCODING_LEGACY
+};
+
+const TiXmlEncoding TIXML_DEFAULT_ENCODING = TIXML_ENCODING_UNKNOWN;
+
+/** TiXmlBase is a base class for every class in TinyXml.
+ It does little except to establish that TinyXml classes
+ can be printed and provide some utility functions.
+
+ In XML, the document and elements can contain
+ other elements and other types of nodes.
+
+ @verbatim
+ A Document can contain: Element (container or leaf)
+ Comment (leaf)
+ Unknown (leaf)
+ Declaration( leaf )
+
+ An Element can contain: Element (container or leaf)
+ Text (leaf)
+ Attributes (not on tree)
+ Comment (leaf)
+ Unknown (leaf)
+
+ A Decleration contains: Attributes (not on tree)
+ @endverbatim
+*/
+class TiXmlBase
+{
+ friend class TiXmlNode;
+ friend class TiXmlElement;
+ friend class TiXmlDocument;
+
+public:
+ TiXmlBase() : userData(0) {}
+ virtual ~TiXmlBase() {}
+
+ /** All TinyXml classes can print themselves to a filestream
+ or the string class (TiXmlString in non-STL mode, std::string
+ in STL mode.) Either or both cfile and str can be null.
+
+ This is a formatted print, and will insert
+ tabs and newlines.
+
+ (For an unformatted stream, use the << operator.)
+ */
+ virtual void Print( FILE* cfile, int depth ) const = 0;
+
+ /** The world does not agree on whether white space should be kept or
+ not. In order to make everyone happy, these global, static functions
+ are provided to set whether or not TinyXml will condense all white space
+ into a single space or not. The default is to condense. Note changing this
+ value is not thread safe.
+ */
+ static void SetCondenseWhiteSpace( bool condense ) { condenseWhiteSpace = condense; }
+
+ /// Return the current white space setting.
+ static bool IsWhiteSpaceCondensed() { return condenseWhiteSpace; }
+
+ /** Return the position, in the original source file, of this node or attribute.
+ The row and column are 1-based. (That is the first row and first column is
+ 1,1). If the returns values are 0 or less, then the parser does not have
+ a row and column value.
+
+ Generally, the row and column value will be set when the TiXmlDocument::Load(),
+ TiXmlDocument::LoadFile(), or any TiXmlNode::Parse() is called. It will NOT be set
+ when the DOM was created from operator>>.
+
+ The values reflect the initial load. Once the DOM is modified programmatically
+ (by adding or changing nodes and attributes) the new values will NOT update to
+ reflect changes in the document.
+
+ There is a minor performance cost to computing the row and column. Computation
+ can be disabled if TiXmlDocument::SetTabSize() is called with 0 as the value.
+
+ @sa TiXmlDocument::SetTabSize()
+ */
+ int Row() const { return location.row + 1; }
+ int Column() const { return location.col + 1; } ///< See Row()
+
+ void SetUserData( void* user ) { userData = user; } ///< Set a pointer to arbitrary user data.
+ void* GetUserData() { return userData; } ///< Get a pointer to arbitrary user data.
+ const void* GetUserData() const { return userData; } ///< Get a pointer to arbitrary user data.
+
+ // Table that returs, for a given lead byte, the total number of bytes
+ // in the UTF-8 sequence.
+ static const int utf8ByteTable[256];
+
+ virtual const char* Parse( const char* p,
+ TiXmlParsingData* data,
+ TiXmlEncoding encoding /*= TIXML_ENCODING_UNKNOWN */ ) = 0;
+
+ /** Expands entities in a string. Note this should not contian the tag's '<', '>', etc,
+ or they will be transformed into entities!
+ */
+ static void EncodeString( const TIXML_STRING& str, TIXML_STRING* out );
+
+ enum
+ {
+ TIXML_NO_ERROR = 0,
+ TIXML_ERROR,
+ TIXML_ERROR_OPENING_FILE,
+ TIXML_ERROR_PARSING_ELEMENT,
+ TIXML_ERROR_FAILED_TO_READ_ELEMENT_NAME,
+ TIXML_ERROR_READING_ELEMENT_VALUE,
+ TIXML_ERROR_READING_ATTRIBUTES,
+ TIXML_ERROR_PARSING_EMPTY,
+ TIXML_ERROR_READING_END_TAG,
+ TIXML_ERROR_PARSING_UNKNOWN,
+ TIXML_ERROR_PARSING_COMMENT,
+ TIXML_ERROR_PARSING_DECLARATION,
+ TIXML_ERROR_DOCUMENT_EMPTY,
+ TIXML_ERROR_EMBEDDED_NULL,
+ TIXML_ERROR_PARSING_CDATA,
+ TIXML_ERROR_DOCUMENT_TOP_ONLY,
+
+ TIXML_ERROR_STRING_COUNT
+ };
+
+protected:
+
+ static const char* SkipWhiteSpace( const char*, TiXmlEncoding encoding );
+
+ inline static bool IsWhiteSpace( char c )
+ {
+ return ( isspace( (unsigned char) c ) || c == '\n' || c == '\r' );
+ }
+ inline static bool IsWhiteSpace( int c )
+ {
+ if ( c < 256 )
+ return IsWhiteSpace( (char) c );
+ return false; // Again, only truly correct for English/Latin...but usually works.
+ }
+
+ #ifdef TIXML_USE_STL
+ static bool StreamWhiteSpace( std::istream * in, TIXML_STRING * tag );
+ static bool StreamTo( std::istream * in, int character, TIXML_STRING * tag );
+ #endif
+
+ /* Reads an XML name into the string provided. Returns
+ a pointer just past the last character of the name,
+ or 0 if the function has an error.
+ */
+ static const char* ReadName( const char* p, TIXML_STRING* name, TiXmlEncoding encoding );
+
+ /* Reads text. Returns a pointer past the given end tag.
+ Wickedly complex options, but it keeps the (sensitive) code in one place.
+ */
+ static const char* ReadText( const char* in, // where to start
+ TIXML_STRING* text, // the string read
+ bool ignoreWhiteSpace, // whether to keep the white space
+ const char* endTag, // what ends this text
+ bool ignoreCase, // whether to ignore case in the end tag
+ TiXmlEncoding encoding ); // the current encoding
+
+ // If an entity has been found, transform it into a character.
+ static const char* GetEntity( const char* in, char* value, int* length, TiXmlEncoding encoding );
+
+ // Get a character, while interpreting entities.
+ // The length can be from 0 to 4 bytes.
+ inline static const char* GetChar( const char* p, char* _value, int* length, TiXmlEncoding encoding )
+ {
+ assert( p );
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ *length = utf8ByteTable[ *((const unsigned char*)p) ];
+ assert( *length >= 0 && *length < 5 );
+ }
+ else
+ {
+ *length = 1;
+ }
+
+ if ( *length == 1 )
+ {
+ if ( *p == '&' )
+ return GetEntity( p, _value, length, encoding );
+ *_value = *p;
+ return p+1;
+ }
+ else if ( *length )
+ {
+ //strncpy( _value, p, *length ); // lots of compilers don't like this function (unsafe),
+ // and the null terminator isn't needed
+ for( int i=0; p[i] && i<*length; ++i ) {
+ _value[i] = p[i];
+ }
+ return p + (*length);
+ }
+ else
+ {
+ // Not valid text.
+ return 0;
+ }
+ }
+
+ // Return true if the next characters in the stream are any of the endTag sequences.
+ // Ignore case only works for english, and should only be relied on when comparing
+ // to English words: StringEqual( p, "version", true ) is fine.
+ static bool StringEqual( const char* p,
+ const char* endTag,
+ bool ignoreCase,
+ TiXmlEncoding encoding );
+
+ static const char* errorString[ TIXML_ERROR_STRING_COUNT ];
+
+ TiXmlCursor location;
+
+ /// Field containing a generic user pointer
+ void* userData;
+
+ // None of these methods are reliable for any language except English.
+ // Good for approximation, not great for accuracy.
+ static int IsAlpha( unsigned char anyByte, TiXmlEncoding encoding );
+ static int IsAlphaNum( unsigned char anyByte, TiXmlEncoding encoding );
+ inline static int ToLower( int v, TiXmlEncoding encoding )
+ {
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ if ( v < 128 ) return tolower( v );
+ return v;
+ }
+ else
+ {
+ return tolower( v );
+ }
+ }
+ static void ConvertUTF32ToUTF8( unsigned long input, char* output, int* length );
+
+private:
+ TiXmlBase( const TiXmlBase& ); // not implemented.
+ void operator=( const TiXmlBase& base ); // not allowed.
+
+ struct Entity
+ {
+ const char* str;
+ unsigned int strLength;
+ char chr;
+ };
+ enum
+ {
+ NUM_ENTITY = 5,
+ MAX_ENTITY_LENGTH = 6
+
+ };
+ static Entity entity[ NUM_ENTITY ];
+ static bool condenseWhiteSpace;
+};
+
+
+/** The parent class for everything in the Document Object Model.
+ (Except for attributes).
+ Nodes have siblings, a parent, and children. A node can be
+ in a document, or stand on its own. The type of a TiXmlNode
+ can be queried, and it can be cast to its more defined type.
+*/
+class TiXmlNode : public TiXmlBase
+{
+ friend class TiXmlDocument;
+ friend class TiXmlElement;
+
+public:
+ #ifdef TIXML_USE_STL
+
+ /** An input stream operator, for every class. Tolerant of newlines and
+ formatting, but doesn't expect them.
+ */
+ friend std::istream& operator >> (std::istream& in, TiXmlNode& base);
+
+ /** An output stream operator, for every class. Note that this outputs
+ without any newlines or formatting, as opposed to Print(), which
+ includes tabs and new lines.
+
+ The operator<< and operator>> are not completely symmetric. Writing
+ a node to a stream is very well defined. You'll get a nice stream
+ of output, without any extra whitespace or newlines.
+
+ But reading is not as well defined. (As it always is.) If you create
+ a TiXmlElement (for example) and read that from an input stream,
+ the text needs to define an element or junk will result. This is
+ true of all input streams, but it's worth keeping in mind.
+
+ A TiXmlDocument will read nodes until it reads a root element, and
+ all the children of that root element.
+ */
+ friend std::ostream& operator<< (std::ostream& out, const TiXmlNode& base);
+
+ /// Appends the XML node or attribute to a std::string.
+ friend std::string& operator<< (std::string& out, const TiXmlNode& base );
+
+ #endif
+
+ /** The types of XML nodes supported by TinyXml. (All the
+ unsupported types are picked up by UNKNOWN.)
+ */
+ enum NodeType
+ {
+ TINYXML_DOCUMENT,
+ TINYXML_ELEMENT,
+ TINYXML_COMMENT,
+ TINYXML_UNKNOWN,
+ TINYXML_TEXT,
+ TINYXML_DECLARATION,
+ TINYXML_TYPECOUNT
+ };
+
+ virtual ~TiXmlNode();
+
+ /** The meaning of 'value' changes for the specific type of
+ TiXmlNode.
+ @verbatim
+ Document: filename of the xml file
+ Element: name of the element
+ Comment: the comment text
+ Unknown: the tag contents
+ Text: the text string
+ @endverbatim
+
+ The subclasses will wrap this function.
+ */
+ const char *Value() const { return value.c_str (); }
+
+ #ifdef TIXML_USE_STL
+ /** Return Value() as a std::string. If you only use STL,
+ this is more efficient than calling Value().
+ Only available in STL mode.
+ */
+ const std::string& ValueStr() const { return value; }
+ #endif
+
+ const TIXML_STRING& ValueTStr() const { return value; }
+
+ /** Changes the value of the node. Defined as:
+ @verbatim
+ Document: filename of the xml file
+ Element: name of the element
+ Comment: the comment text
+ Unknown: the tag contents
+ Text: the text string
+ @endverbatim
+ */
+ void SetValue(const char * _value) { value = _value;}
+
+ #ifdef TIXML_USE_STL
+ /// STL std::string form.
+ void SetValue( const std::string& _value ) { value = _value; }
+ #endif
+
+ /// Delete all the children of this node. Does not affect 'this'.
+ void Clear();
+
+ /// One step up the DOM.
+ TiXmlNode* Parent() { return parent; }
+ const TiXmlNode* Parent() const { return parent; }
+
+ const TiXmlNode* FirstChild() const { return firstChild; } ///< The first child of this node. Will be null if there are no children.
+ TiXmlNode* FirstChild() { return firstChild; }
+ const TiXmlNode* FirstChild( const char * value ) const; ///< The first child of this node with the matching 'value'. Will be null if none found.
+ /// The first child of this node with the matching 'value'. Will be null if none found.
+ TiXmlNode* FirstChild( const char * _value ) {
+ // Call through to the const version - safe since nothing is changed. Exiting syntax: cast this to a const (always safe)
+ // call the method, cast the return back to non-const.
+ return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->FirstChild( _value ));
+ }
+ const TiXmlNode* LastChild() const { return lastChild; } /// The last child of this node. Will be null if there are no children.
+ TiXmlNode* LastChild() { return lastChild; }
+
+ const TiXmlNode* LastChild( const char * value ) const; /// The last child of this node matching 'value'. Will be null if there are no children.
+ TiXmlNode* LastChild( const char * _value ) {
+ return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->LastChild( _value ));
+ }
+
+ #ifdef TIXML_USE_STL
+ const TiXmlNode* FirstChild( const std::string& _value ) const { return FirstChild (_value.c_str ()); } ///< STL std::string form.
+ TiXmlNode* FirstChild( const std::string& _value ) { return FirstChild (_value.c_str ()); } ///< STL std::string form.
+ const TiXmlNode* LastChild( const std::string& _value ) const { return LastChild (_value.c_str ()); } ///< STL std::string form.
+ TiXmlNode* LastChild( const std::string& _value ) { return LastChild (_value.c_str ()); } ///< STL std::string form.
+ #endif
+
+ /** An alternate way to walk the children of a node.
+ One way to iterate over nodes is:
+ @verbatim
+ for( child = parent->FirstChild(); child; child = child->NextSibling() )
+ @endverbatim
+
+ IterateChildren does the same thing with the syntax:
+ @verbatim
+ child = 0;
+ while( child = parent->IterateChildren( child ) )
+ @endverbatim
+
+ IterateChildren takes the previous child as input and finds
+ the next one. If the previous child is null, it returns the
+ first. IterateChildren will return null when done.
+ */
+ const TiXmlNode* IterateChildren( const TiXmlNode* previous ) const;
+ TiXmlNode* IterateChildren( const TiXmlNode* previous ) {
+ return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( previous ) );
+ }
+
+ /// This flavor of IterateChildren searches for children with a particular 'value'
+ const TiXmlNode* IterateChildren( const char * value, const TiXmlNode* previous ) const;
+ TiXmlNode* IterateChildren( const char * _value, const TiXmlNode* previous ) {
+ return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( _value, previous ) );
+ }
+
+ #ifdef TIXML_USE_STL
+ const TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) const { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form.
+ TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form.
+ #endif
+
+ /** Add a new node related to this. Adds a child past the LastChild.
+ Returns a pointer to the new object or NULL if an error occured.
+ */
+ TiXmlNode* InsertEndChild( const TiXmlNode& addThis );
+
+
+ /** Add a new node related to this. Adds a child past the LastChild.
+
+ NOTE: the node to be added is passed by pointer, and will be
+ henceforth owned (and deleted) by tinyXml. This method is efficient
+ and avoids an extra copy, but should be used with care as it
+ uses a different memory model than the other insert functions.
+
+ @sa InsertEndChild
+ */
+ TiXmlNode* LinkEndChild( TiXmlNode* addThis );
+
+ /** Add a new node related to this. Adds a child before the specified child.
+ Returns a pointer to the new object or NULL if an error occured.
+ */
+ TiXmlNode* InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis );
+
+ /** Add a new node related to this. Adds a child after the specified child.
+ Returns a pointer to the new object or NULL if an error occured.
+ */
+ TiXmlNode* InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis );
+
+ /** Replace a child of this node.
+ Returns a pointer to the new object or NULL if an error occured.
+ */
+ TiXmlNode* ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis );
+
+ /// Delete a child of this node.
+ bool RemoveChild( TiXmlNode* removeThis );
+
+ /// Navigate to a sibling node.
+ const TiXmlNode* PreviousSibling() const { return prev; }
+ TiXmlNode* PreviousSibling() { return prev; }
+
+ /// Navigate to a sibling node.
+ const TiXmlNode* PreviousSibling( const char * ) const;
+ TiXmlNode* PreviousSibling( const char *_prev ) {
+ return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->PreviousSibling( _prev ) );
+ }
+
+ #ifdef TIXML_USE_STL
+ const TiXmlNode* PreviousSibling( const std::string& _value ) const { return PreviousSibling (_value.c_str ()); } ///< STL std::string form.
+ TiXmlNode* PreviousSibling( const std::string& _value ) { return PreviousSibling (_value.c_str ()); } ///< STL std::string form.
+ const TiXmlNode* NextSibling( const std::string& _value) const { return NextSibling (_value.c_str ()); } ///< STL std::string form.
+ TiXmlNode* NextSibling( const std::string& _value) { return NextSibling (_value.c_str ()); } ///< STL std::string form.
+ #endif
+
+ /// Navigate to a sibling node.
+ const TiXmlNode* NextSibling() const { return next; }
+ TiXmlNode* NextSibling() { return next; }
+
+ /// Navigate to a sibling node with the given 'value'.
+ const TiXmlNode* NextSibling( const char * ) const;
+ TiXmlNode* NextSibling( const char* _next ) {
+ return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->NextSibling( _next ) );
+ }
+
+ /** Convenience function to get through elements.
+ Calls NextSibling and ToElement. Will skip all non-Element
+ nodes. Returns 0 if there is not another element.
+ */
+ const TiXmlElement* NextSiblingElement() const;
+ TiXmlElement* NextSiblingElement() {
+ return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement() );
+ }
+
+ /** Convenience function to get through elements.
+ Calls NextSibling and ToElement. Will skip all non-Element
+ nodes. Returns 0 if there is not another element.
+ */
+ const TiXmlElement* NextSiblingElement( const char * ) const;
+ TiXmlElement* NextSiblingElement( const char *_next ) {
+ return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement( _next ) );
+ }
+
+ #ifdef TIXML_USE_STL
+ const TiXmlElement* NextSiblingElement( const std::string& _value) const { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form.
+ TiXmlElement* NextSiblingElement( const std::string& _value) { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form.
+ #endif
+
+ /// Convenience function to get through elements.
+ const TiXmlElement* FirstChildElement() const;
+ TiXmlElement* FirstChildElement() {
+ return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement() );
+ }
+
+ /// Convenience function to get through elements.
+ const TiXmlElement* FirstChildElement( const char * _value ) const;
+ TiXmlElement* FirstChildElement( const char * _value ) {
+ return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement( _value ) );
+ }
+
+ #ifdef TIXML_USE_STL
+ const TiXmlElement* FirstChildElement( const std::string& _value ) const { return FirstChildElement (_value.c_str ()); } ///< STL std::string form.
+ TiXmlElement* FirstChildElement( const std::string& _value ) { return FirstChildElement (_value.c_str ()); } ///< STL std::string form.
+ #endif
+
+ /** Query the type (as an enumerated value, above) of this node.
+ The possible types are: TINYXML_DOCUMENT, TINYXML_ELEMENT, TINYXML_COMMENT,
+ TINYXML_UNKNOWN, TINYXML_TEXT, and TINYXML_DECLARATION.
+ */
+ int Type() const { return type; }
+
+ /** Return a pointer to the Document this node lives in.
+ Returns null if not in a document.
+ */
+ const TiXmlDocument* GetDocument() const;
+ TiXmlDocument* GetDocument() {
+ return const_cast< TiXmlDocument* >( (const_cast< const TiXmlNode* >(this))->GetDocument() );
+ }
+
+ /// Returns true if this node has no children.
+ bool NoChildren() const { return !firstChild; }
+
+ virtual const TiXmlDocument* ToDocument() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual const TiXmlElement* ToElement() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual const TiXmlComment* ToComment() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual const TiXmlUnknown* ToUnknown() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual const TiXmlText* ToText() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual const TiXmlDeclaration* ToDeclaration() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+
+ virtual TiXmlDocument* ToDocument() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual TiXmlElement* ToElement() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual TiXmlComment* ToComment() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual TiXmlUnknown* ToUnknown() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual TiXmlText* ToText() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+ virtual TiXmlDeclaration* ToDeclaration() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type.
+
+ /** Create an exact duplicate of this node and return it. The memory must be deleted
+ by the caller.
+ */
+ virtual TiXmlNode* Clone() const = 0;
+
+ /** Accept a hierchical visit the nodes in the TinyXML DOM. Every node in the
+ XML tree will be conditionally visited and the host will be called back
+ via the TiXmlVisitor interface.
+
+ This is essentially a SAX interface for TinyXML. (Note however it doesn't re-parse
+ the XML for the callbacks, so the performance of TinyXML is unchanged by using this
+ interface versus any other.)
+
+ The interface has been based on ideas from:
+
+ - http://www.saxproject.org/
+ - http://c2.com/cgi/wiki?HierarchicalVisitorPattern
+
+ Which are both good references for "visiting".
+
+ An example of using Accept():
+ @verbatim
+ TiXmlPrinter printer;
+ tinyxmlDoc.Accept( &printer );
+ const char* xmlcstr = printer.CStr();
+ @endverbatim
+ */
+ virtual bool Accept( TiXmlVisitor* visitor ) const = 0;
+
+protected:
+ TiXmlNode( NodeType _type );
+
+ // Copy to the allocated object. Shared functionality between Clone, Copy constructor,
+ // and the assignment operator.
+ void CopyTo( TiXmlNode* target ) const;
+
+ #ifdef TIXML_USE_STL
+ // The real work of the input operator.
+ virtual void StreamIn( std::istream* in, TIXML_STRING* tag ) = 0;
+ #endif
+
+ // Figure out what is at *p, and parse it. Returns null if it is not an xml node.
+ TiXmlNode* Identify( const char* start, TiXmlEncoding encoding );
+
+ TiXmlNode* parent;
+ NodeType type;
+
+ TiXmlNode* firstChild;
+ TiXmlNode* lastChild;
+
+ TIXML_STRING value;
+
+ TiXmlNode* prev;
+ TiXmlNode* next;
+
+private:
+ TiXmlNode( const TiXmlNode& ); // not implemented.
+ void operator=( const TiXmlNode& base ); // not allowed.
+};
+
+
+/** An attribute is a name-value pair. Elements have an arbitrary
+ number of attributes, each with a unique name.
+
+ @note The attributes are not TiXmlNodes, since they are not
+ part of the tinyXML document object model. There are other
+ suggested ways to look at this problem.
+*/
+class TiXmlAttribute : public TiXmlBase
+{
+ friend class TiXmlAttributeSet;
+
+public:
+ /// Construct an empty attribute.
+ TiXmlAttribute() : TiXmlBase()
+ {
+ document = 0;
+ prev = next = 0;
+ }
+
+ #ifdef TIXML_USE_STL
+ /// std::string constructor.
+ TiXmlAttribute( const std::string& _name, const std::string& _value )
+ {
+ name = _name;
+ value = _value;
+ document = 0;
+ prev = next = 0;
+ }
+ #endif
+
+ /// Construct an attribute with a name and value.
+ TiXmlAttribute( const char * _name, const char * _value )
+ {
+ name = _name;
+ value = _value;
+ document = 0;
+ prev = next = 0;
+ }
+
+ const char* Name() const { return name.c_str(); } ///< Return the name of this attribute.
+ const char* Value() const { return value.c_str(); } ///< Return the value of this attribute.
+ #ifdef TIXML_USE_STL
+ const std::string& ValueStr() const { return value; } ///< Return the value of this attribute.
+ #endif
+ int IntValue() const; ///< Return the value of this attribute, converted to an integer.
+ double DoubleValue() const; ///< Return the value of this attribute, converted to a double.
+
+ // Get the tinyxml string representation
+ const TIXML_STRING& NameTStr() const { return name; }
+
+ /** QueryIntValue examines the value string. It is an alternative to the
+ IntValue() method with richer error checking.
+ If the value is an integer, it is stored in 'value' and
+ the call returns TIXML_SUCCESS. If it is not
+ an integer, it returns TIXML_WRONG_TYPE.
+
+ A specialized but useful call. Note that for success it returns 0,
+ which is the opposite of almost all other TinyXml calls.
+ */
+ int QueryIntValue( int* _value ) const;
+ /// QueryDoubleValue examines the value string. See QueryIntValue().
+ int QueryDoubleValue( double* _value ) const;
+
+ void SetName( const char* _name ) { name = _name; } ///< Set the name of this attribute.
+ void SetValue( const char* _value ) { value = _value; } ///< Set the value.
+
+ void SetIntValue( int _value ); ///< Set the value from an integer.
+ void SetDoubleValue( double _value ); ///< Set the value from a double.
+
+ #ifdef TIXML_USE_STL
+ /// STL std::string form.
+ void SetName( const std::string& _name ) { name = _name; }
+ /// STL std::string form.
+ void SetValue( const std::string& _value ) { value = _value; }
+ #endif
+
+ /// Get the next sibling attribute in the DOM. Returns null at end.
+ const TiXmlAttribute* Next() const;
+ TiXmlAttribute* Next() {
+ return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Next() );
+ }
+
+ /// Get the previous sibling attribute in the DOM. Returns null at beginning.
+ const TiXmlAttribute* Previous() const;
+ TiXmlAttribute* Previous() {
+ return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Previous() );
+ }
+
+ bool operator==( const TiXmlAttribute& rhs ) const { return rhs.name == name; }
+ bool operator<( const TiXmlAttribute& rhs ) const { return name < rhs.name; }
+ bool operator>( const TiXmlAttribute& rhs ) const { return name > rhs.name; }
+
+ /* Attribute parsing starts: first letter of the name
+ returns: the next char after the value end quote
+ */
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ // Prints this Attribute to a FILE stream.
+ virtual void Print( FILE* cfile, int depth ) const {
+ Print( cfile, depth, 0 );
+ }
+ void Print( FILE* cfile, int depth, TIXML_STRING* str ) const;
+
+ // [internal use]
+ // Set the document pointer so the attribute can report errors.
+ void SetDocument( TiXmlDocument* doc ) { document = doc; }
+
+private:
+ TiXmlAttribute( const TiXmlAttribute& ); // not implemented.
+ void operator=( const TiXmlAttribute& base ); // not allowed.
+
+ TiXmlDocument* document; // A pointer back to a document, for error reporting.
+ TIXML_STRING name;
+ TIXML_STRING value;
+ TiXmlAttribute* prev;
+ TiXmlAttribute* next;
+};
+
+
+/* A class used to manage a group of attributes.
+ It is only used internally, both by the ELEMENT and the DECLARATION.
+
+ The set can be changed transparent to the Element and Declaration
+ classes that use it, but NOT transparent to the Attribute
+ which has to implement a next() and previous() method. Which makes
+ it a bit problematic and prevents the use of STL.
+
+ This version is implemented with circular lists because:
+ - I like circular lists
+ - it demonstrates some independence from the (typical) doubly linked list.
+*/
+class TiXmlAttributeSet
+{
+public:
+ TiXmlAttributeSet();
+ ~TiXmlAttributeSet();
+
+ void Add( TiXmlAttribute* attribute );
+ void Remove( TiXmlAttribute* attribute );
+
+ const TiXmlAttribute* First() const { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; }
+ TiXmlAttribute* First() { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; }
+ const TiXmlAttribute* Last() const { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; }
+ TiXmlAttribute* Last() { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; }
+
+ TiXmlAttribute* Find( const char* _name ) const;
+ TiXmlAttribute* FindOrCreate( const char* _name );
+
+# ifdef TIXML_USE_STL
+ TiXmlAttribute* Find( const std::string& _name ) const;
+ TiXmlAttribute* FindOrCreate( const std::string& _name );
+# endif
+
+
+private:
+ //*ME: Because of hidden/disabled copy-construktor in TiXmlAttribute (sentinel-element),
+ //*ME: this class must be also use a hidden/disabled copy-constructor !!!
+ TiXmlAttributeSet( const TiXmlAttributeSet& ); // not allowed
+ void operator=( const TiXmlAttributeSet& ); // not allowed (as TiXmlAttribute)
+
+ TiXmlAttribute sentinel;
+};
+
+
+/** The element is a container class. It has a value, the element name,
+ and can contain other elements, text, comments, and unknowns.
+ Elements also contain an arbitrary number of attributes.
+*/
+class TiXmlElement : public TiXmlNode
+{
+public:
+ /// Construct an element.
+ TiXmlElement (const char * in_value);
+
+ #ifdef TIXML_USE_STL
+ /// std::string constructor.
+ TiXmlElement( const std::string& _value );
+ #endif
+
+ TiXmlElement( const TiXmlElement& );
+
+ TiXmlElement& operator=( const TiXmlElement& base );
+
+ virtual ~TiXmlElement();
+
+ /** Given an attribute name, Attribute() returns the value
+ for the attribute of that name, or null if none exists.
+ */
+ const char* Attribute( const char* name ) const;
+
+ /** Given an attribute name, Attribute() returns the value
+ for the attribute of that name, or null if none exists.
+ If the attribute exists and can be converted to an integer,
+ the integer value will be put in the return 'i', if 'i'
+ is non-null.
+ */
+ const char* Attribute( const char* name, int* i ) const;
+
+ /** Given an attribute name, Attribute() returns the value
+ for the attribute of that name, or null if none exists.
+ If the attribute exists and can be converted to an double,
+ the double value will be put in the return 'd', if 'd'
+ is non-null.
+ */
+ const char* Attribute( const char* name, double* d ) const;
+
+ /** QueryIntAttribute examines the attribute - it is an alternative to the
+ Attribute() method with richer error checking.
+ If the attribute is an integer, it is stored in 'value' and
+ the call returns TIXML_SUCCESS. If it is not
+ an integer, it returns TIXML_WRONG_TYPE. If the attribute
+ does not exist, then TIXML_NO_ATTRIBUTE is returned.
+ */
+ int QueryIntAttribute( const char* name, int* _value ) const;
+ /// QueryUnsignedAttribute examines the attribute - see QueryIntAttribute().
+ int QueryUnsignedAttribute( const char* name, unsigned* _value ) const;
+ /** QueryBoolAttribute examines the attribute - see QueryIntAttribute().
+ Note that '1', 'true', or 'yes' are considered true, while '0', 'false'
+ and 'no' are considered false.
+ */
+ int QueryBoolAttribute( const char* name, bool* _value ) const;
+ /// QueryDoubleAttribute examines the attribute - see QueryIntAttribute().
+ int QueryDoubleAttribute( const char* name, double* _value ) const;
+ /// QueryFloatAttribute examines the attribute - see QueryIntAttribute().
+ int QueryFloatAttribute( const char* name, float* _value ) const {
+ double d;
+ int result = QueryDoubleAttribute( name, &d );
+ if ( result == TIXML_SUCCESS ) {
+ *_value = (float)d;
+ }
+ return result;
+ }
+
+ #ifdef TIXML_USE_STL
+ /// QueryStringAttribute examines the attribute - see QueryIntAttribute().
+ int QueryStringAttribute( const char* name, std::string* _value ) const {
+ const char* cstr = Attribute( name );
+ if ( cstr ) {
+ *_value = std::string( cstr );
+ return TIXML_SUCCESS;
+ }
+ return TIXML_NO_ATTRIBUTE;
+ }
+
+ /** Template form of the attribute query which will try to read the
+ attribute into the specified type. Very easy, very powerful, but
+ be careful to make sure to call this with the correct type.
+
+ NOTE: This method doesn't work correctly for 'string' types that contain spaces.
+
+ @return TIXML_SUCCESS, TIXML_WRONG_TYPE, or TIXML_NO_ATTRIBUTE
+ */
+ template< typename T > int QueryValueAttribute( const std::string& name, T* outValue ) const
+ {
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( !node )
+ return TIXML_NO_ATTRIBUTE;
+
+ std::stringstream sstream( node->ValueStr() );
+ sstream >> *outValue;
+ if ( !sstream.fail() )
+ return TIXML_SUCCESS;
+ return TIXML_WRONG_TYPE;
+ }
+
+ int QueryValueAttribute( const std::string& name, std::string* outValue ) const
+ {
+ const TiXmlAttribute* node = attributeSet.Find( name );
+ if ( !node )
+ return TIXML_NO_ATTRIBUTE;
+ *outValue = node->ValueStr();
+ return TIXML_SUCCESS;
+ }
+ #endif
+
+ /** Sets an attribute of name to a given value. The attribute
+ will be created if it does not exist, or changed if it does.
+ */
+ void SetAttribute( const char* name, const char * _value );
+
+ #ifdef TIXML_USE_STL
+ const std::string* Attribute( const std::string& name ) const;
+ const std::string* Attribute( const std::string& name, int* i ) const;
+ const std::string* Attribute( const std::string& name, double* d ) const;
+ int QueryIntAttribute( const std::string& name, int* _value ) const;
+ int QueryDoubleAttribute( const std::string& name, double* _value ) const;
+
+ /// STL std::string form.
+ void SetAttribute( const std::string& name, const std::string& _value );
+ ///< STL std::string form.
+ void SetAttribute( const std::string& name, int _value );
+ ///< STL std::string form.
+ void SetDoubleAttribute( const std::string& name, double value );
+ #endif
+
+ /** Sets an attribute of name to a given value. The attribute
+ will be created if it does not exist, or changed if it does.
+ */
+ void SetAttribute( const char * name, int value );
+
+ /** Sets an attribute of name to a given value. The attribute
+ will be created if it does not exist, or changed if it does.
+ */
+ void SetDoubleAttribute( const char * name, double value );
+
+ /** Deletes an attribute with the given name.
+ */
+ void RemoveAttribute( const char * name );
+ #ifdef TIXML_USE_STL
+ void RemoveAttribute( const std::string& name ) { RemoveAttribute (name.c_str ()); } ///< STL std::string form.
+ #endif
+
+ const TiXmlAttribute* FirstAttribute() const { return attributeSet.First(); } ///< Access the first attribute in this element.
+ TiXmlAttribute* FirstAttribute() { return attributeSet.First(); }
+ const TiXmlAttribute* LastAttribute() const { return attributeSet.Last(); } ///< Access the last attribute in this element.
+ TiXmlAttribute* LastAttribute() { return attributeSet.Last(); }
+
+ /** Convenience function for easy access to the text inside an element. Although easy
+ and concise, GetText() is limited compared to getting the TiXmlText child
+ and accessing it directly.
+
+ If the first child of 'this' is a TiXmlText, the GetText()
+ returns the character string of the Text node, else null is returned.
+
+ This is a convenient method for getting the text of simple contained text:
+ @verbatim
+ <foo>This is text</foo>
+ const char* str = fooElement->GetText();
+ @endverbatim
+
+ 'str' will be a pointer to "This is text".
+
+ Note that this function can be misleading. If the element foo was created from
+ this XML:
+ @verbatim
+ <foo><b>This is text</b></foo>
+ @endverbatim
+
+ then the value of str would be null. The first child node isn't a text node, it is
+ another element. From this XML:
+ @verbatim
+ <foo>This is <b>text</b></foo>
+ @endverbatim
+ GetText() will return "This is ".
+
+ WARNING: GetText() accesses a child node - don't become confused with the
+ similarly named TiXmlHandle::Text() and TiXmlNode::ToText() which are
+ safe type casts on the referenced node.
+ */
+ const char* GetText() const;
+
+ /// Creates a new Element and returns it - the returned element is a copy.
+ virtual TiXmlNode* Clone() const;
+ // Print the Element to a FILE stream.
+ virtual void Print( FILE* cfile, int depth ) const;
+
+ /* Attribtue parsing starts: next char past '<'
+ returns: next char past '>'
+ */
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ virtual const TiXmlElement* ToElement() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlElement* ToElement() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* visitor ) const;
+
+protected:
+
+ void CopyTo( TiXmlElement* target ) const;
+ void ClearThis(); // like clear, but initializes 'this' object as well
+
+ // Used to be public [internal use]
+ #ifdef TIXML_USE_STL
+ virtual void StreamIn( std::istream * in, TIXML_STRING * tag );
+ #endif
+ /* [internal use]
+ Reads the "value" of the element -- another element, or text.
+ This should terminate with the current end tag.
+ */
+ const char* ReadValue( const char* in, TiXmlParsingData* prevData, TiXmlEncoding encoding );
+
+private:
+ TiXmlAttributeSet attributeSet;
+};
+
+
+/** An XML comment.
+*/
+class TiXmlComment : public TiXmlNode
+{
+public:
+ /// Constructs an empty comment.
+ TiXmlComment() : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) {}
+ /// Construct a comment from text.
+ TiXmlComment( const char* _value ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) {
+ SetValue( _value );
+ }
+ TiXmlComment( const TiXmlComment& );
+ TiXmlComment& operator=( const TiXmlComment& base );
+
+ virtual ~TiXmlComment() {}
+
+ /// Returns a copy of this Comment.
+ virtual TiXmlNode* Clone() const;
+ // Write this Comment to a FILE stream.
+ virtual void Print( FILE* cfile, int depth ) const;
+
+ /* Attribtue parsing starts: at the ! of the !--
+ returns: next char past '>'
+ */
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ virtual const TiXmlComment* ToComment() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlComment* ToComment() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* visitor ) const;
+
+protected:
+ void CopyTo( TiXmlComment* target ) const;
+
+ // used to be public
+ #ifdef TIXML_USE_STL
+ virtual void StreamIn( std::istream * in, TIXML_STRING * tag );
+ #endif
+// virtual void StreamOut( TIXML_OSTREAM * out ) const;
+
+private:
+
+};
+
+
+/** XML text. A text node can have 2 ways to output the next. "normal" output
+ and CDATA. It will default to the mode it was parsed from the XML file and
+ you generally want to leave it alone, but you can change the output mode with
+ SetCDATA() and query it with CDATA().
+*/
+class TiXmlText : public TiXmlNode
+{
+ friend class TiXmlElement;
+public:
+ /** Constructor for text element. By default, it is treated as
+ normal, encoded text. If you want it be output as a CDATA text
+ element, set the parameter _cdata to 'true'
+ */
+ TiXmlText (const char * initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT)
+ {
+ SetValue( initValue );
+ cdata = false;
+ }
+ virtual ~TiXmlText() {}
+
+ #ifdef TIXML_USE_STL
+ /// Constructor.
+ TiXmlText( const std::string& initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT)
+ {
+ SetValue( initValue );
+ cdata = false;
+ }
+ #endif
+
+ TiXmlText( const TiXmlText& copy ) : TiXmlNode( TiXmlNode::TINYXML_TEXT ) { copy.CopyTo( this ); }
+ TiXmlText& operator=( const TiXmlText& base ) { base.CopyTo( this ); return *this; }
+
+ // Write this text object to a FILE stream.
+ virtual void Print( FILE* cfile, int depth ) const;
+
+ /// Queries whether this represents text using a CDATA section.
+ bool CDATA() const { return cdata; }
+ /// Turns on or off a CDATA representation of text.
+ void SetCDATA( bool _cdata ) { cdata = _cdata; }
+
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ virtual const TiXmlText* ToText() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlText* ToText() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* content ) const;
+
+protected :
+ /// [internal use] Creates a new Element and returns it.
+ virtual TiXmlNode* Clone() const;
+ void CopyTo( TiXmlText* target ) const;
+
+ bool Blank() const; // returns true if all white space and new lines
+ // [internal use]
+ #ifdef TIXML_USE_STL
+ virtual void StreamIn( std::istream * in, TIXML_STRING * tag );
+ #endif
+
+private:
+ bool cdata; // true if this should be input and output as a CDATA style text element
+};
+
+
+/** In correct XML the declaration is the first entry in the file.
+ @verbatim
+ <?xml version="1.0" standalone="yes"?>
+ @endverbatim
+
+ TinyXml will happily read or write files without a declaration,
+ however. There are 3 possible attributes to the declaration:
+ version, encoding, and standalone.
+
+ Note: In this version of the code, the attributes are
+ handled as special cases, not generic attributes, simply
+ because there can only be at most 3 and they are always the same.
+*/
+class TiXmlDeclaration : public TiXmlNode
+{
+public:
+ /// Construct an empty declaration.
+ TiXmlDeclaration() : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) {}
+
+#ifdef TIXML_USE_STL
+ /// Constructor.
+ TiXmlDeclaration( const std::string& _version,
+ const std::string& _encoding,
+ const std::string& _standalone );
+#endif
+
+ /// Construct.
+ TiXmlDeclaration( const char* _version,
+ const char* _encoding,
+ const char* _standalone );
+
+ TiXmlDeclaration( const TiXmlDeclaration& copy );
+ TiXmlDeclaration& operator=( const TiXmlDeclaration& copy );
+
+ virtual ~TiXmlDeclaration() {}
+
+ /// Version. Will return an empty string if none was found.
+ const char *Version() const { return version.c_str (); }
+ /// Encoding. Will return an empty string if none was found.
+ const char *Encoding() const { return encoding.c_str (); }
+ /// Is this a standalone document?
+ const char *Standalone() const { return standalone.c_str (); }
+
+ /// Creates a copy of this Declaration and returns it.
+ virtual TiXmlNode* Clone() const;
+ // Print this declaration to a FILE stream.
+ virtual void Print( FILE* cfile, int depth, TIXML_STRING* str ) const;
+ virtual void Print( FILE* cfile, int depth ) const {
+ Print( cfile, depth, 0 );
+ }
+
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ virtual const TiXmlDeclaration* ToDeclaration() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlDeclaration* ToDeclaration() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* visitor ) const;
+
+protected:
+ void CopyTo( TiXmlDeclaration* target ) const;
+ // used to be public
+ #ifdef TIXML_USE_STL
+ virtual void StreamIn( std::istream * in, TIXML_STRING * tag );
+ #endif
+
+private:
+
+ TIXML_STRING version;
+ TIXML_STRING encoding;
+ TIXML_STRING standalone;
+};
+
+
+/** Any tag that tinyXml doesn't recognize is saved as an
+ unknown. It is a tag of text, but should not be modified.
+ It will be written back to the XML, unchanged, when the file
+ is saved.
+
+ DTD tags get thrown into TiXmlUnknowns.
+*/
+class TiXmlUnknown : public TiXmlNode
+{
+public:
+ TiXmlUnknown() : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) {}
+ virtual ~TiXmlUnknown() {}
+
+ TiXmlUnknown( const TiXmlUnknown& copy ) : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) { copy.CopyTo( this ); }
+ TiXmlUnknown& operator=( const TiXmlUnknown& copy ) { copy.CopyTo( this ); return *this; }
+
+ /// Creates a copy of this Unknown and returns it.
+ virtual TiXmlNode* Clone() const;
+ // Print this Unknown to a FILE stream.
+ virtual void Print( FILE* cfile, int depth ) const;
+
+ virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding );
+
+ virtual const TiXmlUnknown* ToUnknown() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlUnknown* ToUnknown() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* content ) const;
+
+protected:
+ void CopyTo( TiXmlUnknown* target ) const;
+
+ #ifdef TIXML_USE_STL
+ virtual void StreamIn( std::istream * in, TIXML_STRING * tag );
+ #endif
+
+private:
+
+};
+
+
+/** Always the top level node. A document binds together all the
+ XML pieces. It can be saved, loaded, and printed to the screen.
+ The 'value' of a document node is the xml file name.
+*/
+class TiXmlDocument : public TiXmlNode
+{
+public:
+ /// Create an empty document, that has no name.
+ TiXmlDocument();
+ /// Create a document with a name. The name of the document is also the filename of the xml.
+ TiXmlDocument( const char * documentName );
+
+ #ifdef TIXML_USE_STL
+ /// Constructor.
+ TiXmlDocument( const std::string& documentName );
+ #endif
+
+ TiXmlDocument( const TiXmlDocument& copy );
+ TiXmlDocument& operator=( const TiXmlDocument& copy );
+
+ virtual ~TiXmlDocument() {}
+
+ /** Load a file using the current document value.
+ Returns true if successful. Will delete any existing
+ document data before loading.
+ */
+ bool LoadFile( TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING );
+ /// Save a file using the current document value. Returns true if successful.
+ bool SaveFile() const;
+ /// Load a file using the given filename. Returns true if successful.
+ bool LoadFile( const char * filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING );
+ /// Save a file using the given filename. Returns true if successful.
+ bool SaveFile( const char * filename ) const;
+ /** Load a file using the given FILE*. Returns true if successful. Note that this method
+ doesn't stream - the entire object pointed at by the FILE*
+ will be interpreted as an XML file. TinyXML doesn't stream in XML from the current
+ file location. Streaming may be added in the future.
+ */
+ bool LoadFile( FILE*, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING );
+ /// Save a file using the given FILE*. Returns true if successful.
+ bool SaveFile( FILE* ) const;
+
+ #ifdef TIXML_USE_STL
+ bool LoadFile( const std::string& filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ) ///< STL std::string version.
+ {
+ return LoadFile( filename.c_str(), encoding );
+ }
+ bool SaveFile( const std::string& filename ) const ///< STL std::string version.
+ {
+ return SaveFile( filename.c_str() );
+ }
+ #endif
+
+ /** Parse the given null terminated block of xml data. Passing in an encoding to this
+ method (either TIXML_ENCODING_LEGACY or TIXML_ENCODING_UTF8 will force TinyXml
+ to use that encoding, regardless of what TinyXml might otherwise try to detect.
+ */
+ virtual const char* Parse( const char* p, TiXmlParsingData* data = 0, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING );
+
+ /** Get the root element -- the only top level element -- of the document.
+ In well formed XML, there should only be one. TinyXml is tolerant of
+ multiple elements at the document level.
+ */
+ const TiXmlElement* RootElement() const { return FirstChildElement(); }
+ TiXmlElement* RootElement() { return FirstChildElement(); }
+
+ /** If an error occurs, Error will be set to true. Also,
+ - The ErrorId() will contain the integer identifier of the error (not generally useful)
+ - The ErrorDesc() method will return the name of the error. (very useful)
+ - The ErrorRow() and ErrorCol() will return the location of the error (if known)
+ */
+ bool Error() const { return error; }
+
+ /// Contains a textual (english) description of the error if one occurs.
+ const char * ErrorDesc() const { return errorDesc.c_str (); }
+
+ /** Generally, you probably want the error string ( ErrorDesc() ). But if you
+ prefer the ErrorId, this function will fetch it.
+ */
+ int ErrorId() const { return errorId; }
+
+ /** Returns the location (if known) of the error. The first column is column 1,
+ and the first row is row 1. A value of 0 means the row and column wasn't applicable
+ (memory errors, for example, have no row/column) or the parser lost the error. (An
+ error in the error reporting, in that case.)
+
+ @sa SetTabSize, Row, Column
+ */
+ int ErrorRow() const { return errorLocation.row+1; }
+ int ErrorCol() const { return errorLocation.col+1; } ///< The column where the error occured. See ErrorRow()
+
+ /** SetTabSize() allows the error reporting functions (ErrorRow() and ErrorCol())
+ to report the correct values for row and column. It does not change the output
+ or input in any way.
+
+ By calling this method, with a tab size
+ greater than 0, the row and column of each node and attribute is stored
+ when the file is loaded. Very useful for tracking the DOM back in to
+ the source file.
+
+ The tab size is required for calculating the location of nodes. If not
+ set, the default of 4 is used. The tabsize is set per document. Setting
+ the tabsize to 0 disables row/column tracking.
+
+ Note that row and column tracking is not supported when using operator>>.
+
+ The tab size needs to be enabled before the parse or load. Correct usage:
+ @verbatim
+ TiXmlDocument doc;
+ doc.SetTabSize( 8 );
+ doc.Load( "myfile.xml" );
+ @endverbatim
+
+ @sa Row, Column
+ */
+ void SetTabSize( int _tabsize ) { tabsize = _tabsize; }
+
+ int TabSize() const { return tabsize; }
+
+ /** If you have handled the error, it can be reset with this call. The error
+ state is automatically cleared if you Parse a new XML block.
+ */
+ void ClearError() { error = false;
+ errorId = 0;
+ errorDesc = "";
+ errorLocation.row = errorLocation.col = 0;
+ //errorLocation.last = 0;
+ }
+
+ /** Write the document to standard out using formatted printing ("pretty print"). */
+ void Print() const { Print( stdout, 0 ); }
+
+ /* Write the document to a string using formatted printing ("pretty print"). This
+ will allocate a character array (new char[]) and return it as a pointer. The
+ calling code pust call delete[] on the return char* to avoid a memory leak.
+ */
+ //char* PrintToMemory() const;
+
+ /// Print this Document to a FILE stream.
+ virtual void Print( FILE* cfile, int depth = 0 ) const;
+ // [internal use]
+ void SetError( int err, const char* errorLocation, TiXmlParsingData* prevData, TiXmlEncoding encoding );
+
+ virtual const TiXmlDocument* ToDocument() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+ virtual TiXmlDocument* ToDocument() { return this; } ///< Cast to a more defined type. Will return null not of the requested type.
+
+ /** Walk the XML tree visiting this node and all of its children.
+ */
+ virtual bool Accept( TiXmlVisitor* content ) const;
+
+protected :
+ // [internal use]
+ virtual TiXmlNode* Clone() const;
+ #ifdef TIXML_USE_STL
+ virtual void StreamIn( std::istream * in, TIXML_STRING * tag );
+ #endif
+
+private:
+ void CopyTo( TiXmlDocument* target ) const;
+
+ bool error;
+ int errorId;
+ TIXML_STRING errorDesc;
+ int tabsize;
+ TiXmlCursor errorLocation;
+ bool useMicrosoftBOM; // the UTF-8 BOM were found when read. Note this, and try to write.
+};
+
+
+/**
+ A TiXmlHandle is a class that wraps a node pointer with null checks; this is
+ an incredibly useful thing. Note that TiXmlHandle is not part of the TinyXml
+ DOM structure. It is a separate utility class.
+
+ Take an example:
+ @verbatim
+ <Document>
+ <Element attributeA = "valueA">
+ <Child attributeB = "value1" />
+ <Child attributeB = "value2" />
+ </Element>
+ <Document>
+ @endverbatim
+
+ Assuming you want the value of "attributeB" in the 2nd "Child" element, it's very
+ easy to write a *lot* of code that looks like:
+
+ @verbatim
+ TiXmlElement* root = document.FirstChildElement( "Document" );
+ if ( root )
+ {
+ TiXmlElement* element = root->FirstChildElement( "Element" );
+ if ( element )
+ {
+ TiXmlElement* child = element->FirstChildElement( "Child" );
+ if ( child )
+ {
+ TiXmlElement* child2 = child->NextSiblingElement( "Child" );
+ if ( child2 )
+ {
+ // Finally do something useful.
+ @endverbatim
+
+ And that doesn't even cover "else" cases. TiXmlHandle addresses the verbosity
+ of such code. A TiXmlHandle checks for null pointers so it is perfectly safe
+ and correct to use:
+
+ @verbatim
+ TiXmlHandle docHandle( &document );
+ TiXmlElement* child2 = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", 1 ).ToElement();
+ if ( child2 )
+ {
+ // do something useful
+ @endverbatim
+
+ Which is MUCH more concise and useful.
+
+ It is also safe to copy handles - internally they are nothing more than node pointers.
+ @verbatim
+ TiXmlHandle handleCopy = handle;
+ @endverbatim
+
+ What they should not be used for is iteration:
+
+ @verbatim
+ int i=0;
+ while ( true )
+ {
+ TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", i ).ToElement();
+ if ( !child )
+ break;
+ // do something
+ ++i;
+ }
+ @endverbatim
+
+ It seems reasonable, but it is in fact two embedded while loops. The Child method is
+ a linear walk to find the element, so this code would iterate much more than it needs
+ to. Instead, prefer:
+
+ @verbatim
+ TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).FirstChild( "Child" ).ToElement();
+
+ for( child; child; child=child->NextSiblingElement() )
+ {
+ // do something
+ }
+ @endverbatim
+*/
+class TiXmlHandle
+{
+public:
+ /// Create a handle from any node (at any depth of the tree.) This can be a null pointer.
+ TiXmlHandle( TiXmlNode* _node ) { this->node = _node; }
+ /// Copy constructor
+ TiXmlHandle( const TiXmlHandle& ref ) { this->node = ref.node; }
+ TiXmlHandle operator=( const TiXmlHandle& ref ) { if ( &ref != this ) this->node = ref.node; return *this; }
+
+ /// Return a handle to the first child node.
+ TiXmlHandle FirstChild() const;
+ /// Return a handle to the first child node with the given name.
+ TiXmlHandle FirstChild( const char * value ) const;
+ /// Return a handle to the first child element.
+ TiXmlHandle FirstChildElement() const;
+ /// Return a handle to the first child element with the given name.
+ TiXmlHandle FirstChildElement( const char * value ) const;
+
+ /** Return a handle to the "index" child with the given name.
+ The first child is 0, the second 1, etc.
+ */
+ TiXmlHandle Child( const char* value, int index ) const;
+ /** Return a handle to the "index" child.
+ The first child is 0, the second 1, etc.
+ */
+ TiXmlHandle Child( int index ) const;
+ /** Return a handle to the "index" child element with the given name.
+ The first child element is 0, the second 1, etc. Note that only TiXmlElements
+ are indexed: other types are not counted.
+ */
+ TiXmlHandle ChildElement( const char* value, int index ) const;
+ /** Return a handle to the "index" child element.
+ The first child element is 0, the second 1, etc. Note that only TiXmlElements
+ are indexed: other types are not counted.
+ */
+ TiXmlHandle ChildElement( int index ) const;
+
+ #ifdef TIXML_USE_STL
+ TiXmlHandle FirstChild( const std::string& _value ) const { return FirstChild( _value.c_str() ); }
+ TiXmlHandle FirstChildElement( const std::string& _value ) const { return FirstChildElement( _value.c_str() ); }
+
+ TiXmlHandle Child( const std::string& _value, int index ) const { return Child( _value.c_str(), index ); }
+ TiXmlHandle ChildElement( const std::string& _value, int index ) const { return ChildElement( _value.c_str(), index ); }
+ #endif
+
+ /** Return the handle as a TiXmlNode. This may return null.
+ */
+ TiXmlNode* ToNode() const { return node; }
+ /** Return the handle as a TiXmlElement. This may return null.
+ */
+ TiXmlElement* ToElement() const { return ( ( node && node->ToElement() ) ? node->ToElement() : 0 ); }
+ /** Return the handle as a TiXmlText. This may return null.
+ */
+ TiXmlText* ToText() const { return ( ( node && node->ToText() ) ? node->ToText() : 0 ); }
+ /** Return the handle as a TiXmlUnknown. This may return null.
+ */
+ TiXmlUnknown* ToUnknown() const { return ( ( node && node->ToUnknown() ) ? node->ToUnknown() : 0 ); }
+
+ /** @deprecated use ToNode.
+ Return the handle as a TiXmlNode. This may return null.
+ */
+ TiXmlNode* Node() const { return ToNode(); }
+ /** @deprecated use ToElement.
+ Return the handle as a TiXmlElement. This may return null.
+ */
+ TiXmlElement* Element() const { return ToElement(); }
+ /** @deprecated use ToText()
+ Return the handle as a TiXmlText. This may return null.
+ */
+ TiXmlText* Text() const { return ToText(); }
+ /** @deprecated use ToUnknown()
+ Return the handle as a TiXmlUnknown. This may return null.
+ */
+ TiXmlUnknown* Unknown() const { return ToUnknown(); }
+
+private:
+ TiXmlNode* node;
+};
+
+
+/** Print to memory functionality. The TiXmlPrinter is useful when you need to:
+
+ -# Print to memory (especially in non-STL mode)
+ -# Control formatting (line endings, etc.)
+
+ When constructed, the TiXmlPrinter is in its default "pretty printing" mode.
+ Before calling Accept() you can call methods to control the printing
+ of the XML document. After TiXmlNode::Accept() is called, the printed document can
+ be accessed via the CStr(), Str(), and Size() methods.
+
+ TiXmlPrinter uses the Visitor API.
+ @verbatim
+ TiXmlPrinter printer;
+ printer.SetIndent( "\t" );
+
+ doc.Accept( &printer );
+ fprintf( stdout, "%s", printer.CStr() );
+ @endverbatim
+*/
+class TiXmlPrinter : public TiXmlVisitor
+{
+public:
+ TiXmlPrinter() : depth( 0 ), simpleTextPrint( false ),
+ buffer(), indent( " " ), lineBreak( "\n" ) {}
+
+ virtual bool VisitEnter( const TiXmlDocument& doc );
+ virtual bool VisitExit( const TiXmlDocument& doc );
+
+ virtual bool VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute );
+ virtual bool VisitExit( const TiXmlElement& element );
+
+ virtual bool Visit( const TiXmlDeclaration& declaration );
+ virtual bool Visit( const TiXmlText& text );
+ virtual bool Visit( const TiXmlComment& comment );
+ virtual bool Visit( const TiXmlUnknown& unknown );
+
+ /** Set the indent characters for printing. By default 4 spaces
+ but tab (\t) is also useful, or null/empty string for no indentation.
+ */
+ void SetIndent( const char* _indent ) { indent = _indent ? _indent : "" ; }
+ /// Query the indention string.
+ const char* Indent() { return indent.c_str(); }
+ /** Set the line breaking string. By default set to newline (\n).
+ Some operating systems prefer other characters, or can be
+ set to the null/empty string for no indenation.
+ */
+ void SetLineBreak( const char* _lineBreak ) { lineBreak = _lineBreak ? _lineBreak : ""; }
+ /// Query the current line breaking string.
+ const char* LineBreak() { return lineBreak.c_str(); }
+
+ /** Switch over to "stream printing" which is the most dense formatting without
+ linebreaks. Common when the XML is needed for network transmission.
+ */
+ void SetStreamPrinting() { indent = "";
+ lineBreak = "";
+ }
+ /// Return the result.
+ const char* CStr() { return buffer.c_str(); }
+ /// Return the length of the result string.
+ size_t Size() { return buffer.size(); }
+
+ #ifdef TIXML_USE_STL
+ /// Return the result.
+ const std::string& Str() { return buffer; }
+ #endif
+
+private:
+ void DoIndent() {
+ for( int i=0; i<depth; ++i )
+ buffer += indent;
+ }
+ void DoLineBreak() {
+ buffer += lineBreak;
+ }
+
+ int depth;
+ bool simpleTextPrint;
+ TIXML_STRING buffer;
+ TIXML_STRING indent;
+ TIXML_STRING lineBreak;
+};
+
+
+#ifdef _MSC_VER
+#pragma warning( pop )
+#endif
+
+#endif
diff --git a/deps/win/tinyxml/tinyxmlerror.cpp b/deps/win/tinyxml/tinyxmlerror.cpp
new file mode 100644
index 0000000..538c21d
--- /dev/null
+++ b/deps/win/tinyxml/tinyxmlerror.cpp
@@ -0,0 +1,52 @@
+/*
+www.sourceforge.net/projects/tinyxml
+Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com)
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+#include "tinyxml.h"
+
+// The goal of the seperate error file is to make the first
+// step towards localization. tinyxml (currently) only supports
+// english error messages, but the could now be translated.
+//
+// It also cleans up the code a bit.
+//
+
+const char* TiXmlBase::errorString[ TiXmlBase::TIXML_ERROR_STRING_COUNT ] =
+{
+ "No error",
+ "Error",
+ "Failed to open file",
+ "Error parsing Element.",
+ "Failed to read Element name",
+ "Error reading Element value.",
+ "Error reading Attributes.",
+ "Error: empty tag.",
+ "Error reading end tag.",
+ "Error parsing Unknown.",
+ "Error parsing Comment.",
+ "Error parsing Declaration.",
+ "Error document empty.",
+ "Error null (0) or unexpected EOF found in input stream.",
+ "Error parsing CDATA.",
+ "Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.",
+};
diff --git a/deps/win/tinyxml/tinyxmlparser.cpp b/deps/win/tinyxml/tinyxmlparser.cpp
new file mode 100644
index 0000000..eb871e5
--- /dev/null
+++ b/deps/win/tinyxml/tinyxmlparser.cpp
@@ -0,0 +1,1640 @@
+/*
+www.sourceforge.net/projects/tinyxml
+Original code by Lee Thomason (www.grinninglizard.com)
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+#include <ctype.h>
+#include <stddef.h>
+
+#include "tinyxml.h"
+
+//#define DEBUG_PARSER
+#if defined( DEBUG_PARSER )
+# if defined( DEBUG ) && defined( _MSC_VER )
+# include <windows.h>
+# define TIXML_LOG OutputDebugString
+# else
+# define TIXML_LOG printf
+# endif
+#endif
+
+// Note tha "PutString" hardcodes the same list. This
+// is less flexible than it appears. Changing the entries
+// or order will break putstring.
+TiXmlBase::Entity TiXmlBase::entity[ TiXmlBase::NUM_ENTITY ] =
+{
+ { "&", 5, '&' },
+ { "<", 4, '<' },
+ { ">", 4, '>' },
+ { """, 6, '\"' },
+ { "'", 6, '\'' }
+};
+
+// Bunch of unicode info at:
+// http://www.unicode.org/faq/utf_bom.html
+// Including the basic of this table, which determines the #bytes in the
+// sequence from the lead byte. 1 placed for invalid sequences --
+// although the result will be junk, pass it through as much as possible.
+// Beware of the non-characters in UTF-8:
+// ef bb bf (Microsoft "lead bytes")
+// ef bf be
+// ef bf bf
+
+const unsigned char TIXML_UTF_LEAD_0 = 0xefU;
+const unsigned char TIXML_UTF_LEAD_1 = 0xbbU;
+const unsigned char TIXML_UTF_LEAD_2 = 0xbfU;
+
+const int TiXmlBase::utf8ByteTable[256] =
+{
+ // 0 1 2 3 4 5 6 7 8 9 a b c d e f
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x00
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x10
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x20
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x30
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x40
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x50
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x60
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x70 End of ASCII range
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x80 0x80 to 0xc1 invalid
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x90
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xa0
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xb0
+ 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xc0 0xc2 to 0xdf 2 byte
+ 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xd0
+ 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // 0xe0 0xe0 to 0xef 3 byte
+ 4, 4, 4, 4, 4, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // 0xf0 0xf0 to 0xf4 4 byte, 0xf5 and higher invalid
+};
+
+
+void TiXmlBase::ConvertUTF32ToUTF8( unsigned long input, char* output, int* length )
+{
+ const unsigned long BYTE_MASK = 0xBF;
+ const unsigned long BYTE_MARK = 0x80;
+ const unsigned long FIRST_BYTE_MARK[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC };
+
+ if (input < 0x80)
+ *length = 1;
+ else if ( input < 0x800 )
+ *length = 2;
+ else if ( input < 0x10000 )
+ *length = 3;
+ else if ( input < 0x200000 )
+ *length = 4;
+ else
+ { *length = 0; return; } // This code won't covert this correctly anyway.
+
+ output += *length;
+
+ // Scary scary fall throughs.
+ #pragma GCC diagnostic ignored "-Wswitch-default"
+ switch (*length)
+ {
+ case 4:
+ --output;
+ *output = (char)((input | BYTE_MARK) & BYTE_MASK);
+ input >>= 6;
+ case 3:
+ --output;
+ *output = (char)((input | BYTE_MARK) & BYTE_MASK);
+ input >>= 6;
+ case 2:
+ --output;
+ *output = (char)((input | BYTE_MARK) & BYTE_MASK);
+ input >>= 6;
+ case 1:
+ --output;
+ *output = (char)(input | FIRST_BYTE_MARK[*length]);
+ }
+ #pragma GCC diagnostic pop
+}
+
+
+/*static*/ int TiXmlBase::IsAlpha( unsigned char anyByte, TiXmlEncoding /*encoding*/ )
+{
+ // This will only work for low-ascii, everything else is assumed to be a valid
+ // letter. I'm not sure this is the best approach, but it is quite tricky trying
+ // to figure out alhabetical vs. not across encoding. So take a very
+ // conservative approach.
+
+// if ( encoding == TIXML_ENCODING_UTF8 )
+// {
+ if ( anyByte < 127 )
+ return isalpha( anyByte );
+ else
+ return 1; // What else to do? The unicode set is huge...get the english ones right.
+// }
+// else
+// {
+// return isalpha( anyByte );
+// }
+}
+
+
+/*static*/ int TiXmlBase::IsAlphaNum( unsigned char anyByte, TiXmlEncoding /*encoding*/ )
+{
+ // This will only work for low-ascii, everything else is assumed to be a valid
+ // letter. I'm not sure this is the best approach, but it is quite tricky trying
+ // to figure out alhabetical vs. not across encoding. So take a very
+ // conservative approach.
+
+// if ( encoding == TIXML_ENCODING_UTF8 )
+// {
+ if ( anyByte < 127 )
+ return isalnum( anyByte );
+ else
+ return 1; // What else to do? The unicode set is huge...get the english ones right.
+// }
+// else
+// {
+// return isalnum( anyByte );
+// }
+}
+
+
+class TiXmlParsingData
+{
+ friend class TiXmlDocument;
+ public:
+ void Stamp( const char* now, TiXmlEncoding encoding );
+
+ const TiXmlCursor& Cursor() const { return cursor; }
+
+ private:
+ // Only used by the document!
+ TiXmlParsingData( const char* start, int _tabsize, int row, int col )
+ {
+ assert( start );
+ stamp = start;
+ tabsize = _tabsize;
+ cursor.row = row;
+ cursor.col = col;
+ }
+
+ TiXmlCursor cursor;
+ const char* stamp;
+ int tabsize;
+};
+
+
+void TiXmlParsingData::Stamp( const char* now, TiXmlEncoding encoding )
+{
+ assert( now );
+
+ // Do nothing if the tabsize is 0.
+ if ( tabsize < 1 )
+ {
+ return;
+ }
+
+ // Get the current row, column.
+ int row = cursor.row;
+ int col = cursor.col;
+ const char* p = stamp;
+ assert( p );
+
+ while ( p < now )
+ {
+ // Treat p as unsigned, so we have a happy compiler.
+ const unsigned char* pU = (const unsigned char*)p;
+
+ // Code contributed by Fletcher Dunn: (modified by lee)
+ switch (*pU) {
+ case 0:
+ // We *should* never get here, but in case we do, don't
+ // advance past the terminating null character, ever
+ return;
+
+ case '\r':
+ // bump down to the next line
+ ++row;
+ col = 0;
+ // Eat the character
+ ++p;
+
+ // Check for \r\n sequence, and treat this as a single character
+ if (*p == '\n') {
+ ++p;
+ }
+ break;
+
+ case '\n':
+ // bump down to the next line
+ ++row;
+ col = 0;
+
+ // Eat the character
+ ++p;
+
+ // Check for \n\r sequence, and treat this as a single
+ // character. (Yes, this bizarre thing does occur still
+ // on some arcane platforms...)
+ if (*p == '\r') {
+ ++p;
+ }
+ break;
+
+ case '\t':
+ // Eat the character
+ ++p;
+
+ // Skip to next tab stop
+ col = (col / tabsize + 1) * tabsize;
+ break;
+
+ case TIXML_UTF_LEAD_0:
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ if ( *(p+1) && *(p+2) )
+ {
+ // In these cases, don't advance the column. These are
+ // 0-width spaces.
+ if ( *(pU+1)==TIXML_UTF_LEAD_1 && *(pU+2)==TIXML_UTF_LEAD_2 )
+ p += 3;
+ else if ( *(pU+1)==0xbfU && *(pU+2)==0xbeU )
+ p += 3;
+ else if ( *(pU+1)==0xbfU && *(pU+2)==0xbfU )
+ p += 3;
+ else
+ { p +=3; ++col; } // A normal character.
+ }
+ }
+ else
+ {
+ ++p;
+ ++col;
+ }
+ break;
+
+ default:
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ // Eat the 1 to 4 byte utf8 character.
+ int step = TiXmlBase::utf8ByteTable[*((const unsigned char*)p)];
+ if ( step == 0 )
+ step = 1; // Error case from bad encoding, but handle gracefully.
+ p += step;
+
+ // Just advance one column, of course.
+ ++col;
+ }
+ else
+ {
+ ++p;
+ ++col;
+ }
+ break;
+ }
+ }
+ cursor.row = row;
+ cursor.col = col;
+ assert( cursor.row >= -1 );
+ assert( cursor.col >= -1 );
+ stamp = p;
+ assert( stamp );
+}
+
+
+const char* TiXmlBase::SkipWhiteSpace( const char* p, TiXmlEncoding encoding )
+{
+ if ( !p || !*p )
+ {
+ return 0;
+ }
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ while ( *p )
+ {
+ const unsigned char* pU = (const unsigned char*)p;
+
+ // Skip the stupid Microsoft UTF-8 Byte order marks
+ if ( *(pU+0)==TIXML_UTF_LEAD_0
+ && *(pU+1)==TIXML_UTF_LEAD_1
+ && *(pU+2)==TIXML_UTF_LEAD_2 )
+ {
+ p += 3;
+ continue;
+ }
+ else if(*(pU+0)==TIXML_UTF_LEAD_0
+ && *(pU+1)==0xbfU
+ && *(pU+2)==0xbeU )
+ {
+ p += 3;
+ continue;
+ }
+ else if(*(pU+0)==TIXML_UTF_LEAD_0
+ && *(pU+1)==0xbfU
+ && *(pU+2)==0xbfU )
+ {
+ p += 3;
+ continue;
+ }
+
+ if ( IsWhiteSpace( *p ) ) // Still using old rules for white space.
+ ++p;
+ else
+ break;
+ }
+ }
+ else
+ {
+ while ( *p && IsWhiteSpace( *p ) )
+ ++p;
+ }
+
+ return p;
+}
+
+#ifdef TIXML_USE_STL
+/*static*/ bool TiXmlBase::StreamWhiteSpace( std::istream * in, TIXML_STRING * tag )
+{
+ for( ;; )
+ {
+ if ( !in->good() ) return false;
+
+ int c = in->peek();
+ // At this scope, we can't get to a document. So fail silently.
+ if ( !IsWhiteSpace( c ) || c <= 0 )
+ return true;
+
+ *tag += (char) in->get();
+ }
+}
+
+/*static*/ bool TiXmlBase::StreamTo( std::istream * in, int character, TIXML_STRING * tag )
+{
+ //assert( character > 0 && character < 128 ); // else it won't work in utf-8
+ while ( in->good() )
+ {
+ int c = in->peek();
+ if ( c == character )
+ return true;
+ if ( c <= 0 ) // Silent failure: can't get document at this scope
+ return false;
+
+ in->get();
+ *tag += (char) c;
+ }
+ return false;
+}
+#endif
+
+// One of TinyXML's more performance demanding functions. Try to keep the memory overhead down. The
+// "assign" optimization removes over 10% of the execution time.
+//
+const char* TiXmlBase::ReadName( const char* p, TIXML_STRING * name, TiXmlEncoding encoding )
+{
+ // Oddly, not supported on some comilers,
+ //name->clear();
+ // So use this:
+ *name = "";
+ assert( p );
+
+ // Names start with letters or underscores.
+ // Of course, in unicode, tinyxml has no idea what a letter *is*. The
+ // algorithm is generous.
+ //
+ // After that, they can be letters, underscores, numbers,
+ // hyphens, or colons. (Colons are valid ony for namespaces,
+ // but tinyxml can't tell namespaces from names.)
+ if ( p && *p
+ && ( IsAlpha( (unsigned char) *p, encoding ) || *p == '_' ) )
+ {
+ const char* start = p;
+ while( p && *p
+ && ( IsAlphaNum( (unsigned char ) *p, encoding )
+ || *p == '_'
+ || *p == '-'
+ || *p == '.'
+ || *p == ':' ) )
+ {
+ //(*name) += *p; // expensive
+ ++p;
+ }
+ if ( p-start > 0 ) {
+ name->assign( start, p-start );
+ }
+ return p;
+ }
+ return 0;
+}
+
+const char* TiXmlBase::GetEntity( const char* p, char* value, int* length, TiXmlEncoding encoding )
+{
+ // Presume an entity, and pull it out.
+ TIXML_STRING ent;
+ int i;
+ *length = 0;
+
+ if ( *(p+1) && *(p+1) == '#' && *(p+2) )
+ {
+ unsigned long ucs = 0;
+ ptrdiff_t delta = 0;
+ unsigned mult = 1;
+
+ if ( *(p+2) == 'x' )
+ {
+ // Hexadecimal.
+ if ( !*(p+3) ) return 0;
+
+ const char* q = p+3;
+ q = strchr( q, ';' );
+
+ if ( !q || !*q ) return 0;
+
+ delta = q-p;
+ --q;
+
+ while ( *q != 'x' )
+ {
+ if ( *q >= '0' && *q <= '9' )
+ ucs += mult * (*q - '0');
+ else if ( *q >= 'a' && *q <= 'f' )
+ ucs += mult * (*q - 'a' + 10);
+ else if ( *q >= 'A' && *q <= 'F' )
+ ucs += mult * (*q - 'A' + 10 );
+ else
+ return 0;
+ mult *= 16;
+ --q;
+ }
+ }
+ else
+ {
+ // Decimal.
+ if ( !*(p+2) ) return 0;
+
+ const char* q = p+2;
+ q = strchr( q, ';' );
+
+ if ( !q || !*q ) return 0;
+
+ delta = q-p;
+ --q;
+
+ while ( *q != '#' )
+ {
+ if ( *q >= '0' && *q <= '9' )
+ ucs += mult * (*q - '0');
+ else
+ return 0;
+ mult *= 10;
+ --q;
+ }
+ }
+ if ( encoding == TIXML_ENCODING_UTF8 )
+ {
+ // convert the UCS to UTF-8
+ ConvertUTF32ToUTF8( ucs, value, length );
+ }
+ else
+ {
+ *value = (char)ucs;
+ *length = 1;
+ }
+ return p + delta + 1;
+ }
+
+ // Now try to match it.
+ for( i=0; i<NUM_ENTITY; ++i )
+ {
+ if ( strncmp( entity[i].str, p, entity[i].strLength ) == 0 )
+ {
+ assert( strlen( entity[i].str ) == entity[i].strLength );
+ *value = entity[i].chr;
+ *length = 1;
+ return ( p + entity[i].strLength );
+ }
+ }
+
+ // So it wasn't an entity, its unrecognized, or something like that.
+ *value = *p; // Don't put back the last one, since we return it!
+ //*length = 1; // Leave unrecognized entities - this doesn't really work.
+ // Just writes strange XML.
+ return p+1;
+}
+
+
+bool TiXmlBase::StringEqual( const char* p,
+ const char* tag,
+ bool ignoreCase,
+ TiXmlEncoding encoding )
+{
+ assert( p );
+ assert( tag );
+ if ( !p || !*p )
+ {
+ assert( 0 );
+ return false;
+ }
+
+ const char* q = p;
+
+ if ( ignoreCase )
+ {
+ while ( *q && *tag && ToLower( *q, encoding ) == ToLower( *tag, encoding ) )
+ {
+ ++q;
+ ++tag;
+ }
+
+ if ( *tag == 0 )
+ return true;
+ }
+ else
+ {
+ while ( *q && *tag && *q == *tag )
+ {
+ ++q;
+ ++tag;
+ }
+
+ if ( *tag == 0 ) // Have we found the end of the tag, and everything equal?
+ return true;
+ }
+ return false;
+}
+
+const char* TiXmlBase::ReadText( const char* p,
+ TIXML_STRING * text,
+ bool trimWhiteSpace,
+ const char* endTag,
+ bool caseInsensitive,
+ TiXmlEncoding encoding )
+{
+ *text = "";
+ if ( !trimWhiteSpace // certain tags always keep whitespace
+ || !condenseWhiteSpace ) // if true, whitespace is always kept
+ {
+ // Keep all the white space.
+ while ( p && *p
+ && !StringEqual( p, endTag, caseInsensitive, encoding )
+ )
+ {
+ int len;
+ char cArr[4] = { 0, 0, 0, 0 };
+ p = GetChar( p, cArr, &len, encoding );
+ text->append( cArr, len );
+ }
+ }
+ else
+ {
+ bool whitespace = false;
+
+ // Remove leading white space:
+ p = SkipWhiteSpace( p, encoding );
+ while ( p && *p
+ && !StringEqual( p, endTag, caseInsensitive, encoding ) )
+ {
+ if ( *p == '\r' || *p == '\n' )
+ {
+ whitespace = true;
+ ++p;
+ }
+ else if ( IsWhiteSpace( *p ) )
+ {
+ whitespace = true;
+ ++p;
+ }
+ else
+ {
+ // If we've found whitespace, add it before the
+ // new character. Any whitespace just becomes a space.
+ if ( whitespace )
+ {
+ (*text) += ' ';
+ whitespace = false;
+ }
+ int len;
+ char cArr[4] = { 0, 0, 0, 0 };
+ p = GetChar( p, cArr, &len, encoding );
+ if ( len == 1 )
+ (*text) += cArr[0]; // more efficient
+ else
+ text->append( cArr, len );
+ }
+ }
+ }
+ if ( p && *p )
+ p += strlen( endTag );
+ return ( p && *p ) ? p : 0;
+}
+
+#ifdef TIXML_USE_STL
+
+void TiXmlDocument::StreamIn( std::istream * in, TIXML_STRING * tag )
+{
+ // The basic issue with a document is that we don't know what we're
+ // streaming. Read something presumed to be a tag (and hope), then
+ // identify it, and call the appropriate stream method on the tag.
+ //
+ // This "pre-streaming" will never read the closing ">" so the
+ // sub-tag can orient itself.
+
+ if ( !StreamTo( in, '<', tag ) )
+ {
+ SetError( TIXML_ERROR_PARSING_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+
+ while ( in->good() )
+ {
+ int tagIndex = (int) tag->length();
+ while ( in->good() && in->peek() != '>' )
+ {
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ break;
+ }
+ (*tag) += (char) c;
+ }
+
+ if ( in->good() )
+ {
+ // We now have something we presume to be a node of
+ // some sort. Identify it, and call the node to
+ // continue streaming.
+ TiXmlNode* node = Identify( tag->c_str() + tagIndex, TIXML_DEFAULT_ENCODING );
+
+ if ( node )
+ {
+ node->StreamIn( in, tag );
+ bool isElement = node->ToElement() != 0;
+ delete node;
+ node = 0;
+
+ // If this is the root element, we're done. Parsing will be
+ // done by the >> operator.
+ if ( isElement )
+ {
+ return;
+ }
+ }
+ else
+ {
+ SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+ }
+ }
+ // We should have returned sooner.
+ SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN );
+}
+
+#endif
+
+const char* TiXmlDocument::Parse( const char* p, TiXmlParsingData* prevData, TiXmlEncoding encoding )
+{
+ ClearError();
+
+ // Parse away, at the document level. Since a document
+ // contains nothing but other tags, most of what happens
+ // here is skipping white space.
+ if ( !p || !*p )
+ {
+ SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ // Note that, for a document, this needs to come
+ // before the while space skip, so that parsing
+ // starts from the pointer we are given.
+ location.Clear();
+ if ( prevData )
+ {
+ location.row = prevData->cursor.row;
+ location.col = prevData->cursor.col;
+ }
+ else
+ {
+ location.row = 0;
+ location.col = 0;
+ }
+ TiXmlParsingData data( p, TabSize(), location.row, location.col );
+ location = data.Cursor();
+
+ if ( encoding == TIXML_ENCODING_UNKNOWN )
+ {
+ // Check for the Microsoft UTF-8 lead bytes.
+ const unsigned char* pU = (const unsigned char*)p;
+ if ( *(pU+0) && *(pU+0) == TIXML_UTF_LEAD_0
+ && *(pU+1) && *(pU+1) == TIXML_UTF_LEAD_1
+ && *(pU+2) && *(pU+2) == TIXML_UTF_LEAD_2 )
+ {
+ encoding = TIXML_ENCODING_UTF8;
+ useMicrosoftBOM = true;
+ }
+ }
+
+ p = SkipWhiteSpace( p, encoding );
+ if ( !p )
+ {
+ SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return 0;
+ }
+
+ while ( p && *p )
+ {
+ TiXmlNode* node = Identify( p, encoding );
+ if ( node )
+ {
+ p = node->Parse( p, &data, encoding );
+ LinkEndChild( node );
+ }
+ else
+ {
+ break;
+ }
+
+ // Did we get encoding info?
+ if ( encoding == TIXML_ENCODING_UNKNOWN
+ && node->ToDeclaration() )
+ {
+ TiXmlDeclaration* dec = node->ToDeclaration();
+ const char* enc = dec->Encoding();
+ assert( enc );
+
+ if ( *enc == 0 )
+ encoding = TIXML_ENCODING_UTF8;
+ else if ( StringEqual( enc, "UTF-8", true, TIXML_ENCODING_UNKNOWN ) )
+ encoding = TIXML_ENCODING_UTF8;
+ else if ( StringEqual( enc, "UTF8", true, TIXML_ENCODING_UNKNOWN ) )
+ encoding = TIXML_ENCODING_UTF8; // incorrect, but be nice
+ else
+ encoding = TIXML_ENCODING_LEGACY;
+ }
+
+ p = SkipWhiteSpace( p, encoding );
+ }
+
+ // Was this empty?
+ if ( !firstChild ) {
+ SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, encoding );
+ return 0;
+ }
+
+ // All is well.
+ return p;
+}
+
+void TiXmlDocument::SetError( int err, const char* pError, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ // The first error in a chain is more accurate - don't set again!
+ if ( error )
+ return;
+
+ assert( err > 0 && err < TIXML_ERROR_STRING_COUNT );
+ error = true;
+ errorId = err;
+ errorDesc = errorString[ errorId ];
+
+ errorLocation.Clear();
+ if ( pError && data )
+ {
+ data->Stamp( pError, encoding );
+ errorLocation = data->Cursor();
+ }
+}
+
+
+TiXmlNode* TiXmlNode::Identify( const char* p, TiXmlEncoding encoding )
+{
+ TiXmlNode* returnNode = 0;
+
+ p = SkipWhiteSpace( p, encoding );
+ if( !p || !*p || *p != '<' )
+ {
+ return 0;
+ }
+
+ p = SkipWhiteSpace( p, encoding );
+
+ if ( !p || !*p )
+ {
+ return 0;
+ }
+
+ // What is this thing?
+ // - Elements start with a letter or underscore, but xml is reserved.
+ // - Comments: <!--
+ // - Decleration: <?xml
+ // - Everthing else is unknown to tinyxml.
+ //
+
+ const char* xmlHeader = { "<?xml" };
+ const char* commentHeader = { "<!--" };
+ const char* dtdHeader = { "<!" };
+ const char* cdataHeader = { "<![CDATA[" };
+
+ if ( StringEqual( p, xmlHeader, true, encoding ) )
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing Declaration\n" );
+ #endif
+ returnNode = new TiXmlDeclaration();
+ }
+ else if ( StringEqual( p, commentHeader, false, encoding ) )
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing Comment\n" );
+ #endif
+ returnNode = new TiXmlComment();
+ }
+ else if ( StringEqual( p, cdataHeader, false, encoding ) )
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing CDATA\n" );
+ #endif
+ TiXmlText* text = new TiXmlText( "" );
+ text->SetCDATA( true );
+ returnNode = text;
+ }
+ else if ( StringEqual( p, dtdHeader, false, encoding ) )
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing Unknown(1)\n" );
+ #endif
+ returnNode = new TiXmlUnknown();
+ }
+ else if ( IsAlpha( *(p+1), encoding )
+ || *(p+1) == '_' )
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing Element\n" );
+ #endif
+ returnNode = new TiXmlElement( "" );
+ }
+ else
+ {
+ #ifdef DEBUG_PARSER
+ TIXML_LOG( "XML parsing Unknown(2)\n" );
+ #endif
+ returnNode = new TiXmlUnknown();
+ }
+
+ if ( returnNode )
+ {
+ // Set the parent, so it can report errors
+ returnNode->parent = this;
+ }
+ return returnNode;
+}
+
+#ifdef TIXML_USE_STL
+
+void TiXmlElement::StreamIn (std::istream * in, TIXML_STRING * tag)
+{
+ // We're called with some amount of pre-parsing. That is, some of "this"
+ // element is in "tag". Go ahead and stream to the closing ">"
+ while( in->good() )
+ {
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+ (*tag) += (char) c ;
+
+ if ( c == '>' )
+ break;
+ }
+
+ if ( tag->length() < 3 ) return;
+
+ // Okay...if we are a "/>" tag, then we're done. We've read a complete tag.
+ // If not, identify and stream.
+
+ if ( tag->at( tag->length() - 1 ) == '>'
+ && tag->at( tag->length() - 2 ) == '/' )
+ {
+ // All good!
+ return;
+ }
+ else if ( tag->at( tag->length() - 1 ) == '>' )
+ {
+ // There is more. Could be:
+ // text
+ // cdata text (which looks like another node)
+ // closing tag
+ // another node.
+ for ( ;; )
+ {
+ StreamWhiteSpace( in, tag );
+
+ // Do we have text?
+ if ( in->good() && in->peek() != '<' )
+ {
+ // Yep, text.
+ TiXmlText text( "" );
+ text.StreamIn( in, tag );
+
+ // What follows text is a closing tag or another node.
+ // Go around again and figure it out.
+ continue;
+ }
+
+ // We now have either a closing tag...or another node.
+ // We should be at a "<", regardless.
+ if ( !in->good() ) return;
+ assert( in->peek() == '<' );
+ int tagIndex = (int) tag->length();
+
+ bool closingTag = false;
+ bool firstCharFound = false;
+
+ for( ;; )
+ {
+ if ( !in->good() )
+ return;
+
+ int c = in->peek();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+
+ if ( c == '>' )
+ break;
+
+ *tag += (char) c;
+ in->get();
+
+ // Early out if we find the CDATA id.
+ if ( c == '[' && tag->size() >= 9 )
+ {
+ size_t len = tag->size();
+ const char* start = tag->c_str() + len - 9;
+ if ( strcmp( start, "<![CDATA[" ) == 0 ) {
+ assert( !closingTag );
+ break;
+ }
+ }
+
+ if ( !firstCharFound && c != '<' && !IsWhiteSpace( c ) )
+ {
+ firstCharFound = true;
+ if ( c == '/' )
+ closingTag = true;
+ }
+ }
+ // If it was a closing tag, then read in the closing '>' to clean up the input stream.
+ // If it was not, the streaming will be done by the tag.
+ if ( closingTag )
+ {
+ if ( !in->good() )
+ return;
+
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+ assert( c == '>' );
+ *tag += (char) c;
+
+ // We are done, once we've found our closing tag.
+ return;
+ }
+ else
+ {
+ // If not a closing tag, id it, and stream.
+ const char* tagloc = tag->c_str() + tagIndex;
+ TiXmlNode* node = Identify( tagloc, TIXML_DEFAULT_ENCODING );
+ if ( !node )
+ return;
+ node->StreamIn( in, tag );
+ delete node;
+ node = 0;
+
+ // No return: go around from the beginning: text, closing tag, or node.
+ }
+ }
+ }
+}
+#endif
+
+const char* TiXmlElement::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ p = SkipWhiteSpace( p, encoding );
+ TiXmlDocument* document = GetDocument();
+
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_ELEMENT, 0, 0, encoding );
+ return 0;
+ }
+
+ if ( data )
+ {
+ data->Stamp( p, encoding );
+ location = data->Cursor();
+ }
+
+ if ( *p != '<' )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_ELEMENT, p, data, encoding );
+ return 0;
+ }
+
+ p = SkipWhiteSpace( p+1, encoding );
+
+ // Read the name.
+ const char* pErr = p;
+
+ p = ReadName( p, &value, encoding );
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_FAILED_TO_READ_ELEMENT_NAME, pErr, data, encoding );
+ return 0;
+ }
+
+ TIXML_STRING endTag ("</");
+ endTag += value;
+
+ // Check for and read attributes. Also look for an empty
+ // tag or an end tag.
+ while ( p && *p )
+ {
+ pErr = p;
+ p = SkipWhiteSpace( p, encoding );
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, pErr, data, encoding );
+ return 0;
+ }
+ if ( *p == '/' )
+ {
+ ++p;
+ // Empty tag.
+ if ( *p != '>' )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_EMPTY, p, data, encoding );
+ return 0;
+ }
+ return (p+1);
+ }
+ else if ( *p == '>' )
+ {
+ // Done with attributes (if there were any.)
+ // Read the value -- which can include other
+ // elements -- read the end tag, and return.
+ ++p;
+ p = ReadValue( p, data, encoding ); // Note this is an Element method, and will set the error if one happens.
+ if ( !p || !*p ) {
+ // We were looking for the end tag, but found nothing.
+ // Fix for [ 1663758 ] Failure to report error on bad XML
+ if ( document ) document->SetError( TIXML_ERROR_READING_END_TAG, p, data, encoding );
+ return 0;
+ }
+
+ // We should find the end tag now
+ // note that:
+ // </foo > and
+ // </foo>
+ // are both valid end tags.
+ if ( StringEqual( p, endTag.c_str(), false, encoding ) )
+ {
+ p += endTag.length();
+ p = SkipWhiteSpace( p, encoding );
+ if ( p && *p && *p == '>' ) {
+ ++p;
+ return p;
+ }
+ if ( document ) document->SetError( TIXML_ERROR_READING_END_TAG, p, data, encoding );
+ return 0;
+ }
+ else
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_END_TAG, p, data, encoding );
+ return 0;
+ }
+ }
+ else
+ {
+ // Try to read an attribute:
+ TiXmlAttribute* attrib = new TiXmlAttribute();
+ if ( !attrib )
+ {
+ return 0;
+ }
+
+ attrib->SetDocument( document );
+ pErr = p;
+ p = attrib->Parse( p, data, encoding );
+
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_ELEMENT, pErr, data, encoding );
+ delete attrib;
+ return 0;
+ }
+
+ // Handle the strange case of double attributes:
+ #ifdef TIXML_USE_STL
+ TiXmlAttribute* node = attributeSet.Find( attrib->NameTStr() );
+ #else
+ TiXmlAttribute* node = attributeSet.Find( attrib->Name() );
+ #endif
+ if ( node )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_ELEMENT, pErr, data, encoding );
+ delete attrib;
+ return 0;
+ }
+
+ attributeSet.Add( attrib );
+ }
+ }
+ return p;
+}
+
+
+const char* TiXmlElement::ReadValue( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ TiXmlDocument* document = GetDocument();
+
+ // Read in text and elements in any order.
+ const char* pWithWhiteSpace = p;
+ p = SkipWhiteSpace( p, encoding );
+
+ while ( p && *p )
+ {
+ if ( *p != '<' )
+ {
+ // Take what we have, make a text element.
+ TiXmlText* textNode = new TiXmlText( "" );
+
+ if ( !textNode )
+ {
+ return 0;
+ }
+
+ if ( TiXmlBase::IsWhiteSpaceCondensed() )
+ {
+ p = textNode->Parse( p, data, encoding );
+ }
+ else
+ {
+ // Special case: we want to keep the white space
+ // so that leading spaces aren't removed.
+ p = textNode->Parse( pWithWhiteSpace, data, encoding );
+ }
+
+ if ( !textNode->Blank() )
+ LinkEndChild( textNode );
+ else
+ delete textNode;
+ }
+ else
+ {
+ // We hit a '<'
+ // Have we hit a new element or an end tag? This could also be
+ // a TiXmlText in the "CDATA" style.
+ if ( StringEqual( p, "</", false, encoding ) )
+ {
+ return p;
+ }
+ else
+ {
+ TiXmlNode* node = Identify( p, encoding );
+ if ( node )
+ {
+ p = node->Parse( p, data, encoding );
+ LinkEndChild( node );
+ }
+ else
+ {
+ return 0;
+ }
+ }
+ }
+ pWithWhiteSpace = p;
+ p = SkipWhiteSpace( p, encoding );
+ }
+
+ if ( !p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_ELEMENT_VALUE, 0, 0, encoding );
+ }
+ return p;
+}
+
+
+#ifdef TIXML_USE_STL
+void TiXmlUnknown::StreamIn( std::istream * in, TIXML_STRING * tag )
+{
+ while ( in->good() )
+ {
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+ (*tag) += (char) c;
+
+ if ( c == '>' )
+ {
+ // All is well.
+ return;
+ }
+ }
+}
+#endif
+
+
+const char* TiXmlUnknown::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ TiXmlDocument* document = GetDocument();
+ p = SkipWhiteSpace( p, encoding );
+
+ if ( data )
+ {
+ data->Stamp( p, encoding );
+ location = data->Cursor();
+ }
+ if ( !p || !*p || *p != '<' )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_UNKNOWN, p, data, encoding );
+ return 0;
+ }
+ ++p;
+ value = "";
+
+ while ( p && *p && *p != '>' )
+ {
+ value += *p;
+ ++p;
+ }
+
+ if ( !p )
+ {
+ if ( document )
+ document->SetError( TIXML_ERROR_PARSING_UNKNOWN, 0, 0, encoding );
+ }
+ if ( p && *p == '>' )
+ return p+1;
+ return p;
+}
+
+#ifdef TIXML_USE_STL
+void TiXmlComment::StreamIn( std::istream * in, TIXML_STRING * tag )
+{
+ while ( in->good() )
+ {
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+
+ (*tag) += (char) c;
+
+ if ( c == '>'
+ && tag->at( tag->length() - 2 ) == '-'
+ && tag->at( tag->length() - 3 ) == '-' )
+ {
+ // All is well.
+ return;
+ }
+ }
+}
+#endif
+
+
+const char* TiXmlComment::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ TiXmlDocument* document = GetDocument();
+ value = "";
+
+ p = SkipWhiteSpace( p, encoding );
+
+ if ( data )
+ {
+ data->Stamp( p, encoding );
+ location = data->Cursor();
+ }
+ const char* startTag = "<!--";
+ const char* endTag = "-->";
+
+ if ( !StringEqual( p, startTag, false, encoding ) )
+ {
+ if ( document )
+ document->SetError( TIXML_ERROR_PARSING_COMMENT, p, data, encoding );
+ return 0;
+ }
+ p += strlen( startTag );
+
+ // [ 1475201 ] TinyXML parses entities in comments
+ // Oops - ReadText doesn't work, because we don't want to parse the entities.
+ // p = ReadText( p, &value, false, endTag, false, encoding );
+ //
+ // from the XML spec:
+ /*
+ [Definition: Comments may appear anywhere in a document outside other markup; in addition,
+ they may appear within the document type declaration at places allowed by the grammar.
+ They are not part of the document's character data; an XML processor MAY, but need not,
+ make it possible for an application to retrieve the text of comments. For compatibility,
+ the string "--" (double-hyphen) MUST NOT occur within comments.] Parameter entity
+ references MUST NOT be recognized within comments.
+
+ An example of a comment:
+
+ <!-- declarations for <head> & <body> -->
+ */
+
+ value = "";
+ // Keep all the white space.
+ while ( p && *p && !StringEqual( p, endTag, false, encoding ) )
+ {
+ value.append( p, 1 );
+ ++p;
+ }
+ if ( p && *p )
+ p += strlen( endTag );
+
+ return p;
+}
+
+
+const char* TiXmlAttribute::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ p = SkipWhiteSpace( p, encoding );
+ if ( !p || !*p ) return 0;
+
+ if ( data )
+ {
+ data->Stamp( p, encoding );
+ location = data->Cursor();
+ }
+ // Read the name, the '=' and the value.
+ const char* pErr = p;
+ p = ReadName( p, &name, encoding );
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, pErr, data, encoding );
+ return 0;
+ }
+ p = SkipWhiteSpace( p, encoding );
+ if ( !p || !*p || *p != '=' )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding );
+ return 0;
+ }
+
+ ++p; // skip '='
+ p = SkipWhiteSpace( p, encoding );
+ if ( !p || !*p )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding );
+ return 0;
+ }
+
+ const char* end;
+ const char SINGLE_QUOTE = '\'';
+ const char DOUBLE_QUOTE = '\"';
+
+ if ( *p == SINGLE_QUOTE )
+ {
+ ++p;
+ end = "\'"; // single quote in string
+ p = ReadText( p, &value, false, end, false, encoding );
+ }
+ else if ( *p == DOUBLE_QUOTE )
+ {
+ ++p;
+ end = "\""; // double quote in string
+ p = ReadText( p, &value, false, end, false, encoding );
+ }
+ else
+ {
+ // All attribute values should be in single or double quotes.
+ // But this is such a common error that the parser will try
+ // its best, even without them.
+ value = "";
+ while ( p && *p // existence
+ && !IsWhiteSpace( *p ) // whitespace
+ && *p != '/' && *p != '>' ) // tag end
+ {
+ if ( *p == SINGLE_QUOTE || *p == DOUBLE_QUOTE ) {
+ // [ 1451649 ] Attribute values with trailing quotes not handled correctly
+ // We did not have an opening quote but seem to have a
+ // closing one. Give up and throw an error.
+ if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding );
+ return 0;
+ }
+ value += *p;
+ ++p;
+ }
+ }
+ return p;
+}
+
+#ifdef TIXML_USE_STL
+void TiXmlText::StreamIn( std::istream * in, TIXML_STRING * tag )
+{
+ while ( in->good() )
+ {
+ int c = in->peek();
+ if ( !cdata && (c == '<' ) )
+ {
+ return;
+ }
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+
+ (*tag) += (char) c;
+ in->get(); // "commits" the peek made above
+
+ if ( cdata && c == '>' && tag->size() >= 3 ) {
+ size_t len = tag->size();
+ if ( (*tag)[len-2] == ']' && (*tag)[len-3] == ']' ) {
+ // terminator of cdata.
+ return;
+ }
+ }
+ }
+}
+#endif
+
+const char* TiXmlText::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding )
+{
+ value = "";
+ TiXmlDocument* document = GetDocument();
+
+ if ( data )
+ {
+ data->Stamp( p, encoding );
+ location = data->Cursor();
+ }
+
+ const char* const startTag = "<![CDATA[";
+ const char* const endTag = "]]>";
+
+ if ( cdata || StringEqual( p, startTag, false, encoding ) )
+ {
+ cdata = true;
+
+ if ( !StringEqual( p, startTag, false, encoding ) )
+ {
+ if ( document )
+ document->SetError( TIXML_ERROR_PARSING_CDATA, p, data, encoding );
+ return 0;
+ }
+ p += strlen( startTag );
+
+ // Keep all the white space, ignore the encoding, etc.
+ while ( p && *p
+ && !StringEqual( p, endTag, false, encoding )
+ )
+ {
+ value += *p;
+ ++p;
+ }
+
+ TIXML_STRING dummy;
+ p = ReadText( p, &dummy, false, endTag, false, encoding );
+ return p;
+ }
+ else
+ {
+ bool ignoreWhite = true;
+
+ const char* end = "<";
+ p = ReadText( p, &value, ignoreWhite, end, false, encoding );
+ if ( p && *p )
+ return p-1; // don't truncate the '<'
+ return 0;
+ }
+}
+
+#ifdef TIXML_USE_STL
+void TiXmlDeclaration::StreamIn( std::istream * in, TIXML_STRING * tag )
+{
+ while ( in->good() )
+ {
+ int c = in->get();
+ if ( c <= 0 )
+ {
+ TiXmlDocument* document = GetDocument();
+ if ( document )
+ document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN );
+ return;
+ }
+ (*tag) += (char) c;
+
+ if ( c == '>' )
+ {
+ // All is well.
+ return;
+ }
+ }
+}
+#endif
+
+const char* TiXmlDeclaration::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding _encoding )
+{
+ p = SkipWhiteSpace( p, _encoding );
+ // Find the beginning, find the end, and look for
+ // the stuff in-between.
+ TiXmlDocument* document = GetDocument();
+ if ( !p || !*p || !StringEqual( p, "<?xml", true, _encoding ) )
+ {
+ if ( document ) document->SetError( TIXML_ERROR_PARSING_DECLARATION, 0, 0, _encoding );
+ return 0;
+ }
+ if ( data )
+ {
+ data->Stamp( p, _encoding );
+ location = data->Cursor();
+ }
+ p += 5;
+
+ version = "";
+ encoding = "";
+ standalone = "";
+
+ while ( p && *p )
+ {
+ if ( *p == '>' )
+ {
+ ++p;
+ return p;
+ }
+
+ p = SkipWhiteSpace( p, _encoding );
+ if ( StringEqual( p, "version", true, _encoding ) )
+ {
+ TiXmlAttribute attrib;
+ p = attrib.Parse( p, data, _encoding );
+ version = attrib.Value();
+ }
+ else if ( StringEqual( p, "encoding", true, _encoding ) )
+ {
+ TiXmlAttribute attrib;
+ p = attrib.Parse( p, data, _encoding );
+ encoding = attrib.Value();
+ }
+ else if ( StringEqual( p, "standalone", true, _encoding ) )
+ {
+ TiXmlAttribute attrib;
+ p = attrib.Parse( p, data, _encoding );
+ standalone = attrib.Value();
+ }
+ else
+ {
+ // Read over whatever it is.
+ while( p && *p && *p != '>' && !IsWhiteSpace( *p ) )
+ ++p;
+ }
+ }
+ return 0;
+}
+
+bool TiXmlText::Blank() const
+{
+ for ( unsigned i=0; i<value.length(); i++ )
+ if ( !IsWhiteSpace( value[i] ) )
+ return false;
+ return true;
+}
+
diff --git a/doc/gazebo.in b/doc/gazebo.in
index da8f056..9d29700 100644
--- a/doc/gazebo.in
+++ b/doc/gazebo.in
@@ -662,7 +662,8 @@ WARN_LOGFILE = /tmp/gazebo_dox.warn
# with spaces.
INPUT = @CMAKE_SOURCE_DIR@/doc/mainpage.html \
- @CMAKE_SOURCE_DIR@/gazebo
+ @CMAKE_SOURCE_DIR@/gazebo \
+ @CMAKE_SOURCE_DIR@/plugins
# This tag can be used to specify the character encoding of the source files
# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is
@@ -695,7 +696,7 @@ RECURSIVE = YES
# Note that relative paths are relative to the directory from which doxygen is
# run.
-EXCLUDE = @CMAKE_SOURCE_DIR@/gazebo/rendering/ogretest
+EXCLUDE = @CMAKE_SOURCE_DIR@/gazebo/rendering/ogretest \
@CMAKE_SOURCE_DIR@/gazebo/rendering/skyx \
@CMAKE_SOURCE_DIR@/gazebo/rendering/selection_buffer \
@CMAKE_SOURCE_DIR@/gazebo/rendering/deferred_shading
@@ -1219,7 +1220,7 @@ MATHJAX_EXTENSIONS =
# typically be disabled. For large projects the javascript based search engine
# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution.
-SEARCHENGINE = YES
+SEARCHENGINE = NO
# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
# implemented using a PHP enabled web server instead of at the web client
@@ -1663,7 +1664,7 @@ CLASS_GRAPH = YES
# indirect implementation dependencies (inheritance, containment, and
# class references variables) of the class with other documented classes.
-COLLABORATION_GRAPH = YES
+COLLABORATION_GRAPH = NO
# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen
# will generate a graph for groups, showing the direct groups dependencies
diff --git a/doc/header.html b/doc/header.html
index 53a706e..44e9cdb 100644
--- a/doc/header.html
+++ b/doc/header.html
@@ -55,6 +55,7 @@
</dl>
</div>
<div>
+ <!--
<div id="MSearchBox" class="MSearchBoxInactive">
<span>
<img id="MSearchSelect" src="search/mag_sel.png"
@@ -66,7 +67,7 @@
onblur="searchBox.OnSearchFieldFocus(false)"
onkeyup="searchBox.OnSearchFieldChange(event)"/>
</span>
- </div> <!-- End MSearchBox -->
+ </div> End MSearchBox -->
<div id="MSearchResultsWindow" style="position: static; display: block; border: none; background-color: #ffffff; width: 18em;">
<iframe src="javascript:void(0)" frameborder="0"
diff --git a/doc/mainpage.html b/doc/mainpage.html
index 682f3bd..ed9bdab 100644
--- a/doc/mainpage.html
+++ b/doc/mainpage.html
@@ -1,16 +1,16 @@
/** \mainpage Gazebo API Reference
-This documentation provides useful information about the Gazebo API. The code
+This documentation provides useful information about the Gazebo API. The code
reference is divided into the groups below. Should you find problems with this
- documentation - typos, unclear phrases, or insufficient detail - please
-create a <a href="https://bitbucket.org/osrf/gazebo/issues/new">new bitbucket
-issue</a>. Include sufficient detail to quickly locate the problematic
-documentation, and set the issue's fields accordingly: Assignee - blank;
-Kind - bug; Priority - minor; Version - blank.
+ documentation - typos, unclear phrases, or insufficient detail - please
+create a <a href="https://bitbucket.org/osrf/gazebo/issues/new">new bitbucket
+issue</a>. Include sufficient detail to quickly locate the problematic
+documentation, and set the issue's fields accordingly: Assignee - blank;
+Kind - bug; Priority - minor; Component - documentation; Version - blank.
<dl>
- <dt>Class</dt>
+ <dt>Classes</dt>
<dd><a href="classes.html">List</a> - Index of all classes in Gazebo, organized alphabetically</dd>
<dd><a href="hierarchy.html">Hierarchy</a> - Index of classes, organized hierachically according to their inheritance</dd>
@@ -20,17 +20,20 @@ Kind - bug; Priority - minor; Version - blank.
<dl>
<dt><a href="modules.html" style="font-weight:bold">Modules</a></dt>
+
<dd><a href="group__gazebo__common.html">Common</a>: Classes and files used ubiquitously across Gazebo</dd>
<dd><a href="group__gazebo__event.html">Events</a>: For creating and destroying Gazebo events</dd>
+ <dd><a href="group__gazebo__gui.html">GUI</a>: A set of GUI related classes, functions, and definitions.</dd>
+
<dd><a href="group__gazebo__math.html">Math</a>: A set of classes that encapsulate math related properties and functions.</dd>
<dd><a href="group__gazebo__msgs.html">Messages</a>: All messages and helper functions.</dd>
<dd><a href="group__gazebo__physics.html">Physics</a>: Classes for physics and dynamics</dd>
- <dd><a href="group__gazebo__rendering.html">Rendering</a>: A set of rendering related class, functions, and definitions.</dd>
+ <dd><a href="group__gazebo__rendering.html">Rendering</a>: A set of rendering related classes, functions, and definitions.</dd>
<dd><a href="group__gazebo__sensors.html">Sensors</a>: A set of sensor classes, functions, and definitions.</dd>
diff --git a/examples/ignition/CMakeLists.txt b/examples/ignition/CMakeLists.txt
new file mode 100644
index 0000000..c3fe056
--- /dev/null
+++ b/examples/ignition/CMakeLists.txt
@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+find_package(ignition-math2)
+find_package(Gazebo)
+
+set(CMAKE_CXX_FLAGS ${IGNITION-MATH_CXX_FLAGS})
+include_directories(${GAZEBO_INCLUDE_DIRS} ${IGNITION-MATH_INCLUDE_DIRS})
+link_directories(${GAZEBO_LIBRARY_DIRS} ${IGNITION-MATH_LIBRARY_DIRS})
+
+add_executable(vector3 vector3.cc)
+target_link_libraries(vector3 ${GAZEBO_LIBRARIES} ${IGNITION-MATH_LIBRARIES})
+
+add_executable(box box.cc)
+target_link_libraries(box ${GAZEBO_LIBRARIES} ${IGNITION-MATH_LIBRARIES})
+
+add_executable(angle angle.cc)
+target_link_libraries(angle ${GAZEBO_LIBRARIES} ${IGNITION-MATH_LIBRARIES})
diff --git a/examples/ignition/angle.cc b/examples/ignition/angle.cc
new file mode 100644
index 0000000..7c46d60
--- /dev/null
+++ b/examples/ignition/angle.cc
@@ -0,0 +1,45 @@
+/*
+ * 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/Angle.hh>
+#include <ignition/math/Angle.hh>
+
+/////////////////////////////////////////////////
+// Migration from gazebo::math::Angle to ignition::math::Angle examples
+// See Ignition documentation http://ignitionrobotics.org/libraries/math
+int main()
+{
+ // Construction
+ gazebo::math::Angle gzAngle(0.1);
+ ignition::math::Angle ignAngle(0.1);
+
+ // Get
+ std::cout << gzAngle.Radian() << std::endl;
+ std::cout << ignAngle.Radian() << std::endl;
+
+ // Output
+ std::cout << gzAngle << std::endl;
+ std::cout << ignAngle << std::endl;
+
+ // Convert from gazebo to ignition
+ ignAngle = gzAngle.Ign();
+
+ // Set gazebo from ignition
+ gzAngle = ignAngle;
+
+ return 0;
+}
diff --git a/examples/ignition/box.cc b/examples/ignition/box.cc
new file mode 100644
index 0000000..78e5423
--- /dev/null
+++ b/examples/ignition/box.cc
@@ -0,0 +1,46 @@
+/*
+ * 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/Box.hh>
+#include <ignition/math/Box.hh>
+
+/////////////////////////////////////////////////
+// Migration from gazebo::math::Box to ignition::math::Box examples
+// See Ignition documentation http://ignitionrobotics.org/libraries/math
+int main()
+{
+ // Construction
+ gazebo::math::Box gzBox(gazebo::math::Vector3(1, 2, 3),
+ gazebo::math::Vector3(4, 5, 6));
+ ignition::math::Box ignBox(1, 2, 3, 4, 5, 6);
+
+ // Get
+ std::cout << gzBox.min << std::endl;
+ std::cout << ignBox.Min() << std::endl;
+
+ // Output
+ std::cout << gzBox << std::endl;
+ std::cout << ignBox << std::endl;
+
+ // Convert from gazebo to ignition
+ ignBox = gzBox.Ign();
+
+ // Set gazebo from ignition
+ gzBox = ignBox;
+
+ return 0;
+}
diff --git a/examples/ignition/vector3.cc b/examples/ignition/vector3.cc
new file mode 100644
index 0000000..c695a68
--- /dev/null
+++ b/examples/ignition/vector3.cc
@@ -0,0 +1,50 @@
+/*
+ * 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/Vector3.hh>
+#include <ignition/math/Vector3.hh>
+
+/////////////////////////////////////////////////
+// Migration from gazebo::math::Vector3 to ignition::math::Vector3 examples
+// See Ignition documentation http://ignitionrobotics.org/libraries/math
+int main()
+{
+ // Construction
+ gazebo::math::Vector3 gzVec(1, 2, 3);
+ ignition::math::Vector3d ignVec(1, 2, 3);
+
+ // Set
+ gzVec.x = 2.5;
+ ignVec.X() = 3.5;
+ ignVec.X(4.5);
+
+ // Get
+ std::cout << gzVec.x << std::endl;
+ std::cout << ignVec.X() << std::endl;
+
+ // Output
+ std::cout << gzVec << std::endl;
+ std::cout << ignVec << std::endl;
+
+ // Convert from ignition to gazebo
+ gzVec = ignVec;
+
+ // Convert gazebo to igntion
+ ignVec = gzVec.Ign();
+
+ return 0;
+}
diff --git a/examples/plugins/CMakeLists.txt b/examples/plugins/CMakeLists.txt
index 04eef8c..e0f13a5 100644
--- a/examples/plugins/CMakeLists.txt
+++ b/examples/plugins/CMakeLists.txt
@@ -3,10 +3,12 @@ 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 b272a69..8fd7064 100644
--- a/examples/plugins/animate_joints/CMakeLists.txt
+++ b/examples/plugins/animate_joints/CMakeLists.txt
@@ -7,6 +7,7 @@ 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)
diff --git a/examples/plugins/animate_pose/CMakeLists.txt b/examples/plugins/animate_pose/CMakeLists.txt
index 8cacaaa..93168d1 100644
--- a/examples/plugins/animate_pose/CMakeLists.txt
+++ b/examples/plugins/animate_pose/CMakeLists.txt
@@ -1,11 +1,9 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
+find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
add_library(animate_pose SHARED animate_pose.cc)
target_link_libraries(animate_pose ${GAZEBO_LIBRARIES})
diff --git a/examples/plugins/animate_pose/animate_pose.cc b/examples/plugins/animate_pose/animate_pose.cc
index 46523b4..3dc5a84 100644
--- a/examples/plugins/animate_pose/animate_pose.cc
+++ b/examples/plugins/animate_pose/animate_pose.cc
@@ -14,6 +14,9 @@
* limitations under the License.
*
*/
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Quaternion.hh>
+
#include "gazebo/common/CommonTypes.hh"
#include "gazebo/common/Animation.hh"
#include "gazebo/common/KeyFrame.hh"
@@ -32,12 +35,12 @@ namespace gazebo
gazebo::common::PoseKeyFrame *key;
key = anim->CreateKeyFrame(0);
- key->SetTranslation(math::Vector3(0, 0, 0));
- key->SetRotation(math::Quaternion(0, 0, 0));
+ key->Translation(ignition::math::Vector3d(0, 0, 0));
+ key->Rotation(ignition::math::Quaterniond(0, 0, 0));
key = anim->CreateKeyFrame(1000.0);
- key->SetTranslation(math::Vector3(5, 0, 0));
- key->SetRotation(math::Quaternion(0, 0, 1.5707));
+ key->Translation(ignition::math::Vector3d(5, 0, 0));
+ key->Rotation(ignition::math::Quaterniond(0, 0, 1.5707));
_parent->SetAnimation(anim);
}
diff --git a/examples/plugins/camera/CMakeLists.txt b/examples/plugins/camera/CMakeLists.txt
index 4ae767d..984fae4 100644
--- a/examples/plugins/camera/CMakeLists.txt
+++ b/examples/plugins/camera/CMakeLists.txt
@@ -1,11 +1,9 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
+find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
add_library(camera_dump SHARED camera_dump.cc)
target_link_libraries(camera_dump ${GAZEBO_libraries} CameraPlugin)
diff --git a/examples/plugins/camera/camera_dump.cc b/examples/plugins/camera/camera_dump.cc
index 0221b99..0669477 100644
--- a/examples/plugins/camera/camera_dump.cc
+++ b/examples/plugins/camera/camera_dump.cc
@@ -15,7 +15,7 @@
*
*/
#include "gazebo/gazebo.hh"
-#include "plugins/CameraPlugin.hh"
+#include "gazebo/plugins/CameraPlugin.hh"
namespace gazebo
{
diff --git a/examples/plugins/custom_messages/CMakeLists.txt b/examples/plugins/custom_messages/CMakeLists.txt
index 0492ccc..fafac79 100644
--- a/examples/plugins/custom_messages/CMakeLists.txt
+++ b/examples/plugins/custom_messages/CMakeLists.txt
@@ -1,14 +1,13 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
+find_package(gazebo REQUIRED)
+
include_directories(
${GAZEBO_INCLUDE_DIRS}
${CMAKE_CURRENT_BINARY_DIR}/msgs
)
link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
add_subdirectory(msgs)
diff --git a/examples/plugins/factory/CMakeLists.txt b/examples/plugins/factory/CMakeLists.txt
index 9578742..7f60277 100644
--- a/examples/plugins/factory/CMakeLists.txt
+++ b/examples/plugins/factory/CMakeLists.txt
@@ -1,12 +1,9 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
find_package(GAZEBO)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
add_library(factory SHARED factory.cc)
target_link_libraries(factory
diff --git a/examples/plugins/factory/factory.cc b/examples/plugins/factory/factory.cc
index 23e7aec..fc2a42d 100644
--- a/examples/plugins/factory/factory.cc
+++ b/examples/plugins/factory/factory.cc
@@ -14,6 +14,7 @@
* limitations under the License.
*
*/
+#include <ignition/math/Pose3.hh>
#include "gazebo/physics/physics.hh"
#include "gazebo/common/common.hh"
#include "gazebo/gazebo.hh"
@@ -51,7 +52,7 @@ class Factory : public WorldPlugin
</model>\
</sdf>");
// Demonstrate using a custom model name.
- sdf::ElementPtr model = sphereSDF.root->GetElement("model");
+ sdf::ElementPtr model = sphereSDF.Root()->GetElement("model");
model->GetAttribute("name")->SetFromString("unique_sphere");
_parent->InsertModelSDF(sphereSDF);
@@ -75,7 +76,9 @@ class Factory : public WorldPlugin
// Pose to initialize the model to
msgs::Set(msg.mutable_pose(),
- math::Pose(math::Vector3(1, -2, 0), math::Quaternion(0, 0, 0)));
+ ignition::math::Pose3d(
+ ignition::math::Vector3d(1, -2, 0),
+ ignition::math::Quaterniond(0, 0, 0)));
// Send the message
factoryPub->Publish(msg);
diff --git a/examples/plugins/gui_overlay_plugin_spawn/CMakeLists.txt b/examples/plugins/gui_overlay_plugin_spawn/CMakeLists.txt
index 50d1a52..8ade35d 100644
--- a/examples/plugins/gui_overlay_plugin_spawn/CMakeLists.txt
+++ b/examples/plugins/gui_overlay_plugin_spawn/CMakeLists.txt
@@ -1,31 +1,21 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package (Qt4)
-
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
- pkg_check_modules(OGRE OGRE)
- pkg_check_modules(OGRE-Terrain OGRE-Terrain)
-endif()
-
find_package(Protobuf REQUIRED)
+find_package(gazebo REQUIRED)
include (${QT_USE_FILE})
add_definitions(${QT_DEFINITIONS})
-include_directories(SYSTEM ${GAZEBO_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS}
- ${OGRE-Terrain_INCLUDE_DIRS})
+include_directories(SYSTEM ${GAZEBO_INCLUDE_DIRS})
+link_directories(${GAZEBO_LIBRARY_DIRS})
-link_directories(${GAZEBO_LIBRARY_DIRS} ${OGRE_LIBRARY_DIRS}
- ${OGRE-Terrain_LIBRARY_DIRS})
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
QT4_WRAP_CPP(headers_MOC GUIExampleSpawnWidget.hh)
add_library(gui_example_spawn_widget SHARED ${headers_MOC}
GUIExampleSpawnWidget.cc)
target_link_libraries(gui_example_spawn_widget ${GAZEBO_LIBRARIES}
- ${OGRE_LIBRARIES}
- ${OGRE-Terrain_LIBRARIES}
${QT_LIBRARIES}
${PROTOBUF_LIBRARIES})
diff --git a/examples/plugins/gui_overlay_plugin_time/CMakeLists.txt b/examples/plugins/gui_overlay_plugin_time/CMakeLists.txt
index 2892732..4cdf62f 100644
--- a/examples/plugins/gui_overlay_plugin_time/CMakeLists.txt
+++ b/examples/plugins/gui_overlay_plugin_time/CMakeLists.txt
@@ -1,24 +1,15 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package (Qt4)
-
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
- pkg_check_modules(OGRE OGRE)
- pkg_check_modules(OGRE-Terrain OGRE-Terrain)
-endif()
-
find_package(Protobuf REQUIRED)
+find_package(gazebo REQUIRED)
include (${QT_USE_FILE})
add_definitions(${QT_DEFINITIONS})
-include_directories(SYSTEM ${GAZEBO_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS}
- ${OGRE-Terrain_INCLUDE_DIRS})
-
-link_directories(${GAZEBO_LIBRARY_DIRS} ${OGRE_LIBRARY_DIRS}
- ${OGRE-Terrain_LIBRARY_DIRS})
+include_directories(SYSTEM ${GAZEBO_INCLUDE_DIRS})
+link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
QT4_WRAP_CPP(headers_MOC GUIExampleTimeWidget.hh)
diff --git a/examples/plugins/hello_world/CMakeLists.txt b/examples/plugins/hello_world/CMakeLists.txt
index d7be4f1..9a30934 100644
--- a/examples/plugins/hello_world/CMakeLists.txt
+++ b/examples/plugins/hello_world/CMakeLists.txt
@@ -1,11 +1,9 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
+find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${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 9d40b86..c48088a 100644
--- a/examples/plugins/mainwindow_example/CMakeLists.txt
+++ b/examples/plugins/mainwindow_example/CMakeLists.txt
@@ -2,30 +2,19 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package (Qt4)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
- pkg_check_modules(OGRE OGRE)
- pkg_check_modules(OGRE-Terrain OGRE-Terrain)
-endif()
-
find_package(Protobuf REQUIRED)
include (${QT_USE_FILE})
add_definitions(${QT_DEFINITIONS})
-include_directories(SYSTEM ${GAZEBO_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS}
- ${OGRE-Terrain_INCLUDE_DIRS})
-
-link_directories(${GAZEBO_LIBRARY_DIRS} ${OGRE_LIBRARY_DIRS}
- ${OGRE-Terrain_LIBRARY_DIRS})
+include_directories(SYSTEM ${GAZEBO_INCLUDE_DIRS})
+link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
QT4_WRAP_CPP(headers_MOC MainWindowExampleWidget.hh)
add_library(main_window_example_widget SHARED ${headers_MOC}
MainWindowExampleWidget.cc)
target_link_libraries(main_window_example_widget ${GAZEBO_LIBRARIES}
- ${OGRE_LIBRARIES}
- ${OGRE-Terrain_LIBRARIES}
${QT_LIBRARIES}
${PROTOBUF_LIBRARIES})
diff --git a/examples/plugins/model_move/CMakeLists.txt b/examples/plugins/model_move/CMakeLists.txt
new file mode 100644
index 0000000..cfc825b
--- /dev/null
+++ b/examples/plugins/model_move/CMakeLists.txt
@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+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})
+
+# Create libraries and executables
+
+add_executable(path_publisher path_publisher.cc)
+target_link_libraries(path_publisher ${GAZEBO_LIBRARIES} ${PROTOBUF_LIBRARIES} ${Boost_LIBRARIES})
+
+add_library(model_move SHARED model_move.cc)
+target_link_libraries(model_move ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${SDF_LIBRARIES} ${PROTOBUF_LIBRARIES})
diff --git a/examples/plugins/model_move/README.md b/examples/plugins/model_move/README.md
new file mode 100644
index 0000000..4a25110
--- /dev/null
+++ b/examples/plugins/model_move/README.md
@@ -0,0 +1,80 @@
+# Model Movement Plugin
+
+This plugin example demonstates moving a model to a specified set of
+goal points.
+
+The movement is not kinematic aware as it uses Pose Animation
+described in this
+[tutorial](http://gazebosim.org/tutorials?tut=animated_box).
+
+## Build Instructions
+
+From this directory
+
+```
+$ mkdir build
+$ cd build
+$ cmake ../
+$ make
+```
+
+## Run Instructions
+
+There are several ways to interact with the animation. Below you can find
+how to run the standalone example, which defines the animation
+programatically. It is also possible to define the animation parameters in
+the SDF file.
+
+### Run the standalone publisher
+
+1. Go to the build directory and run gazebo with the example world
+
+ ```
+ $ cd build
+ ```
+
+ ```
+ $ gazebo ../model_move.world
+ ```
+
+1. Publish the path publisher
+
+ ```
+ open a new terminal
+ ```
+
+ ```
+ $ cd build
+ ```
+
+ ```
+ $ ./path_publisher
+ ```
+
+### Use predefined animation from the SDF file
+
+Go to the build directory and run gazebo with the example world
+
+```
+$ cd build
+$ gazebo ../model_move_with_movement.world
+```
+
+To change the input parameters, you can modify the world file sdf:
+
+```
+<goals> container of the list of poses to be reached by the model
+<pose> 6 floats (position, orientation) to be treated as goals
+```
+
+Example (in sdf file):
+
+```
+<plugin name="model_move" filename="libmodel_move.so">
+ <goals>
+ <pose>5 5 0 0 0 0</pose>
+ <pose>5 -5 0 0 0 0</pose>
+ <pose>0 0 0 0 0 0</pose>
+ </goals>
+</plugin>
+```
diff --git a/examples/plugins/model_move/model_move.cc b/examples/plugins/model_move/model_move.cc
new file mode 100644
index 0000000..96dbf03
--- /dev/null
+++ b/examples/plugins/model_move/model_move.cc
@@ -0,0 +1,178 @@
+/*
+ * 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/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/transport/transport.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Pose3.hh>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string>
+#include <sstream>
+
+#include "model_move.hh"
+
+using namespace gazebo;
+
+// Register this plugin with the simulator
+GZ_REGISTER_MODEL_PLUGIN(ModelMove);
+
+/////////////////////////////////////////////////
+ModelMove::ModelMove()
+{
+}
+
+/////////////////////////////////////////////////
+void ModelMove::Move(const math::Vector3 &_start, const math::Vector3 &_end,
+ math::Vector3 &_translation)
+{
+ int duration = floor(_start.Distance(_end.x, _end.y, _end.z));
+ math::Vector3 diff = _end - _start;
+ float xStep = diff.x / duration;
+ float yStep = diff.y / duration;
+ float zStep = diff.z / duration;
+ int currFrame = this->anim->GetKeyFrameCount();
+
+ for (int i = 1; i <= duration; ++i)
+ {
+ gazebo::common::PoseKeyFrame *key = this->anim->CreateKeyFrame(
+ i + currFrame);
+
+ key->Translation(ignition::math::Vector3d(
+ _translation.x + xStep * i,
+ _translation.y + yStep * i,
+ _translation.z + zStep * i));
+ key->Rotation(ignition::math::Quaterniond(0, 0, 0));
+ }
+
+ _translation.Set(_translation.x + xStep * duration,
+ _translation.y + yStep * duration,
+ _translation.z + zStep * duration);
+}
+
+/////////////////////////////////////////////////
+void ModelMove::InitiateMove()
+{
+ // get distance from starting point to the first of the goals
+ float pathLength = this->startPosition.Distance(this->pathGoals[0].pos);
+
+ // to calculate the full distance, add the distance between goals
+ for (unsigned int i = 0; i < this->pathGoals.size()-1; ++i)
+ pathLength += this->pathGoals[i].pos.Distance(this->pathGoals[i+1].pos);
+
+ // create the animation
+ this->anim = gazebo::common::PoseAnimationPtr(
+ new gazebo::common::PoseAnimation("test", pathLength + 1, false));
+
+ gazebo::common::PoseKeyFrame *key;
+
+ // set starting location of the box
+ key = this->anim->CreateKeyFrame(0);
+ key->Translation(ignition::math::Vector3d(0, 0, 0));
+ key->Rotation(ignition::math::Quaterniond(0, 0, 0));
+
+ math::Vector3 translation = math::Vector3(0, 0, 0);
+
+ // Move to the startPosition to first goal
+ this->Move(this->startPosition, this->pathGoals[0].pos, translation);
+
+ for (unsigned int i = 0; i < this->pathGoals.size()-1; ++i)
+ this->Move(this->pathGoals[i].pos, this->pathGoals[i+1].pos, translation);
+
+ // set the animation
+ this->model->SetAnimation(this->anim);
+}
+
+/////////////////////////////////////////////////
+void ModelMove::OnPathMsg(ConstPoseAnimationPtr &_msg)
+{
+ gzmsg << "[model_move] Received path message" << std::endl;
+
+ // Store message poses into the pathGoals and launch movement
+ for (unsigned int i = 0; i < _msg->pose_size(); ++i)
+ this->pathGoals.push_back(gazebo::msgs::ConvertIgn(_msg->pose(i)));
+
+ this->InitiateMove();
+}
+
+/////////////////////////////////////////////////
+bool ModelMove::LoadGoalsFromSDF(const sdf::ElementPtr _sdf)
+{
+ gzmsg << "[model_move] Processing path goals defined in the SDF file"
+ << std::endl;
+ GZ_ASSERT(_sdf, "_sdf element is null");
+
+ if (!_sdf->HasElement("pose"))
+ {
+ gzerr << "[model_move] SDF with goals tag but without pose/s element/s"
+ << std::endl;
+ return false;
+ }
+
+ sdf::ElementPtr poseElem = _sdf->GetElement("pose");
+
+ while (poseElem)
+ {
+ this->pathGoals.push_back(poseElem->Get<math::Pose>());
+ poseElem = poseElem->GetNextElement("pose");
+ }
+
+ GZ_ASSERT(this->pathGoals.size() > 0, "pathGoals should not be zero");
+
+ return true;
+}
+
+/////////////////////////////////////////////////
+void ModelMove::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
+{
+ // Store the pointer to the model
+ this->model = _parent;
+ this->node = transport::NodePtr(new transport::Node());
+ this->node->Init(this->model->GetWorld()->GetName());
+
+ // Get parameters from sdf, if provided
+ if (_sdf->HasElement("goals"))
+ {
+ if (this->LoadGoalsFromSDF(_sdf->GetElement("goals")))
+ {
+ // Ready to start the move. Store the initial pose of the model
+ // and call initiateMove
+ ignition::math::Vector3d sdfPose =
+ _sdf->GetParent()->GetElement("pose")
+ ->Get<ignition::math::Pose3d>().Pos();
+ this->startPosition =
+ math::Vector3(sdfPose.X(), sdfPose.Y(), sdfPose.Z());
+
+ this->InitiateMove();
+ }
+ else
+ {
+ gzerr << "[model_move] Problems on loading goals from sdf "
+ << "made the movement impossible" << std::endl;
+ }
+ }
+
+ // Create the subscriber
+ std::string pathTopicName = std::string("~/") + _parent->GetName()
+ + "/model_move";
+ this->pathSubscriber = node->Subscribe(
+ pathTopicName, &ModelMove::OnPathMsg, this);
+ gzmsg << "[model_move] Subscribed to receive paths in: "<< pathTopicName
+ << std::endl;
+}
diff --git a/examples/plugins/model_move/model_move.hh b/examples/plugins/model_move/model_move.hh
new file mode 100644
index 0000000..b46b935
--- /dev/null
+++ b/examples/plugins/model_move/model_move.hh
@@ -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.
+ *
+*/
+
+#ifndef _GAZEBO_MODEL_MOVE_HH_
+#define _GAZEBO_MODEL_MOVE_HH_
+
+#include <gazebo/msgs/pose_animation.pb.h>
+#include <vector>
+
+namespace gazebo
+{
+ /// \brief A plugin to transport a model from point to point using
+ /// pose animation.
+ class ModelMove : public ModelPlugin
+ {
+ /// \brief Constructor
+ public: ModelMove();
+
+ /// \brief Perform movement of the model
+ /// \param[in] _start starting point
+ /// \param[in] _end goal point
+ /// \param[in, out] _translation translation done before start
+ private: void Move(const math::Vector3 &_start, const math::Vector3 &_end,
+ math::Vector3 &_translation);
+
+ /// \brief Parse goals defined in the SDF
+ /// \param[in] _sdf sdf pointer corresponding to goals element
+ /// \return True if parsing was succesfull or not
+ private: bool LoadGoalsFromSDF(const sdf::ElementPtr _sdf);
+
+ /// \brief prepare the movement of the model.
+ /// Initialize the animation and related variables, call the
+ /// move method between each goal
+ public: void InitiateMove();
+
+ /// \brief Callback to run when recieve a path message.
+ /// \param[in] _msg path message received to animate.
+ public: void OnPathMsg(ConstPoseAnimationPtr &_msg);
+
+ /// \brief Plugin Load function
+ /// \param[in] _parent Model pointer to the model defining this plugin
+ /// \param[in] _sdf pointer to the SDF of the model
+ public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+
+ /// \brief Pointer to the model that defines this plugin
+ private: physics::ModelPtr model;
+
+ /// \brief pointer to the move animation to perform
+ private: gazebo::common::PoseAnimationPtr anim;
+
+ /// \brief Transport node used to communicate with the transport system
+ private: transport::NodePtr node;
+
+ /// \brief Subscriber to get path messages
+ private: transport::SubscriberPtr pathSubscriber;
+
+ /// \brief Starting point of the path to follow
+ private: math::Vector3 startPosition;
+
+ /// \brief Path to follow
+ private: std::vector<math::Pose> pathGoals;
+ };
+}
+#endif
diff --git a/examples/plugins/model_move/model_move.world b/examples/plugins/model_move/model_move.world
new file mode 100644
index 0000000..5833a6b
--- /dev/null
+++ b/examples/plugins/model_move/model_move.world
@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<sdf version="1.4">
+ <world name="modelmove_world">
+
+ <!-- Ground Plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <model name="box">
+ <pose>0 0 0 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <plugin name="model_move" filename="libmodel_move.so"/>
+ </model>
+ </world>
+</sdf>
diff --git a/examples/plugins/model_move/model_move_with_movement.world b/examples/plugins/model_move/model_move_with_movement.world
new file mode 100644
index 0000000..cbaa5ee
--- /dev/null
+++ b/examples/plugins/model_move/model_move_with_movement.world
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<sdf version="1.4">
+ <world name="modelmove_world">
+
+ <!-- Ground Plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <model name="box">
+ <pose>0 0 0 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <plugin name="model_move" filename="libmodel_move.so">
+ <goals>
+ <pose>5 5 0 0 0 0</pose>
+ <pose>5 -5 0 0 0 0</pose>
+ <pose>0 0 0 0 0 0</pose>
+ </goals>
+ </plugin>
+
+ </model>
+ </world>
+</sdf>
diff --git a/examples/plugins/model_move/path_publisher.cc b/examples/plugins/model_move/path_publisher.cc
new file mode 100644
index 0000000..dea3086
--- /dev/null
+++ b/examples/plugins/model_move/path_publisher.cc
@@ -0,0 +1,61 @@
+/*
+ * 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 <iostream>
+#include <math.h>
+#include <ignition/math/Pose3.hh>
+
+#include "gazebo/gazebo.hh"
+#include "gazebo/common/common.hh"
+#include "gazebo/math/Vector3.hh"
+#include "gazebo/transport/transport.hh"
+#include "gazebo/physics/physics.hh"
+#include "gazebo/msgs/msgs.hh"
+
+using namespace gazebo;
+
+int main(int _argc, char *_argv[])
+{
+ msgs::PoseAnimation msg;
+
+ msg.set_model_name("box");
+ msgs::Pose *p = msg.add_pose();
+ msgs::Set(p, ignition::math::Pose3d(5, 5, 0, 0, 0, 0));
+ p = msg.add_pose();
+ msgs::Set(p, ignition::math::Pose3d(5, -5, 0, 0, 0, 0));
+ p = msg.add_pose();
+ msgs::Set(p, ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
+
+ transport::init();
+ transport::run();
+ transport::NodePtr node(new gazebo::transport::Node());
+ node->Init("default");
+
+ // modelmove_world is the name of the testing world
+ const std::string topicName = "/gazebo/modelmove_world/" + msg.model_name()
+ + "/model_move";
+ gazebo::transport::PublisherPtr pathPub =
+ node->Advertise<msgs::PoseAnimation>(topicName);
+
+ std::cout << "Waiting for connection in " << topicName << std::endl;
+ pathPub->WaitForConnection();
+ pathPub->Publish(msg);
+
+ std::cout << "Path published!" << std::endl;
+
+ gazebo::transport::fini();
+ return 0;
+}
diff --git a/examples/plugins/model_push/CMakeLists.txt b/examples/plugins/model_push/CMakeLists.txt
index c211177..1f141ac 100644
--- a/examples/plugins/model_push/CMakeLists.txt
+++ b/examples/plugins/model_push/CMakeLists.txt
@@ -1,11 +1,9 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
+find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${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_visuals/CMakeLists.txt b/examples/plugins/model_visuals/CMakeLists.txt
index 3987994..c2d998c 100644
--- a/examples/plugins/model_visuals/CMakeLists.txt
+++ b/examples/plugins/model_visuals/CMakeLists.txt
@@ -1,11 +1,9 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
+find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
add_library(model_visuals SHARED model_visuals.cc)
target_link_libraries(model_visuals ${GAZEBO_libraries})
diff --git a/examples/plugins/parameters/CMakeLists.txt b/examples/plugins/parameters/CMakeLists.txt
index 787b225..7a98997 100644
--- a/examples/plugins/parameters/CMakeLists.txt
+++ b/examples/plugins/parameters/CMakeLists.txt
@@ -7,6 +7,7 @@ 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}")
add_library(parameters SHARED parameters.cc)
target_link_libraries(parameters ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
diff --git a/examples/plugins/presentation.cc b/examples/plugins/presentation.cc
index eaabd0b..9d3ea5b 100644
--- a/examples/plugins/presentation.cc
+++ b/examples/plugins/presentation.cc
@@ -32,7 +32,7 @@ namespace gazebo
class State
{
public: State(const std::string &_slide,
- const math::Vector3 _offset,
+ const math::Vector3 &_offset,
double _pitch, double _yaw, double _duration)
: slide(_slide), offset(_offset),
pitch(_pitch), yaw(_yaw), duration(_duration) {}
diff --git a/examples/plugins/projector/CMakeLists.txt b/examples/plugins/projector/CMakeLists.txt
index 5f628b5..f737f37 100644
--- a/examples/plugins/projector/CMakeLists.txt
+++ b/examples/plugins/projector/CMakeLists.txt
@@ -7,6 +7,7 @@ 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}")
add_library(projector_plugin SHARED projector.cc)
target_link_libraries(projector_plugin ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
diff --git a/examples/plugins/system_gui_plugin/CMakeLists.txt b/examples/plugins/system_gui_plugin/CMakeLists.txt
index 56f0c3c..c16b234 100644
--- a/examples/plugins/system_gui_plugin/CMakeLists.txt
+++ b/examples/plugins/system_gui_plugin/CMakeLists.txt
@@ -1,9 +1,9 @@
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}")
add_library(system_gui SHARED system_gui.cc)
target_link_libraries(system_gui ${GAZEBO_LIBRARIES})
diff --git a/examples/plugins/world_edit/CMakeLists.txt b/examples/plugins/world_edit/CMakeLists.txt
index 34fc043..311d50a 100644
--- a/examples/plugins/world_edit/CMakeLists.txt
+++ b/examples/plugins/world_edit/CMakeLists.txt
@@ -1,11 +1,10 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
+find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+
add_library(world_edit SHARED world_edit.cc)
target_link_libraries(world_edit ${GAZEBO_LIBRARIES})
diff --git a/examples/plugins/world_edit/world_edit.cc b/examples/plugins/world_edit/world_edit.cc
index 21732cd..bbde8e0 100644
--- a/examples/plugins/world_edit/world_edit.cc
+++ b/examples/plugins/world_edit/world_edit.cc
@@ -15,6 +15,7 @@
*
*/
#include <sdf/sdf.hh>
+#include <ignition/math/Pose3.hh>
#include "gazebo/gazebo.hh"
#include "gazebo/common/Plugin.hh"
#include "gazebo/msgs/msgs.hh"
@@ -47,7 +48,8 @@ namespace gazebo
physicsMsg.set_max_step_size(0.01);
// Change gravity
- msgs::Set(physicsMsg.mutable_gravity(), math::Vector3(0.01, 0, 0.1));
+ msgs::Set(physicsMsg.mutable_gravity(),
+ ignition::math::Vector3d(0.01, 0, 0.1));
physicsPub->Publish(physicsMsg);
}
};
diff --git a/examples/stand_alone/actuator/CMakeLists.txt b/examples/stand_alone/actuator/CMakeLists.txt
new file mode 100644
index 0000000..166c3cc
--- /dev/null
+++ b/examples/stand_alone/actuator/CMakeLists.txt
@@ -0,0 +1,28 @@
+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}")
+
+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/actuator/actuator_example.world b/examples/stand_alone/actuator/actuator_example.world
new file mode 100644
index 0000000..57c34b2
--- /dev/null
+++ b/examples/stand_alone/actuator/actuator_example.world
@@ -0,0 +1,226 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="actuator_example_world">
+
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <!-- A ground plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <model name='actuator_example'>
+ <link name='top'>
+ <visual name='top_visual'>
+ <pose>0 0 1.5 0 -0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.5</radius>
+ <length>1</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='top_collision'>
+ <geometry>
+ <cylinder>
+ <radius>0.5</radius>
+ <length>1</length>
+ </cylinder>
+ </geometry>
+ <pose>0 0 1.5 0 0 0</pose>
+ </collision>
+ <gravity>1</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ </inertial>
+ </link>
+ <link name='base'>
+ <visual name='base_visual'>
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>1.5</radius>
+ <length>1</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='base_collision'>
+ <geometry>
+ <cylinder>
+ <radius>1.5</radius>
+ <length>1</length>
+ </cylinder>
+ </geometry>
+ <pose>0 0 0.5 0 0 0</pose>
+ </collision>
+ <gravity>1</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <inertial>
+ <mass>1</mass>
+ <pose>0 0 0 0 -0 0</pose>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ </inertial>
+ </link>
+ <joint name='JOINT_0' type='revolute'>
+ <parent>base</parent>
+ <child>top</child>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ <limit>
+ <lower>1e-16</lower>
+ <upper>1e16</upper>
+ </limit>
+ </axis>
+ <pose>0 0 1 0 -0 0</pose>
+ </joint>
+ <static>0</static>
+ <allow_auto_disable>1</allow_auto_disable>
+
+ <plugin name="actuator_plugin" filename="libActuatorPlugin.so">
+ <actuator>
+ <name>actuator_0</name>
+ <joint>JOINT_0</joint>
+ <index>0</index>
+ <type>electric_motor</type>
+ <power>20</power>
+ <max_velocity>6</max_velocity>
+ <max_torque>10.0</max_torque>
+ </actuator>
+ </plugin>
+ </model>
+
+ <model name='unactuated_example'>
+ <pose>20 0 0 0 0 0</pose>
+ <link name='top'>
+ <visual name='top_visual'>
+ <pose>0 0 1.5 0 -0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.5</radius>
+ <length>1</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='top_collision'>
+ <geometry>
+ <cylinder>
+ <radius>0.5</radius>
+ <length>1</length>
+ </cylinder>
+ </geometry>
+ <pose>0 0 1.5 0 0 0</pose>
+ </collision>
+ <gravity>1</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ </inertial>
+ </link>
+ <link name='base'>
+ <visual name='base_visual'>
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>1.5</radius>
+ <length>1</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name='base_collision'>
+ <geometry>
+ <cylinder>
+ <radius>1.5</radius>
+ <length>1</length>
+ </cylinder>
+ </geometry>
+ <pose>0 0 0.5 0 0 0</pose>
+ </collision>
+ <gravity>1</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <inertial>
+ <mass>1</mass>
+ <pose>0 0 0 0 -0 0</pose>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ </inertial>
+ </link>
+ <joint name='JOINT_0' type='revolute'>
+ <parent>base</parent>
+ <child>top</child>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <use_parent_model_frame>0</use_parent_model_frame>
+ <limit>
+ <lower>1e-16</lower>
+ <upper>1e16</upper>
+ </limit>
+ </axis>
+ <pose>0 0 1 0 0 0</pose>
+ </joint>
+ <static>0</static>
+ <allow_auto_disable>1</allow_auto_disable>
+ </model>
+
+ </world>
+</sdf>
diff --git a/examples/stand_alone/actuator/data/plot.py b/examples/stand_alone/actuator/data/plot.py
new file mode 100644
index 0000000..8caa93f
--- /dev/null
+++ b/examples/stand_alone/actuator/data/plot.py
@@ -0,0 +1,69 @@
+import matplotlib.pyplot as pyplot
+import numpy
+
+#data = numpy.genfromtext('data.csv', delimiter='\t', skip_header=1,
+# names=['actuated_joint_pos', 'actuated_joint_vel',
+# 'actuated_joint_torque', 'unactuated_joint_pos',
+# 'unactuated_joint_vel', 'unactuated_joint_torque'])
+
+data = numpy.loadtxt('data.csv', delimiter='\t', skiprows=1)
+
+# expected headers:
+# 0: actuated_joint_pos 1: actuated_joint_vel 2: actuated_joint_torque
+# 3: unactuated_joint_pos 4: unactuated_joint_vel 5: unactuated_joint_torque
+time = range(len(data[:, 0]))
+
+# Joint positions over time
+passive = pyplot.plot(time, data[:, 3], label='Passive')
+actuated = pyplot.plot(time, data[:, 0], label='Actuated')
+pyplot.legend(loc='upper left')
+pyplot.xlabel('Time')
+pyplot.ylabel('Joint position')
+pyplot.savefig("actuator_pos_time.png")
+pyplot.show()
+
+# Joint velocities over time
+pyplot.plot(time, data[:, 4], label='Passive')
+pyplot.plot(time, data[:, 1], label='Actuated')
+
+pyplot.legend(loc='upper left')
+pyplot.xlabel('Time')
+pyplot.ylabel('Joint velocity')
+pyplot.savefig("actuator_vel_time.png")
+pyplot.show()
+
+# Joint torques over time
+pyplot.plot(time, data[:, 5], label='Passive')
+
+pyplot.legend()
+pyplot.xlabel('Time')
+pyplot.ylabel('Joint torque')
+pyplot.savefig("actuator_torque_time_passive.png")
+pyplot.show()
+
+pyplot.plot(time, data[:, 2], label='Actuated', color='g')
+pyplot.legend()
+pyplot.xlabel('Time')
+pyplot.ylabel('Joint torque')
+pyplot.savefig("actuator_torque_time_actuated.png")
+pyplot.show()
+
+# Torque vs. velocity (velocity on x-axis)
+
+pyplot.plot(data[:, 4], data[:, 5], label='Passive')
+
+pyplot.legend()
+pyplot.xlabel('Joint velocity')
+pyplot.ylabel('Joint torque')
+pyplot.xlim(0, max(data[:, 4]))
+pyplot.savefig("actuator_torque_speed_passive.png")
+pyplot.show()
+
+pyplot.plot(data[:, 1], data[:, 2], label='Actuated', color='g')
+pyplot.legend()
+pyplot.xlabel('Joint velocity')
+pyplot.ylabel('Joint torque')
+pyplot.xlim(0, max(data[:, 1]))
+
+pyplot.savefig("actuator_torque_speed_actuated.png")
+pyplot.show()
diff --git a/examples/stand_alone/actuator/main.cc b/examples/stand_alone/actuator/main.cc
new file mode 100644
index 0000000..729d33e
--- /dev/null
+++ b/examples/stand_alone/actuator/main.cc
@@ -0,0 +1,214 @@
+/*
+ * 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/gazebo.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/physics/physics.hh>
+
+/////////////////////////////////////////////////
+int main(int _argc, char **_argv)
+{
+ boost::program_options::options_description desc("Data collection options");
+
+ // Maximum number of collection iterations.
+ int maxIterations = 2000;
+ // Number of timesteps between samples.
+ int sampleTimesteps = 1;
+ // How much to scale the actuator's maximum torque by
+ float maxTorqueAdj = 2;
+
+ desc.add_options()("max_iter,i", boost::program_options::value<int>(),
+ "number of collection iterations")
+ ("sample_ts,s", boost::program_options::value<int>(),
+ "number of timesteps between samples")
+ ("torque_scale,t", boost::program_options::value<float>(),
+ "scale factor for applied torque");
+
+ boost::program_options::variables_map vm;
+ boost::program_options::store(boost::program_options::parse_command_line(
+ _argc, _argv, desc), vm);
+ boost::program_options::notify(vm);
+
+ if (vm.count("max_iter"))
+ {
+ maxIterations = vm["max_iter"].as<int>();
+ }
+
+ if (vm.count("sample_ts"))
+ {
+ sampleTimesteps = vm["sample_ts"].as<int>();
+ }
+
+ if (vm.count("torque_scale"))
+ {
+ maxTorqueAdj = vm["sample_ts"].as<float>();
+ }
+
+ // The joint index which we are collecting data on.
+ unsigned int index;
+ // The torque we are commanding to the actuator
+ float maxTorque;
+
+ // Initialize gazebo.
+ gazebo::setupServer(_argc, _argv);
+
+ // Load a world with two models: one actuated, one not
+ gazebo::physics::WorldPtr world =
+ gazebo::loadWorld("../actuator_example.world");
+ if (!world)
+ {
+ std::cout << "Could not load world actuator_example" << std::endl;
+ return -1;
+ }
+
+ // Get the models and pointers to their joints
+ std::vector<std::string> modelNames;
+ modelNames.push_back("actuator_example");
+ modelNames.push_back("unactuated_example");
+
+ std::string jointName;
+
+ std::vector<gazebo::physics::JointPtr> joints;
+
+ for (unsigned int i = 0; i < modelNames.size(); i++)
+ {
+ gazebo::physics::ModelPtr model = world->GetModel(modelNames[i]);
+ if (!model)
+ {
+ std::cout << "Couldn't find model: " << modelNames[i] << std::endl;
+ return -1;
+ }
+
+ if (modelNames[i].compare("actuator_example") == 0)
+ {
+ const sdf::ElementPtr modelSDF = model->GetSDF();
+ // Find the ActuatorPlugin SDF block
+ if (!modelSDF->HasElement("plugin"))
+ {
+ std::cout << "ERROR: couldn't find index element." << std::endl;
+ return -1;
+ }
+ sdf::ElementPtr elem = modelSDF->GetElement("plugin");
+ while (elem->GetAttribute("filename")->GetAsString().
+ compare("libActuatorPlugin.so"))
+ {
+ elem = elem->GetNextElement("plugin");
+ }
+
+ if (!elem->HasElement("actuator"))
+ {
+ std::cout << "ERROR: couldn't find actuator element" << std::endl;
+ return -1;
+ }
+ elem = elem->GetElement("actuator");
+
+ if (!elem->HasElement("index"))
+ {
+ std::cout << "ERROR: couldn't find index element." << std::endl;
+ return -1;
+ }
+ index = elem->GetElement("index")->Get<unsigned int>();
+
+ if (!elem->HasElement("max_torque"))
+ {
+ std::cout << "ERROR: couldn't find max_torque element." << std::endl;
+ return -1;
+ }
+ maxTorque = maxTorqueAdj*elem->GetElement("max_torque")->Get<float>();
+
+ if (!elem->HasElement("joint"))
+ {
+ std::cout << "ERROR: couldn't find joint element." << std::endl;
+ return -1;
+ }
+ jointName = elem->GetElement("joint")->Get<std::string>();
+ }
+
+ gazebo::physics::JointPtr joint = model->GetJoint(jointName);
+ if (!joint)
+ {
+ std::cout << "Couldn't find joint " << jointName << " for model "
+ << modelNames[i] << std::endl;
+ }
+ else
+ {
+ joints.push_back(joint);
+ }
+ }
+
+ // Create a data directory
+ boost::filesystem::path path("../data");
+ if (!boost::filesystem::exists(path))
+ {
+ boost::filesystem::create_directories(path);
+ }
+ // Open a file for recording data
+ std::ofstream fileStream;
+ fileStream.open((path.string() + "/data.csv").c_str());
+ // Push initial file headings
+ fileStream << "actuated_joint_pos\tactuated_joint_vel\t"
+ << "actuated_joint_torque\tunactuated_joint_pos\t"
+ << "unactuated_joint_vel\tunactuated_joint_torque" << std::endl;
+
+ // Run the simulation for a fixed number of iterations.
+ std::cout << "Collecting data for " << maxIterations << " iterations."
+ << std::endl;
+
+ for (unsigned int i = 0; i < maxIterations; ++i)
+ {
+ for (unsigned int j = 0; j < joints.size(); ++j)
+ {
+ if (!joints[j])
+ {
+ std::cout << "got NULL joint in actuator example main.cc" << std::endl;
+ continue;
+ }
+ // Command a constant force on each joint.
+ // This causes the joints to accelerate linearly
+ // The linear velocity data gives a range of values, allowing us to
+ // construct a torque vs. velocity graph
+ // By commanding a torque above the actuator's maximum torque value
+ // we are forcing the maximum force state of the actuator
+ joints[j]->SetForce(index, maxTorque);
+ }
+
+ gazebo::runWorld(world, sampleTimesteps);
+
+ // Print joint position, velocity and torques for each model to file
+ for (unsigned int j = 0; j < joints.size(); ++j)
+ {
+ fileStream << joints[j]->GetAngle(index) << "\t"
+ << joints[j]->GetVelocity(index) << "\t"
+ << joints[j]->GetForce(index);
+ if (j == joints.size() - 1)
+ {
+ fileStream << std::endl;
+ }
+ else
+ {
+ fileStream << "\t";
+ }
+ }
+ }
+
+ // Close everything.
+ fileStream.close();
+ gazebo::shutdown();
+
+ std::cout << "Finished data collection. Closing Gazebo." << std::endl;
+ return 0;
+}
diff --git a/examples/stand_alone/animated_box/CMakeLists.txt b/examples/stand_alone/animated_box/CMakeLists.txt
index ef1c4cb..b9c709a 100644
--- a/examples/stand_alone/animated_box/CMakeLists.txt
+++ b/examples/stand_alone/animated_box/CMakeLists.txt
@@ -8,14 +8,10 @@ find_package(Boost REQUIRED COMPONENTS system)
include_directories(${Boost_INCLUDE_DIRS})
link_directories(${Boost_LIBRARY_DIRS})
-
find_package(Protobuf REQUIRED)
+find_package(gazebo REQUIRED)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
-
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
# include appropriate directories
include_directories(${GAZEBO_INCLUDE_DIRS})
@@ -31,4 +27,4 @@ target_link_libraries(integrated_main ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${P
add_executable(independent_listener independent_listener.cc)
-target_link_libraries(independent_listener ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
\ No newline at end of file
+target_link_libraries(independent_listener ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
diff --git a/examples/stand_alone/animated_box/animated_box.cc b/examples/stand_alone/animated_box/animated_box.cc
index ef6a159..7c18548 100644
--- a/examples/stand_alone/animated_box/animated_box.cc
+++ b/examples/stand_alone/animated_box/animated_box.cc
@@ -16,6 +16,7 @@
*/
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
+#include <ignition/math.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>
@@ -40,33 +41,33 @@ namespace gazebo
// set starting location of the box
key = anim->CreateKeyFrame(0);
- key->SetTranslation(math::Vector3(0, 0, 0));
- key->SetRotation(math::Quaternion(0, 0, 0));
+ key->Translation(ignition::math::Vector3d(0, 0, 0));
+ key->Rotation(ignition::math::Quaterniond(0, 0, 0));
// set waypoint location after 2 seconds
key = anim->CreateKeyFrame(2.0);
- key->SetTranslation(math::Vector3(-50, -50, 0));
- key->SetRotation(math::Quaternion(0, 0, 1.5707));
+ key->Translation(ignition::math::Vector3d(-50, -50, 0));
+ key->Rotation(ignition::math::Quaterniond(0, 0, 1.5707));
key = anim->CreateKeyFrame(4.0);
- key->SetTranslation(math::Vector3(10, 20, 0));
- key->SetRotation(math::Quaternion(0, 0, 1.5707));
+ key->Translation(ignition::math::Vector3d(10, 20, 0));
+ key->Rotation(ignition::math::Quaterniond(0, 0, 1.5707));
key = anim->CreateKeyFrame(6.0);
- key->SetTranslation(math::Vector3(-10, 20, 0));
- key->SetRotation(math::Quaternion(0, 0, 1.5707));
+ key->Translation(ignition::math::Vector3d(-10, 20, 0));
+ key->Rotation(ignition::math::Quaterniond(0, 0, 1.5707));
key = anim->CreateKeyFrame(8.0);
- key->SetTranslation(math::Vector3(10, -20, 0));
- key->SetRotation(math::Quaternion(0, 0, 1.5707));
+ key->Translation(ignition::math::Vector3d(10, -20, 0));
+ key->Rotation(ignition::math::Quaterniond(0, 0, 1.5707));
// set final location equal to starting location
key = anim->CreateKeyFrame(10);
- key->SetTranslation(math::Vector3(0, 0, 0));
- key->SetRotation(math::Quaternion(0, 0, 0));
+ key->Translation(ignition::math::Vector3d(0, 0, 0));
+ key->Rotation(ignition::math::Quaterniond(0, 0, 0));
// set the animation
_parent->SetAnimation(anim);
diff --git a/examples/stand_alone/animated_box/independent_listener.cc b/examples/stand_alone/animated_box/independent_listener.cc
index f0c94b9..56ea34d 100644
--- a/examples/stand_alone/animated_box/independent_listener.cc
+++ b/examples/stand_alone/animated_box/independent_listener.cc
@@ -17,7 +17,7 @@
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
-#include <gazebo/gazebo.hh>
+#include <gazebo/gazebo_client.hh>
#include <iostream>
@@ -53,7 +53,7 @@ void posesStampedCallback(ConstPosesStampedPtr &posesStamped)
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());
@@ -68,5 +68,5 @@ int main(int _argc, char **_argv)
gazebo::common::Time::MSleep(10);
// Make sure to shut everything down.
- gazebo::shutdown();
+ gazebo::client::shutdown();
}
diff --git a/examples/stand_alone/arrange/CMakeLists.txt b/examples/stand_alone/arrange/CMakeLists.txt
index 1b82123..11457e9 100644
--- a/examples/stand_alone/arrange/CMakeLists.txt
+++ b/examples/stand_alone/arrange/CMakeLists.txt
@@ -1,14 +1,12 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
-
+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}")
add_executable(arrange_plugin_test arrange_plugin_test.cc)
target_link_libraries(arrange_plugin_test ${GAZEBO_LIBRARIES} ${PROTOBUF_LIBRARIES})
diff --git a/examples/stand_alone/arrange/arrange_plugin_test.cc b/examples/stand_alone/arrange/arrange_plugin_test.cc
index 824c810..57b820d 100644
--- a/examples/stand_alone/arrange/arrange_plugin_test.cc
+++ b/examples/stand_alone/arrange/arrange_plugin_test.cc
@@ -15,7 +15,7 @@
*
*/
-#include <gazebo/gazebo.hh>
+#include <gazebo/gazebo_client.hh>
#include <vector>
#include <iostream>
@@ -24,7 +24,7 @@
int main(int _argc, char** _argv)
{
- gazebo::setupClient(_argc, _argv);
+ gazebo::client::setup(_argc, _argv);
std::vector<std::string> arrangementNames;
@@ -52,5 +52,5 @@ int main(int _argc, char** _argv)
gazebo::common::Time::MSleep(1000);
}
- gazebo::shutdown();
+ gazebo::client::shutdown();
}
diff --git a/examples/stand_alone/clone_simulation/CMakeLists.txt b/examples/stand_alone/clone_simulation/CMakeLists.txt
index 799b038..f03b6f8 100644
--- a/examples/stand_alone/clone_simulation/CMakeLists.txt
+++ b/examples/stand_alone/clone_simulation/CMakeLists.txt
@@ -1,17 +1,14 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
-
include(FindBoost)
-find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem regex)
+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}")
add_executable(cloner cloner.cc)
target_link_libraries(cloner ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES})
diff --git a/examples/stand_alone/custom_main/CMakeLists.txt b/examples/stand_alone/custom_main/CMakeLists.txt
index 2573a6c..5104317 100644
--- a/examples/stand_alone/custom_main/CMakeLists.txt
+++ b/examples/stand_alone/custom_main/CMakeLists.txt
@@ -1,17 +1,15 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
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}")
add_executable(custom_main custom_main.cc)
target_link_libraries(custom_main ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
diff --git a/examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt b/examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt
new file mode 100644
index 0000000..e8f487f
--- /dev/null
+++ b/examples/stand_alone/custom_main_pkgconfig/CMakeLists.txt
@@ -0,0 +1,19 @@
+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}")
+
+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)
diff --git a/examples/stand_alone/custom_main_pkgconfig/README.md b/examples/stand_alone/custom_main_pkgconfig/README.md
new file mode 100644
index 0000000..b497f85
--- /dev/null
+++ b/examples/stand_alone/custom_main_pkgconfig/README.md
@@ -0,0 +1,6 @@
+## Custom Main pkg-config
+
+This example builds a stand-alone application using Gazebo as a library. The
+`CMakeLists.txt` in this directory uses pkg-config to find Gazebo. Other
+examples use Gazebo's installed cmake config file instead of pkg-config. We
+recommend using Gazebo's cmake config instead of pkg-config.
diff --git a/examples/stand_alone/custom_main_pkgconfig/custom_main.cc b/examples/stand_alone/custom_main_pkgconfig/custom_main.cc
new file mode 100644
index 0000000..844b1ac
--- /dev/null
+++ b/examples/stand_alone/custom_main_pkgconfig/custom_main.cc
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2012-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/gazebo.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/physics/physics.hh>
+
+/////////////////////////////////////////////////
+int main(int _argc, char **_argv)
+{
+ // Initialize gazebo.
+ gazebo::setupServer(_argc, _argv);
+
+ // Load a world
+ gazebo::physics::WorldPtr world = gazebo::loadWorld("worlds/empty.world");
+
+ // This is your custom main loop. In this example the main loop is just a
+ // for loop with 2 iterations.
+ for (unsigned int i = 0; i < 2; ++i)
+ {
+ // Run simulation for 100 steps.
+ gazebo::runWorld(world, 100);
+ }
+
+ // Close everything.
+ gazebo::shutdown();
+}
diff --git a/examples/stand_alone/listener/CMakeLists.txt b/examples/stand_alone/listener/CMakeLists.txt
index 0732849..e947e6f 100644
--- a/examples/stand_alone/listener/CMakeLists.txt
+++ b/examples/stand_alone/listener/CMakeLists.txt
@@ -1,17 +1,14 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
-
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}")
add_executable(listener listener.cc)
target_link_libraries(listener ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
diff --git a/examples/stand_alone/listener/listener.cc b/examples/stand_alone/listener/listener.cc
index 5c8aa43..f52bb97 100644
--- a/examples/stand_alone/listener/listener.cc
+++ b/examples/stand_alone/listener/listener.cc
@@ -17,7 +17,7 @@
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
-#include <gazebo/gazebo.hh>
+#include <gazebo/gazebo_client.hh>
#include <iostream>
@@ -33,7 +33,7 @@ void cb(ConstWorldStatisticsPtr &_msg)
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());
@@ -47,5 +47,5 @@ int main(int _argc, char **_argv)
gazebo::common::Time::MSleep(10);
// Make sure to shut everything down.
- gazebo::shutdown();
+ gazebo::client::shutdown();
}
diff --git a/examples/stand_alone/publisher/CMakeLists.txt b/examples/stand_alone/publisher/CMakeLists.txt
index 7d926f0..7b75618 100644
--- a/examples/stand_alone/publisher/CMakeLists.txt
+++ b/examples/stand_alone/publisher/CMakeLists.txt
@@ -1,17 +1,14 @@
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
- pkg_check_modules(GAZEBO gazebo)
-endif()
-
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}")
add_executable(publisher publisher.cc)
target_link_libraries(publisher ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
diff --git a/examples/stand_alone/publisher/publisher.cc b/examples/stand_alone/publisher/publisher.cc
index ff1d40d..821d7cb 100644
--- a/examples/stand_alone/publisher/publisher.cc
+++ b/examples/stand_alone/publisher/publisher.cc
@@ -15,19 +15,18 @@
*
*/
-#include <gazebo/gazebo.hh>
+#include <gazebo/gazebo_client.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/math/gzmath.hh>
#include <iostream>
-
/////////////////////////////////////////////////
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());
@@ -57,5 +56,5 @@ int main(int _argc, char **_argv)
}
// Make sure to shut everything down.
- gazebo::shutdown();
+ gazebo::client::shutdown();
}
diff --git a/examples/stand_alone/test_fixture/CMakeLists.txt b/examples/stand_alone/test_fixture/CMakeLists.txt
new file mode 100644
index 0000000..b370e23
--- /dev/null
+++ b/examples/stand_alone/test_fixture/CMakeLists.txt
@@ -0,0 +1,23 @@
+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
+ ${PROJECT_SOURCE_DIR}/gtest
+)
+
+link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+
+# Build gtest
+add_library(gtest STATIC gtest/src/gtest-all.cc)
+add_library(gtest_main STATIC gtest/src/gtest_main.cc)
+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})
diff --git a/examples/stand_alone/test_fixture/gtest/cmake/internal_utils.cmake b/examples/stand_alone/test_fixture/gtest/cmake/internal_utils.cmake
new file mode 100644
index 0000000..8cb2189
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/cmake/internal_utils.cmake
@@ -0,0 +1,227 @@
+# Defines functions and macros useful for building Google Test and
+# Google Mock.
+#
+# Note:
+#
+# - This file will be run twice when building Google Mock (once via
+# Google Test's CMakeLists.txt, and once via Google Mock's).
+# Therefore it shouldn't have any side effects other than defining
+# the functions and macros.
+#
+# - The functions/macros defined in this file may depend on Google
+# Test and Google Mock's option() definitions, and thus must be
+# called *after* the options have been defined.
+
+# Tweaks CMake's default compiler/linker settings to suit Google Test's needs.
+#
+# This must be a macro(), as inside a function string() can only
+# update variables in the function scope.
+macro(fix_default_compiler_settings_)
+ if (MSVC)
+ # For MSVC, CMake sets certain flags to defaults we want to override.
+ # This replacement code is taken from sample in the CMake Wiki at
+ # http://www.cmake.org/Wiki/CMake_FAQ#Dynamic_Replace.
+ foreach (flag_var
+ CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE
+ CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO)
+ if (NOT BUILD_SHARED_LIBS AND NOT gtest_force_shared_crt)
+ # When Google Test is built as a shared library, it should also use
+ # shared runtime libraries. Otherwise, it may end up with multiple
+ # copies of runtime library data in different modules, resulting in
+ # hard-to-find crashes. When it is built as a static library, it is
+ # preferable to use CRT as static libraries, as we don't have to rely
+ # on CRT DLLs being available. CMake always defaults to using shared
+ # CRT libraries, so we override that default here.
+ string(REPLACE "/MD" "-MT" ${flag_var} "${${flag_var}}")
+ endif()
+
+ # We prefer more strict warning checking for building Google Test.
+ # Replaces /W3 with /W4 in defaults.
+ string(REPLACE "/W3" "-W4" ${flag_var} "${${flag_var}}")
+ endforeach()
+ endif()
+endmacro()
+
+# Defines the compiler/linker flags used to build Google Test and
+# Google Mock. You can tweak these definitions to suit your need. A
+# variable's value is empty before it's explicitly assigned to.
+macro(config_compiler_and_linker)
+ if (NOT gtest_disable_pthreads)
+ # Defines CMAKE_USE_PTHREADS_INIT and CMAKE_THREAD_LIBS_INIT.
+ find_package(Threads)
+ endif()
+
+ fix_default_compiler_settings_()
+ if (MSVC)
+ # Newlines inside flags variables break CMake's NMake generator.
+ # TODO(vladl at google.com): Add -RTCs and -RTCu to debug builds.
+ set(cxx_base_flags "-GS -W4 -WX -wd4127 -wd4251 -wd4275 -nologo -J -Zi")
+ if (MSVC_VERSION LESS 1400)
+ # Suppress spurious warnings MSVC 7.1 sometimes issues.
+ # Forcing value to bool.
+ set(cxx_base_flags "${cxx_base_flags} -wd4800")
+ # Copy constructor and assignment operator could not be generated.
+ set(cxx_base_flags "${cxx_base_flags} -wd4511 -wd4512")
+ # Compatibility warnings not applicable to Google Test.
+ # Resolved overload was found by argument-dependent lookup.
+ set(cxx_base_flags "${cxx_base_flags} -wd4675")
+ endif()
+ set(cxx_base_flags "${cxx_base_flags} -D_UNICODE -DUNICODE -DWIN32 -D_WIN32")
+ set(cxx_base_flags "${cxx_base_flags} -DSTRICT -DWIN32_LEAN_AND_MEAN")
+ set(cxx_exception_flags "-EHsc -D_HAS_EXCEPTIONS=1")
+ set(cxx_no_exception_flags "-D_HAS_EXCEPTIONS=0")
+ set(cxx_no_rtti_flags "-GR-")
+ elseif (CMAKE_COMPILER_IS_GNUCXX)
+ set(cxx_base_flags "-Wall -Wshadow")
+ set(cxx_exception_flags "-fexceptions")
+ set(cxx_no_exception_flags "-fno-exceptions")
+ # Until version 4.3.2, GCC doesn't define a macro to indicate
+ # whether RTTI is enabled. Therefore we define GTEST_HAS_RTTI
+ # explicitly.
+ set(cxx_no_rtti_flags "-fno-rtti -DGTEST_HAS_RTTI=0")
+ set(cxx_strict_flags
+ "-Wextra -Wno-unused-parameter -Wno-missing-field-initializers")
+ elseif (CMAKE_CXX_COMPILER_ID STREQUAL "SunPro")
+ set(cxx_exception_flags "-features=except")
+ # Sun Pro doesn't provide macros to indicate whether exceptions and
+ # RTTI are enabled, so we define GTEST_HAS_* explicitly.
+ set(cxx_no_exception_flags "-features=no%except -DGTEST_HAS_EXCEPTIONS=0")
+ set(cxx_no_rtti_flags "-features=no%rtti -DGTEST_HAS_RTTI=0")
+ elseif (CMAKE_CXX_COMPILER_ID STREQUAL "VisualAge" OR
+ CMAKE_CXX_COMPILER_ID STREQUAL "XL")
+ # CMake 2.8 changes Visual Age's compiler ID to "XL".
+ set(cxx_exception_flags "-qeh")
+ set(cxx_no_exception_flags "-qnoeh")
+ # Until version 9.0, Visual Age doesn't define a macro to indicate
+ # whether RTTI is enabled. Therefore we define GTEST_HAS_RTTI
+ # explicitly.
+ set(cxx_no_rtti_flags "-qnortti -DGTEST_HAS_RTTI=0")
+ elseif (CMAKE_CXX_COMPILER_ID STREQUAL "HP")
+ set(cxx_base_flags "-AA -mt")
+ set(cxx_exception_flags "-DGTEST_HAS_EXCEPTIONS=1")
+ set(cxx_no_exception_flags "+noeh -DGTEST_HAS_EXCEPTIONS=0")
+ # RTTI can not be disabled in HP aCC compiler.
+ set(cxx_no_rtti_flags "")
+ endif()
+
+ if (CMAKE_USE_PTHREADS_INIT) # The pthreads library is available and allowed.
+ set(cxx_base_flags "${cxx_base_flags} -DGTEST_HAS_PTHREAD=1")
+ else()
+ set(cxx_base_flags "${cxx_base_flags} -DGTEST_HAS_PTHREAD=0")
+ endif()
+
+ # For building gtest's own tests and samples.
+ set(cxx_exception "${CMAKE_CXX_FLAGS} ${cxx_base_flags} ${cxx_exception_flags}")
+ set(cxx_no_exception
+ "${CMAKE_CXX_FLAGS} ${cxx_base_flags} ${cxx_no_exception_flags}")
+ set(cxx_default "${cxx_exception}")
+ set(cxx_no_rtti "${cxx_default} ${cxx_no_rtti_flags}")
+ set(cxx_use_own_tuple "${cxx_default} -DGTEST_USE_OWN_TR1_TUPLE=1")
+
+ # For building the gtest libraries.
+ set(cxx_strict "${cxx_default} ${cxx_strict_flags}")
+endmacro()
+
+# Defines the gtest & gtest_main libraries. User tests should link
+# with one of them.
+function(cxx_library_with_type name type cxx_flags)
+ # type can be either STATIC or SHARED to denote a static or shared library.
+ # ARGN refers to additional arguments after 'cxx_flags'.
+ add_library(${name} ${type} ${ARGN})
+ set_target_properties(${name}
+ PROPERTIES
+ COMPILE_FLAGS "${cxx_flags}")
+ if (BUILD_SHARED_LIBS OR type STREQUAL "SHARED")
+ set_target_properties(${name}
+ PROPERTIES
+ COMPILE_DEFINITIONS "GTEST_CREATE_SHARED_LIBRARY=1")
+ endif()
+ if (CMAKE_USE_PTHREADS_INIT)
+ target_link_libraries(${name} ${CMAKE_THREAD_LIBS_INIT})
+ endif()
+endfunction()
+
+########################################################################
+#
+# Helper functions for creating build targets.
+
+function(cxx_shared_library name cxx_flags)
+ cxx_library_with_type(${name} SHARED "${cxx_flags}" ${ARGN})
+endfunction()
+
+function(cxx_library name cxx_flags)
+ cxx_library_with_type(${name} "" "${cxx_flags}" ${ARGN})
+endfunction()
+
+# cxx_executable_with_flags(name cxx_flags libs srcs...)
+#
+# creates a named C++ executable that depends on the given libraries and
+# is built from the given source files with the given compiler flags.
+function(cxx_executable_with_flags name cxx_flags libs)
+ add_executable(${name} ${ARGN})
+ if (cxx_flags)
+ set_target_properties(${name}
+ PROPERTIES
+ COMPILE_FLAGS "${cxx_flags}")
+ endif()
+ if (BUILD_SHARED_LIBS)
+ set_target_properties(${name}
+ PROPERTIES
+ COMPILE_DEFINITIONS "GTEST_LINKED_AS_SHARED_LIBRARY=1")
+ endif()
+ # To support mixing linking in static and dynamic libraries, link each
+ # library in with an extra call to target_link_libraries.
+ foreach (lib "${libs}")
+ target_link_libraries(${name} ${lib})
+ endforeach()
+endfunction()
+
+# cxx_executable(name dir lib srcs...)
+#
+# creates a named target that depends on the given libs and is built
+# from the given source files. dir/name.cc is implicitly included in
+# the source file list.
+function(cxx_executable name dir libs)
+ cxx_executable_with_flags(
+ ${name} "${cxx_default}" "${libs}" "${dir}/${name}.cc" ${ARGN})
+endfunction()
+
+# Sets PYTHONINTERP_FOUND and PYTHON_EXECUTABLE.
+find_package(PythonInterp)
+
+# cxx_test_with_flags(name cxx_flags libs srcs...)
+#
+# creates a named C++ test that depends on the given libs and is built
+# from the given source files with the given compiler flags.
+function(cxx_test_with_flags name cxx_flags libs)
+ cxx_executable_with_flags(${name} "${cxx_flags}" "${libs}" ${ARGN})
+ add_test(${name} ${name})
+endfunction()
+
+# cxx_test(name libs srcs...)
+#
+# creates a named test target that depends on the given libs and is
+# built from the given source files. Unlike cxx_test_with_flags,
+# test/name.cc is already implicitly included in the source file list.
+function(cxx_test name libs)
+ cxx_test_with_flags("${name}" "${cxx_default}" "${libs}"
+ "test/${name}.cc" ${ARGN})
+endfunction()
+
+# py_test(name)
+#
+# creates a Python test with the given name whose main module is in
+# test/name.py. It does nothing if Python is not installed.
+function(py_test name)
+ # We are not supporting Python tests on Linux yet as they consider
+ # all Linux environments to be google3 and try to use google3 features.
+ if (PYTHONINTERP_FOUND)
+ # ${CMAKE_BINARY_DIR} is known at configuration time, so we can
+ # directly bind it from cmake. ${CTEST_CONFIGURATION_TYPE} is known
+ # only at ctest runtime (by calling ctest -c <Configuration>), so
+ # we have to escape $ to delay variable substitution here.
+ add_test(${name}
+ ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/test/${name}.py
+ --build_dir=${CMAKE_CURRENT_BINARY_DIR}/\${CTEST_CONFIGURATION_TYPE})
+ endif()
+endfunction()
diff --git a/examples/stand_alone/test_fixture/gtest/gtest-1.7.0.diff b/examples/stand_alone/test_fixture/gtest/gtest-1.7.0.diff
new file mode 100644
index 0000000..e878396
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/gtest-1.7.0.diff
@@ -0,0 +1,44 @@
+diff -r a5e72dd0ecf3 test/gtest/include/gtest/gtest-typed-test.h
+--- a/test/gtest/include/gtest/gtest-typed-test.h Mon Nov 04 11:47:43 2013 -0800
++++ b/test/gtest/include/gtest/gtest-typed-test.h Mon Nov 04 11:49:12 2013 -0800
+@@ -31,6 +31,7 @@
+
+ #ifndef GTEST_INCLUDE_GTEST_GTEST_TYPED_TEST_H_
+ #define GTEST_INCLUDE_GTEST_GTEST_TYPED_TEST_H_
++#pragma GCC system_header
+
+ // This header implements typed tests and type-parameterized tests.
+
+diff -r a5e72dd0ecf3 test/gtest/src/gtest.cc
+--- a/test/gtest/src/gtest.cc Mon Nov 04 11:47:43 2013 -0800
++++ b/test/gtest/src/gtest.cc Mon Nov 04 11:49:12 2013 -0800
+@@ -33,6 +33,7 @@
+
+ #include "gtest/gtest.h"
+ #include "gtest/gtest-spi.h"
++#pragma GCC system_header
+
+ #include <ctype.h>
+ #include <math.h>
+diff -r c33b44f8a9a1 test/gtest/include/gtest/internal/gtest-port.h
+--- a/test/gtest/include/gtest/internal/gtest-port.h Wed Nov 06 11:23:38 2013 -0800
++++ b/test/gtest/include/gtest/internal/gtest-port.h Wed Nov 06 17:12:57 2013 -0800
+@@ -39,6 +39,7 @@
+
+ #ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PORT_H_
+ #define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PORT_H_
++#pragma GCC system_header
+
+ // The user can define the following macros in the build script to
+ // control Google Test's behavior. If the user doesn't define a macro
+diff -r e980730656c1 test/gtest/include/gtest/gtest-printers.h
+--- a/test/gtest/include/gtest/gtest-printers.h Wed Nov 06 17:13:57 2013 -0800
++++ b/test/gtest/include/gtest/gtest-printers.h Thu Nov 07 09:29:28 2013 -0800
+@@ -94,6 +94,7 @@
+
+ #ifndef GTEST_INCLUDE_GTEST_GTEST_PRINTERS_H_
+ #define GTEST_INCLUDE_GTEST_GTEST_PRINTERS_H_
++#pragma GCC system_header
+
+ #include <ostream> // NOLINT
+ #include <sstream>
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-death-test.h b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-death-test.h
new file mode 100644
index 0000000..957a69c
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-death-test.h
@@ -0,0 +1,294 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+//
+// The Google C++ Testing Framework (Google Test)
+//
+// This header file defines the public API for death tests. It is
+// #included by gtest.h so a user doesn't need to include this
+// directly.
+
+#ifndef GTEST_INCLUDE_GTEST_GTEST_DEATH_TEST_H_
+#define GTEST_INCLUDE_GTEST_GTEST_DEATH_TEST_H_
+
+#include "gtest/internal/gtest-death-test-internal.h"
+
+namespace testing {
+
+// This flag controls the style of death tests. Valid values are "threadsafe",
+// meaning that the death test child process will re-execute the test binary
+// from the start, running only a single death test, or "fast",
+// meaning that the child process will execute the test logic immediately
+// after forking.
+GTEST_DECLARE_string_(death_test_style);
+
+#if GTEST_HAS_DEATH_TEST
+
+namespace internal {
+
+// Returns a Boolean value indicating whether the caller is currently
+// executing in the context of the death test child process. Tools such as
+// Valgrind heap checkers may need this to modify their behavior in death
+// tests. IMPORTANT: This is an internal utility. Using it may break the
+// implementation of death tests. User code MUST NOT use it.
+GTEST_API_ bool InDeathTestChild();
+
+} // namespace internal
+
+// The following macros are useful for writing death tests.
+
+// Here's what happens when an ASSERT_DEATH* or EXPECT_DEATH* is
+// executed:
+//
+// 1. It generates a warning if there is more than one active
+// thread. This is because it's safe to fork() or clone() only
+// when there is a single thread.
+//
+// 2. The parent process clone()s a sub-process and runs the death
+// test in it; the sub-process exits with code 0 at the end of the
+// death test, if it hasn't exited already.
+//
+// 3. The parent process waits for the sub-process to terminate.
+//
+// 4. The parent process checks the exit code and error message of
+// the sub-process.
+//
+// Examples:
+//
+// ASSERT_DEATH(server.SendMessage(56, "Hello"), "Invalid port number");
+// for (int i = 0; i < 5; i++) {
+// EXPECT_DEATH(server.ProcessRequest(i),
+// "Invalid request .* in ProcessRequest()")
+// << "Failed to die on request " << i;
+// }
+//
+// ASSERT_EXIT(server.ExitNow(), ::testing::ExitedWithCode(0), "Exiting");
+//
+// bool KilledBySIGHUP(int exit_code) {
+// return WIFSIGNALED(exit_code) && WTERMSIG(exit_code) == SIGHUP;
+// }
+//
+// ASSERT_EXIT(client.HangUpServer(), KilledBySIGHUP, "Hanging up!");
+//
+// On the regular expressions used in death tests:
+//
+// On POSIX-compliant systems (*nix), we use the <regex.h> library,
+// which uses the POSIX extended regex syntax.
+//
+// On other platforms (e.g. Windows), we only support a simple regex
+// syntax implemented as part of Google Test. This limited
+// implementation should be enough most of the time when writing
+// death tests; though it lacks many features you can find in PCRE
+// or POSIX extended regex syntax. For example, we don't support
+// union ("x|y"), grouping ("(xy)"), brackets ("[xy]"), and
+// repetition count ("x{5,7}"), among others.
+//
+// Below is the syntax that we do support. We chose it to be a
+// subset of both PCRE and POSIX extended regex, so it's easy to
+// learn wherever you come from. In the following: 'A' denotes a
+// literal character, period (.), or a single \\ escape sequence;
+// 'x' and 'y' denote regular expressions; 'm' and 'n' are for
+// natural numbers.
+//
+// c matches any literal character c
+// \\d matches any decimal digit
+// \\D matches any character that's not a decimal digit
+// \\f matches \f
+// \\n matches \n
+// \\r matches \r
+// \\s matches any ASCII whitespace, including \n
+// \\S matches any character that's not a whitespace
+// \\t matches \t
+// \\v matches \v
+// \\w matches any letter, _, or decimal digit
+// \\W matches any character that \\w doesn't match
+// \\c matches any literal character c, which must be a punctuation
+// . matches any single character except \n
+// A? matches 0 or 1 occurrences of A
+// A* matches 0 or many occurrences of A
+// A+ matches 1 or many occurrences of A
+// ^ matches the beginning of a string (not that of each line)
+// $ matches the end of a string (not that of each line)
+// xy matches x followed by y
+//
+// If you accidentally use PCRE or POSIX extended regex features
+// not implemented by us, you will get a run-time failure. In that
+// case, please try to rewrite your regular expression within the
+// above syntax.
+//
+// This implementation is *not* meant to be as highly tuned or robust
+// as a compiled regex library, but should perform well enough for a
+// death test, which already incurs significant overhead by launching
+// a child process.
+//
+// Known caveats:
+//
+// A "threadsafe" style death test obtains the path to the test
+// program from argv[0] and re-executes it in the sub-process. For
+// simplicity, the current implementation doesn't search the PATH
+// when launching the sub-process. This means that the user must
+// invoke the test program via a path that contains at least one
+// path separator (e.g. path/to/foo_test and
+// /absolute/path/to/bar_test are fine, but foo_test is not). This
+// is rarely a problem as people usually don't put the test binary
+// directory in PATH.
+//
+// TODO(wan at google.com): make thread-safe death tests search the PATH.
+
+// Asserts that a given statement causes the program to exit, with an
+// integer exit status that satisfies predicate, and emitting error output
+// that matches regex.
+# define ASSERT_EXIT(statement, predicate, regex) \
+ GTEST_DEATH_TEST_(statement, predicate, regex, GTEST_FATAL_FAILURE_)
+
+// Like ASSERT_EXIT, but continues on to successive tests in the
+// test case, if any:
+# define EXPECT_EXIT(statement, predicate, regex) \
+ GTEST_DEATH_TEST_(statement, predicate, regex, GTEST_NONFATAL_FAILURE_)
+
+// Asserts that a given statement causes the program to exit, either by
+// explicitly exiting with a nonzero exit code or being killed by a
+// signal, and emitting error output that matches regex.
+# define ASSERT_DEATH(statement, regex) \
+ ASSERT_EXIT(statement, ::testing::internal::ExitedUnsuccessfully, regex)
+
+// Like ASSERT_DEATH, but continues on to successive tests in the
+// test case, if any:
+# define EXPECT_DEATH(statement, regex) \
+ EXPECT_EXIT(statement, ::testing::internal::ExitedUnsuccessfully, regex)
+
+// Two predicate classes that can be used in {ASSERT,EXPECT}_EXIT*:
+
+// Tests that an exit code describes a normal exit with a given exit code.
+class GTEST_API_ ExitedWithCode {
+ public:
+ explicit ExitedWithCode(int exit_code);
+ bool operator()(int exit_status) const;
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ExitedWithCode& other);
+
+ const int exit_code_;
+};
+
+# if !GTEST_OS_WINDOWS
+// Tests that an exit code describes an exit due to termination by a
+// given signal.
+class GTEST_API_ KilledBySignal {
+ public:
+ explicit KilledBySignal(int signum);
+ bool operator()(int exit_status) const;
+ private:
+ const int signum_;
+};
+# endif // !GTEST_OS_WINDOWS
+
+// EXPECT_DEBUG_DEATH asserts that the given statements die in debug mode.
+// The death testing framework causes this to have interesting semantics,
+// since the sideeffects of the call are only visible in opt mode, and not
+// in debug mode.
+//
+// In practice, this can be used to test functions that utilize the
+// LOG(DFATAL) macro using the following style:
+//
+// int DieInDebugOr12(int* sideeffect) {
+// if (sideeffect) {
+// *sideeffect = 12;
+// }
+// LOG(DFATAL) << "death";
+// return 12;
+// }
+//
+// TEST(TestCase, TestDieOr12WorksInDgbAndOpt) {
+// int sideeffect = 0;
+// // Only asserts in dbg.
+// EXPECT_DEBUG_DEATH(DieInDebugOr12(&sideeffect), "death");
+//
+// #ifdef NDEBUG
+// // opt-mode has sideeffect visible.
+// EXPECT_EQ(12, sideeffect);
+// #else
+// // dbg-mode no visible sideeffect.
+// EXPECT_EQ(0, sideeffect);
+// #endif
+// }
+//
+// This will assert that DieInDebugReturn12InOpt() crashes in debug
+// mode, usually due to a DCHECK or LOG(DFATAL), but returns the
+// appropriate fallback value (12 in this case) in opt mode. If you
+// need to test that a function has appropriate side-effects in opt
+// mode, include assertions against the side-effects. A general
+// pattern for this is:
+//
+// EXPECT_DEBUG_DEATH({
+// // Side-effects here will have an effect after this statement in
+// // opt mode, but none in debug mode.
+// EXPECT_EQ(12, DieInDebugOr12(&sideeffect));
+// }, "death");
+//
+# ifdef NDEBUG
+
+# define EXPECT_DEBUG_DEATH(statement, regex) \
+ GTEST_EXECUTE_STATEMENT_(statement, regex)
+
+# define ASSERT_DEBUG_DEATH(statement, regex) \
+ GTEST_EXECUTE_STATEMENT_(statement, regex)
+
+# else
+
+# define EXPECT_DEBUG_DEATH(statement, regex) \
+ EXPECT_DEATH(statement, regex)
+
+# define ASSERT_DEBUG_DEATH(statement, regex) \
+ ASSERT_DEATH(statement, regex)
+
+# endif // NDEBUG for EXPECT_DEBUG_DEATH
+#endif // GTEST_HAS_DEATH_TEST
+
+// EXPECT_DEATH_IF_SUPPORTED(statement, regex) and
+// ASSERT_DEATH_IF_SUPPORTED(statement, regex) expand to real death tests if
+// death tests are supported; otherwise they just issue a warning. This is
+// useful when you are combining death test assertions with normal test
+// assertions in one test.
+#if GTEST_HAS_DEATH_TEST
+# define EXPECT_DEATH_IF_SUPPORTED(statement, regex) \
+ EXPECT_DEATH(statement, regex)
+# define ASSERT_DEATH_IF_SUPPORTED(statement, regex) \
+ ASSERT_DEATH(statement, regex)
+#else
+# define EXPECT_DEATH_IF_SUPPORTED(statement, regex) \
+ GTEST_UNSUPPORTED_DEATH_TEST_(statement, regex, )
+# define ASSERT_DEATH_IF_SUPPORTED(statement, regex) \
+ GTEST_UNSUPPORTED_DEATH_TEST_(statement, regex, return)
+#endif
+
+} // namespace testing
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_DEATH_TEST_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-message.h b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-message.h
new file mode 100644
index 0000000..fe879bc
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-message.h
@@ -0,0 +1,250 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+//
+// The Google C++ Testing Framework (Google Test)
+//
+// This header file defines the Message class.
+//
+// IMPORTANT NOTE: Due to limitation of the C++ language, we have to
+// leave some internal implementation details in this header file.
+// They are clearly marked by comments like this:
+//
+// // INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+//
+// Such code is NOT meant to be used by a user directly, and is subject
+// to CHANGE WITHOUT NOTICE. Therefore DO NOT DEPEND ON IT in a user
+// program!
+
+#ifndef GTEST_INCLUDE_GTEST_GTEST_MESSAGE_H_
+#define GTEST_INCLUDE_GTEST_GTEST_MESSAGE_H_
+
+#include <limits>
+
+#include "gtest/internal/gtest-port.h"
+
+// Ensures that there is at least one operator<< in the global namespace.
+// See Message& operator<<(...) below for why.
+void operator<<(const testing::internal::Secret&, int);
+
+namespace testing {
+
+// The Message class works like an ostream repeater.
+//
+// Typical usage:
+//
+// 1. You stream a bunch of values to a Message object.
+// It will remember the text in a stringstream.
+// 2. Then you stream the Message object to an ostream.
+// This causes the text in the Message to be streamed
+// to the ostream.
+//
+// For example;
+//
+// testing::Message foo;
+// foo << 1 << " != " << 2;
+// std::cout << foo;
+//
+// will print "1 != 2".
+//
+// Message is not intended to be inherited from. In particular, its
+// destructor is not virtual.
+//
+// Note that stringstream behaves differently in gcc and in MSVC. You
+// can stream a NULL char pointer to it in the former, but not in the
+// latter (it causes an access violation if you do). The Message
+// class hides this difference by treating a NULL char pointer as
+// "(null)".
+class GTEST_API_ Message {
+ private:
+ // The type of basic IO manipulators (endl, ends, and flush) for
+ // narrow streams.
+ typedef std::ostream& (*BasicNarrowIoManip)(std::ostream&);
+
+ public:
+ // Constructs an empty Message.
+ Message();
+
+ // Copy constructor.
+ Message(const Message& msg) : ss_(new ::std::stringstream) { // NOLINT
+ *ss_ << msg.GetString();
+ }
+
+ // Constructs a Message from a C-string.
+ explicit Message(const char* str) : ss_(new ::std::stringstream) {
+ *ss_ << str;
+ }
+
+#if GTEST_OS_SYMBIAN
+ // Streams a value (either a pointer or not) to this object.
+ template <typename T>
+ inline Message& operator <<(const T& value) {
+ StreamHelper(typename internal::is_pointer<T>::type(), value);
+ return *this;
+ }
+#else
+ // Streams a non-pointer value to this object.
+ template <typename T>
+ inline Message& operator <<(const T& val) {
+ // Some libraries overload << for STL containers. These
+ // overloads are defined in the global namespace instead of ::std.
+ //
+ // C++'s symbol lookup rule (i.e. Koenig lookup) says that these
+ // overloads are visible in either the std namespace or the global
+ // namespace, but not other namespaces, including the testing
+ // namespace which Google Test's Message class is in.
+ //
+ // To allow STL containers (and other types that has a << operator
+ // defined in the global namespace) to be used in Google Test
+ // assertions, testing::Message must access the custom << operator
+ // from the global namespace. With this using declaration,
+ // overloads of << defined in the global namespace and those
+ // visible via Koenig lookup are both exposed in this function.
+ using ::operator <<;
+ *ss_ << val;
+ return *this;
+ }
+
+ // Streams a pointer value to this object.
+ //
+ // This function is an overload of the previous one. When you
+ // stream a pointer to a Message, this definition will be used as it
+ // is more specialized. (The C++ Standard, section
+ // [temp.func.order].) If you stream a non-pointer, then the
+ // previous definition will be used.
+ //
+ // The reason for this overload is that streaming a NULL pointer to
+ // ostream is undefined behavior. Depending on the compiler, you
+ // may get "0", "(nil)", "(null)", or an access violation. To
+ // ensure consistent result across compilers, we always treat NULL
+ // as "(null)".
+ template <typename T>
+ inline Message& operator <<(T* const& pointer) { // NOLINT
+ if (pointer == NULL) {
+ *ss_ << "(null)";
+ } else {
+ *ss_ << pointer;
+ }
+ return *this;
+ }
+#endif // GTEST_OS_SYMBIAN
+
+ // Since the basic IO manipulators are overloaded for both narrow
+ // and wide streams, we have to provide this specialized definition
+ // of operator <<, even though its body is the same as the
+ // templatized version above. Without this definition, streaming
+ // endl or other basic IO manipulators to Message will confuse the
+ // compiler.
+ Message& operator <<(BasicNarrowIoManip val) {
+ *ss_ << val;
+ return *this;
+ }
+
+ // Instead of 1/0, we want to see true/false for bool values.
+ Message& operator <<(bool b) {
+ return *this << (b ? "true" : "false");
+ }
+
+ // These two overloads allow streaming a wide C string to a Message
+ // using the UTF-8 encoding.
+ Message& operator <<(const wchar_t* wide_c_str);
+ Message& operator <<(wchar_t* wide_c_str);
+
+#if GTEST_HAS_STD_WSTRING
+ // Converts the given wide string to a narrow string using the UTF-8
+ // encoding, and streams the result to this Message object.
+ Message& operator <<(const ::std::wstring& wstr);
+#endif // GTEST_HAS_STD_WSTRING
+
+#if GTEST_HAS_GLOBAL_WSTRING
+ // Converts the given wide string to a narrow string using the UTF-8
+ // encoding, and streams the result to this Message object.
+ Message& operator <<(const ::wstring& wstr);
+#endif // GTEST_HAS_GLOBAL_WSTRING
+
+ // Gets the text streamed to this object so far as an std::string.
+ // Each '\0' character in the buffer is replaced with "\\0".
+ //
+ // INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+ std::string GetString() const;
+
+ private:
+
+#if GTEST_OS_SYMBIAN
+ // These are needed as the Nokia Symbian Compiler cannot decide between
+ // const T& and const T* in a function template. The Nokia compiler _can_
+ // decide between class template specializations for T and T*, so a
+ // tr1::type_traits-like is_pointer works, and we can overload on that.
+ template <typename T>
+ inline void StreamHelper(internal::true_type /*is_pointer*/, T* pointer) {
+ if (pointer == NULL) {
+ *ss_ << "(null)";
+ } else {
+ *ss_ << pointer;
+ }
+ }
+ template <typename T>
+ inline void StreamHelper(internal::false_type /*is_pointer*/,
+ const T& value) {
+ // See the comments in Message& operator <<(const T&) above for why
+ // we need this using statement.
+ using ::operator <<;
+ *ss_ << value;
+ }
+#endif // GTEST_OS_SYMBIAN
+
+ // We'll hold the text streamed to this object here.
+ const internal::scoped_ptr< ::std::stringstream> ss_;
+
+ // We declare (but don't implement) this to prevent the compiler
+ // from implementing the assignment operator.
+ void operator=(const Message&);
+};
+
+// Streams a Message to an ostream.
+inline std::ostream& operator <<(std::ostream& os, const Message& sb) {
+ return os << sb.GetString();
+}
+
+namespace internal {
+
+// Converts a streamable value to an std::string. A NULL pointer is
+// converted to "(null)". When the input value is a ::string,
+// ::std::string, ::wstring, or ::std::wstring object, each NUL
+// character in it is replaced with "\\0".
+template <typename T>
+std::string StreamableToString(const T& streamable) {
+ return (Message() << streamable).GetString();
+}
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_MESSAGE_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-param-test.h b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-param-test.h
new file mode 100644
index 0000000..d6702c8
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-param-test.h
@@ -0,0 +1,1421 @@
+// This file was GENERATED by command:
+// pump.py gtest-param-test.h.pump
+// DO NOT EDIT BY HAND!!!
+
+// Copyright 2008, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Authors: vladl at google.com (Vlad Losev)
+//
+// Macros and functions for implementing parameterized tests
+// in Google C++ Testing Framework (Google Test)
+//
+// This file is generated by a SCRIPT. DO NOT EDIT BY HAND!
+//
+#ifndef GTEST_INCLUDE_GTEST_GTEST_PARAM_TEST_H_
+#define GTEST_INCLUDE_GTEST_GTEST_PARAM_TEST_H_
+
+
+// Value-parameterized tests allow you to test your code with different
+// parameters without writing multiple copies of the same test.
+//
+// Here is how you use value-parameterized tests:
+
+#if 0
+
+// To write value-parameterized tests, first you should define a fixture
+// class. It is usually derived from testing::TestWithParam<T> (see below for
+// another inheritance scheme that's sometimes useful in more complicated
+// class hierarchies), where the type of your parameter values.
+// TestWithParam<T> is itself derived from testing::Test. T can be any
+// copyable type. If it's a raw pointer, you are responsible for managing the
+// lifespan of the pointed values.
+
+class FooTest : public ::testing::TestWithParam<const char*> {
+ // You can implement all the usual class fixture members here.
+};
+
+// Then, use the TEST_P macro to define as many parameterized tests
+// for this fixture as you want. The _P suffix is for "parameterized"
+// or "pattern", whichever you prefer to think.
+
+TEST_P(FooTest, DoesBlah) {
+ // Inside a test, access the test parameter with the GetParam() method
+ // of the TestWithParam<T> class:
+ EXPECT_TRUE(foo.Blah(GetParam()));
+ ...
+}
+
+TEST_P(FooTest, HasBlahBlah) {
+ ...
+}
+
+// Finally, you can use INSTANTIATE_TEST_CASE_P to instantiate the test
+// case with any set of parameters you want. Google Test defines a number
+// of functions for generating test parameters. They return what we call
+// (surprise!) parameter generators. Here is a summary of them, which
+// are all in the testing namespace:
+//
+//
+// Range(begin, end [, step]) - Yields values {begin, begin+step,
+// begin+step+step, ...}. The values do not
+// include end. step defaults to 1.
+// Values(v1, v2, ..., vN) - Yields values {v1, v2, ..., vN}.
+// ValuesIn(container) - Yields values from a C-style array, an STL
+// ValuesIn(begin,end) container, or an iterator range [begin, end).
+// Bool() - Yields sequence {false, true}.
+// Combine(g1, g2, ..., gN) - Yields all combinations (the Cartesian product
+// for the math savvy) of the values generated
+// by the N generators.
+//
+// For more details, see comments at the definitions of these functions below
+// in this file.
+//
+// The following statement will instantiate tests from the FooTest test case
+// each with parameter values "meeny", "miny", and "moe".
+
+INSTANTIATE_TEST_CASE_P(InstantiationName,
+ FooTest,
+ Values("meeny", "miny", "moe"));
+
+// To distinguish different instances of the pattern, (yes, you
+// can instantiate it more then once) the first argument to the
+// INSTANTIATE_TEST_CASE_P macro is a prefix that will be added to the
+// actual test case name. Remember to pick unique prefixes for different
+// instantiations. The tests from the instantiation above will have
+// these names:
+//
+// * InstantiationName/FooTest.DoesBlah/0 for "meeny"
+// * InstantiationName/FooTest.DoesBlah/1 for "miny"
+// * InstantiationName/FooTest.DoesBlah/2 for "moe"
+// * InstantiationName/FooTest.HasBlahBlah/0 for "meeny"
+// * InstantiationName/FooTest.HasBlahBlah/1 for "miny"
+// * InstantiationName/FooTest.HasBlahBlah/2 for "moe"
+//
+// You can use these names in --gtest_filter.
+//
+// This statement will instantiate all tests from FooTest again, each
+// with parameter values "cat" and "dog":
+
+const char* pets[] = {"cat", "dog"};
+INSTANTIATE_TEST_CASE_P(AnotherInstantiationName, FooTest, ValuesIn(pets));
+
+// The tests from the instantiation above will have these names:
+//
+// * AnotherInstantiationName/FooTest.DoesBlah/0 for "cat"
+// * AnotherInstantiationName/FooTest.DoesBlah/1 for "dog"
+// * AnotherInstantiationName/FooTest.HasBlahBlah/0 for "cat"
+// * AnotherInstantiationName/FooTest.HasBlahBlah/1 for "dog"
+//
+// Please note that INSTANTIATE_TEST_CASE_P will instantiate all tests
+// in the given test case, whether their definitions come before or
+// AFTER the INSTANTIATE_TEST_CASE_P statement.
+//
+// Please also note that generator expressions (including parameters to the
+// generators) are evaluated in InitGoogleTest(), after main() has started.
+// This allows the user on one hand, to adjust generator parameters in order
+// to dynamically determine a set of tests to run and on the other hand,
+// give the user a chance to inspect the generated tests with Google Test
+// reflection API before RUN_ALL_TESTS() is executed.
+//
+// You can see samples/sample7_unittest.cc and samples/sample8_unittest.cc
+// for more examples.
+//
+// In the future, we plan to publish the API for defining new parameter
+// generators. But for now this interface remains part of the internal
+// implementation and is subject to change.
+//
+//
+// A parameterized test fixture must be derived from testing::Test and from
+// testing::WithParamInterface<T>, where T is the type of the parameter
+// values. Inheriting from TestWithParam<T> satisfies that requirement because
+// TestWithParam<T> inherits from both Test and WithParamInterface. In more
+// complicated hierarchies, however, it is occasionally useful to inherit
+// separately from Test and WithParamInterface. For example:
+
+class BaseTest : public ::testing::Test {
+ // You can inherit all the usual members for a non-parameterized test
+ // fixture here.
+};
+
+class DerivedTest : public BaseTest, public ::testing::WithParamInterface<int> {
+ // The usual test fixture members go here too.
+};
+
+TEST_F(BaseTest, HasFoo) {
+ // This is an ordinary non-parameterized test.
+}
+
+TEST_P(DerivedTest, DoesBlah) {
+ // GetParam works just the same here as if you inherit from TestWithParam.
+ EXPECT_TRUE(foo.Blah(GetParam()));
+}
+
+#endif // 0
+
+#include "gtest/internal/gtest-port.h"
+
+#if !GTEST_OS_SYMBIAN
+# include <utility>
+#endif
+
+// scripts/fuse_gtest.py depends on gtest's own header being #included
+// *unconditionally*. Therefore these #includes cannot be moved
+// inside #if GTEST_HAS_PARAM_TEST.
+#include "gtest/internal/gtest-internal.h"
+#include "gtest/internal/gtest-param-util.h"
+#include "gtest/internal/gtest-param-util-generated.h"
+
+#if GTEST_HAS_PARAM_TEST
+
+namespace testing {
+
+// Functions producing parameter generators.
+//
+// Google Test uses these generators to produce parameters for value-
+// parameterized tests. When a parameterized test case is instantiated
+// with a particular generator, Google Test creates and runs tests
+// for each element in the sequence produced by the generator.
+//
+// In the following sample, tests from test case FooTest are instantiated
+// each three times with parameter values 3, 5, and 8:
+//
+// class FooTest : public TestWithParam<int> { ... };
+//
+// TEST_P(FooTest, TestThis) {
+// }
+// TEST_P(FooTest, TestThat) {
+// }
+// INSTANTIATE_TEST_CASE_P(TestSequence, FooTest, Values(3, 5, 8));
+//
+
+// Range() returns generators providing sequences of values in a range.
+//
+// Synopsis:
+// Range(start, end)
+// - returns a generator producing a sequence of values {start, start+1,
+// start+2, ..., }.
+// Range(start, end, step)
+// - returns a generator producing a sequence of values {start, start+step,
+// start+step+step, ..., }.
+// Notes:
+// * The generated sequences never include end. For example, Range(1, 5)
+// returns a generator producing a sequence {1, 2, 3, 4}. Range(1, 9, 2)
+// returns a generator producing {1, 3, 5, 7}.
+// * start and end must have the same type. That type may be any integral or
+// floating-point type or a user defined type satisfying these conditions:
+// * It must be assignable (have operator=() defined).
+// * It must have operator+() (operator+(int-compatible type) for
+// two-operand version).
+// * It must have operator<() defined.
+// Elements in the resulting sequences will also have that type.
+// * Condition start < end must be satisfied in order for resulting sequences
+// to contain any elements.
+//
+template <typename T, typename IncrementT>
+internal::ParamGenerator<T> Range(T start, T end, IncrementT step) {
+ return internal::ParamGenerator<T>(
+ new internal::RangeGenerator<T, IncrementT>(start, end, step));
+}
+
+template <typename T>
+internal::ParamGenerator<T> Range(T start, T end) {
+ return Range(start, end, 1);
+}
+
+// ValuesIn() function allows generation of tests with parameters coming from
+// a container.
+//
+// Synopsis:
+// ValuesIn(const T (&array)[N])
+// - returns a generator producing sequences with elements from
+// a C-style array.
+// ValuesIn(const Container& container)
+// - returns a generator producing sequences with elements from
+// an STL-style container.
+// ValuesIn(Iterator begin, Iterator end)
+// - returns a generator producing sequences with elements from
+// a range [begin, end) defined by a pair of STL-style iterators. These
+// iterators can also be plain C pointers.
+//
+// Please note that ValuesIn copies the values from the containers
+// passed in and keeps them to generate tests in RUN_ALL_TESTS().
+//
+// Examples:
+//
+// This instantiates tests from test case StringTest
+// each with C-string values of "foo", "bar", and "baz":
+//
+// const char* strings[] = {"foo", "bar", "baz"};
+// INSTANTIATE_TEST_CASE_P(StringSequence, SrtingTest, ValuesIn(strings));
+//
+// This instantiates tests from test case StlStringTest
+// each with STL strings with values "a" and "b":
+//
+// ::std::vector< ::std::string> GetParameterStrings() {
+// ::std::vector< ::std::string> v;
+// v.push_back("a");
+// v.push_back("b");
+// return v;
+// }
+//
+// INSTANTIATE_TEST_CASE_P(CharSequence,
+// StlStringTest,
+// ValuesIn(GetParameterStrings()));
+//
+//
+// This will also instantiate tests from CharTest
+// each with parameter values 'a' and 'b':
+//
+// ::std::list<char> GetParameterChars() {
+// ::std::list<char> list;
+// list.push_back('a');
+// list.push_back('b');
+// return list;
+// }
+// ::std::list<char> l = GetParameterChars();
+// INSTANTIATE_TEST_CASE_P(CharSequence2,
+// CharTest,
+// ValuesIn(l.begin(), l.end()));
+//
+template <typename ForwardIterator>
+internal::ParamGenerator<
+ typename ::testing::internal::IteratorTraits<ForwardIterator>::value_type>
+ValuesIn(ForwardIterator begin, ForwardIterator end) {
+ typedef typename ::testing::internal::IteratorTraits<ForwardIterator>
+ ::value_type ParamType;
+ return internal::ParamGenerator<ParamType>(
+ new internal::ValuesInIteratorRangeGenerator<ParamType>(begin, end));
+}
+
+template <typename T, size_t N>
+internal::ParamGenerator<T> ValuesIn(const T (&array)[N]) {
+ return ValuesIn(array, array + N);
+}
+
+template <class Container>
+internal::ParamGenerator<typename Container::value_type> ValuesIn(
+ const Container& container) {
+ return ValuesIn(container.begin(), container.end());
+}
+
+// Values() allows generating tests from explicitly specified list of
+// parameters.
+//
+// Synopsis:
+// Values(T v1, T v2, ..., T vN)
+// - returns a generator producing sequences with elements v1, v2, ..., vN.
+//
+// For example, this instantiates tests from test case BarTest each
+// with values "one", "two", and "three":
+//
+// INSTANTIATE_TEST_CASE_P(NumSequence, BarTest, Values("one", "two", "three"));
+//
+// This instantiates tests from test case BazTest each with values 1, 2, 3.5.
+// The exact type of values will depend on the type of parameter in BazTest.
+//
+// INSTANTIATE_TEST_CASE_P(FloatingNumbers, BazTest, Values(1, 2, 3.5));
+//
+// Currently, Values() supports from 1 to 50 parameters.
+//
+template <typename T1>
+internal::ValueArray1<T1> Values(T1 v1) {
+ return internal::ValueArray1<T1>(v1);
+}
+
+template <typename T1, typename T2>
+internal::ValueArray2<T1, T2> Values(T1 v1, T2 v2) {
+ return internal::ValueArray2<T1, T2>(v1, v2);
+}
+
+template <typename T1, typename T2, typename T3>
+internal::ValueArray3<T1, T2, T3> Values(T1 v1, T2 v2, T3 v3) {
+ return internal::ValueArray3<T1, T2, T3>(v1, v2, v3);
+}
+
+template <typename T1, typename T2, typename T3, typename T4>
+internal::ValueArray4<T1, T2, T3, T4> Values(T1 v1, T2 v2, T3 v3, T4 v4) {
+ return internal::ValueArray4<T1, T2, T3, T4>(v1, v2, v3, v4);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5>
+internal::ValueArray5<T1, T2, T3, T4, T5> Values(T1 v1, T2 v2, T3 v3, T4 v4,
+ T5 v5) {
+ return internal::ValueArray5<T1, T2, T3, T4, T5>(v1, v2, v3, v4, v5);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6>
+internal::ValueArray6<T1, T2, T3, T4, T5, T6> Values(T1 v1, T2 v2, T3 v3,
+ T4 v4, T5 v5, T6 v6) {
+ return internal::ValueArray6<T1, T2, T3, T4, T5, T6>(v1, v2, v3, v4, v5, v6);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7>
+internal::ValueArray7<T1, T2, T3, T4, T5, T6, T7> Values(T1 v1, T2 v2, T3 v3,
+ T4 v4, T5 v5, T6 v6, T7 v7) {
+ return internal::ValueArray7<T1, T2, T3, T4, T5, T6, T7>(v1, v2, v3, v4, v5,
+ v6, v7);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8>
+internal::ValueArray8<T1, T2, T3, T4, T5, T6, T7, T8> Values(T1 v1, T2 v2,
+ T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8) {
+ return internal::ValueArray8<T1, T2, T3, T4, T5, T6, T7, T8>(v1, v2, v3, v4,
+ v5, v6, v7, v8);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9>
+internal::ValueArray9<T1, T2, T3, T4, T5, T6, T7, T8, T9> Values(T1 v1, T2 v2,
+ T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9) {
+ return internal::ValueArray9<T1, T2, T3, T4, T5, T6, T7, T8, T9>(v1, v2, v3,
+ v4, v5, v6, v7, v8, v9);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10>
+internal::ValueArray10<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10> Values(T1 v1,
+ T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10) {
+ return internal::ValueArray10<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10>(v1,
+ v2, v3, v4, v5, v6, v7, v8, v9, v10);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11>
+internal::ValueArray11<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10,
+ T11> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11) {
+ return internal::ValueArray11<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10,
+ T11>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12>
+internal::ValueArray12<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12) {
+ return internal::ValueArray12<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13>
+internal::ValueArray13<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13) {
+ return internal::ValueArray13<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14>
+internal::ValueArray14<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14) {
+ return internal::ValueArray14<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13,
+ v14);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15>
+internal::ValueArray15<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8,
+ T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15) {
+ return internal::ValueArray15<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12,
+ v13, v14, v15);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16>
+internal::ValueArray16<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7,
+ T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15,
+ T16 v16) {
+ return internal::ValueArray16<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11,
+ v12, v13, v14, v15, v16);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17>
+internal::ValueArray17<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7,
+ T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15,
+ T16 v16, T17 v17) {
+ return internal::ValueArray17<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10,
+ v11, v12, v13, v14, v15, v16, v17);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18>
+internal::ValueArray18<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6,
+ T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15,
+ T16 v16, T17 v17, T18 v18) {
+ return internal::ValueArray18<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18>(v1, v2, v3, v4, v5, v6, v7, v8, v9,
+ v10, v11, v12, v13, v14, v15, v16, v17, v18);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19>
+internal::ValueArray19<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5,
+ T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14,
+ T15 v15, T16 v16, T17 v17, T18 v18, T19 v19) {
+ return internal::ValueArray19<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19>(v1, v2, v3, v4, v5, v6, v7, v8,
+ v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20>
+internal::ValueArray20<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20> Values(T1 v1, T2 v2, T3 v3, T4 v4,
+ T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13,
+ T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20) {
+ return internal::ValueArray20<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20>(v1, v2, v3, v4, v5, v6, v7,
+ v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21>
+internal::ValueArray21<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21> Values(T1 v1, T2 v2, T3 v3, T4 v4,
+ T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13,
+ T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21) {
+ return internal::ValueArray21<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21>(v1, v2, v3, v4, v5, v6,
+ v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22>
+internal::ValueArray22<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22> Values(T1 v1, T2 v2, T3 v3,
+ T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12,
+ T13 v13, T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20,
+ T21 v21, T22 v22) {
+ return internal::ValueArray22<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22>(v1, v2, v3, v4,
+ v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19,
+ v20, v21, v22);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23>
+internal::ValueArray23<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23> Values(T1 v1, T2 v2,
+ T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12,
+ T13 v13, T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20,
+ T21 v21, T22 v22, T23 v23) {
+ return internal::ValueArray23<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23>(v1, v2, v3,
+ v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19,
+ v20, v21, v22, v23);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24>
+internal::ValueArray24<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24> Values(T1 v1, T2 v2,
+ T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12,
+ T13 v13, T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20,
+ T21 v21, T22 v22, T23 v23, T24 v24) {
+ return internal::ValueArray24<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24>(v1, v2,
+ v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18,
+ v19, v20, v21, v22, v23, v24);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25>
+internal::ValueArray25<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25> Values(T1 v1,
+ T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11,
+ T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19,
+ T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25) {
+ return internal::ValueArray25<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25>(v1,
+ v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17,
+ v18, v19, v20, v21, v22, v23, v24, v25);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26>
+internal::ValueArray26<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26) {
+ return internal::ValueArray26<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15,
+ v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27>
+internal::ValueArray27<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27) {
+ return internal::ValueArray27<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14,
+ v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28>
+internal::ValueArray28<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28) {
+ return internal::ValueArray28<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13,
+ v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27,
+ v28);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29>
+internal::ValueArray29<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29) {
+ return internal::ValueArray29<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12,
+ v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26,
+ v27, v28, v29);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30>
+internal::ValueArray30<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8,
+ T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16,
+ T17 v17, T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24,
+ T25 v25, T26 v26, T27 v27, T28 v28, T29 v29, T30 v30) {
+ return internal::ValueArray30<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11,
+ v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25,
+ v26, v27, v28, v29, v30);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31>
+internal::ValueArray31<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7,
+ T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15,
+ T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23,
+ T24 v24, T25 v25, T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31) {
+ return internal::ValueArray31<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10,
+ v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24,
+ v25, v26, v27, v28, v29, v30, v31);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32>
+internal::ValueArray32<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7,
+ T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15,
+ T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23,
+ T24 v24, T25 v25, T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31,
+ T32 v32) {
+ return internal::ValueArray32<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32>(v1, v2, v3, v4, v5, v6, v7, v8, v9,
+ v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23,
+ v24, v25, v26, v27, v28, v29, v30, v31, v32);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33>
+internal::ValueArray33<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6,
+ T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15,
+ T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23,
+ T24 v24, T25 v25, T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31,
+ T32 v32, T33 v33) {
+ return internal::ValueArray33<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33>(v1, v2, v3, v4, v5, v6, v7, v8,
+ v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23,
+ v24, v25, v26, v27, v28, v29, v30, v31, v32, v33);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34>
+internal::ValueArray34<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5,
+ T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14,
+ T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21, T22 v22,
+ T23 v23, T24 v24, T25 v25, T26 v26, T27 v27, T28 v28, T29 v29, T30 v30,
+ T31 v31, T32 v32, T33 v33, T34 v34) {
+ return internal::ValueArray34<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34>(v1, v2, v3, v4, v5, v6, v7,
+ v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22,
+ v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35>
+internal::ValueArray35<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35> Values(T1 v1, T2 v2, T3 v3, T4 v4,
+ T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13,
+ T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21,
+ T22 v22, T23 v23, T24 v24, T25 v25, T26 v26, T27 v27, T28 v28, T29 v29,
+ T30 v30, T31 v31, T32 v32, T33 v33, T34 v34, T35 v35) {
+ return internal::ValueArray35<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35>(v1, v2, v3, v4, v5, v6,
+ v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21,
+ v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36>
+internal::ValueArray36<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36> Values(T1 v1, T2 v2, T3 v3, T4 v4,
+ T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13,
+ T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21,
+ T22 v22, T23 v23, T24 v24, T25 v25, T26 v26, T27 v27, T28 v28, T29 v29,
+ T30 v30, T31 v31, T32 v32, T33 v33, T34 v34, T35 v35, T36 v36) {
+ return internal::ValueArray36<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36>(v1, v2, v3, v4,
+ v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19,
+ v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33,
+ v34, v35, v36);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37>
+internal::ValueArray37<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37> Values(T1 v1, T2 v2, T3 v3,
+ T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12,
+ T13 v13, T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20,
+ T21 v21, T22 v22, T23 v23, T24 v24, T25 v25, T26 v26, T27 v27, T28 v28,
+ T29 v29, T30 v30, T31 v31, T32 v32, T33 v33, T34 v34, T35 v35, T36 v36,
+ T37 v37) {
+ return internal::ValueArray37<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37>(v1, v2, v3,
+ v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19,
+ v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33,
+ v34, v35, v36, v37);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38>
+internal::ValueArray38<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38> Values(T1 v1, T2 v2,
+ T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12,
+ T13 v13, T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20,
+ T21 v21, T22 v22, T23 v23, T24 v24, T25 v25, T26 v26, T27 v27, T28 v28,
+ T29 v29, T30 v30, T31 v31, T32 v32, T33 v33, T34 v34, T35 v35, T36 v36,
+ T37 v37, T38 v38) {
+ return internal::ValueArray38<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38>(v1, v2,
+ v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18,
+ v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32,
+ v33, v34, v35, v36, v37, v38);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39>
+internal::ValueArray39<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39> Values(T1 v1, T2 v2,
+ T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12,
+ T13 v13, T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20,
+ T21 v21, T22 v22, T23 v23, T24 v24, T25 v25, T26 v26, T27 v27, T28 v28,
+ T29 v29, T30 v30, T31 v31, T32 v32, T33 v33, T34 v34, T35 v35, T36 v36,
+ T37 v37, T38 v38, T39 v39) {
+ return internal::ValueArray39<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39>(v1,
+ v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17,
+ v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31,
+ v32, v33, v34, v35, v36, v37, v38, v39);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40>
+internal::ValueArray40<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40> Values(T1 v1,
+ T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11,
+ T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19,
+ T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25, T26 v26, T27 v27,
+ T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33, T34 v34, T35 v35,
+ T36 v36, T37 v37, T38 v38, T39 v39, T40 v40) {
+ return internal::ValueArray40<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15,
+ v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28, v29,
+ v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41>
+internal::ValueArray41<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41) {
+ return internal::ValueArray41<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40, T41>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14,
+ v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27, v28,
+ v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42>
+internal::ValueArray42<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41,
+ T42> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42) {
+ return internal::ValueArray42<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40, T41, T42>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13,
+ v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26, v27,
+ v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40, v41,
+ v42);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43>
+internal::ValueArray43<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42,
+ T43> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42, T43 v43) {
+ return internal::ValueArray43<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40, T41, T42, T43>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12,
+ v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25, v26,
+ v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39, v40,
+ v41, v42, v43);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44>
+internal::ValueArray44<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42, T43 v43, T44 v44) {
+ return internal::ValueArray44<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40, T41, T42, T43, T44>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11,
+ v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24, v25,
+ v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38, v39,
+ v40, v41, v42, v43, v44);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45>
+internal::ValueArray45<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8,
+ T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16,
+ T17 v17, T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24,
+ T25 v25, T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32,
+ T33 v33, T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40,
+ T41 v41, T42 v42, T43 v43, T44 v44, T45 v45) {
+ return internal::ValueArray45<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40, T41, T42, T43, T44, T45>(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10,
+ v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23, v24,
+ v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37, v38,
+ v39, v40, v41, v42, v43, v44, v45);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46>
+internal::ValueArray46<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45, T46> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7,
+ T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15,
+ T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23,
+ T24 v24, T25 v25, T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31,
+ T32 v32, T33 v33, T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39,
+ T40 v40, T41 v41, T42 v42, T43 v43, T44 v44, T45 v45, T46 v46) {
+ return internal::ValueArray46<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40, T41, T42, T43, T44, T45, T46>(v1, v2, v3, v4, v5, v6, v7, v8, v9,
+ v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23,
+ v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37,
+ v38, v39, v40, v41, v42, v43, v44, v45, v46);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47>
+internal::ValueArray47<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45, T46, T47> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7,
+ T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15,
+ T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23,
+ T24 v24, T25 v25, T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31,
+ T32 v32, T33 v33, T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39,
+ T40 v40, T41 v41, T42 v42, T43 v43, T44 v44, T45 v45, T46 v46, T47 v47) {
+ return internal::ValueArray47<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40, T41, T42, T43, T44, T45, T46, T47>(v1, v2, v3, v4, v5, v6, v7, v8,
+ v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22, v23,
+ v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36, v37,
+ v38, v39, v40, v41, v42, v43, v44, v45, v46, v47);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48>
+internal::ValueArray48<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45, T46, T47, T48> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6,
+ T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15,
+ T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23,
+ T24 v24, T25 v25, T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31,
+ T32 v32, T33 v33, T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39,
+ T40 v40, T41 v41, T42 v42, T43 v43, T44 v44, T45 v45, T46 v46, T47 v47,
+ T48 v48) {
+ return internal::ValueArray48<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40, T41, T42, T43, T44, T45, T46, T47, T48>(v1, v2, v3, v4, v5, v6, v7,
+ v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21, v22,
+ v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35, v36,
+ v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48, typename T49>
+internal::ValueArray49<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45, T46, T47, T48, T49> Values(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5,
+ T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13, T14 v14,
+ T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21, T22 v22,
+ T23 v23, T24 v24, T25 v25, T26 v26, T27 v27, T28 v28, T29 v29, T30 v30,
+ T31 v31, T32 v32, T33 v33, T34 v34, T35 v35, T36 v36, T37 v37, T38 v38,
+ T39 v39, T40 v40, T41 v41, T42 v42, T43 v43, T44 v44, T45 v45, T46 v46,
+ T47 v47, T48 v48, T49 v49) {
+ return internal::ValueArray49<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40, T41, T42, T43, T44, T45, T46, T47, T48, T49>(v1, v2, v3, v4, v5, v6,
+ v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19, v20, v21,
+ v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33, v34, v35,
+ v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47, v48, v49);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48, typename T49, typename T50>
+internal::ValueArray50<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45, T46, T47, T48, T49, T50> Values(T1 v1, T2 v2, T3 v3, T4 v4,
+ T5 v5, T6 v6, T7 v7, T8 v8, T9 v9, T10 v10, T11 v11, T12 v12, T13 v13,
+ T14 v14, T15 v15, T16 v16, T17 v17, T18 v18, T19 v19, T20 v20, T21 v21,
+ T22 v22, T23 v23, T24 v24, T25 v25, T26 v26, T27 v27, T28 v28, T29 v29,
+ T30 v30, T31 v31, T32 v32, T33 v33, T34 v34, T35 v35, T36 v36, T37 v37,
+ T38 v38, T39 v39, T40 v40, T41 v41, T42 v42, T43 v43, T44 v44, T45 v45,
+ T46 v46, T47 v47, T48 v48, T49 v49, T50 v50) {
+ return internal::ValueArray50<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26, T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40, T41, T42, T43, T44, T45, T46, T47, T48, T49, T50>(v1, v2, v3, v4,
+ v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16, v17, v18, v19,
+ v20, v21, v22, v23, v24, v25, v26, v27, v28, v29, v30, v31, v32, v33,
+ v34, v35, v36, v37, v38, v39, v40, v41, v42, v43, v44, v45, v46, v47,
+ v48, v49, v50);
+}
+
+// Bool() allows generating tests with parameters in a set of (false, true).
+//
+// Synopsis:
+// Bool()
+// - returns a generator producing sequences with elements {false, true}.
+//
+// It is useful when testing code that depends on Boolean flags. Combinations
+// of multiple flags can be tested when several Bool()'s are combined using
+// Combine() function.
+//
+// In the following example all tests in the test case FlagDependentTest
+// will be instantiated twice with parameters false and true.
+//
+// class FlagDependentTest : public testing::TestWithParam<bool> {
+// virtual void SetUp() {
+// external_flag = GetParam();
+// }
+// }
+// INSTANTIATE_TEST_CASE_P(BoolSequence, FlagDependentTest, Bool());
+//
+inline internal::ParamGenerator<bool> Bool() {
+ return Values(false, true);
+}
+
+# if GTEST_HAS_COMBINE
+// Combine() allows the user to combine two or more sequences to produce
+// values of a Cartesian product of those sequences' elements.
+//
+// Synopsis:
+// Combine(gen1, gen2, ..., genN)
+// - returns a generator producing sequences with elements coming from
+// the Cartesian product of elements from the sequences generated by
+// gen1, gen2, ..., genN. The sequence elements will have a type of
+// tuple<T1, T2, ..., TN> where T1, T2, ..., TN are the types
+// of elements from sequences produces by gen1, gen2, ..., genN.
+//
+// Combine can have up to 10 arguments. This number is currently limited
+// by the maximum number of elements in the tuple implementation used by Google
+// Test.
+//
+// Example:
+//
+// This will instantiate tests in test case AnimalTest each one with
+// the parameter values tuple("cat", BLACK), tuple("cat", WHITE),
+// tuple("dog", BLACK), and tuple("dog", WHITE):
+//
+// enum Color { BLACK, GRAY, WHITE };
+// class AnimalTest
+// : public testing::TestWithParam<tuple<const char*, Color> > {...};
+//
+// TEST_P(AnimalTest, AnimalLooksNice) {...}
+//
+// INSTANTIATE_TEST_CASE_P(AnimalVariations, AnimalTest,
+// Combine(Values("cat", "dog"),
+// Values(BLACK, WHITE)));
+//
+// This will instantiate tests in FlagDependentTest with all variations of two
+// Boolean flags:
+//
+// class FlagDependentTest
+// : public testing::TestWithParam<tuple<bool, bool> > {
+// virtual void SetUp() {
+// // Assigns external_flag_1 and external_flag_2 values from the tuple.
+// tie(external_flag_1, external_flag_2) = GetParam();
+// }
+// };
+//
+// TEST_P(FlagDependentTest, TestFeature1) {
+// // Test your code using external_flag_1 and external_flag_2 here.
+// }
+// INSTANTIATE_TEST_CASE_P(TwoBoolSequence, FlagDependentTest,
+// Combine(Bool(), Bool()));
+//
+template <typename Generator1, typename Generator2>
+internal::CartesianProductHolder2<Generator1, Generator2> Combine(
+ const Generator1& g1, const Generator2& g2) {
+ return internal::CartesianProductHolder2<Generator1, Generator2>(
+ g1, g2);
+}
+
+template <typename Generator1, typename Generator2, typename Generator3>
+internal::CartesianProductHolder3<Generator1, Generator2, Generator3> Combine(
+ const Generator1& g1, const Generator2& g2, const Generator3& g3) {
+ return internal::CartesianProductHolder3<Generator1, Generator2, Generator3>(
+ g1, g2, g3);
+}
+
+template <typename Generator1, typename Generator2, typename Generator3,
+ typename Generator4>
+internal::CartesianProductHolder4<Generator1, Generator2, Generator3,
+ Generator4> Combine(
+ const Generator1& g1, const Generator2& g2, const Generator3& g3,
+ const Generator4& g4) {
+ return internal::CartesianProductHolder4<Generator1, Generator2, Generator3,
+ Generator4>(
+ g1, g2, g3, g4);
+}
+
+template <typename Generator1, typename Generator2, typename Generator3,
+ typename Generator4, typename Generator5>
+internal::CartesianProductHolder5<Generator1, Generator2, Generator3,
+ Generator4, Generator5> Combine(
+ const Generator1& g1, const Generator2& g2, const Generator3& g3,
+ const Generator4& g4, const Generator5& g5) {
+ return internal::CartesianProductHolder5<Generator1, Generator2, Generator3,
+ Generator4, Generator5>(
+ g1, g2, g3, g4, g5);
+}
+
+template <typename Generator1, typename Generator2, typename Generator3,
+ typename Generator4, typename Generator5, typename Generator6>
+internal::CartesianProductHolder6<Generator1, Generator2, Generator3,
+ Generator4, Generator5, Generator6> Combine(
+ const Generator1& g1, const Generator2& g2, const Generator3& g3,
+ const Generator4& g4, const Generator5& g5, const Generator6& g6) {
+ return internal::CartesianProductHolder6<Generator1, Generator2, Generator3,
+ Generator4, Generator5, Generator6>(
+ g1, g2, g3, g4, g5, g6);
+}
+
+template <typename Generator1, typename Generator2, typename Generator3,
+ typename Generator4, typename Generator5, typename Generator6,
+ typename Generator7>
+internal::CartesianProductHolder7<Generator1, Generator2, Generator3,
+ Generator4, Generator5, Generator6, Generator7> Combine(
+ const Generator1& g1, const Generator2& g2, const Generator3& g3,
+ const Generator4& g4, const Generator5& g5, const Generator6& g6,
+ const Generator7& g7) {
+ return internal::CartesianProductHolder7<Generator1, Generator2, Generator3,
+ Generator4, Generator5, Generator6, Generator7>(
+ g1, g2, g3, g4, g5, g6, g7);
+}
+
+template <typename Generator1, typename Generator2, typename Generator3,
+ typename Generator4, typename Generator5, typename Generator6,
+ typename Generator7, typename Generator8>
+internal::CartesianProductHolder8<Generator1, Generator2, Generator3,
+ Generator4, Generator5, Generator6, Generator7, Generator8> Combine(
+ const Generator1& g1, const Generator2& g2, const Generator3& g3,
+ const Generator4& g4, const Generator5& g5, const Generator6& g6,
+ const Generator7& g7, const Generator8& g8) {
+ return internal::CartesianProductHolder8<Generator1, Generator2, Generator3,
+ Generator4, Generator5, Generator6, Generator7, Generator8>(
+ g1, g2, g3, g4, g5, g6, g7, g8);
+}
+
+template <typename Generator1, typename Generator2, typename Generator3,
+ typename Generator4, typename Generator5, typename Generator6,
+ typename Generator7, typename Generator8, typename Generator9>
+internal::CartesianProductHolder9<Generator1, Generator2, Generator3,
+ Generator4, Generator5, Generator6, Generator7, Generator8,
+ Generator9> Combine(
+ const Generator1& g1, const Generator2& g2, const Generator3& g3,
+ const Generator4& g4, const Generator5& g5, const Generator6& g6,
+ const Generator7& g7, const Generator8& g8, const Generator9& g9) {
+ return internal::CartesianProductHolder9<Generator1, Generator2, Generator3,
+ Generator4, Generator5, Generator6, Generator7, Generator8, Generator9>(
+ g1, g2, g3, g4, g5, g6, g7, g8, g9);
+}
+
+template <typename Generator1, typename Generator2, typename Generator3,
+ typename Generator4, typename Generator5, typename Generator6,
+ typename Generator7, typename Generator8, typename Generator9,
+ typename Generator10>
+internal::CartesianProductHolder10<Generator1, Generator2, Generator3,
+ Generator4, Generator5, Generator6, Generator7, Generator8, Generator9,
+ Generator10> Combine(
+ const Generator1& g1, const Generator2& g2, const Generator3& g3,
+ const Generator4& g4, const Generator5& g5, const Generator6& g6,
+ const Generator7& g7, const Generator8& g8, const Generator9& g9,
+ const Generator10& g10) {
+ return internal::CartesianProductHolder10<Generator1, Generator2, Generator3,
+ Generator4, Generator5, Generator6, Generator7, Generator8, Generator9,
+ Generator10>(
+ g1, g2, g3, g4, g5, g6, g7, g8, g9, g10);
+}
+# endif // GTEST_HAS_COMBINE
+
+
+
+# define TEST_P(test_case_name, test_name) \
+ class GTEST_TEST_CLASS_NAME_(test_case_name, test_name) \
+ : public test_case_name { \
+ public: \
+ GTEST_TEST_CLASS_NAME_(test_case_name, test_name)() {} \
+ virtual void TestBody(); \
+ private: \
+ static int AddToRegistry() { \
+ ::testing::UnitTest::GetInstance()->parameterized_test_registry(). \
+ GetTestCasePatternHolder<test_case_name>(\
+ #test_case_name, __FILE__, __LINE__)->AddTestPattern(\
+ #test_case_name, \
+ #test_name, \
+ new ::testing::internal::TestMetaFactory< \
+ GTEST_TEST_CLASS_NAME_(test_case_name, test_name)>()); \
+ return 0; \
+ } \
+ static int gtest_registering_dummy_; \
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(\
+ GTEST_TEST_CLASS_NAME_(test_case_name, test_name)); \
+ }; \
+ int GTEST_TEST_CLASS_NAME_(test_case_name, \
+ test_name)::gtest_registering_dummy_ = \
+ GTEST_TEST_CLASS_NAME_(test_case_name, test_name)::AddToRegistry(); \
+ void GTEST_TEST_CLASS_NAME_(test_case_name, test_name)::TestBody()
+
+# define INSTANTIATE_TEST_CASE_P(prefix, test_case_name, generator) \
+ ::testing::internal::ParamGenerator<test_case_name::ParamType> \
+ gtest_##prefix##test_case_name##_EvalGenerator_() { return generator; } \
+ int gtest_##prefix##test_case_name##_dummy_ = \
+ ::testing::UnitTest::GetInstance()->parameterized_test_registry(). \
+ GetTestCasePatternHolder<test_case_name>(\
+ #test_case_name, __FILE__, __LINE__)->AddTestCaseInstantiation(\
+ #prefix, \
+ >est_##prefix##test_case_name##_EvalGenerator_, \
+ __FILE__, __LINE__)
+
+} // namespace testing
+
+#endif // GTEST_HAS_PARAM_TEST
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_PARAM_TEST_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-param-test.h.pump b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-param-test.h.pump
new file mode 100644
index 0000000..2dc9303
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-param-test.h.pump
@@ -0,0 +1,487 @@
+$$ -*- mode: c++; -*-
+$var n = 50 $$ Maximum length of Values arguments we want to support.
+$var maxtuple = 10 $$ Maximum number of Combine arguments we want to support.
+// Copyright 2008, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Authors: vladl at google.com (Vlad Losev)
+//
+// Macros and functions for implementing parameterized tests
+// in Google C++ Testing Framework (Google Test)
+//
+// This file is generated by a SCRIPT. DO NOT EDIT BY HAND!
+//
+#ifndef GTEST_INCLUDE_GTEST_GTEST_PARAM_TEST_H_
+#define GTEST_INCLUDE_GTEST_GTEST_PARAM_TEST_H_
+
+
+// Value-parameterized tests allow you to test your code with different
+// parameters without writing multiple copies of the same test.
+//
+// Here is how you use value-parameterized tests:
+
+#if 0
+
+// To write value-parameterized tests, first you should define a fixture
+// class. It is usually derived from testing::TestWithParam<T> (see below for
+// another inheritance scheme that's sometimes useful in more complicated
+// class hierarchies), where the type of your parameter values.
+// TestWithParam<T> is itself derived from testing::Test. T can be any
+// copyable type. If it's a raw pointer, you are responsible for managing the
+// lifespan of the pointed values.
+
+class FooTest : public ::testing::TestWithParam<const char*> {
+ // You can implement all the usual class fixture members here.
+};
+
+// Then, use the TEST_P macro to define as many parameterized tests
+// for this fixture as you want. The _P suffix is for "parameterized"
+// or "pattern", whichever you prefer to think.
+
+TEST_P(FooTest, DoesBlah) {
+ // Inside a test, access the test parameter with the GetParam() method
+ // of the TestWithParam<T> class:
+ EXPECT_TRUE(foo.Blah(GetParam()));
+ ...
+}
+
+TEST_P(FooTest, HasBlahBlah) {
+ ...
+}
+
+// Finally, you can use INSTANTIATE_TEST_CASE_P to instantiate the test
+// case with any set of parameters you want. Google Test defines a number
+// of functions for generating test parameters. They return what we call
+// (surprise!) parameter generators. Here is a summary of them, which
+// are all in the testing namespace:
+//
+//
+// Range(begin, end [, step]) - Yields values {begin, begin+step,
+// begin+step+step, ...}. The values do not
+// include end. step defaults to 1.
+// Values(v1, v2, ..., vN) - Yields values {v1, v2, ..., vN}.
+// ValuesIn(container) - Yields values from a C-style array, an STL
+// ValuesIn(begin,end) container, or an iterator range [begin, end).
+// Bool() - Yields sequence {false, true}.
+// Combine(g1, g2, ..., gN) - Yields all combinations (the Cartesian product
+// for the math savvy) of the values generated
+// by the N generators.
+//
+// For more details, see comments at the definitions of these functions below
+// in this file.
+//
+// The following statement will instantiate tests from the FooTest test case
+// each with parameter values "meeny", "miny", and "moe".
+
+INSTANTIATE_TEST_CASE_P(InstantiationName,
+ FooTest,
+ Values("meeny", "miny", "moe"));
+
+// To distinguish different instances of the pattern, (yes, you
+// can instantiate it more then once) the first argument to the
+// INSTANTIATE_TEST_CASE_P macro is a prefix that will be added to the
+// actual test case name. Remember to pick unique prefixes for different
+// instantiations. The tests from the instantiation above will have
+// these names:
+//
+// * InstantiationName/FooTest.DoesBlah/0 for "meeny"
+// * InstantiationName/FooTest.DoesBlah/1 for "miny"
+// * InstantiationName/FooTest.DoesBlah/2 for "moe"
+// * InstantiationName/FooTest.HasBlahBlah/0 for "meeny"
+// * InstantiationName/FooTest.HasBlahBlah/1 for "miny"
+// * InstantiationName/FooTest.HasBlahBlah/2 for "moe"
+//
+// You can use these names in --gtest_filter.
+//
+// This statement will instantiate all tests from FooTest again, each
+// with parameter values "cat" and "dog":
+
+const char* pets[] = {"cat", "dog"};
+INSTANTIATE_TEST_CASE_P(AnotherInstantiationName, FooTest, ValuesIn(pets));
+
+// The tests from the instantiation above will have these names:
+//
+// * AnotherInstantiationName/FooTest.DoesBlah/0 for "cat"
+// * AnotherInstantiationName/FooTest.DoesBlah/1 for "dog"
+// * AnotherInstantiationName/FooTest.HasBlahBlah/0 for "cat"
+// * AnotherInstantiationName/FooTest.HasBlahBlah/1 for "dog"
+//
+// Please note that INSTANTIATE_TEST_CASE_P will instantiate all tests
+// in the given test case, whether their definitions come before or
+// AFTER the INSTANTIATE_TEST_CASE_P statement.
+//
+// Please also note that generator expressions (including parameters to the
+// generators) are evaluated in InitGoogleTest(), after main() has started.
+// This allows the user on one hand, to adjust generator parameters in order
+// to dynamically determine a set of tests to run and on the other hand,
+// give the user a chance to inspect the generated tests with Google Test
+// reflection API before RUN_ALL_TESTS() is executed.
+//
+// You can see samples/sample7_unittest.cc and samples/sample8_unittest.cc
+// for more examples.
+//
+// In the future, we plan to publish the API for defining new parameter
+// generators. But for now this interface remains part of the internal
+// implementation and is subject to change.
+//
+//
+// A parameterized test fixture must be derived from testing::Test and from
+// testing::WithParamInterface<T>, where T is the type of the parameter
+// values. Inheriting from TestWithParam<T> satisfies that requirement because
+// TestWithParam<T> inherits from both Test and WithParamInterface. In more
+// complicated hierarchies, however, it is occasionally useful to inherit
+// separately from Test and WithParamInterface. For example:
+
+class BaseTest : public ::testing::Test {
+ // You can inherit all the usual members for a non-parameterized test
+ // fixture here.
+};
+
+class DerivedTest : public BaseTest, public ::testing::WithParamInterface<int> {
+ // The usual test fixture members go here too.
+};
+
+TEST_F(BaseTest, HasFoo) {
+ // This is an ordinary non-parameterized test.
+}
+
+TEST_P(DerivedTest, DoesBlah) {
+ // GetParam works just the same here as if you inherit from TestWithParam.
+ EXPECT_TRUE(foo.Blah(GetParam()));
+}
+
+#endif // 0
+
+#include "gtest/internal/gtest-port.h"
+
+#if !GTEST_OS_SYMBIAN
+# include <utility>
+#endif
+
+// scripts/fuse_gtest.py depends on gtest's own header being #included
+// *unconditionally*. Therefore these #includes cannot be moved
+// inside #if GTEST_HAS_PARAM_TEST.
+#include "gtest/internal/gtest-internal.h"
+#include "gtest/internal/gtest-param-util.h"
+#include "gtest/internal/gtest-param-util-generated.h"
+
+#if GTEST_HAS_PARAM_TEST
+
+namespace testing {
+
+// Functions producing parameter generators.
+//
+// Google Test uses these generators to produce parameters for value-
+// parameterized tests. When a parameterized test case is instantiated
+// with a particular generator, Google Test creates and runs tests
+// for each element in the sequence produced by the generator.
+//
+// In the following sample, tests from test case FooTest are instantiated
+// each three times with parameter values 3, 5, and 8:
+//
+// class FooTest : public TestWithParam<int> { ... };
+//
+// TEST_P(FooTest, TestThis) {
+// }
+// TEST_P(FooTest, TestThat) {
+// }
+// INSTANTIATE_TEST_CASE_P(TestSequence, FooTest, Values(3, 5, 8));
+//
+
+// Range() returns generators providing sequences of values in a range.
+//
+// Synopsis:
+// Range(start, end)
+// - returns a generator producing a sequence of values {start, start+1,
+// start+2, ..., }.
+// Range(start, end, step)
+// - returns a generator producing a sequence of values {start, start+step,
+// start+step+step, ..., }.
+// Notes:
+// * The generated sequences never include end. For example, Range(1, 5)
+// returns a generator producing a sequence {1, 2, 3, 4}. Range(1, 9, 2)
+// returns a generator producing {1, 3, 5, 7}.
+// * start and end must have the same type. That type may be any integral or
+// floating-point type or a user defined type satisfying these conditions:
+// * It must be assignable (have operator=() defined).
+// * It must have operator+() (operator+(int-compatible type) for
+// two-operand version).
+// * It must have operator<() defined.
+// Elements in the resulting sequences will also have that type.
+// * Condition start < end must be satisfied in order for resulting sequences
+// to contain any elements.
+//
+template <typename T, typename IncrementT>
+internal::ParamGenerator<T> Range(T start, T end, IncrementT step) {
+ return internal::ParamGenerator<T>(
+ new internal::RangeGenerator<T, IncrementT>(start, end, step));
+}
+
+template <typename T>
+internal::ParamGenerator<T> Range(T start, T end) {
+ return Range(start, end, 1);
+}
+
+// ValuesIn() function allows generation of tests with parameters coming from
+// a container.
+//
+// Synopsis:
+// ValuesIn(const T (&array)[N])
+// - returns a generator producing sequences with elements from
+// a C-style array.
+// ValuesIn(const Container& container)
+// - returns a generator producing sequences with elements from
+// an STL-style container.
+// ValuesIn(Iterator begin, Iterator end)
+// - returns a generator producing sequences with elements from
+// a range [begin, end) defined by a pair of STL-style iterators. These
+// iterators can also be plain C pointers.
+//
+// Please note that ValuesIn copies the values from the containers
+// passed in and keeps them to generate tests in RUN_ALL_TESTS().
+//
+// Examples:
+//
+// This instantiates tests from test case StringTest
+// each with C-string values of "foo", "bar", and "baz":
+//
+// const char* strings[] = {"foo", "bar", "baz"};
+// INSTANTIATE_TEST_CASE_P(StringSequence, SrtingTest, ValuesIn(strings));
+//
+// This instantiates tests from test case StlStringTest
+// each with STL strings with values "a" and "b":
+//
+// ::std::vector< ::std::string> GetParameterStrings() {
+// ::std::vector< ::std::string> v;
+// v.push_back("a");
+// v.push_back("b");
+// return v;
+// }
+//
+// INSTANTIATE_TEST_CASE_P(CharSequence,
+// StlStringTest,
+// ValuesIn(GetParameterStrings()));
+//
+//
+// This will also instantiate tests from CharTest
+// each with parameter values 'a' and 'b':
+//
+// ::std::list<char> GetParameterChars() {
+// ::std::list<char> list;
+// list.push_back('a');
+// list.push_back('b');
+// return list;
+// }
+// ::std::list<char> l = GetParameterChars();
+// INSTANTIATE_TEST_CASE_P(CharSequence2,
+// CharTest,
+// ValuesIn(l.begin(), l.end()));
+//
+template <typename ForwardIterator>
+internal::ParamGenerator<
+ typename ::testing::internal::IteratorTraits<ForwardIterator>::value_type>
+ValuesIn(ForwardIterator begin, ForwardIterator end) {
+ typedef typename ::testing::internal::IteratorTraits<ForwardIterator>
+ ::value_type ParamType;
+ return internal::ParamGenerator<ParamType>(
+ new internal::ValuesInIteratorRangeGenerator<ParamType>(begin, end));
+}
+
+template <typename T, size_t N>
+internal::ParamGenerator<T> ValuesIn(const T (&array)[N]) {
+ return ValuesIn(array, array + N);
+}
+
+template <class Container>
+internal::ParamGenerator<typename Container::value_type> ValuesIn(
+ const Container& container) {
+ return ValuesIn(container.begin(), container.end());
+}
+
+// Values() allows generating tests from explicitly specified list of
+// parameters.
+//
+// Synopsis:
+// Values(T v1, T v2, ..., T vN)
+// - returns a generator producing sequences with elements v1, v2, ..., vN.
+//
+// For example, this instantiates tests from test case BarTest each
+// with values "one", "two", and "three":
+//
+// INSTANTIATE_TEST_CASE_P(NumSequence, BarTest, Values("one", "two", "three"));
+//
+// This instantiates tests from test case BazTest each with values 1, 2, 3.5.
+// The exact type of values will depend on the type of parameter in BazTest.
+//
+// INSTANTIATE_TEST_CASE_P(FloatingNumbers, BazTest, Values(1, 2, 3.5));
+//
+// Currently, Values() supports from 1 to $n parameters.
+//
+$range i 1..n
+$for i [[
+$range j 1..i
+
+template <$for j, [[typename T$j]]>
+internal::ValueArray$i<$for j, [[T$j]]> Values($for j, [[T$j v$j]]) {
+ return internal::ValueArray$i<$for j, [[T$j]]>($for j, [[v$j]]);
+}
+
+]]
+
+// Bool() allows generating tests with parameters in a set of (false, true).
+//
+// Synopsis:
+// Bool()
+// - returns a generator producing sequences with elements {false, true}.
+//
+// It is useful when testing code that depends on Boolean flags. Combinations
+// of multiple flags can be tested when several Bool()'s are combined using
+// Combine() function.
+//
+// In the following example all tests in the test case FlagDependentTest
+// will be instantiated twice with parameters false and true.
+//
+// class FlagDependentTest : public testing::TestWithParam<bool> {
+// virtual void SetUp() {
+// external_flag = GetParam();
+// }
+// }
+// INSTANTIATE_TEST_CASE_P(BoolSequence, FlagDependentTest, Bool());
+//
+inline internal::ParamGenerator<bool> Bool() {
+ return Values(false, true);
+}
+
+# if GTEST_HAS_COMBINE
+// Combine() allows the user to combine two or more sequences to produce
+// values of a Cartesian product of those sequences' elements.
+//
+// Synopsis:
+// Combine(gen1, gen2, ..., genN)
+// - returns a generator producing sequences with elements coming from
+// the Cartesian product of elements from the sequences generated by
+// gen1, gen2, ..., genN. The sequence elements will have a type of
+// tuple<T1, T2, ..., TN> where T1, T2, ..., TN are the types
+// of elements from sequences produces by gen1, gen2, ..., genN.
+//
+// Combine can have up to $maxtuple arguments. This number is currently limited
+// by the maximum number of elements in the tuple implementation used by Google
+// Test.
+//
+// Example:
+//
+// This will instantiate tests in test case AnimalTest each one with
+// the parameter values tuple("cat", BLACK), tuple("cat", WHITE),
+// tuple("dog", BLACK), and tuple("dog", WHITE):
+//
+// enum Color { BLACK, GRAY, WHITE };
+// class AnimalTest
+// : public testing::TestWithParam<tuple<const char*, Color> > {...};
+//
+// TEST_P(AnimalTest, AnimalLooksNice) {...}
+//
+// INSTANTIATE_TEST_CASE_P(AnimalVariations, AnimalTest,
+// Combine(Values("cat", "dog"),
+// Values(BLACK, WHITE)));
+//
+// This will instantiate tests in FlagDependentTest with all variations of two
+// Boolean flags:
+//
+// class FlagDependentTest
+// : public testing::TestWithParam<tuple<bool, bool> > {
+// virtual void SetUp() {
+// // Assigns external_flag_1 and external_flag_2 values from the tuple.
+// tie(external_flag_1, external_flag_2) = GetParam();
+// }
+// };
+//
+// TEST_P(FlagDependentTest, TestFeature1) {
+// // Test your code using external_flag_1 and external_flag_2 here.
+// }
+// INSTANTIATE_TEST_CASE_P(TwoBoolSequence, FlagDependentTest,
+// Combine(Bool(), Bool()));
+//
+$range i 2..maxtuple
+$for i [[
+$range j 1..i
+
+template <$for j, [[typename Generator$j]]>
+internal::CartesianProductHolder$i<$for j, [[Generator$j]]> Combine(
+ $for j, [[const Generator$j& g$j]]) {
+ return internal::CartesianProductHolder$i<$for j, [[Generator$j]]>(
+ $for j, [[g$j]]);
+}
+
+]]
+# endif // GTEST_HAS_COMBINE
+
+
+
+# define TEST_P(test_case_name, test_name) \
+ class GTEST_TEST_CLASS_NAME_(test_case_name, test_name) \
+ : public test_case_name { \
+ public: \
+ GTEST_TEST_CLASS_NAME_(test_case_name, test_name)() {} \
+ virtual void TestBody(); \
+ private: \
+ static int AddToRegistry() { \
+ ::testing::UnitTest::GetInstance()->parameterized_test_registry(). \
+ GetTestCasePatternHolder<test_case_name>(\
+ #test_case_name, __FILE__, __LINE__)->AddTestPattern(\
+ #test_case_name, \
+ #test_name, \
+ new ::testing::internal::TestMetaFactory< \
+ GTEST_TEST_CLASS_NAME_(test_case_name, test_name)>()); \
+ return 0; \
+ } \
+ static int gtest_registering_dummy_; \
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(\
+ GTEST_TEST_CLASS_NAME_(test_case_name, test_name)); \
+ }; \
+ int GTEST_TEST_CLASS_NAME_(test_case_name, \
+ test_name)::gtest_registering_dummy_ = \
+ GTEST_TEST_CLASS_NAME_(test_case_name, test_name)::AddToRegistry(); \
+ void GTEST_TEST_CLASS_NAME_(test_case_name, test_name)::TestBody()
+
+# define INSTANTIATE_TEST_CASE_P(prefix, test_case_name, generator) \
+ ::testing::internal::ParamGenerator<test_case_name::ParamType> \
+ gtest_##prefix##test_case_name##_EvalGenerator_() { return generator; } \
+ int gtest_##prefix##test_case_name##_dummy_ = \
+ ::testing::UnitTest::GetInstance()->parameterized_test_registry(). \
+ GetTestCasePatternHolder<test_case_name>(\
+ #test_case_name, __FILE__, __LINE__)->AddTestCaseInstantiation(\
+ #prefix, \
+ >est_##prefix##test_case_name##_EvalGenerator_, \
+ __FILE__, __LINE__)
+
+} // namespace testing
+
+#endif // GTEST_HAS_PARAM_TEST
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_PARAM_TEST_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-printers.h b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-printers.h
new file mode 100644
index 0000000..b00f91e
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-printers.h
@@ -0,0 +1,856 @@
+// Copyright 2007, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+
+// Google Test - The Google C++ Testing Framework
+//
+// This file implements a universal value printer that can print a
+// value of any type T:
+//
+// void ::testing::internal::UniversalPrinter<T>::Print(value, ostream_ptr);
+//
+// A user can teach this function how to print a class type T by
+// defining either operator<<() or PrintTo() in the namespace that
+// defines T. More specifically, the FIRST defined function in the
+// following list will be used (assuming T is defined in namespace
+// foo):
+//
+// 1. foo::PrintTo(const T&, ostream*)
+// 2. operator<<(ostream&, const T&) defined in either foo or the
+// global namespace.
+//
+// If none of the above is defined, it will print the debug string of
+// the value if it is a protocol buffer, or print the raw bytes in the
+// value otherwise.
+//
+// To aid debugging: when T is a reference type, the address of the
+// value is also printed; when T is a (const) char pointer, both the
+// pointer value and the NUL-terminated string it points to are
+// printed.
+//
+// We also provide some convenient wrappers:
+//
+// // Prints a value to a string. For a (const or not) char
+// // pointer, the NUL-terminated string (but not the pointer) is
+// // printed.
+// std::string ::testing::PrintToString(const T& value);
+//
+// // Prints a value tersely: for a reference type, the referenced
+// // value (but not the address) is printed; for a (const or not) char
+// // pointer, the NUL-terminated string (but not the pointer) is
+// // printed.
+// void ::testing::internal::UniversalTersePrint(const T& value, ostream*);
+//
+// // Prints value using the type inferred by the compiler. The difference
+// // from UniversalTersePrint() is that this function prints both the
+// // pointer and the NUL-terminated string for a (const or not) char pointer.
+// void ::testing::internal::UniversalPrint(const T& value, ostream*);
+//
+// // Prints the fields of a tuple tersely to a string vector, one
+// // element for each field. Tuple support must be enabled in
+// // gtest-port.h.
+// std::vector<string> UniversalTersePrintTupleFieldsToStrings(
+// const Tuple& value);
+//
+// Known limitation:
+//
+// The print primitives print the elements of an STL-style container
+// using the compiler-inferred type of *iter where iter is a
+// const_iterator of the container. When const_iterator is an input
+// iterator but not a forward iterator, this inferred type may not
+// match value_type, and the print output may be incorrect. In
+// practice, this is rarely a problem as for most containers
+// const_iterator is a forward iterator. We'll fix this if there's an
+// actual need for it. Note that this fix cannot rely on value_type
+// being defined as many user-defined container types don't have
+// value_type.
+
+#ifndef GTEST_INCLUDE_GTEST_GTEST_PRINTERS_H_
+#define GTEST_INCLUDE_GTEST_GTEST_PRINTERS_H_
+#pragma GCC system_header
+
+#include <ostream> // NOLINT
+#include <sstream>
+#include <string>
+#include <utility>
+#include <vector>
+#include "gtest/internal/gtest-port.h"
+#include "gtest/internal/gtest-internal.h"
+
+namespace testing {
+
+// Definitions in the 'internal' and 'internal2' name spaces are
+// subject to change without notice. DO NOT USE THEM IN USER CODE!
+namespace internal2 {
+
+// Prints the given number of bytes in the given object to the given
+// ostream.
+GTEST_API_ void PrintBytesInObjectTo(const unsigned char* obj_bytes,
+ size_t count,
+ ::std::ostream* os);
+
+// For selecting which printer to use when a given type has neither <<
+// nor PrintTo().
+enum TypeKind {
+ kProtobuf, // a protobuf type
+ kConvertibleToInteger, // a type implicitly convertible to BiggestInt
+ // (e.g. a named or unnamed enum type)
+ kOtherType // anything else
+};
+
+// TypeWithoutFormatter<T, kTypeKind>::PrintValue(value, os) is called
+// by the universal printer to print a value of type T when neither
+// operator<< nor PrintTo() is defined for T, where kTypeKind is the
+// "kind" of T as defined by enum TypeKind.
+template <typename T, TypeKind kTypeKind>
+class TypeWithoutFormatter {
+ public:
+ // This default version is called when kTypeKind is kOtherType.
+ static void PrintValue(const T& value, ::std::ostream* os) {
+ PrintBytesInObjectTo(reinterpret_cast<const unsigned char*>(&value),
+ sizeof(value), os);
+ }
+};
+
+// We print a protobuf using its ShortDebugString() when the string
+// doesn't exceed this many characters; otherwise we print it using
+// DebugString() for better readability.
+const size_t kProtobufOneLinerMaxLength = 50;
+
+template <typename T>
+class TypeWithoutFormatter<T, kProtobuf> {
+ public:
+ static void PrintValue(const T& value, ::std::ostream* os) {
+ const ::testing::internal::string short_str = value.ShortDebugString();
+ const ::testing::internal::string pretty_str =
+ short_str.length() <= kProtobufOneLinerMaxLength ?
+ short_str : ("\n" + value.DebugString());
+ *os << ("<" + pretty_str + ">");
+ }
+};
+
+template <typename T>
+class TypeWithoutFormatter<T, kConvertibleToInteger> {
+ public:
+ // Since T has no << operator or PrintTo() but can be implicitly
+ // converted to BiggestInt, we print it as a BiggestInt.
+ //
+ // Most likely T is an enum type (either named or unnamed), in which
+ // case printing it as an integer is the desired behavior. In case
+ // T is not an enum, printing it as an integer is the best we can do
+ // given that it has no user-defined printer.
+ static void PrintValue(const T& value, ::std::ostream* os) {
+ const internal::BiggestInt kBigInt = value;
+ *os << kBigInt;
+ }
+};
+
+// Prints the given value to the given ostream. If the value is a
+// protocol message, its debug string is printed; if it's an enum or
+// of a type implicitly convertible to BiggestInt, it's printed as an
+// integer; otherwise the bytes in the value are printed. This is
+// what UniversalPrinter<T>::Print() does when it knows nothing about
+// type T and T has neither << operator nor PrintTo().
+//
+// A user can override this behavior for a class type Foo by defining
+// a << operator in the namespace where Foo is defined.
+//
+// We put this operator in namespace 'internal2' instead of 'internal'
+// to simplify the implementation, as much code in 'internal' needs to
+// use << in STL, which would conflict with our own << were it defined
+// in 'internal'.
+//
+// Note that this operator<< takes a generic std::basic_ostream<Char,
+// CharTraits> type instead of the more restricted std::ostream. If
+// we define it to take an std::ostream instead, we'll get an
+// "ambiguous overloads" compiler error when trying to print a type
+// Foo that supports streaming to std::basic_ostream<Char,
+// CharTraits>, as the compiler cannot tell whether
+// operator<<(std::ostream&, const T&) or
+// operator<<(std::basic_stream<Char, CharTraits>, const Foo&) is more
+// specific.
+template <typename Char, typename CharTraits, typename T>
+::std::basic_ostream<Char, CharTraits>& operator<<(
+ ::std::basic_ostream<Char, CharTraits>& os, const T& x) {
+ TypeWithoutFormatter<T,
+ (internal::IsAProtocolMessage<T>::value ? kProtobuf :
+ internal::ImplicitlyConvertible<const T&, internal::BiggestInt>::value ?
+ kConvertibleToInteger : kOtherType)>::PrintValue(x, &os);
+ return os;
+}
+
+} // namespace internal2
+} // namespace testing
+
+// This namespace MUST NOT BE NESTED IN ::testing, or the name look-up
+// magic needed for implementing UniversalPrinter won't work.
+namespace testing_internal {
+
+// Used to print a value that is not an STL-style container when the
+// user doesn't define PrintTo() for it.
+template <typename T>
+void DefaultPrintNonContainerTo(const T& value, ::std::ostream* os) {
+ // With the following statement, during unqualified name lookup,
+ // testing::internal2::operator<< appears as if it was declared in
+ // the nearest enclosing namespace that contains both
+ // ::testing_internal and ::testing::internal2, i.e. the global
+ // namespace. For more details, refer to the C++ Standard section
+ // 7.3.4-1 [namespace.udir]. This allows us to fall back onto
+ // testing::internal2::operator<< in case T doesn't come with a <<
+ // operator.
+ //
+ // We cannot write 'using ::testing::internal2::operator<<;', which
+ // gcc 3.3 fails to compile due to a compiler bug.
+ using namespace ::testing::internal2; // NOLINT
+
+ // Assuming T is defined in namespace foo, in the next statement,
+ // the compiler will consider all of:
+ //
+ // 1. foo::operator<< (thanks to Koenig look-up),
+ // 2. ::operator<< (as the current namespace is enclosed in ::),
+ // 3. testing::internal2::operator<< (thanks to the using statement above).
+ //
+ // The operator<< whose type matches T best will be picked.
+ //
+ // We deliberately allow #2 to be a candidate, as sometimes it's
+ // impossible to define #1 (e.g. when foo is ::std, defining
+ // anything in it is undefined behavior unless you are a compiler
+ // vendor.).
+ *os << value;
+}
+
+} // namespace testing_internal
+
+namespace testing {
+namespace internal {
+
+// UniversalPrinter<T>::Print(value, ostream_ptr) prints the given
+// value to the given ostream. The caller must ensure that
+// 'ostream_ptr' is not NULL, or the behavior is undefined.
+//
+// We define UniversalPrinter as a class template (as opposed to a
+// function template), as we need to partially specialize it for
+// reference types, which cannot be done with function templates.
+template <typename T>
+class UniversalPrinter;
+
+template <typename T>
+void UniversalPrint(const T& value, ::std::ostream* os);
+
+// Used to print an STL-style container when the user doesn't define
+// a PrintTo() for it.
+template <typename C>
+void DefaultPrintTo(IsContainer /* dummy */,
+ false_type /* is not a pointer */,
+ const C& container, ::std::ostream* os) {
+ const size_t kMaxCount = 32; // The maximum number of elements to print.
+ *os << '{';
+ size_t count = 0;
+ for (typename C::const_iterator it = container.begin();
+ it != container.end(); ++it, ++count) {
+ if (count > 0) {
+ *os << ',';
+ if (count == kMaxCount) { // Enough has been printed.
+ *os << " ...";
+ break;
+ }
+ }
+ *os << ' ';
+ // We cannot call PrintTo(*it, os) here as PrintTo() doesn't
+ // handle *it being a native array.
+ internal::UniversalPrint(*it, os);
+ }
+
+ if (count > 0) {
+ *os << ' ';
+ }
+ *os << '}';
+}
+
+// Used to print a pointer that is neither a char pointer nor a member
+// pointer, when the user doesn't define PrintTo() for it. (A member
+// variable pointer or member function pointer doesn't really point to
+// a location in the address space. Their representation is
+// implementation-defined. Therefore they will be printed as raw
+// bytes.)
+template <typename T>
+void DefaultPrintTo(IsNotContainer /* dummy */,
+ true_type /* is a pointer */,
+ T* p, ::std::ostream* os) {
+ if (p == NULL) {
+ *os << "NULL";
+ } else {
+ // C++ doesn't allow casting from a function pointer to any object
+ // pointer.
+ //
+ // IsTrue() silences warnings: "Condition is always true",
+ // "unreachable code".
+ if (IsTrue(ImplicitlyConvertible<T*, const void*>::value)) {
+ // T is not a function type. We just call << to print p,
+ // relying on ADL to pick up user-defined << for their pointer
+ // types, if any.
+ *os << p;
+ } else {
+ // T is a function type, so '*os << p' doesn't do what we want
+ // (it just prints p as bool). We want to print p as a const
+ // void*. However, we cannot cast it to const void* directly,
+ // even using reinterpret_cast, as earlier versions of gcc
+ // (e.g. 3.4.5) cannot compile the cast when p is a function
+ // pointer. Casting to UInt64 first solves the problem.
+ *os << reinterpret_cast<const void*>(
+ reinterpret_cast<internal::UInt64>(p));
+ }
+ }
+}
+
+// Used to print a non-container, non-pointer value when the user
+// doesn't define PrintTo() for it.
+template <typename T>
+void DefaultPrintTo(IsNotContainer /* dummy */,
+ false_type /* is not a pointer */,
+ const T& value, ::std::ostream* os) {
+ ::testing_internal::DefaultPrintNonContainerTo(value, os);
+}
+
+// Prints the given value using the << operator if it has one;
+// otherwise prints the bytes in it. This is what
+// UniversalPrinter<T>::Print() does when PrintTo() is not specialized
+// or overloaded for type T.
+//
+// A user can override this behavior for a class type Foo by defining
+// an overload of PrintTo() in the namespace where Foo is defined. We
+// give the user this option as sometimes defining a << operator for
+// Foo is not desirable (e.g. the coding style may prevent doing it,
+// or there is already a << operator but it doesn't do what the user
+// wants).
+template <typename T>
+void PrintTo(const T& value, ::std::ostream* os) {
+ // DefaultPrintTo() is overloaded. The type of its first two
+ // arguments determine which version will be picked. If T is an
+ // STL-style container, the version for container will be called; if
+ // T is a pointer, the pointer version will be called; otherwise the
+ // generic version will be called.
+ //
+ // Note that we check for container types here, prior to we check
+ // for protocol message types in our operator<<. The rationale is:
+ //
+ // For protocol messages, we want to give people a chance to
+ // override Google Mock's format by defining a PrintTo() or
+ // operator<<. For STL containers, other formats can be
+ // incompatible with Google Mock's format for the container
+ // elements; therefore we check for container types here to ensure
+ // that our format is used.
+ //
+ // The second argument of DefaultPrintTo() is needed to bypass a bug
+ // in Symbian's C++ compiler that prevents it from picking the right
+ // overload between:
+ //
+ // PrintTo(const T& x, ...);
+ // PrintTo(T* x, ...);
+ DefaultPrintTo(IsContainerTest<T>(0), is_pointer<T>(), value, os);
+}
+
+// The following list of PrintTo() overloads tells
+// UniversalPrinter<T>::Print() how to print standard types (built-in
+// types, strings, plain arrays, and pointers).
+
+// Overloads for various char types.
+GTEST_API_ void PrintTo(unsigned char c, ::std::ostream* os);
+GTEST_API_ void PrintTo(signed char c, ::std::ostream* os);
+inline void PrintTo(char c, ::std::ostream* os) {
+ // When printing a plain char, we always treat it as unsigned. This
+ // way, the output won't be affected by whether the compiler thinks
+ // char is signed or not.
+ PrintTo(static_cast<unsigned char>(c), os);
+}
+
+// Overloads for other simple built-in types.
+inline void PrintTo(bool x, ::std::ostream* os) {
+ *os << (x ? "true" : "false");
+}
+
+// Overload for wchar_t type.
+// Prints a wchar_t as a symbol if it is printable or as its internal
+// code otherwise and also as its decimal code (except for L'\0').
+// The L'\0' char is printed as "L'\\0'". The decimal code is printed
+// as signed integer when wchar_t is implemented by the compiler
+// as a signed type and is printed as an unsigned integer when wchar_t
+// is implemented as an unsigned type.
+GTEST_API_ void PrintTo(wchar_t wc, ::std::ostream* os);
+
+// Overloads for C strings.
+GTEST_API_ void PrintTo(const char* s, ::std::ostream* os);
+inline void PrintTo(char* s, ::std::ostream* os) {
+ PrintTo(ImplicitCast_<const char*>(s), os);
+}
+
+// signed/unsigned char is often used for representing binary data, so
+// we print pointers to it as void* to be safe.
+inline void PrintTo(const signed char* s, ::std::ostream* os) {
+ PrintTo(ImplicitCast_<const void*>(s), os);
+}
+inline void PrintTo(signed char* s, ::std::ostream* os) {
+ PrintTo(ImplicitCast_<const void*>(s), os);
+}
+inline void PrintTo(const unsigned char* s, ::std::ostream* os) {
+ PrintTo(ImplicitCast_<const void*>(s), os);
+}
+inline void PrintTo(unsigned char* s, ::std::ostream* os) {
+ PrintTo(ImplicitCast_<const void*>(s), os);
+}
+
+// MSVC can be configured to define wchar_t as a typedef of unsigned
+// short. It defines _NATIVE_WCHAR_T_DEFINED when wchar_t is a native
+// type. When wchar_t is a typedef, defining an overload for const
+// wchar_t* would cause unsigned short* be printed as a wide string,
+// possibly causing invalid memory accesses.
+#if !defined(_MSC_VER) || defined(_NATIVE_WCHAR_T_DEFINED)
+// Overloads for wide C strings
+GTEST_API_ void PrintTo(const wchar_t* s, ::std::ostream* os);
+inline void PrintTo(wchar_t* s, ::std::ostream* os) {
+ PrintTo(ImplicitCast_<const wchar_t*>(s), os);
+}
+#endif
+
+// Overload for C arrays. Multi-dimensional arrays are printed
+// properly.
+
+// Prints the given number of elements in an array, without printing
+// the curly braces.
+template <typename T>
+void PrintRawArrayTo(const T a[], size_t count, ::std::ostream* os) {
+ UniversalPrint(a[0], os);
+ for (size_t i = 1; i != count; i++) {
+ *os << ", ";
+ UniversalPrint(a[i], os);
+ }
+}
+
+// Overloads for ::string and ::std::string.
+#if GTEST_HAS_GLOBAL_STRING
+GTEST_API_ void PrintStringTo(const ::string&s, ::std::ostream* os);
+inline void PrintTo(const ::string& s, ::std::ostream* os) {
+ PrintStringTo(s, os);
+}
+#endif // GTEST_HAS_GLOBAL_STRING
+
+GTEST_API_ void PrintStringTo(const ::std::string&s, ::std::ostream* os);
+inline void PrintTo(const ::std::string& s, ::std::ostream* os) {
+ PrintStringTo(s, os);
+}
+
+// Overloads for ::wstring and ::std::wstring.
+#if GTEST_HAS_GLOBAL_WSTRING
+GTEST_API_ void PrintWideStringTo(const ::wstring&s, ::std::ostream* os);
+inline void PrintTo(const ::wstring& s, ::std::ostream* os) {
+ PrintWideStringTo(s, os);
+}
+#endif // GTEST_HAS_GLOBAL_WSTRING
+
+#if GTEST_HAS_STD_WSTRING
+GTEST_API_ void PrintWideStringTo(const ::std::wstring&s, ::std::ostream* os);
+inline void PrintTo(const ::std::wstring& s, ::std::ostream* os) {
+ PrintWideStringTo(s, os);
+}
+#endif // GTEST_HAS_STD_WSTRING
+
+#if GTEST_HAS_TR1_TUPLE
+// Overload for ::std::tr1::tuple. Needed for printing function arguments,
+// which are packed as tuples.
+
+// Helper function for printing a tuple. T must be instantiated with
+// a tuple type.
+template <typename T>
+void PrintTupleTo(const T& t, ::std::ostream* os);
+
+// Overloaded PrintTo() for tuples of various arities. We support
+// tuples of up-to 10 fields. The following implementation works
+// regardless of whether tr1::tuple is implemented using the
+// non-standard variadic template feature or not.
+
+inline void PrintTo(const ::std::tr1::tuple<>& t, ::std::ostream* os) {
+ PrintTupleTo(t, os);
+}
+
+template <typename T1>
+void PrintTo(const ::std::tr1::tuple<T1>& t, ::std::ostream* os) {
+ PrintTupleTo(t, os);
+}
+
+template <typename T1, typename T2>
+void PrintTo(const ::std::tr1::tuple<T1, T2>& t, ::std::ostream* os) {
+ PrintTupleTo(t, os);
+}
+
+template <typename T1, typename T2, typename T3>
+void PrintTo(const ::std::tr1::tuple<T1, T2, T3>& t, ::std::ostream* os) {
+ PrintTupleTo(t, os);
+}
+
+template <typename T1, typename T2, typename T3, typename T4>
+void PrintTo(const ::std::tr1::tuple<T1, T2, T3, T4>& t, ::std::ostream* os) {
+ PrintTupleTo(t, os);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5>
+void PrintTo(const ::std::tr1::tuple<T1, T2, T3, T4, T5>& t,
+ ::std::ostream* os) {
+ PrintTupleTo(t, os);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6>
+void PrintTo(const ::std::tr1::tuple<T1, T2, T3, T4, T5, T6>& t,
+ ::std::ostream* os) {
+ PrintTupleTo(t, os);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7>
+void PrintTo(const ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7>& t,
+ ::std::ostream* os) {
+ PrintTupleTo(t, os);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8>
+void PrintTo(const ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7, T8>& t,
+ ::std::ostream* os) {
+ PrintTupleTo(t, os);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9>
+void PrintTo(const ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7, T8, T9>& t,
+ ::std::ostream* os) {
+ PrintTupleTo(t, os);
+}
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10>
+void PrintTo(
+ const ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10>& t,
+ ::std::ostream* os) {
+ PrintTupleTo(t, os);
+}
+#endif // GTEST_HAS_TR1_TUPLE
+
+// Overload for std::pair.
+template <typename T1, typename T2>
+void PrintTo(const ::std::pair<T1, T2>& value, ::std::ostream* os) {
+ *os << '(';
+ // We cannot use UniversalPrint(value.first, os) here, as T1 may be
+ // a reference type. The same for printing value.second.
+ UniversalPrinter<T1>::Print(value.first, os);
+ *os << ", ";
+ UniversalPrinter<T2>::Print(value.second, os);
+ *os << ')';
+}
+
+// Implements printing a non-reference type T by letting the compiler
+// pick the right overload of PrintTo() for T.
+template <typename T>
+class UniversalPrinter {
+ public:
+ // MSVC warns about adding const to a function type, so we want to
+ // disable the warning.
+#ifdef _MSC_VER
+# pragma warning(push) // Saves the current warning state.
+# pragma warning(disable:4180) // Temporarily disables warning 4180.
+#endif // _MSC_VER
+
+ // Note: we deliberately don't call this PrintTo(), as that name
+ // conflicts with ::testing::internal::PrintTo in the body of the
+ // function.
+ static void Print(const T& value, ::std::ostream* os) {
+ // By default, ::testing::internal::PrintTo() is used for printing
+ // the value.
+ //
+ // Thanks to Koenig look-up, if T is a class and has its own
+ // PrintTo() function defined in its namespace, that function will
+ // be visible here. Since it is more specific than the generic ones
+ // in ::testing::internal, it will be picked by the compiler in the
+ // following statement - exactly what we want.
+ PrintTo(value, os);
+ }
+
+#ifdef _MSC_VER
+# pragma warning(pop) // Restores the warning state.
+#endif // _MSC_VER
+};
+
+// UniversalPrintArray(begin, len, os) prints an array of 'len'
+// elements, starting at address 'begin'.
+template <typename T>
+void UniversalPrintArray(const T* begin, size_t len, ::std::ostream* os) {
+ if (len == 0) {
+ *os << "{}";
+ } else {
+ *os << "{ ";
+ const size_t kThreshold = 18;
+ const size_t kChunkSize = 8;
+ // If the array has more than kThreshold elements, we'll have to
+ // omit some details by printing only the first and the last
+ // kChunkSize elements.
+ // TODO(wan at google.com): let the user control the threshold using a flag.
+ if (len <= kThreshold) {
+ PrintRawArrayTo(begin, len, os);
+ } else {
+ PrintRawArrayTo(begin, kChunkSize, os);
+ *os << ", ..., ";
+ PrintRawArrayTo(begin + len - kChunkSize, kChunkSize, os);
+ }
+ *os << " }";
+ }
+}
+// This overload prints a (const) char array compactly.
+GTEST_API_ void UniversalPrintArray(
+ const char* begin, size_t len, ::std::ostream* os);
+
+// This overload prints a (const) wchar_t array compactly.
+GTEST_API_ void UniversalPrintArray(
+ const wchar_t* begin, size_t len, ::std::ostream* os);
+
+// Implements printing an array type T[N].
+template <typename T, size_t N>
+class UniversalPrinter<T[N]> {
+ public:
+ // Prints the given array, omitting some elements when there are too
+ // many.
+ static void Print(const T (&a)[N], ::std::ostream* os) {
+ UniversalPrintArray(a, N, os);
+ }
+};
+
+// Implements printing a reference type T&.
+template <typename T>
+class UniversalPrinter<T&> {
+ public:
+ // MSVC warns about adding const to a function type, so we want to
+ // disable the warning.
+#ifdef _MSC_VER
+# pragma warning(push) // Saves the current warning state.
+# pragma warning(disable:4180) // Temporarily disables warning 4180.
+#endif // _MSC_VER
+
+ static void Print(const T& value, ::std::ostream* os) {
+ // Prints the address of the value. We use reinterpret_cast here
+ // as static_cast doesn't compile when T is a function type.
+ *os << "@" << reinterpret_cast<const void*>(&value) << " ";
+
+ // Then prints the value itself.
+ UniversalPrint(value, os);
+ }
+
+#ifdef _MSC_VER
+# pragma warning(pop) // Restores the warning state.
+#endif // _MSC_VER
+};
+
+// Prints a value tersely: for a reference type, the referenced value
+// (but not the address) is printed; for a (const) char pointer, the
+// NUL-terminated string (but not the pointer) is printed.
+
+template <typename T>
+class UniversalTersePrinter {
+ public:
+ static void Print(const T& value, ::std::ostream* os) {
+ UniversalPrint(value, os);
+ }
+};
+template <typename T>
+class UniversalTersePrinter<T&> {
+ public:
+ static void Print(const T& value, ::std::ostream* os) {
+ UniversalPrint(value, os);
+ }
+};
+template <typename T, size_t N>
+class UniversalTersePrinter<T[N]> {
+ public:
+ static void Print(const T (&value)[N], ::std::ostream* os) {
+ UniversalPrinter<T[N]>::Print(value, os);
+ }
+};
+template <>
+class UniversalTersePrinter<const char*> {
+ public:
+ static void Print(const char* str, ::std::ostream* os) {
+ if (str == NULL) {
+ *os << "NULL";
+ } else {
+ UniversalPrint(string(str), os);
+ }
+ }
+};
+template <>
+class UniversalTersePrinter<char*> {
+ public:
+ static void Print(char* str, ::std::ostream* os) {
+ UniversalTersePrinter<const char*>::Print(str, os);
+ }
+};
+
+#if GTEST_HAS_STD_WSTRING
+template <>
+class UniversalTersePrinter<const wchar_t*> {
+ public:
+ static void Print(const wchar_t* str, ::std::ostream* os) {
+ if (str == NULL) {
+ *os << "NULL";
+ } else {
+ UniversalPrint(::std::wstring(str), os);
+ }
+ }
+};
+#endif
+
+template <>
+class UniversalTersePrinter<wchar_t*> {
+ public:
+ static void Print(wchar_t* str, ::std::ostream* os) {
+ UniversalTersePrinter<const wchar_t*>::Print(str, os);
+ }
+};
+
+template <typename T>
+void UniversalTersePrint(const T& value, ::std::ostream* os) {
+ UniversalTersePrinter<T>::Print(value, os);
+}
+
+// Prints a value using the type inferred by the compiler. The
+// difference between this and UniversalTersePrint() is that for a
+// (const) char pointer, this prints both the pointer and the
+// NUL-terminated string.
+template <typename T>
+void UniversalPrint(const T& value, ::std::ostream* os) {
+ // A workarond for the bug in VC++ 7.1 that prevents us from instantiating
+ // UniversalPrinter with T directly.
+ typedef T T1;
+ UniversalPrinter<T1>::Print(value, os);
+}
+
+#if GTEST_HAS_TR1_TUPLE
+typedef ::std::vector<string> Strings;
+
+// This helper template allows PrintTo() for tuples and
+// UniversalTersePrintTupleFieldsToStrings() to be defined by
+// induction on the number of tuple fields. The idea is that
+// TuplePrefixPrinter<N>::PrintPrefixTo(t, os) prints the first N
+// fields in tuple t, and can be defined in terms of
+// TuplePrefixPrinter<N - 1>.
+
+// The inductive case.
+template <size_t N>
+struct TuplePrefixPrinter {
+ // Prints the first N fields of a tuple.
+ template <typename Tuple>
+ static void PrintPrefixTo(const Tuple& t, ::std::ostream* os) {
+ TuplePrefixPrinter<N - 1>::PrintPrefixTo(t, os);
+ *os << ", ";
+ UniversalPrinter<typename ::std::tr1::tuple_element<N - 1, Tuple>::type>
+ ::Print(::std::tr1::get<N - 1>(t), os);
+ }
+
+ // Tersely prints the first N fields of a tuple to a string vector,
+ // one element for each field.
+ template <typename Tuple>
+ static void TersePrintPrefixToStrings(const Tuple& t, Strings* strings) {
+ TuplePrefixPrinter<N - 1>::TersePrintPrefixToStrings(t, strings);
+ ::std::stringstream ss;
+ UniversalTersePrint(::std::tr1::get<N - 1>(t), &ss);
+ strings->push_back(ss.str());
+ }
+};
+
+// Base cases.
+template <>
+struct TuplePrefixPrinter<0> {
+ template <typename Tuple>
+ static void PrintPrefixTo(const Tuple&, ::std::ostream*) {}
+
+ template <typename Tuple>
+ static void TersePrintPrefixToStrings(const Tuple&, Strings*) {}
+};
+// We have to specialize the entire TuplePrefixPrinter<> class
+// template here, even though the definition of
+// TersePrintPrefixToStrings() is the same as the generic version, as
+// Embarcadero (formerly CodeGear, formerly Borland) C++ doesn't
+// support specializing a method template of a class template.
+template <>
+struct TuplePrefixPrinter<1> {
+ template <typename Tuple>
+ static void PrintPrefixTo(const Tuple& t, ::std::ostream* os) {
+ UniversalPrinter<typename ::std::tr1::tuple_element<0, Tuple>::type>::
+ Print(::std::tr1::get<0>(t), os);
+ }
+
+ template <typename Tuple>
+ static void TersePrintPrefixToStrings(const Tuple& t, Strings* strings) {
+ ::std::stringstream ss;
+ UniversalTersePrint(::std::tr1::get<0>(t), &ss);
+ strings->push_back(ss.str());
+ }
+};
+
+// Helper function for printing a tuple. T must be instantiated with
+// a tuple type.
+template <typename T>
+void PrintTupleTo(const T& t, ::std::ostream* os) {
+ *os << "(";
+ TuplePrefixPrinter< ::std::tr1::tuple_size<T>::value>::
+ PrintPrefixTo(t, os);
+ *os << ")";
+}
+
+// Prints the fields of a tuple tersely to a string vector, one
+// element for each field. See the comment before
+// UniversalTersePrint() for how we define "tersely".
+template <typename Tuple>
+Strings UniversalTersePrintTupleFieldsToStrings(const Tuple& value) {
+ Strings result;
+ TuplePrefixPrinter< ::std::tr1::tuple_size<Tuple>::value>::
+ TersePrintPrefixToStrings(value, &result);
+ return result;
+}
+#endif // GTEST_HAS_TR1_TUPLE
+
+} // namespace internal
+
+template <typename T>
+::std::string PrintToString(const T& value) {
+ ::std::stringstream ss;
+ internal::UniversalTersePrinter<T>::Print(value, &ss);
+ return ss.str();
+}
+
+} // namespace testing
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_PRINTERS_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-spi.h b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-spi.h
new file mode 100644
index 0000000..f63fa9a
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-spi.h
@@ -0,0 +1,232 @@
+// Copyright 2007, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+//
+// Utilities for testing Google Test itself and code that uses Google Test
+// (e.g. frameworks built on top of Google Test).
+
+#ifndef GTEST_INCLUDE_GTEST_GTEST_SPI_H_
+#define GTEST_INCLUDE_GTEST_GTEST_SPI_H_
+
+#include "gtest/gtest.h"
+
+namespace testing {
+
+// This helper class can be used to mock out Google Test failure reporting
+// so that we can test Google Test or code that builds on Google Test.
+//
+// An object of this class appends a TestPartResult object to the
+// TestPartResultArray object given in the constructor whenever a Google Test
+// failure is reported. It can either intercept only failures that are
+// generated in the same thread that created this object or it can intercept
+// all generated failures. The scope of this mock object can be controlled with
+// the second argument to the two arguments constructor.
+class GTEST_API_ ScopedFakeTestPartResultReporter
+ : public TestPartResultReporterInterface {
+ public:
+ // The two possible mocking modes of this object.
+ enum InterceptMode {
+ INTERCEPT_ONLY_CURRENT_THREAD, // Intercepts only thread local failures.
+ INTERCEPT_ALL_THREADS // Intercepts all failures.
+ };
+
+ // The c'tor sets this object as the test part result reporter used
+ // by Google Test. The 'result' parameter specifies where to report the
+ // results. This reporter will only catch failures generated in the current
+ // thread. DEPRECATED
+ explicit ScopedFakeTestPartResultReporter(TestPartResultArray* result);
+
+ // Same as above, but you can choose the interception scope of this object.
+ ScopedFakeTestPartResultReporter(InterceptMode intercept_mode,
+ TestPartResultArray* result);
+
+ // The d'tor restores the previous test part result reporter.
+ virtual ~ScopedFakeTestPartResultReporter();
+
+ // Appends the TestPartResult object to the TestPartResultArray
+ // received in the constructor.
+ //
+ // This method is from the TestPartResultReporterInterface
+ // interface.
+ virtual void ReportTestPartResult(const TestPartResult& result);
+ private:
+ void Init();
+
+ const InterceptMode intercept_mode_;
+ TestPartResultReporterInterface* old_reporter_;
+ TestPartResultArray* const result_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ScopedFakeTestPartResultReporter);
+};
+
+namespace internal {
+
+// A helper class for implementing EXPECT_FATAL_FAILURE() and
+// EXPECT_NONFATAL_FAILURE(). Its destructor verifies that the given
+// TestPartResultArray contains exactly one failure that has the given
+// type and contains the given substring. If that's not the case, a
+// non-fatal failure will be generated.
+class GTEST_API_ SingleFailureChecker {
+ public:
+ // The constructor remembers the arguments.
+ SingleFailureChecker(const TestPartResultArray* results,
+ TestPartResult::Type type,
+ const string& substr);
+ ~SingleFailureChecker();
+ private:
+ const TestPartResultArray* const results_;
+ const TestPartResult::Type type_;
+ const string substr_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(SingleFailureChecker);
+};
+
+} // namespace internal
+
+} // namespace testing
+
+// A set of macros for testing Google Test assertions or code that's expected
+// to generate Google Test fatal failures. It verifies that the given
+// statement will cause exactly one fatal Google Test failure with 'substr'
+// being part of the failure message.
+//
+// There are two different versions of this macro. EXPECT_FATAL_FAILURE only
+// affects and considers failures generated in the current thread and
+// EXPECT_FATAL_FAILURE_ON_ALL_THREADS does the same but for all threads.
+//
+// The verification of the assertion is done correctly even when the statement
+// throws an exception or aborts the current function.
+//
+// Known restrictions:
+// - 'statement' cannot reference local non-static variables or
+// non-static members of the current object.
+// - 'statement' cannot return a value.
+// - You cannot stream a failure message to this macro.
+//
+// Note that even though the implementations of the following two
+// macros are much alike, we cannot refactor them to use a common
+// helper macro, due to some peculiarity in how the preprocessor
+// works. The AcceptsMacroThatExpandsToUnprotectedComma test in
+// gtest_unittest.cc will fail to compile if we do that.
+#define EXPECT_FATAL_FAILURE(statement, substr) \
+ do { \
+ class GTestExpectFatalFailureHelper {\
+ public:\
+ static void Execute() { statement; }\
+ };\
+ ::testing::TestPartResultArray gtest_failures;\
+ ::testing::internal::SingleFailureChecker gtest_checker(\
+ >est_failures, ::testing::TestPartResult::kFatalFailure, (substr));\
+ {\
+ ::testing::ScopedFakeTestPartResultReporter gtest_reporter(\
+ ::testing::ScopedFakeTestPartResultReporter:: \
+ INTERCEPT_ONLY_CURRENT_THREAD, >est_failures);\
+ GTestExpectFatalFailureHelper::Execute();\
+ }\
+ } while (::testing::internal::AlwaysFalse())
+
+#define EXPECT_FATAL_FAILURE_ON_ALL_THREADS(statement, substr) \
+ do { \
+ class GTestExpectFatalFailureHelper {\
+ public:\
+ static void Execute() { statement; }\
+ };\
+ ::testing::TestPartResultArray gtest_failures;\
+ ::testing::internal::SingleFailureChecker gtest_checker(\
+ >est_failures, ::testing::TestPartResult::kFatalFailure, (substr));\
+ {\
+ ::testing::ScopedFakeTestPartResultReporter gtest_reporter(\
+ ::testing::ScopedFakeTestPartResultReporter:: \
+ INTERCEPT_ALL_THREADS, >est_failures);\
+ GTestExpectFatalFailureHelper::Execute();\
+ }\
+ } while (::testing::internal::AlwaysFalse())
+
+// A macro for testing Google Test assertions or code that's expected to
+// generate Google Test non-fatal failures. It asserts that the given
+// statement will cause exactly one non-fatal Google Test failure with 'substr'
+// being part of the failure message.
+//
+// There are two different versions of this macro. EXPECT_NONFATAL_FAILURE only
+// affects and considers failures generated in the current thread and
+// EXPECT_NONFATAL_FAILURE_ON_ALL_THREADS does the same but for all threads.
+//
+// 'statement' is allowed to reference local variables and members of
+// the current object.
+//
+// The verification of the assertion is done correctly even when the statement
+// throws an exception or aborts the current function.
+//
+// Known restrictions:
+// - You cannot stream a failure message to this macro.
+//
+// Note that even though the implementations of the following two
+// macros are much alike, we cannot refactor them to use a common
+// helper macro, due to some peculiarity in how the preprocessor
+// works. If we do that, the code won't compile when the user gives
+// EXPECT_NONFATAL_FAILURE() a statement that contains a macro that
+// expands to code containing an unprotected comma. The
+// AcceptsMacroThatExpandsToUnprotectedComma test in gtest_unittest.cc
+// catches that.
+//
+// For the same reason, we have to write
+// if (::testing::internal::AlwaysTrue()) { statement; }
+// instead of
+// GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement)
+// to avoid an MSVC warning on unreachable code.
+#define EXPECT_NONFATAL_FAILURE(statement, substr) \
+ do {\
+ ::testing::TestPartResultArray gtest_failures;\
+ ::testing::internal::SingleFailureChecker gtest_checker(\
+ >est_failures, ::testing::TestPartResult::kNonFatalFailure, \
+ (substr));\
+ {\
+ ::testing::ScopedFakeTestPartResultReporter gtest_reporter(\
+ ::testing::ScopedFakeTestPartResultReporter:: \
+ INTERCEPT_ONLY_CURRENT_THREAD, >est_failures);\
+ if (::testing::internal::AlwaysTrue()) { statement; }\
+ }\
+ } while (::testing::internal::AlwaysFalse())
+
+#define EXPECT_NONFATAL_FAILURE_ON_ALL_THREADS(statement, substr) \
+ do {\
+ ::testing::TestPartResultArray gtest_failures;\
+ ::testing::internal::SingleFailureChecker gtest_checker(\
+ >est_failures, ::testing::TestPartResult::kNonFatalFailure, \
+ (substr));\
+ {\
+ ::testing::ScopedFakeTestPartResultReporter gtest_reporter(\
+ ::testing::ScopedFakeTestPartResultReporter::INTERCEPT_ALL_THREADS, \
+ >est_failures);\
+ if (::testing::internal::AlwaysTrue()) { statement; }\
+ }\
+ } while (::testing::internal::AlwaysFalse())
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_SPI_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-test-part.h b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-test-part.h
new file mode 100644
index 0000000..77eb844
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-test-part.h
@@ -0,0 +1,179 @@
+// Copyright 2008, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: mheule at google.com (Markus Heule)
+//
+
+#ifndef GTEST_INCLUDE_GTEST_GTEST_TEST_PART_H_
+#define GTEST_INCLUDE_GTEST_GTEST_TEST_PART_H_
+
+#include <iosfwd>
+#include <vector>
+#include "gtest/internal/gtest-internal.h"
+#include "gtest/internal/gtest-string.h"
+
+namespace testing {
+
+// A copyable object representing the result of a test part (i.e. an
+// assertion or an explicit FAIL(), ADD_FAILURE(), or SUCCESS()).
+//
+// Don't inherit from TestPartResult as its destructor is not virtual.
+class GTEST_API_ TestPartResult {
+ public:
+ // The possible outcomes of a test part (i.e. an assertion or an
+ // explicit SUCCEED(), FAIL(), or ADD_FAILURE()).
+ enum Type {
+ kSuccess, // Succeeded.
+ kNonFatalFailure, // Failed but the test can continue.
+ kFatalFailure // Failed and the test should be terminated.
+ };
+
+ // C'tor. TestPartResult does NOT have a default constructor.
+ // Always use this constructor (with parameters) to create a
+ // TestPartResult object.
+ TestPartResult(Type a_type,
+ const char* a_file_name,
+ int a_line_number,
+ const char* a_message)
+ : type_(a_type),
+ file_name_(a_file_name == NULL ? "" : a_file_name),
+ line_number_(a_line_number),
+ summary_(ExtractSummary(a_message)),
+ message_(a_message) {
+ }
+
+ // Gets the outcome of the test part.
+ Type type() const { return type_; }
+
+ // Gets the name of the source file where the test part took place, or
+ // NULL if it's unknown.
+ const char* file_name() const {
+ return file_name_.empty() ? NULL : file_name_.c_str();
+ }
+
+ // Gets the line in the source file where the test part took place,
+ // or -1 if it's unknown.
+ int line_number() const { return line_number_; }
+
+ // Gets the summary of the failure message.
+ const char* summary() const { return summary_.c_str(); }
+
+ // Gets the message associated with the test part.
+ const char* message() const { return message_.c_str(); }
+
+ // Returns true iff the test part passed.
+ bool passed() const { return type_ == kSuccess; }
+
+ // Returns true iff the test part failed.
+ bool failed() const { return type_ != kSuccess; }
+
+ // Returns true iff the test part non-fatally failed.
+ bool nonfatally_failed() const { return type_ == kNonFatalFailure; }
+
+ // Returns true iff the test part fatally failed.
+ bool fatally_failed() const { return type_ == kFatalFailure; }
+
+ private:
+ Type type_;
+
+ // Gets the summary of the failure message by omitting the stack
+ // trace in it.
+ static std::string ExtractSummary(const char* message);
+
+ // The name of the source file where the test part took place, or
+ // "" if the source file is unknown.
+ std::string file_name_;
+ // The line in the source file where the test part took place, or -1
+ // if the line number is unknown.
+ int line_number_;
+ std::string summary_; // The test failure summary.
+ std::string message_; // The test failure message.
+};
+
+// Prints a TestPartResult object.
+std::ostream& operator<<(std::ostream& os, const TestPartResult& result);
+
+// An array of TestPartResult objects.
+//
+// Don't inherit from TestPartResultArray as its destructor is not
+// virtual.
+class GTEST_API_ TestPartResultArray {
+ public:
+ TestPartResultArray() {}
+
+ // Appends the given TestPartResult to the array.
+ void Append(const TestPartResult& result);
+
+ // Returns the TestPartResult at the given index (0-based).
+ const TestPartResult& GetTestPartResult(int index) const;
+
+ // Returns the number of TestPartResult objects in the array.
+ int size() const;
+
+ private:
+ std::vector<TestPartResult> array_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(TestPartResultArray);
+};
+
+// This interface knows how to report a test part result.
+class TestPartResultReporterInterface {
+ public:
+ virtual ~TestPartResultReporterInterface() {}
+
+ virtual void ReportTestPartResult(const TestPartResult& result) = 0;
+};
+
+namespace internal {
+
+// This helper class is used by {ASSERT|EXPECT}_NO_FATAL_FAILURE to check if a
+// statement generates new fatal failures. To do so it registers itself as the
+// current test part result reporter. Besides checking if fatal failures were
+// reported, it only delegates the reporting to the former result reporter.
+// The original result reporter is restored in the destructor.
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+class GTEST_API_ HasNewFatalFailureHelper
+ : public TestPartResultReporterInterface {
+ public:
+ HasNewFatalFailureHelper();
+ virtual ~HasNewFatalFailureHelper();
+ virtual void ReportTestPartResult(const TestPartResult& result);
+ bool has_new_fatal_failure() const { return has_new_fatal_failure_; }
+ private:
+ bool has_new_fatal_failure_;
+ TestPartResultReporterInterface* original_reporter_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(HasNewFatalFailureHelper);
+};
+
+} // namespace internal
+
+} // namespace testing
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_TEST_PART_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-typed-test.h b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-typed-test.h
new file mode 100644
index 0000000..d5dc8be
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest-typed-test.h
@@ -0,0 +1,260 @@
+// Copyright 2008 Google Inc.
+// All Rights Reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+
+#ifndef GTEST_INCLUDE_GTEST_GTEST_TYPED_TEST_H_
+#define GTEST_INCLUDE_GTEST_GTEST_TYPED_TEST_H_
+#pragma GCC system_header
+
+// This header implements typed tests and type-parameterized tests.
+
+// Typed (aka type-driven) tests repeat the same test for types in a
+// list. You must know which types you want to test with when writing
+// typed tests. Here's how you do it:
+
+#if 0
+
+// First, define a fixture class template. It should be parameterized
+// by a type. Remember to derive it from testing::Test.
+template <typename T>
+class FooTest : public testing::Test {
+ public:
+ ...
+ typedef std::list<T> List;
+ static T shared_;
+ T value_;
+};
+
+// Next, associate a list of types with the test case, which will be
+// repeated for each type in the list. The typedef is necessary for
+// the macro to parse correctly.
+typedef testing::Types<char, int, unsigned int> MyTypes;
+TYPED_TEST_CASE(FooTest, MyTypes);
+
+// If the type list contains only one type, you can write that type
+// directly without Types<...>:
+// TYPED_TEST_CASE(FooTest, int);
+
+// Then, use TYPED_TEST() instead of TEST_F() to define as many typed
+// tests for this test case as you want.
+TYPED_TEST(FooTest, DoesBlah) {
+ // Inside a test, refer to TypeParam to get the type parameter.
+ // Since we are inside a derived class template, C++ requires use to
+ // visit the members of FooTest via 'this'.
+ TypeParam n = this->value_;
+
+ // To visit static members of the fixture, add the TestFixture::
+ // prefix.
+ n += TestFixture::shared_;
+
+ // To refer to typedefs in the fixture, add the "typename
+ // TestFixture::" prefix.
+ typename TestFixture::List values;
+ values.push_back(n);
+ ...
+}
+
+TYPED_TEST(FooTest, HasPropertyA) { ... }
+
+#endif // 0
+
+// Type-parameterized tests are abstract test patterns parameterized
+// by a type. Compared with typed tests, type-parameterized tests
+// allow you to define the test pattern without knowing what the type
+// parameters are. The defined pattern can be instantiated with
+// different types any number of times, in any number of translation
+// units.
+//
+// If you are designing an interface or concept, you can define a
+// suite of type-parameterized tests to verify properties that any
+// valid implementation of the interface/concept should have. Then,
+// each implementation can easily instantiate the test suite to verify
+// that it conforms to the requirements, without having to write
+// similar tests repeatedly. Here's an example:
+
+#if 0
+
+// First, define a fixture class template. It should be parameterized
+// by a type. Remember to derive it from testing::Test.
+template <typename T>
+class FooTest : public testing::Test {
+ ...
+};
+
+// Next, declare that you will define a type-parameterized test case
+// (the _P suffix is for "parameterized" or "pattern", whichever you
+// prefer):
+TYPED_TEST_CASE_P(FooTest);
+
+// Then, use TYPED_TEST_P() to define as many type-parameterized tests
+// for this type-parameterized test case as you want.
+TYPED_TEST_P(FooTest, DoesBlah) {
+ // Inside a test, refer to TypeParam to get the type parameter.
+ TypeParam n = 0;
+ ...
+}
+
+TYPED_TEST_P(FooTest, HasPropertyA) { ... }
+
+// Now the tricky part: you need to register all test patterns before
+// you can instantiate them. The first argument of the macro is the
+// test case name; the rest are the names of the tests in this test
+// case.
+REGISTER_TYPED_TEST_CASE_P(FooTest,
+ DoesBlah, HasPropertyA);
+
+// Finally, you are free to instantiate the pattern with the types you
+// want. If you put the above code in a header file, you can #include
+// it in multiple C++ source files and instantiate it multiple times.
+//
+// To distinguish different instances of the pattern, the first
+// argument to the INSTANTIATE_* macro is a prefix that will be added
+// to the actual test case name. Remember to pick unique prefixes for
+// different instances.
+typedef testing::Types<char, int, unsigned int> MyTypes;
+INSTANTIATE_TYPED_TEST_CASE_P(My, FooTest, MyTypes);
+
+// If the type list contains only one type, you can write that type
+// directly without Types<...>:
+// INSTANTIATE_TYPED_TEST_CASE_P(My, FooTest, int);
+
+#endif // 0
+
+#include "gtest/internal/gtest-port.h"
+#include "gtest/internal/gtest-type-util.h"
+
+// Implements typed tests.
+
+#if GTEST_HAS_TYPED_TEST
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// Expands to the name of the typedef for the type parameters of the
+// given test case.
+# define GTEST_TYPE_PARAMS_(TestCaseName) gtest_type_params_##TestCaseName##_
+
+// The 'Types' template argument below must have spaces around it
+// since some compilers may choke on '>>' when passing a template
+// instance (e.g. Types<int>)
+# define TYPED_TEST_CASE(CaseName, Types) \
+ typedef ::testing::internal::TypeList< Types >::type \
+ GTEST_TYPE_PARAMS_(CaseName)
+
+# define TYPED_TEST(CaseName, TestName) \
+ template <typename gtest_TypeParam_> \
+ class GTEST_TEST_CLASS_NAME_(CaseName, TestName) \
+ : public CaseName<gtest_TypeParam_> { \
+ private: \
+ typedef CaseName<gtest_TypeParam_> TestFixture; \
+ typedef gtest_TypeParam_ TypeParam; \
+ virtual void TestBody(); \
+ }; \
+ bool gtest_##CaseName##_##TestName##_registered_ GTEST_ATTRIBUTE_UNUSED_ = \
+ ::testing::internal::TypeParameterizedTest< \
+ CaseName, \
+ ::testing::internal::TemplateSel< \
+ GTEST_TEST_CLASS_NAME_(CaseName, TestName)>, \
+ GTEST_TYPE_PARAMS_(CaseName)>::Register(\
+ "", #CaseName, #TestName, 0); \
+ template <typename gtest_TypeParam_> \
+ void GTEST_TEST_CLASS_NAME_(CaseName, TestName)<gtest_TypeParam_>::TestBody()
+
+#endif // GTEST_HAS_TYPED_TEST
+
+// Implements type-parameterized tests.
+
+#if GTEST_HAS_TYPED_TEST_P
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// Expands to the namespace name that the type-parameterized tests for
+// the given type-parameterized test case are defined in. The exact
+// name of the namespace is subject to change without notice.
+# define GTEST_CASE_NAMESPACE_(TestCaseName) \
+ gtest_case_##TestCaseName##_
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// Expands to the name of the variable used to remember the names of
+// the defined tests in the given test case.
+# define GTEST_TYPED_TEST_CASE_P_STATE_(TestCaseName) \
+ gtest_typed_test_case_p_state_##TestCaseName##_
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE DIRECTLY.
+//
+// Expands to the name of the variable used to remember the names of
+// the registered tests in the given test case.
+# define GTEST_REGISTERED_TEST_NAMES_(TestCaseName) \
+ gtest_registered_test_names_##TestCaseName##_
+
+// The variables defined in the type-parameterized test macros are
+// static as typically these macros are used in a .h file that can be
+// #included in multiple translation units linked together.
+# define TYPED_TEST_CASE_P(CaseName) \
+ static ::testing::internal::TypedTestCasePState \
+ GTEST_TYPED_TEST_CASE_P_STATE_(CaseName)
+
+# define TYPED_TEST_P(CaseName, TestName) \
+ namespace GTEST_CASE_NAMESPACE_(CaseName) { \
+ template <typename gtest_TypeParam_> \
+ class TestName : public CaseName<gtest_TypeParam_> { \
+ private: \
+ typedef CaseName<gtest_TypeParam_> TestFixture; \
+ typedef gtest_TypeParam_ TypeParam; \
+ virtual void TestBody(); \
+ }; \
+ static bool gtest_##TestName##_defined_ GTEST_ATTRIBUTE_UNUSED_ = \
+ GTEST_TYPED_TEST_CASE_P_STATE_(CaseName).AddTestName(\
+ __FILE__, __LINE__, #CaseName, #TestName); \
+ } \
+ template <typename gtest_TypeParam_> \
+ void GTEST_CASE_NAMESPACE_(CaseName)::TestName<gtest_TypeParam_>::TestBody()
+
+# define REGISTER_TYPED_TEST_CASE_P(CaseName, ...) \
+ namespace GTEST_CASE_NAMESPACE_(CaseName) { \
+ typedef ::testing::internal::Templates<__VA_ARGS__>::type gtest_AllTests_; \
+ } \
+ static const char* const GTEST_REGISTERED_TEST_NAMES_(CaseName) = \
+ GTEST_TYPED_TEST_CASE_P_STATE_(CaseName).VerifyRegisteredTestNames(\
+ __FILE__, __LINE__, #__VA_ARGS__)
+
+// The 'Types' template argument below must have spaces around it
+// since some compilers may choke on '>>' when passing a template
+// instance (e.g. Types<int>)
+# define INSTANTIATE_TYPED_TEST_CASE_P(Prefix, CaseName, Types) \
+ bool gtest_##Prefix##_##CaseName GTEST_ATTRIBUTE_UNUSED_ = \
+ ::testing::internal::TypeParameterizedTestCase<CaseName, \
+ GTEST_CASE_NAMESPACE_(CaseName)::gtest_AllTests_, \
+ ::testing::internal::TypeList< Types >::type>::Register(\
+ #Prefix, #CaseName, GTEST_REGISTERED_TEST_NAMES_(CaseName))
+
+#endif // GTEST_HAS_TYPED_TEST_P
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_TYPED_TEST_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/gtest.h b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest.h
new file mode 100644
index 0000000..6fa0a39
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest.h
@@ -0,0 +1,2291 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+//
+// The Google C++ Testing Framework (Google Test)
+//
+// This header file defines the public API for Google Test. It should be
+// included by any test program that uses Google Test.
+//
+// IMPORTANT NOTE: Due to limitation of the C++ language, we have to
+// leave some internal implementation details in this header file.
+// They are clearly marked by comments like this:
+//
+// // INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+//
+// Such code is NOT meant to be used by a user directly, and is subject
+// to CHANGE WITHOUT NOTICE. Therefore DO NOT DEPEND ON IT in a user
+// program!
+//
+// Acknowledgment: Google Test borrowed the idea of automatic test
+// registration from Barthelemy Dagenais' (barthelemy at prologique.com)
+// easyUnit framework.
+
+#ifndef GTEST_INCLUDE_GTEST_GTEST_H_
+#define GTEST_INCLUDE_GTEST_GTEST_H_
+
+#include <limits>
+#include <ostream>
+#include <vector>
+
+#include "gtest/internal/gtest-internal.h"
+#include "gtest/internal/gtest-string.h"
+#include "gtest/gtest-death-test.h"
+#include "gtest/gtest-message.h"
+#include "gtest/gtest-param-test.h"
+#include "gtest/gtest-printers.h"
+#include "gtest/gtest_prod.h"
+#include "gtest/gtest-test-part.h"
+#include "gtest/gtest-typed-test.h"
+
+// Depending on the platform, different string classes are available.
+// On Linux, in addition to ::std::string, Google also makes use of
+// class ::string, which has the same interface as ::std::string, but
+// has a different implementation.
+//
+// The user can define GTEST_HAS_GLOBAL_STRING to 1 to indicate that
+// ::string is available AND is a distinct type to ::std::string, or
+// define it to 0 to indicate otherwise.
+//
+// If the user's ::std::string and ::string are the same class due to
+// aliasing, he should define GTEST_HAS_GLOBAL_STRING to 0.
+//
+// If the user doesn't define GTEST_HAS_GLOBAL_STRING, it is defined
+// heuristically.
+
+namespace testing {
+
+// Declares the flags.
+
+// This flag temporary enables the disabled tests.
+GTEST_DECLARE_bool_(also_run_disabled_tests);
+
+// This flag brings the debugger on an assertion failure.
+GTEST_DECLARE_bool_(break_on_failure);
+
+// This flag controls whether Google Test catches all test-thrown exceptions
+// and logs them as failures.
+GTEST_DECLARE_bool_(catch_exceptions);
+
+// This flag enables using colors in terminal output. Available values are
+// "yes" to enable colors, "no" (disable colors), or "auto" (the default)
+// to let Google Test decide.
+GTEST_DECLARE_string_(color);
+
+// This flag sets up the filter to select by name using a glob pattern
+// the tests to run. If the filter is not given all tests are executed.
+GTEST_DECLARE_string_(filter);
+
+// This flag causes the Google Test to list tests. None of the tests listed
+// are actually run if the flag is provided.
+GTEST_DECLARE_bool_(list_tests);
+
+// This flag controls whether Google Test emits a detailed XML report to a file
+// in addition to its normal textual output.
+GTEST_DECLARE_string_(output);
+
+// This flags control whether Google Test prints the elapsed time for each
+// test.
+GTEST_DECLARE_bool_(print_time);
+
+// This flag specifies the random number seed.
+GTEST_DECLARE_int32_(random_seed);
+
+// This flag sets how many times the tests are repeated. The default value
+// is 1. If the value is -1 the tests are repeating forever.
+GTEST_DECLARE_int32_(repeat);
+
+// This flag controls whether Google Test includes Google Test internal
+// stack frames in failure stack traces.
+GTEST_DECLARE_bool_(show_internal_stack_frames);
+
+// When this flag is specified, tests' order is randomized on every iteration.
+GTEST_DECLARE_bool_(shuffle);
+
+// This flag specifies the maximum number of stack frames to be
+// printed in a failure message.
+GTEST_DECLARE_int32_(stack_trace_depth);
+
+// When this flag is specified, a failed assertion will throw an
+// exception if exceptions are enabled, or exit the program with a
+// non-zero code otherwise.
+GTEST_DECLARE_bool_(throw_on_failure);
+
+// When this flag is set with a "host:port" string, on supported
+// platforms test results are streamed to the specified port on
+// the specified host machine.
+GTEST_DECLARE_string_(stream_result_to);
+
+// The upper limit for valid stack trace depths.
+const int kMaxStackTraceDepth = 100;
+
+namespace internal {
+
+class AssertHelper;
+class DefaultGlobalTestPartResultReporter;
+class ExecDeathTest;
+class NoExecDeathTest;
+class FinalSuccessChecker;
+class GTestFlagSaver;
+class StreamingListenerTest;
+class TestResultAccessor;
+class TestEventListenersAccessor;
+class TestEventRepeater;
+class UnitTestRecordPropertyTestHelper;
+class WindowsDeathTest;
+class UnitTestImpl* GetUnitTestImpl();
+void ReportFailureInUnknownLocation(TestPartResult::Type result_type,
+ const std::string& message);
+
+} // namespace internal
+
+// The friend relationship of some of these classes is cyclic.
+// If we don't forward declare them the compiler might confuse the classes
+// in friendship clauses with same named classes on the scope.
+class Test;
+class TestCase;
+class TestInfo;
+class UnitTest;
+
+// A class for indicating whether an assertion was successful. When
+// the assertion wasn't successful, the AssertionResult object
+// remembers a non-empty message that describes how it failed.
+//
+// To create an instance of this class, use one of the factory functions
+// (AssertionSuccess() and AssertionFailure()).
+//
+// This class is useful for two purposes:
+// 1. Defining predicate functions to be used with Boolean test assertions
+// EXPECT_TRUE/EXPECT_FALSE and their ASSERT_ counterparts
+// 2. Defining predicate-format functions to be
+// used with predicate assertions (ASSERT_PRED_FORMAT*, etc).
+//
+// For example, if you define IsEven predicate:
+//
+// testing::AssertionResult IsEven(int n) {
+// if ((n % 2) == 0)
+// return testing::AssertionSuccess();
+// else
+// return testing::AssertionFailure() << n << " is odd";
+// }
+//
+// Then the failed expectation EXPECT_TRUE(IsEven(Fib(5)))
+// will print the message
+//
+// Value of: IsEven(Fib(5))
+// Actual: false (5 is odd)
+// Expected: true
+//
+// instead of a more opaque
+//
+// Value of: IsEven(Fib(5))
+// Actual: false
+// Expected: true
+//
+// in case IsEven is a simple Boolean predicate.
+//
+// If you expect your predicate to be reused and want to support informative
+// messages in EXPECT_FALSE and ASSERT_FALSE (negative assertions show up
+// about half as often as positive ones in our tests), supply messages for
+// both success and failure cases:
+//
+// testing::AssertionResult IsEven(int n) {
+// if ((n % 2) == 0)
+// return testing::AssertionSuccess() << n << " is even";
+// else
+// return testing::AssertionFailure() << n << " is odd";
+// }
+//
+// Then a statement EXPECT_FALSE(IsEven(Fib(6))) will print
+//
+// Value of: IsEven(Fib(6))
+// Actual: true (8 is even)
+// Expected: false
+//
+// NB: Predicates that support negative Boolean assertions have reduced
+// performance in positive ones so be careful not to use them in tests
+// that have lots (tens of thousands) of positive Boolean assertions.
+//
+// To use this class with EXPECT_PRED_FORMAT assertions such as:
+//
+// // Verifies that Foo() returns an even number.
+// EXPECT_PRED_FORMAT1(IsEven, Foo());
+//
+// you need to define:
+//
+// testing::AssertionResult IsEven(const char* expr, int n) {
+// if ((n % 2) == 0)
+// return testing::AssertionSuccess();
+// else
+// return testing::AssertionFailure()
+// << "Expected: " << expr << " is even\n Actual: it's " << n;
+// }
+//
+// If Foo() returns 5, you will see the following message:
+//
+// Expected: Foo() is even
+// Actual: it's 5
+//
+class GTEST_API_ AssertionResult {
+ public:
+ // Copy constructor.
+ // Used in EXPECT_TRUE/FALSE(assertion_result).
+ AssertionResult(const AssertionResult& other);
+ // Used in the EXPECT_TRUE/FALSE(bool_expression).
+ explicit AssertionResult(bool success) : success_(success) {}
+
+ // Returns true iff the assertion succeeded.
+ operator bool() const { return success_; } // NOLINT
+
+ // Returns the assertion's negation. Used with EXPECT/ASSERT_FALSE.
+ AssertionResult operator!() const;
+
+ // Returns the text streamed into this AssertionResult. Test assertions
+ // use it when they fail (i.e., the predicate's outcome doesn't match the
+ // assertion's expectation). When nothing has been streamed into the
+ // object, returns an empty string.
+ const char* message() const {
+ return message_.get() != NULL ? message_->c_str() : "";
+ }
+ // TODO(vladl at google.com): Remove this after making sure no clients use it.
+ // Deprecated; please use message() instead.
+ const char* failure_message() const { return message(); }
+
+ // Streams a custom failure message into this object.
+ template <typename T> AssertionResult& operator<<(const T& value) {
+ AppendMessage(Message() << value);
+ return *this;
+ }
+
+ // Allows streaming basic output manipulators such as endl or flush into
+ // this object.
+ AssertionResult& operator<<(
+ ::std::ostream& (*basic_manipulator)(::std::ostream& stream)) {
+ AppendMessage(Message() << basic_manipulator);
+ return *this;
+ }
+
+ private:
+ // Appends the contents of message to message_.
+ void AppendMessage(const Message& a_message) {
+ if (message_.get() == NULL)
+ message_.reset(new ::std::string);
+ message_->append(a_message.GetString().c_str());
+ }
+
+ // Stores result of the assertion predicate.
+ bool success_;
+ // Stores the message describing the condition in case the expectation
+ // construct is not satisfied with the predicate's outcome.
+ // Referenced via a pointer to avoid taking too much stack frame space
+ // with test assertions.
+ internal::scoped_ptr< ::std::string> message_;
+
+ GTEST_DISALLOW_ASSIGN_(AssertionResult);
+};
+
+// Makes a successful assertion result.
+GTEST_API_ AssertionResult AssertionSuccess();
+
+// Makes a failed assertion result.
+GTEST_API_ AssertionResult AssertionFailure();
+
+// Makes a failed assertion result with the given failure message.
+// Deprecated; use AssertionFailure() << msg.
+GTEST_API_ AssertionResult AssertionFailure(const Message& msg);
+
+// The abstract class that all tests inherit from.
+//
+// In Google Test, a unit test program contains one or many TestCases, and
+// each TestCase contains one or many Tests.
+//
+// When you define a test using the TEST macro, you don't need to
+// explicitly derive from Test - the TEST macro automatically does
+// this for you.
+//
+// The only time you derive from Test is when defining a test fixture
+// to be used a TEST_F. For example:
+//
+// class FooTest : public testing::Test {
+// protected:
+// virtual void SetUp() { ... }
+// virtual void TearDown() { ... }
+// ...
+// };
+//
+// TEST_F(FooTest, Bar) { ... }
+// TEST_F(FooTest, Baz) { ... }
+//
+// Test is not copyable.
+class GTEST_API_ Test {
+ public:
+ friend class TestInfo;
+
+ // Defines types for pointers to functions that set up and tear down
+ // a test case.
+ typedef internal::SetUpTestCaseFunc SetUpTestCaseFunc;
+ typedef internal::TearDownTestCaseFunc TearDownTestCaseFunc;
+
+ // The d'tor is virtual as we intend to inherit from Test.
+ virtual ~Test();
+
+ // Sets up the stuff shared by all tests in this test case.
+ //
+ // Google Test will call Foo::SetUpTestCase() before running the first
+ // test in test case Foo. Hence a sub-class can define its own
+ // SetUpTestCase() method to shadow the one defined in the super
+ // class.
+ static void SetUpTestCase() {}
+
+ // Tears down the stuff shared by all tests in this test case.
+ //
+ // Google Test will call Foo::TearDownTestCase() after running the last
+ // test in test case Foo. Hence a sub-class can define its own
+ // TearDownTestCase() method to shadow the one defined in the super
+ // class.
+ static void TearDownTestCase() {}
+
+ // Returns true iff the current test has a fatal failure.
+ static bool HasFatalFailure();
+
+ // Returns true iff the current test has a non-fatal failure.
+ static bool HasNonfatalFailure();
+
+ // Returns true iff the current test has a (either fatal or
+ // non-fatal) failure.
+ static bool HasFailure() { return HasFatalFailure() || HasNonfatalFailure(); }
+
+ // Logs a property for the current test, test case, or for the entire
+ // invocation of the test program when used outside of the context of a
+ // test case. Only the last value for a given key is remembered. These
+ // are public static so they can be called from utility functions that are
+ // not members of the test fixture. Calls to RecordProperty made during
+ // lifespan of the test (from the moment its constructor starts to the
+ // moment its destructor finishes) will be output in XML as attributes of
+ // the <testcase> element. Properties recorded from fixture's
+ // SetUpTestCase or TearDownTestCase are logged as attributes of the
+ // corresponding <testsuite> element. Calls to RecordProperty made in the
+ // global context (before or after invocation of RUN_ALL_TESTS and from
+ // SetUp/TearDown method of Environment objects registered with Google
+ // Test) will be output as attributes of the <testsuites> element.
+ static void RecordProperty(const std::string& key, const std::string& value);
+ static void RecordProperty(const std::string& key, int value);
+
+ protected:
+ // Creates a Test object.
+ Test();
+
+ // Sets up the test fixture.
+ virtual void SetUp();
+
+ // Tears down the test fixture.
+ virtual void TearDown();
+
+ private:
+ // Returns true iff the current test has the same fixture class as
+ // the first test in the current test case.
+ static bool HasSameFixtureClass();
+
+ // Runs the test after the test fixture has been set up.
+ //
+ // A sub-class must implement this to define the test logic.
+ //
+ // DO NOT OVERRIDE THIS FUNCTION DIRECTLY IN A USER PROGRAM.
+ // Instead, use the TEST or TEST_F macro.
+ virtual void TestBody() = 0;
+
+ // Sets up, executes, and tears down the test.
+ void Run();
+
+ // Deletes self. We deliberately pick an unusual name for this
+ // internal method to avoid clashing with names used in user TESTs.
+ void DeleteSelf_() { delete this; }
+
+ // Uses a GTestFlagSaver to save and restore all Google Test flags.
+ const internal::GTestFlagSaver* const gtest_flag_saver_;
+
+ // Often a user mis-spells SetUp() as Setup() and spends a long time
+ // wondering why it is never called by Google Test. The declaration of
+ // the following method is solely for catching such an error at
+ // compile time:
+ //
+ // - The return type is deliberately chosen to be not void, so it
+ // will be a conflict if a user declares void Setup() in his test
+ // fixture.
+ //
+ // - This method is private, so it will be another compiler error
+ // if a user calls it from his test fixture.
+ //
+ // DO NOT OVERRIDE THIS FUNCTION.
+ //
+ // If you see an error about overriding the following function or
+ // about it being private, you have mis-spelled SetUp() as Setup().
+ struct Setup_should_be_spelled_SetUp {};
+ virtual Setup_should_be_spelled_SetUp* Setup() { return NULL; }
+
+ // We disallow copying Tests.
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(Test);
+};
+
+typedef internal::TimeInMillis TimeInMillis;
+
+// A copyable object representing a user specified test property which can be
+// output as a key/value string pair.
+//
+// Don't inherit from TestProperty as its destructor is not virtual.
+class TestProperty {
+ public:
+ // C'tor. TestProperty does NOT have a default constructor.
+ // Always use this constructor (with parameters) to create a
+ // TestProperty object.
+ TestProperty(const std::string& a_key, const std::string& a_value) :
+ key_(a_key), value_(a_value) {
+ }
+
+ // Gets the user supplied key.
+ const char* key() const {
+ return key_.c_str();
+ }
+
+ // Gets the user supplied value.
+ const char* value() const {
+ return value_.c_str();
+ }
+
+ // Sets a new value, overriding the one supplied in the constructor.
+ void SetValue(const std::string& new_value) {
+ value_ = new_value;
+ }
+
+ private:
+ // The key supplied by the user.
+ std::string key_;
+ // The value supplied by the user.
+ std::string value_;
+};
+
+// The result of a single Test. This includes a list of
+// TestPartResults, a list of TestProperties, a count of how many
+// death tests there are in the Test, and how much time it took to run
+// the Test.
+//
+// TestResult is not copyable.
+class GTEST_API_ TestResult {
+ public:
+ // Creates an empty TestResult.
+ TestResult();
+
+ // D'tor. Do not inherit from TestResult.
+ ~TestResult();
+
+ // Gets the number of all test parts. This is the sum of the number
+ // of successful test parts and the number of failed test parts.
+ int total_part_count() const;
+
+ // Returns the number of the test properties.
+ int test_property_count() const;
+
+ // Returns true iff the test passed (i.e. no test part failed).
+ bool Passed() const { return !Failed(); }
+
+ // Returns true iff the test failed.
+ bool Failed() const;
+
+ // Returns true iff the test fatally failed.
+ bool HasFatalFailure() const;
+
+ // Returns true iff the test has a non-fatal failure.
+ bool HasNonfatalFailure() const;
+
+ // Returns the elapsed time, in milliseconds.
+ TimeInMillis elapsed_time() const { return elapsed_time_; }
+
+ // Returns the i-th test part result among all the results. i can range
+ // from 0 to test_property_count() - 1. If i is not in that range, aborts
+ // the program.
+ const TestPartResult& GetTestPartResult(int i) const;
+
+ // Returns the i-th test property. i can range from 0 to
+ // test_property_count() - 1. If i is not in that range, aborts the
+ // program.
+ const TestProperty& GetTestProperty(int i) const;
+
+ private:
+ friend class TestInfo;
+ friend class TestCase;
+ friend class UnitTest;
+ friend class internal::DefaultGlobalTestPartResultReporter;
+ friend class internal::ExecDeathTest;
+ friend class internal::TestResultAccessor;
+ friend class internal::UnitTestImpl;
+ friend class internal::WindowsDeathTest;
+
+ // Gets the vector of TestPartResults.
+ const std::vector<TestPartResult>& test_part_results() const {
+ return test_part_results_;
+ }
+
+ // Gets the vector of TestProperties.
+ const std::vector<TestProperty>& test_properties() const {
+ return test_properties_;
+ }
+
+ // Sets the elapsed time.
+ void set_elapsed_time(TimeInMillis elapsed) { elapsed_time_ = elapsed; }
+
+ // Adds a test property to the list. The property is validated and may add
+ // a non-fatal failure if invalid (e.g., if it conflicts with reserved
+ // key names). If a property is already recorded for the same key, the
+ // value will be updated, rather than storing multiple values for the same
+ // key. xml_element specifies the element for which the property is being
+ // recorded and is used for validation.
+ void RecordProperty(const std::string& xml_element,
+ const TestProperty& test_property);
+
+ // Adds a failure if the key is a reserved attribute of Google Test
+ // testcase tags. Returns true if the property is valid.
+ // TODO(russr): Validate attribute names are legal and human readable.
+ static bool ValidateTestProperty(const std::string& xml_element,
+ const TestProperty& test_property);
+
+ // Adds a test part result to the list.
+ void AddTestPartResult(const TestPartResult& test_part_result);
+
+ // Returns the death test count.
+ int death_test_count() const { return death_test_count_; }
+
+ // Increments the death test count, returning the new count.
+ int increment_death_test_count() { return ++death_test_count_; }
+
+ // Clears the test part results.
+ void ClearTestPartResults();
+
+ // Clears the object.
+ void Clear();
+
+ // Protects mutable state of the property vector and of owned
+ // properties, whose values may be updated.
+ internal::Mutex test_properites_mutex_;
+
+ // The vector of TestPartResults
+ std::vector<TestPartResult> test_part_results_;
+ // The vector of TestProperties
+ std::vector<TestProperty> test_properties_;
+ // Running count of death tests.
+ int death_test_count_;
+ // The elapsed time, in milliseconds.
+ TimeInMillis elapsed_time_;
+
+ // We disallow copying TestResult.
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(TestResult);
+}; // class TestResult
+
+// A TestInfo object stores the following information about a test:
+//
+// Test case name
+// Test name
+// Whether the test should be run
+// A function pointer that creates the test object when invoked
+// Test result
+//
+// The constructor of TestInfo registers itself with the UnitTest
+// singleton such that the RUN_ALL_TESTS() macro knows which tests to
+// run.
+class GTEST_API_ TestInfo {
+ public:
+ // Destructs a TestInfo object. This function is not virtual, so
+ // don't inherit from TestInfo.
+ ~TestInfo();
+
+ // Returns the test case name.
+ const char* test_case_name() const { return test_case_name_.c_str(); }
+
+ // Returns the test name.
+ const char* name() const { return name_.c_str(); }
+
+ // Returns the name of the parameter type, or NULL if this is not a typed
+ // or a type-parameterized test.
+ const char* type_param() const {
+ if (type_param_.get() != NULL)
+ return type_param_->c_str();
+ return NULL;
+ }
+
+ // Returns the text representation of the value parameter, or NULL if this
+ // is not a value-parameterized test.
+ const char* value_param() const {
+ if (value_param_.get() != NULL)
+ return value_param_->c_str();
+ return NULL;
+ }
+
+ // Returns true if this test should run, that is if the test is not
+ // disabled (or it is disabled but the also_run_disabled_tests flag has
+ // been specified) and its full name matches the user-specified filter.
+ //
+ // Google Test allows the user to filter the tests by their full names.
+ // The full name of a test Bar in test case Foo is defined as
+ // "Foo.Bar". Only the tests that match the filter will run.
+ //
+ // A filter is a colon-separated list of glob (not regex) patterns,
+ // optionally followed by a '-' and a colon-separated list of
+ // negative patterns (tests to exclude). A test is run if it
+ // matches one of the positive patterns and does not match any of
+ // the negative patterns.
+ //
+ // For example, *A*:Foo.* is a filter that matches any string that
+ // contains the character 'A' or starts with "Foo.".
+ bool should_run() const { return should_run_; }
+
+ // Returns true iff this test will appear in the XML report.
+ bool is_reportable() const {
+ // For now, the XML report includes all tests matching the filter.
+ // In the future, we may trim tests that are excluded because of
+ // sharding.
+ return matches_filter_;
+ }
+
+ // Returns the result of the test.
+ const TestResult* result() const { return &result_; }
+
+ private:
+#if GTEST_HAS_DEATH_TEST
+ friend class internal::DefaultDeathTestFactory;
+#endif // GTEST_HAS_DEATH_TEST
+ friend class Test;
+ friend class TestCase;
+ friend class internal::UnitTestImpl;
+ friend class internal::StreamingListenerTest;
+ friend TestInfo* internal::MakeAndRegisterTestInfo(
+ const char* test_case_name,
+ const char* name,
+ const char* type_param,
+ const char* value_param,
+ internal::TypeId fixture_class_id,
+ Test::SetUpTestCaseFunc set_up_tc,
+ Test::TearDownTestCaseFunc tear_down_tc,
+ internal::TestFactoryBase* factory);
+
+ // Constructs a TestInfo object. The newly constructed instance assumes
+ // ownership of the factory object.
+ TestInfo(const std::string& test_case_name,
+ const std::string& name,
+ const char* a_type_param, // NULL if not a type-parameterized test
+ const char* a_value_param, // NULL if not a value-parameterized test
+ internal::TypeId fixture_class_id,
+ internal::TestFactoryBase* factory);
+
+ // Increments the number of death tests encountered in this test so
+ // far.
+ int increment_death_test_count() {
+ return result_.increment_death_test_count();
+ }
+
+ // Creates the test object, runs it, records its result, and then
+ // deletes it.
+ void Run();
+
+ static void ClearTestResult(TestInfo* test_info) {
+ test_info->result_.Clear();
+ }
+
+ // These fields are immutable properties of the test.
+ const std::string test_case_name_; // Test case name
+ const std::string name_; // Test name
+ // Name of the parameter type, or NULL if this is not a typed or a
+ // type-parameterized test.
+ const internal::scoped_ptr<const ::std::string> type_param_;
+ // Text representation of the value parameter, or NULL if this is not a
+ // value-parameterized test.
+ const internal::scoped_ptr<const ::std::string> value_param_;
+ const internal::TypeId fixture_class_id_; // ID of the test fixture class
+ bool should_run_; // True iff this test should run
+ bool is_disabled_; // True iff this test is disabled
+ bool matches_filter_; // True if this test matches the
+ // user-specified filter.
+ internal::TestFactoryBase* const factory_; // The factory that creates
+ // the test object
+
+ // This field is mutable and needs to be reset before running the
+ // test for the second time.
+ TestResult result_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(TestInfo);
+};
+
+// A test case, which consists of a vector of TestInfos.
+//
+// TestCase is not copyable.
+class GTEST_API_ TestCase {
+ public:
+ // Creates a TestCase with the given name.
+ //
+ // TestCase does NOT have a default constructor. Always use this
+ // constructor to create a TestCase object.
+ //
+ // Arguments:
+ //
+ // name: name of the test case
+ // a_type_param: the name of the test's type parameter, or NULL if
+ // this is not a type-parameterized test.
+ // set_up_tc: pointer to the function that sets up the test case
+ // tear_down_tc: pointer to the function that tears down the test case
+ TestCase(const char* name, const char* a_type_param,
+ Test::SetUpTestCaseFunc set_up_tc,
+ Test::TearDownTestCaseFunc tear_down_tc);
+
+ // Destructor of TestCase.
+ virtual ~TestCase();
+
+ // Gets the name of the TestCase.
+ const char* name() const { return name_.c_str(); }
+
+ // Returns the name of the parameter type, or NULL if this is not a
+ // type-parameterized test case.
+ const char* type_param() const {
+ if (type_param_.get() != NULL)
+ return type_param_->c_str();
+ return NULL;
+ }
+
+ // Returns true if any test in this test case should run.
+ bool should_run() const { return should_run_; }
+
+ // Gets the number of successful tests in this test case.
+ int successful_test_count() const;
+
+ // Gets the number of failed tests in this test case.
+ int failed_test_count() const;
+
+ // Gets the number of disabled tests that will be reported in the XML report.
+ int reportable_disabled_test_count() const;
+
+ // Gets the number of disabled tests in this test case.
+ int disabled_test_count() const;
+
+ // Gets the number of tests to be printed in the XML report.
+ int reportable_test_count() const;
+
+ // Get the number of tests in this test case that should run.
+ int test_to_run_count() const;
+
+ // Gets the number of all tests in this test case.
+ int total_test_count() const;
+
+ // Returns true iff the test case passed.
+ bool Passed() const { return !Failed(); }
+
+ // Returns true iff the test case failed.
+ bool Failed() const { return failed_test_count() > 0; }
+
+ // Returns the elapsed time, in milliseconds.
+ TimeInMillis elapsed_time() const { return elapsed_time_; }
+
+ // Returns the i-th test among all the tests. i can range from 0 to
+ // total_test_count() - 1. If i is not in that range, returns NULL.
+ const TestInfo* GetTestInfo(int i) const;
+
+ // Returns the TestResult that holds test properties recorded during
+ // execution of SetUpTestCase and TearDownTestCase.
+ const TestResult& ad_hoc_test_result() const { return ad_hoc_test_result_; }
+
+ private:
+ friend class Test;
+ friend class internal::UnitTestImpl;
+
+ // Gets the (mutable) vector of TestInfos in this TestCase.
+ std::vector<TestInfo*>& test_info_list() { return test_info_list_; }
+
+ // Gets the (immutable) vector of TestInfos in this TestCase.
+ const std::vector<TestInfo*>& test_info_list() const {
+ return test_info_list_;
+ }
+
+ // Returns the i-th test among all the tests. i can range from 0 to
+ // total_test_count() - 1. If i is not in that range, returns NULL.
+ TestInfo* GetMutableTestInfo(int i);
+
+ // Sets the should_run member.
+ void set_should_run(bool should) { should_run_ = should; }
+
+ // Adds a TestInfo to this test case. Will delete the TestInfo upon
+ // destruction of the TestCase object.
+ void AddTestInfo(TestInfo * test_info);
+
+ // Clears the results of all tests in this test case.
+ void ClearResult();
+
+ // Clears the results of all tests in the given test case.
+ static void ClearTestCaseResult(TestCase* test_case) {
+ test_case->ClearResult();
+ }
+
+ // Runs every test in this TestCase.
+ void Run();
+
+ // Runs SetUpTestCase() for this TestCase. This wrapper is needed
+ // for catching exceptions thrown from SetUpTestCase().
+ void RunSetUpTestCase() { (*set_up_tc_)(); }
+
+ // Runs TearDownTestCase() for this TestCase. This wrapper is
+ // needed for catching exceptions thrown from TearDownTestCase().
+ void RunTearDownTestCase() { (*tear_down_tc_)(); }
+
+ // Returns true iff test passed.
+ static bool TestPassed(const TestInfo* test_info) {
+ return test_info->should_run() && test_info->result()->Passed();
+ }
+
+ // Returns true iff test failed.
+ static bool TestFailed(const TestInfo* test_info) {
+ return test_info->should_run() && test_info->result()->Failed();
+ }
+
+ // Returns true iff the test is disabled and will be reported in the XML
+ // report.
+ static bool TestReportableDisabled(const TestInfo* test_info) {
+ return test_info->is_reportable() && test_info->is_disabled_;
+ }
+
+ // Returns true iff test is disabled.
+ static bool TestDisabled(const TestInfo* test_info) {
+ return test_info->is_disabled_;
+ }
+
+ // Returns true iff this test will appear in the XML report.
+ static bool TestReportable(const TestInfo* test_info) {
+ return test_info->is_reportable();
+ }
+
+ // Returns true if the given test should run.
+ static bool ShouldRunTest(const TestInfo* test_info) {
+ return test_info->should_run();
+ }
+
+ // Shuffles the tests in this test case.
+ void ShuffleTests(internal::Random* random);
+
+ // Restores the test order to before the first shuffle.
+ void UnshuffleTests();
+
+ // Name of the test case.
+ std::string name_;
+ // Name of the parameter type, or NULL if this is not a typed or a
+ // type-parameterized test.
+ const internal::scoped_ptr<const ::std::string> type_param_;
+ // The vector of TestInfos in their original order. It owns the
+ // elements in the vector.
+ std::vector<TestInfo*> test_info_list_;
+ // Provides a level of indirection for the test list to allow easy
+ // shuffling and restoring the test order. The i-th element in this
+ // vector is the index of the i-th test in the shuffled test list.
+ std::vector<int> test_indices_;
+ // Pointer to the function that sets up the test case.
+ Test::SetUpTestCaseFunc set_up_tc_;
+ // Pointer to the function that tears down the test case.
+ Test::TearDownTestCaseFunc tear_down_tc_;
+ // True iff any test in this test case should run.
+ bool should_run_;
+ // Elapsed time, in milliseconds.
+ TimeInMillis elapsed_time_;
+ // Holds test properties recorded during execution of SetUpTestCase and
+ // TearDownTestCase.
+ TestResult ad_hoc_test_result_;
+
+ // We disallow copying TestCases.
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(TestCase);
+};
+
+// An Environment object is capable of setting up and tearing down an
+// environment. The user should subclass this to define his own
+// environment(s).
+//
+// An Environment object does the set-up and tear-down in virtual
+// methods SetUp() and TearDown() instead of the constructor and the
+// destructor, as:
+//
+// 1. You cannot safely throw from a destructor. This is a problem
+// as in some cases Google Test is used where exceptions are enabled, and
+// we may want to implement ASSERT_* using exceptions where they are
+// available.
+// 2. You cannot use ASSERT_* directly in a constructor or
+// destructor.
+class Environment {
+ public:
+ // The d'tor is virtual as we need to subclass Environment.
+ virtual ~Environment() {}
+
+ // Override this to define how to set up the environment.
+ virtual void SetUp() {}
+
+ // Override this to define how to tear down the environment.
+ virtual void TearDown() {}
+ private:
+ // If you see an error about overriding the following function or
+ // about it being private, you have mis-spelled SetUp() as Setup().
+ struct Setup_should_be_spelled_SetUp {};
+ virtual Setup_should_be_spelled_SetUp* Setup() { return NULL; }
+};
+
+// The interface for tracing execution of tests. The methods are organized in
+// the order the corresponding events are fired.
+class TestEventListener {
+ public:
+ virtual ~TestEventListener() {}
+
+ // Fired before any test activity starts.
+ virtual void OnTestProgramStart(const UnitTest& unit_test) = 0;
+
+ // Fired before each iteration of tests starts. There may be more than
+ // one iteration if GTEST_FLAG(repeat) is set. iteration is the iteration
+ // index, starting from 0.
+ virtual void OnTestIterationStart(const UnitTest& unit_test,
+ int iteration) = 0;
+
+ // Fired before environment set-up for each iteration of tests starts.
+ virtual void OnEnvironmentsSetUpStart(const UnitTest& unit_test) = 0;
+
+ // Fired after environment set-up for each iteration of tests ends.
+ virtual void OnEnvironmentsSetUpEnd(const UnitTest& unit_test) = 0;
+
+ // Fired before the test case starts.
+ virtual void OnTestCaseStart(const TestCase& test_case) = 0;
+
+ // Fired before the test starts.
+ virtual void OnTestStart(const TestInfo& test_info) = 0;
+
+ // Fired after a failed assertion or a SUCCEED() invocation.
+ virtual void OnTestPartResult(const TestPartResult& test_part_result) = 0;
+
+ // Fired after the test ends.
+ virtual void OnTestEnd(const TestInfo& test_info) = 0;
+
+ // Fired after the test case ends.
+ virtual void OnTestCaseEnd(const TestCase& test_case) = 0;
+
+ // Fired before environment tear-down for each iteration of tests starts.
+ virtual void OnEnvironmentsTearDownStart(const UnitTest& unit_test) = 0;
+
+ // Fired after environment tear-down for each iteration of tests ends.
+ virtual void OnEnvironmentsTearDownEnd(const UnitTest& unit_test) = 0;
+
+ // Fired after each iteration of tests finishes.
+ virtual void OnTestIterationEnd(const UnitTest& unit_test,
+ int iteration) = 0;
+
+ // Fired after all test activities have ended.
+ virtual void OnTestProgramEnd(const UnitTest& unit_test) = 0;
+};
+
+// The convenience class for users who need to override just one or two
+// methods and are not concerned that a possible change to a signature of
+// the methods they override will not be caught during the build. For
+// comments about each method please see the definition of TestEventListener
+// above.
+class EmptyTestEventListener : public TestEventListener {
+ public:
+ virtual void OnTestProgramStart(const UnitTest& /*unit_test*/) {}
+ virtual void OnTestIterationStart(const UnitTest& /*unit_test*/,
+ int /*iteration*/) {}
+ virtual void OnEnvironmentsSetUpStart(const UnitTest& /*unit_test*/) {}
+ virtual void OnEnvironmentsSetUpEnd(const UnitTest& /*unit_test*/) {}
+ virtual void OnTestCaseStart(const TestCase& /*test_case*/) {}
+ virtual void OnTestStart(const TestInfo& /*test_info*/) {}
+ virtual void OnTestPartResult(const TestPartResult& /*test_part_result*/) {}
+ virtual void OnTestEnd(const TestInfo& /*test_info*/) {}
+ virtual void OnTestCaseEnd(const TestCase& /*test_case*/) {}
+ virtual void OnEnvironmentsTearDownStart(const UnitTest& /*unit_test*/) {}
+ virtual void OnEnvironmentsTearDownEnd(const UnitTest& /*unit_test*/) {}
+ virtual void OnTestIterationEnd(const UnitTest& /*unit_test*/,
+ int /*iteration*/) {}
+ virtual void OnTestProgramEnd(const UnitTest& /*unit_test*/) {}
+};
+
+// TestEventListeners lets users add listeners to track events in Google Test.
+class GTEST_API_ TestEventListeners {
+ public:
+ TestEventListeners();
+ ~TestEventListeners();
+
+ // Appends an event listener to the end of the list. Google Test assumes
+ // the ownership of the listener (i.e. it will delete the listener when
+ // the test program finishes).
+ void Append(TestEventListener* listener);
+
+ // Removes the given event listener from the list and returns it. It then
+ // becomes the caller's responsibility to delete the listener. Returns
+ // NULL if the listener is not found in the list.
+ TestEventListener* Release(TestEventListener* listener);
+
+ // Returns the standard listener responsible for the default console
+ // output. Can be removed from the listeners list to shut down default
+ // console output. Note that removing this object from the listener list
+ // with Release transfers its ownership to the caller and makes this
+ // function return NULL the next time.
+ TestEventListener* default_result_printer() const {
+ return default_result_printer_;
+ }
+
+ // Returns the standard listener responsible for the default XML output
+ // controlled by the --gtest_output=xml flag. Can be removed from the
+ // listeners list by users who want to shut down the default XML output
+ // controlled by this flag and substitute it with custom one. Note that
+ // removing this object from the listener list with Release transfers its
+ // ownership to the caller and makes this function return NULL the next
+ // time.
+ TestEventListener* default_xml_generator() const {
+ return default_xml_generator_;
+ }
+
+ private:
+ friend class TestCase;
+ friend class TestInfo;
+ friend class internal::DefaultGlobalTestPartResultReporter;
+ friend class internal::NoExecDeathTest;
+ friend class internal::TestEventListenersAccessor;
+ friend class internal::UnitTestImpl;
+
+ // Returns repeater that broadcasts the TestEventListener events to all
+ // subscribers.
+ TestEventListener* repeater();
+
+ // Sets the default_result_printer attribute to the provided listener.
+ // The listener is also added to the listener list and previous
+ // default_result_printer is removed from it and deleted. The listener can
+ // also be NULL in which case it will not be added to the list. Does
+ // nothing if the previous and the current listener objects are the same.
+ void SetDefaultResultPrinter(TestEventListener* listener);
+
+ // Sets the default_xml_generator attribute to the provided listener. The
+ // listener is also added to the listener list and previous
+ // default_xml_generator is removed from it and deleted. The listener can
+ // also be NULL in which case it will not be added to the list. Does
+ // nothing if the previous and the current listener objects are the same.
+ void SetDefaultXmlGenerator(TestEventListener* listener);
+
+ // Controls whether events will be forwarded by the repeater to the
+ // listeners in the list.
+ bool EventForwardingEnabled() const;
+ void SuppressEventForwarding();
+
+ // The actual list of listeners.
+ internal::TestEventRepeater* repeater_;
+ // Listener responsible for the standard result output.
+ TestEventListener* default_result_printer_;
+ // Listener responsible for the creation of the XML output file.
+ TestEventListener* default_xml_generator_;
+
+ // We disallow copying TestEventListeners.
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(TestEventListeners);
+};
+
+// A UnitTest consists of a vector of TestCases.
+//
+// This is a singleton class. The only instance of UnitTest is
+// created when UnitTest::GetInstance() is first called. This
+// instance is never deleted.
+//
+// UnitTest is not copyable.
+//
+// This class is thread-safe as long as the methods are called
+// according to their specification.
+class GTEST_API_ UnitTest {
+ public:
+ // Gets the singleton UnitTest object. The first time this method
+ // is called, a UnitTest object is constructed and returned.
+ // Consecutive calls will return the same object.
+ static UnitTest* GetInstance();
+
+ // Runs all tests in this UnitTest object and prints the result.
+ // Returns 0 if successful, or 1 otherwise.
+ //
+ // This method can only be called from the main thread.
+ //
+ // INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+ int Run() GTEST_MUST_USE_RESULT_;
+
+ // Returns the working directory when the first TEST() or TEST_F()
+ // was executed. The UnitTest object owns the string.
+ const char* original_working_dir() const;
+
+ // Returns the TestCase object for the test that's currently running,
+ // or NULL if no test is running.
+ const TestCase* current_test_case() const
+ GTEST_LOCK_EXCLUDED_(mutex_);
+
+ // Returns the TestInfo object for the test that's currently running,
+ // or NULL if no test is running.
+ const TestInfo* current_test_info() const
+ GTEST_LOCK_EXCLUDED_(mutex_);
+
+ // Returns the random seed used at the start of the current test run.
+ int random_seed() const;
+
+#if GTEST_HAS_PARAM_TEST
+ // Returns the ParameterizedTestCaseRegistry object used to keep track of
+ // value-parameterized tests and instantiate and register them.
+ //
+ // INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+ internal::ParameterizedTestCaseRegistry& parameterized_test_registry()
+ GTEST_LOCK_EXCLUDED_(mutex_);
+#endif // GTEST_HAS_PARAM_TEST
+
+ // Gets the number of successful test cases.
+ int successful_test_case_count() const;
+
+ // Gets the number of failed test cases.
+ int failed_test_case_count() const;
+
+ // Gets the number of all test cases.
+ int total_test_case_count() const;
+
+ // Gets the number of all test cases that contain at least one test
+ // that should run.
+ int test_case_to_run_count() const;
+
+ // Gets the number of successful tests.
+ int successful_test_count() const;
+
+ // Gets the number of failed tests.
+ int failed_test_count() const;
+
+ // Gets the number of disabled tests that will be reported in the XML report.
+ int reportable_disabled_test_count() const;
+
+ // Gets the number of disabled tests.
+ int disabled_test_count() const;
+
+ // Gets the number of tests to be printed in the XML report.
+ int reportable_test_count() const;
+
+ // Gets the number of all tests.
+ int total_test_count() const;
+
+ // Gets the number of tests that should run.
+ int test_to_run_count() const;
+
+ // Gets the time of the test program start, in ms from the start of the
+ // UNIX epoch.
+ TimeInMillis start_timestamp() const;
+
+ // Gets the elapsed time, in milliseconds.
+ TimeInMillis elapsed_time() const;
+
+ // Returns true iff the unit test passed (i.e. all test cases passed).
+ bool Passed() const;
+
+ // Returns true iff the unit test failed (i.e. some test case failed
+ // or something outside of all tests failed).
+ bool Failed() const;
+
+ // Gets the i-th test case among all the test cases. i can range from 0 to
+ // total_test_case_count() - 1. If i is not in that range, returns NULL.
+ const TestCase* GetTestCase(int i) const;
+
+ // Returns the TestResult containing information on test failures and
+ // properties logged outside of individual test cases.
+ const TestResult& ad_hoc_test_result() const;
+
+ // Returns the list of event listeners that can be used to track events
+ // inside Google Test.
+ TestEventListeners& listeners();
+
+ private:
+ // Registers and returns a global test environment. When a test
+ // program is run, all global test environments will be set-up in
+ // the order they were registered. After all tests in the program
+ // have finished, all global test environments will be torn-down in
+ // the *reverse* order they were registered.
+ //
+ // The UnitTest object takes ownership of the given environment.
+ //
+ // This method can only be called from the main thread.
+ Environment* AddEnvironment(Environment* env);
+
+ // Adds a TestPartResult to the current TestResult object. All
+ // Google Test assertion macros (e.g. ASSERT_TRUE, EXPECT_EQ, etc)
+ // eventually call this to report their results. The user code
+ // should use the assertion macros instead of calling this directly.
+ void AddTestPartResult(TestPartResult::Type result_type,
+ const char* file_name,
+ int line_number,
+ const std::string& message,
+ const std::string& os_stack_trace)
+ GTEST_LOCK_EXCLUDED_(mutex_);
+
+ // Adds a TestProperty to the current TestResult object when invoked from
+ // inside a test, to current TestCase's ad_hoc_test_result_ when invoked
+ // from SetUpTestCase or TearDownTestCase, or to the global property set
+ // when invoked elsewhere. If the result already contains a property with
+ // the same key, the value will be updated.
+ void RecordProperty(const std::string& key, const std::string& value);
+
+ // Gets the i-th test case among all the test cases. i can range from 0 to
+ // total_test_case_count() - 1. If i is not in that range, returns NULL.
+ TestCase* GetMutableTestCase(int i);
+
+ // Accessors for the implementation object.
+ internal::UnitTestImpl* impl() { return impl_; }
+ const internal::UnitTestImpl* impl() const { return impl_; }
+
+ // These classes and funcions are friends as they need to access private
+ // members of UnitTest.
+ friend class Test;
+ friend class internal::AssertHelper;
+ friend class internal::ScopedTrace;
+ friend class internal::StreamingListenerTest;
+ friend class internal::UnitTestRecordPropertyTestHelper;
+ friend Environment* AddGlobalTestEnvironment(Environment* env);
+ friend internal::UnitTestImpl* internal::GetUnitTestImpl();
+ friend void internal::ReportFailureInUnknownLocation(
+ TestPartResult::Type result_type,
+ const std::string& message);
+
+ // Creates an empty UnitTest.
+ UnitTest();
+
+ // D'tor
+ virtual ~UnitTest();
+
+ // Pushes a trace defined by SCOPED_TRACE() on to the per-thread
+ // Google Test trace stack.
+ void PushGTestTrace(const internal::TraceInfo& trace)
+ GTEST_LOCK_EXCLUDED_(mutex_);
+
+ // Pops a trace from the per-thread Google Test trace stack.
+ void PopGTestTrace()
+ GTEST_LOCK_EXCLUDED_(mutex_);
+
+ // Protects mutable state in *impl_. This is mutable as some const
+ // methods need to lock it too.
+ mutable internal::Mutex mutex_;
+
+ // Opaque implementation object. This field is never changed once
+ // the object is constructed. We don't mark it as const here, as
+ // doing so will cause a warning in the constructor of UnitTest.
+ // Mutable state in *impl_ is protected by mutex_.
+ internal::UnitTestImpl* impl_;
+
+ // We disallow copying UnitTest.
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(UnitTest);
+};
+
+// A convenient wrapper for adding an environment for the test
+// program.
+//
+// You should call this before RUN_ALL_TESTS() is called, probably in
+// main(). If you use gtest_main, you need to call this before main()
+// starts for it to take effect. For example, you can define a global
+// variable like this:
+//
+// testing::Environment* const foo_env =
+// testing::AddGlobalTestEnvironment(new FooEnvironment);
+//
+// However, we strongly recommend you to write your own main() and
+// call AddGlobalTestEnvironment() there, as relying on initialization
+// of global variables makes the code harder to read and may cause
+// problems when you register multiple environments from different
+// translation units and the environments have dependencies among them
+// (remember that the compiler doesn't guarantee the order in which
+// global variables from different translation units are initialized).
+inline Environment* AddGlobalTestEnvironment(Environment* env) {
+ return UnitTest::GetInstance()->AddEnvironment(env);
+}
+
+// Initializes Google Test. This must be called before calling
+// RUN_ALL_TESTS(). In particular, it parses a command line for the
+// flags that Google Test recognizes. Whenever a Google Test flag is
+// seen, it is removed from argv, and *argc is decremented.
+//
+// No value is returned. Instead, the Google Test flag variables are
+// updated.
+//
+// Calling the function for the second time has no user-visible effect.
+GTEST_API_ void InitGoogleTest(int* argc, char** argv);
+
+// This overloaded version can be used in Windows programs compiled in
+// UNICODE mode.
+GTEST_API_ void InitGoogleTest(int* argc, wchar_t** argv);
+
+namespace internal {
+
+// FormatForComparison<ToPrint, OtherOperand>::Format(value) formats a
+// value of type ToPrint that is an operand of a comparison assertion
+// (e.g. ASSERT_EQ). OtherOperand is the type of the other operand in
+// the comparison, and is used to help determine the best way to
+// format the value. In particular, when the value is a C string
+// (char pointer) and the other operand is an STL string object, we
+// want to format the C string as a string, since we know it is
+// compared by value with the string object. If the value is a char
+// pointer but the other operand is not an STL string object, we don't
+// know whether the pointer is supposed to point to a NUL-terminated
+// string, and thus want to print it as a pointer to be safe.
+//
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+
+// The default case.
+template <typename ToPrint, typename OtherOperand>
+class FormatForComparison {
+ public:
+ static ::std::string Format(const ToPrint& value) {
+ return ::testing::PrintToString(value);
+ }
+};
+
+// Array.
+template <typename ToPrint, size_t N, typename OtherOperand>
+class FormatForComparison<ToPrint[N], OtherOperand> {
+ public:
+ static ::std::string Format(const ToPrint* value) {
+ return FormatForComparison<const ToPrint*, OtherOperand>::Format(value);
+ }
+};
+
+// By default, print C string as pointers to be safe, as we don't know
+// whether they actually point to a NUL-terminated string.
+
+#define GTEST_IMPL_FORMAT_C_STRING_AS_POINTER_(CharType) \
+ template <typename OtherOperand> \
+ class FormatForComparison<CharType*, OtherOperand> { \
+ public: \
+ static ::std::string Format(CharType* value) { \
+ return ::testing::PrintToString(static_cast<const void*>(value)); \
+ } \
+ }
+
+GTEST_IMPL_FORMAT_C_STRING_AS_POINTER_(char);
+GTEST_IMPL_FORMAT_C_STRING_AS_POINTER_(const char);
+GTEST_IMPL_FORMAT_C_STRING_AS_POINTER_(wchar_t);
+GTEST_IMPL_FORMAT_C_STRING_AS_POINTER_(const wchar_t);
+
+#undef GTEST_IMPL_FORMAT_C_STRING_AS_POINTER_
+
+// If a C string is compared with an STL string object, we know it's meant
+// to point to a NUL-terminated string, and thus can print it as a string.
+
+#define GTEST_IMPL_FORMAT_C_STRING_AS_STRING_(CharType, OtherStringType) \
+ template <> \
+ class FormatForComparison<CharType*, OtherStringType> { \
+ public: \
+ static ::std::string Format(CharType* value) { \
+ return ::testing::PrintToString(value); \
+ } \
+ }
+
+GTEST_IMPL_FORMAT_C_STRING_AS_STRING_(char, ::std::string);
+GTEST_IMPL_FORMAT_C_STRING_AS_STRING_(const char, ::std::string);
+
+#if GTEST_HAS_GLOBAL_STRING
+GTEST_IMPL_FORMAT_C_STRING_AS_STRING_(char, ::string);
+GTEST_IMPL_FORMAT_C_STRING_AS_STRING_(const char, ::string);
+#endif
+
+#if GTEST_HAS_GLOBAL_WSTRING
+GTEST_IMPL_FORMAT_C_STRING_AS_STRING_(wchar_t, ::wstring);
+GTEST_IMPL_FORMAT_C_STRING_AS_STRING_(const wchar_t, ::wstring);
+#endif
+
+#if GTEST_HAS_STD_WSTRING
+GTEST_IMPL_FORMAT_C_STRING_AS_STRING_(wchar_t, ::std::wstring);
+GTEST_IMPL_FORMAT_C_STRING_AS_STRING_(const wchar_t, ::std::wstring);
+#endif
+
+#undef GTEST_IMPL_FORMAT_C_STRING_AS_STRING_
+
+// Formats a comparison assertion (e.g. ASSERT_EQ, EXPECT_LT, and etc)
+// operand to be used in a failure message. The type (but not value)
+// of the other operand may affect the format. This allows us to
+// print a char* as a raw pointer when it is compared against another
+// char* or void*, and print it as a C string when it is compared
+// against an std::string object, for example.
+//
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+template <typename T1, typename T2>
+std::string FormatForComparisonFailureMessage(
+ const T1& value, const T2& /* other_operand */) {
+ return FormatForComparison<T1, T2>::Format(value);
+}
+
+// The helper function for {ASSERT|EXPECT}_EQ.
+template <typename T1, typename T2>
+AssertionResult CmpHelperEQ(const char* expected_expression,
+ const char* actual_expression,
+ const T1& expected,
+ const T2& actual) {
+#ifdef _MSC_VER
+# pragma warning(push) // Saves the current warning state.
+# pragma warning(disable:4389) // Temporarily disables warning on
+ // signed/unsigned mismatch.
+#endif
+
+ if (expected == actual) {
+ return AssertionSuccess();
+ }
+
+#ifdef _MSC_VER
+# pragma warning(pop) // Restores the warning state.
+#endif
+
+ return EqFailure(expected_expression,
+ actual_expression,
+ FormatForComparisonFailureMessage(expected, actual),
+ FormatForComparisonFailureMessage(actual, expected),
+ false);
+}
+
+// With this overloaded version, we allow anonymous enums to be used
+// in {ASSERT|EXPECT}_EQ when compiled with gcc 4, as anonymous enums
+// can be implicitly cast to BiggestInt.
+GTEST_API_ AssertionResult CmpHelperEQ(const char* expected_expression,
+ const char* actual_expression,
+ BiggestInt expected,
+ BiggestInt actual);
+
+// The helper class for {ASSERT|EXPECT}_EQ. The template argument
+// lhs_is_null_literal is true iff the first argument to ASSERT_EQ()
+// is a null pointer literal. The following default implementation is
+// for lhs_is_null_literal being false.
+template <bool lhs_is_null_literal>
+class EqHelper {
+ public:
+ // This templatized version is for the general case.
+ template <typename T1, typename T2>
+ static AssertionResult Compare(const char* expected_expression,
+ const char* actual_expression,
+ const T1& expected,
+ const T2& actual) {
+ return CmpHelperEQ(expected_expression, actual_expression, expected,
+ actual);
+ }
+
+ // With this overloaded version, we allow anonymous enums to be used
+ // in {ASSERT|EXPECT}_EQ when compiled with gcc 4, as anonymous
+ // enums can be implicitly cast to BiggestInt.
+ //
+ // Even though its body looks the same as the above version, we
+ // cannot merge the two, as it will make anonymous enums unhappy.
+ static AssertionResult Compare(const char* expected_expression,
+ const char* actual_expression,
+ BiggestInt expected,
+ BiggestInt actual) {
+ return CmpHelperEQ(expected_expression, actual_expression, expected,
+ actual);
+ }
+};
+
+// This specialization is used when the first argument to ASSERT_EQ()
+// is a null pointer literal, like NULL, false, or 0.
+template <>
+class EqHelper<true> {
+ public:
+ // We define two overloaded versions of Compare(). The first
+ // version will be picked when the second argument to ASSERT_EQ() is
+ // NOT a pointer, e.g. ASSERT_EQ(0, AnIntFunction()) or
+ // EXPECT_EQ(false, a_bool).
+ template <typename T1, typename T2>
+ static AssertionResult Compare(
+ const char* expected_expression,
+ const char* actual_expression,
+ const T1& expected,
+ const T2& actual,
+ // The following line prevents this overload from being considered if T2
+ // is not a pointer type. We need this because ASSERT_EQ(NULL, my_ptr)
+ // expands to Compare("", "", NULL, my_ptr), which requires a conversion
+ // to match the Secret* in the other overload, which would otherwise make
+ // this template match better.
+ typename EnableIf<!is_pointer<T2>::value>::type* = 0) {
+ return CmpHelperEQ(expected_expression, actual_expression, expected,
+ actual);
+ }
+
+ // This version will be picked when the second argument to ASSERT_EQ() is a
+ // pointer, e.g. ASSERT_EQ(NULL, a_pointer).
+ template <typename T>
+ static AssertionResult Compare(
+ const char* expected_expression,
+ const char* actual_expression,
+ // We used to have a second template parameter instead of Secret*. That
+ // template parameter would deduce to 'long', making this a better match
+ // than the first overload even without the first overload's EnableIf.
+ // Unfortunately, gcc with -Wconversion-null warns when "passing NULL to
+ // non-pointer argument" (even a deduced integral argument), so the old
+ // implementation caused warnings in user code.
+ Secret* /* expected (NULL) */,
+ T* actual) {
+ // We already know that 'expected' is a null pointer.
+ return CmpHelperEQ(expected_expression, actual_expression,
+ static_cast<T*>(NULL), actual);
+ }
+};
+
+// A macro for implementing the helper functions needed to implement
+// ASSERT_?? and EXPECT_??. It is here just to avoid copy-and-paste
+// of similar code.
+//
+// For each templatized helper function, we also define an overloaded
+// version for BiggestInt in order to reduce code bloat and allow
+// anonymous enums to be used with {ASSERT|EXPECT}_?? when compiled
+// with gcc 4.
+//
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+#define GTEST_IMPL_CMP_HELPER_(op_name, op)\
+template <typename T1, typename T2>\
+AssertionResult CmpHelper##op_name(const char* expr1, const char* expr2, \
+ const T1& val1, const T2& val2) {\
+ if (val1 op val2) {\
+ return AssertionSuccess();\
+ } else {\
+ return AssertionFailure() \
+ << "Expected: (" << expr1 << ") " #op " (" << expr2\
+ << "), actual: " << FormatForComparisonFailureMessage(val1, val2)\
+ << " vs " << FormatForComparisonFailureMessage(val2, val1);\
+ }\
+}\
+GTEST_API_ AssertionResult CmpHelper##op_name(\
+ const char* expr1, const char* expr2, BiggestInt val1, BiggestInt val2)
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+
+// Implements the helper function for {ASSERT|EXPECT}_NE
+GTEST_IMPL_CMP_HELPER_(NE, !=);
+// Implements the helper function for {ASSERT|EXPECT}_LE
+GTEST_IMPL_CMP_HELPER_(LE, <=);
+// Implements the helper function for {ASSERT|EXPECT}_LT
+GTEST_IMPL_CMP_HELPER_(LT, <);
+// Implements the helper function for {ASSERT|EXPECT}_GE
+GTEST_IMPL_CMP_HELPER_(GE, >=);
+// Implements the helper function for {ASSERT|EXPECT}_GT
+GTEST_IMPL_CMP_HELPER_(GT, >);
+
+#undef GTEST_IMPL_CMP_HELPER_
+
+// The helper function for {ASSERT|EXPECT}_STREQ.
+//
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+GTEST_API_ AssertionResult CmpHelperSTREQ(const char* expected_expression,
+ const char* actual_expression,
+ const char* expected,
+ const char* actual);
+
+// The helper function for {ASSERT|EXPECT}_STRCASEEQ.
+//
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+GTEST_API_ AssertionResult CmpHelperSTRCASEEQ(const char* expected_expression,
+ const char* actual_expression,
+ const char* expected,
+ const char* actual);
+
+// The helper function for {ASSERT|EXPECT}_STRNE.
+//
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+GTEST_API_ AssertionResult CmpHelperSTRNE(const char* s1_expression,
+ const char* s2_expression,
+ const char* s1,
+ const char* s2);
+
+// The helper function for {ASSERT|EXPECT}_STRCASENE.
+//
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+GTEST_API_ AssertionResult CmpHelperSTRCASENE(const char* s1_expression,
+ const char* s2_expression,
+ const char* s1,
+ const char* s2);
+
+
+// Helper function for *_STREQ on wide strings.
+//
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+GTEST_API_ AssertionResult CmpHelperSTREQ(const char* expected_expression,
+ const char* actual_expression,
+ const wchar_t* expected,
+ const wchar_t* actual);
+
+// Helper function for *_STRNE on wide strings.
+//
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+GTEST_API_ AssertionResult CmpHelperSTRNE(const char* s1_expression,
+ const char* s2_expression,
+ const wchar_t* s1,
+ const wchar_t* s2);
+
+} // namespace internal
+
+// IsSubstring() and IsNotSubstring() are intended to be used as the
+// first argument to {EXPECT,ASSERT}_PRED_FORMAT2(), not by
+// themselves. They check whether needle is a substring of haystack
+// (NULL is considered a substring of itself only), and return an
+// appropriate error message when they fail.
+//
+// The {needle,haystack}_expr arguments are the stringified
+// expressions that generated the two real arguments.
+GTEST_API_ AssertionResult IsSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const char* needle, const char* haystack);
+GTEST_API_ AssertionResult IsSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const wchar_t* needle, const wchar_t* haystack);
+GTEST_API_ AssertionResult IsNotSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const char* needle, const char* haystack);
+GTEST_API_ AssertionResult IsNotSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const wchar_t* needle, const wchar_t* haystack);
+GTEST_API_ AssertionResult IsSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const ::std::string& needle, const ::std::string& haystack);
+GTEST_API_ AssertionResult IsNotSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const ::std::string& needle, const ::std::string& haystack);
+
+#if GTEST_HAS_STD_WSTRING
+GTEST_API_ AssertionResult IsSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const ::std::wstring& needle, const ::std::wstring& haystack);
+GTEST_API_ AssertionResult IsNotSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const ::std::wstring& needle, const ::std::wstring& haystack);
+#endif // GTEST_HAS_STD_WSTRING
+
+namespace internal {
+
+// Helper template function for comparing floating-points.
+//
+// Template parameter:
+//
+// RawType: the raw floating-point type (either float or double)
+//
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+template <typename RawType>
+AssertionResult CmpHelperFloatingPointEQ(const char* expected_expression,
+ const char* actual_expression,
+ RawType expected,
+ RawType actual) {
+ const FloatingPoint<RawType> lhs(expected), rhs(actual);
+
+ if (lhs.AlmostEquals(rhs)) {
+ return AssertionSuccess();
+ }
+
+ ::std::stringstream expected_ss;
+ expected_ss << std::setprecision(std::numeric_limits<RawType>::digits10 + 2)
+ << expected;
+
+ ::std::stringstream actual_ss;
+ actual_ss << std::setprecision(std::numeric_limits<RawType>::digits10 + 2)
+ << actual;
+
+ return EqFailure(expected_expression,
+ actual_expression,
+ StringStreamToString(&expected_ss),
+ StringStreamToString(&actual_ss),
+ false);
+}
+
+// Helper function for implementing ASSERT_NEAR.
+//
+// INTERNAL IMPLEMENTATION - DO NOT USE IN A USER PROGRAM.
+GTEST_API_ AssertionResult DoubleNearPredFormat(const char* expr1,
+ const char* expr2,
+ const char* abs_error_expr,
+ double val1,
+ double val2,
+ double abs_error);
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+// A class that enables one to stream messages to assertion macros
+class GTEST_API_ AssertHelper {
+ public:
+ // Constructor.
+ AssertHelper(TestPartResult::Type type,
+ const char* file,
+ int line,
+ const char* message);
+ ~AssertHelper();
+
+ // Message assignment is a semantic trick to enable assertion
+ // streaming; see the GTEST_MESSAGE_ macro below.
+ void operator=(const Message& message) const;
+
+ private:
+ // We put our data in a struct so that the size of the AssertHelper class can
+ // be as small as possible. This is important because gcc is incapable of
+ // re-using stack space even for temporary variables, so every EXPECT_EQ
+ // reserves stack space for another AssertHelper.
+ struct AssertHelperData {
+ AssertHelperData(TestPartResult::Type t,
+ const char* srcfile,
+ int line_num,
+ const char* msg)
+ : type(t), file(srcfile), line(line_num), message(msg) { }
+
+ TestPartResult::Type const type;
+ const char* const file;
+ int const line;
+ std::string const message;
+
+ private:
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(AssertHelperData);
+ };
+
+ AssertHelperData* const data_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(AssertHelper);
+};
+
+} // namespace internal
+
+#if GTEST_HAS_PARAM_TEST
+// The pure interface class that all value-parameterized tests inherit from.
+// A value-parameterized class must inherit from both ::testing::Test and
+// ::testing::WithParamInterface. In most cases that just means inheriting
+// from ::testing::TestWithParam, but more complicated test hierarchies
+// may need to inherit from Test and WithParamInterface at different levels.
+//
+// This interface has support for accessing the test parameter value via
+// the GetParam() method.
+//
+// Use it with one of the parameter generator defining functions, like Range(),
+// Values(), ValuesIn(), Bool(), and Combine().
+//
+// class FooTest : public ::testing::TestWithParam<int> {
+// protected:
+// FooTest() {
+// // Can use GetParam() here.
+// }
+// virtual ~FooTest() {
+// // Can use GetParam() here.
+// }
+// virtual void SetUp() {
+// // Can use GetParam() here.
+// }
+// virtual void TearDown {
+// // Can use GetParam() here.
+// }
+// };
+// TEST_P(FooTest, DoesBar) {
+// // Can use GetParam() method here.
+// Foo foo;
+// ASSERT_TRUE(foo.DoesBar(GetParam()));
+// }
+// INSTANTIATE_TEST_CASE_P(OneToTenRange, FooTest, ::testing::Range(1, 10));
+
+template <typename T>
+class WithParamInterface {
+ public:
+ typedef T ParamType;
+ virtual ~WithParamInterface() {}
+
+ // The current parameter value. Is also available in the test fixture's
+ // constructor. This member function is non-static, even though it only
+ // references static data, to reduce the opportunity for incorrect uses
+ // like writing 'WithParamInterface<bool>::GetParam()' for a test that
+ // uses a fixture whose parameter type is int.
+ const ParamType& GetParam() const {
+ GTEST_CHECK_(parameter_ != NULL)
+ << "GetParam() can only be called inside a value-parameterized test "
+ << "-- did you intend to write TEST_P instead of TEST_F?";
+ return *parameter_;
+ }
+
+ private:
+ // Sets parameter value. The caller is responsible for making sure the value
+ // remains alive and unchanged throughout the current test.
+ static void SetParam(const ParamType* parameter) {
+ parameter_ = parameter;
+ }
+
+ // Static value used for accessing parameter during a test lifetime.
+ static const ParamType* parameter_;
+
+ // TestClass must be a subclass of WithParamInterface<T> and Test.
+ template <class TestClass> friend class internal::ParameterizedTestFactory;
+};
+
+template <typename T>
+const T* WithParamInterface<T>::parameter_ = NULL;
+
+// Most value-parameterized classes can ignore the existence of
+// WithParamInterface, and can just inherit from ::testing::TestWithParam.
+
+template <typename T>
+class TestWithParam : public Test, public WithParamInterface<T> {
+};
+
+#endif // GTEST_HAS_PARAM_TEST
+
+// Macros for indicating success/failure in test code.
+
+// ADD_FAILURE unconditionally adds a failure to the current test.
+// SUCCEED generates a success - it doesn't automatically make the
+// current test successful, as a test is only successful when it has
+// no failure.
+//
+// EXPECT_* verifies that a certain condition is satisfied. If not,
+// it behaves like ADD_FAILURE. In particular:
+//
+// EXPECT_TRUE verifies that a Boolean condition is true.
+// EXPECT_FALSE verifies that a Boolean condition is false.
+//
+// FAIL and ASSERT_* are similar to ADD_FAILURE and EXPECT_*, except
+// that they will also abort the current function on failure. People
+// usually want the fail-fast behavior of FAIL and ASSERT_*, but those
+// writing data-driven tests often find themselves using ADD_FAILURE
+// and EXPECT_* more.
+
+// Generates a nonfatal failure with a generic message.
+#define ADD_FAILURE() GTEST_NONFATAL_FAILURE_("Failed")
+
+// Generates a nonfatal failure at the given source file location with
+// a generic message.
+#define ADD_FAILURE_AT(file, line) \
+ GTEST_MESSAGE_AT_(file, line, "Failed", \
+ ::testing::TestPartResult::kNonFatalFailure)
+
+// Generates a fatal failure with a generic message.
+#define GTEST_FAIL() GTEST_FATAL_FAILURE_("Failed")
+
+// Define this macro to 1 to omit the definition of FAIL(), which is a
+// generic name and clashes with some other libraries.
+#if !GTEST_DONT_DEFINE_FAIL
+# define FAIL() GTEST_FAIL()
+#endif
+
+// Generates a success with a generic message.
+#define GTEST_SUCCEED() GTEST_SUCCESS_("Succeeded")
+
+// Define this macro to 1 to omit the definition of SUCCEED(), which
+// is a generic name and clashes with some other libraries.
+#if !GTEST_DONT_DEFINE_SUCCEED
+# define SUCCEED() GTEST_SUCCEED()
+#endif
+
+// Macros for testing exceptions.
+//
+// * {ASSERT|EXPECT}_THROW(statement, expected_exception):
+// Tests that the statement throws the expected exception.
+// * {ASSERT|EXPECT}_NO_THROW(statement):
+// Tests that the statement doesn't throw any exception.
+// * {ASSERT|EXPECT}_ANY_THROW(statement):
+// Tests that the statement throws an exception.
+
+#define EXPECT_THROW(statement, expected_exception) \
+ GTEST_TEST_THROW_(statement, expected_exception, GTEST_NONFATAL_FAILURE_)
+#define EXPECT_NO_THROW(statement) \
+ GTEST_TEST_NO_THROW_(statement, GTEST_NONFATAL_FAILURE_)
+#define EXPECT_ANY_THROW(statement) \
+ GTEST_TEST_ANY_THROW_(statement, GTEST_NONFATAL_FAILURE_)
+#define ASSERT_THROW(statement, expected_exception) \
+ GTEST_TEST_THROW_(statement, expected_exception, GTEST_FATAL_FAILURE_)
+#define ASSERT_NO_THROW(statement) \
+ GTEST_TEST_NO_THROW_(statement, GTEST_FATAL_FAILURE_)
+#define ASSERT_ANY_THROW(statement) \
+ GTEST_TEST_ANY_THROW_(statement, GTEST_FATAL_FAILURE_)
+
+// Boolean assertions. Condition can be either a Boolean expression or an
+// AssertionResult. For more information on how to use AssertionResult with
+// these macros see comments on that class.
+#define EXPECT_TRUE(condition) \
+ GTEST_TEST_BOOLEAN_(condition, #condition, false, true, \
+ GTEST_NONFATAL_FAILURE_)
+#define EXPECT_FALSE(condition) \
+ GTEST_TEST_BOOLEAN_(!(condition), #condition, true, false, \
+ GTEST_NONFATAL_FAILURE_)
+#define ASSERT_TRUE(condition) \
+ GTEST_TEST_BOOLEAN_(condition, #condition, false, true, \
+ GTEST_FATAL_FAILURE_)
+#define ASSERT_FALSE(condition) \
+ GTEST_TEST_BOOLEAN_(!(condition), #condition, true, false, \
+ GTEST_FATAL_FAILURE_)
+
+// Includes the auto-generated header that implements a family of
+// generic predicate assertion macros.
+#include "gtest/gtest_pred_impl.h"
+
+// Macros for testing equalities and inequalities.
+//
+// * {ASSERT|EXPECT}_EQ(expected, actual): Tests that expected == actual
+// * {ASSERT|EXPECT}_NE(v1, v2): Tests that v1 != v2
+// * {ASSERT|EXPECT}_LT(v1, v2): Tests that v1 < v2
+// * {ASSERT|EXPECT}_LE(v1, v2): Tests that v1 <= v2
+// * {ASSERT|EXPECT}_GT(v1, v2): Tests that v1 > v2
+// * {ASSERT|EXPECT}_GE(v1, v2): Tests that v1 >= v2
+//
+// When they are not, Google Test prints both the tested expressions and
+// their actual values. The values must be compatible built-in types,
+// or you will get a compiler error. By "compatible" we mean that the
+// values can be compared by the respective operator.
+//
+// Note:
+//
+// 1. It is possible to make a user-defined type work with
+// {ASSERT|EXPECT}_??(), but that requires overloading the
+// comparison operators and is thus discouraged by the Google C++
+// Usage Guide. Therefore, you are advised to use the
+// {ASSERT|EXPECT}_TRUE() macro to assert that two objects are
+// equal.
+//
+// 2. The {ASSERT|EXPECT}_??() macros do pointer comparisons on
+// pointers (in particular, C strings). Therefore, if you use it
+// with two C strings, you are testing how their locations in memory
+// are related, not how their content is related. To compare two C
+// strings by content, use {ASSERT|EXPECT}_STR*().
+//
+// 3. {ASSERT|EXPECT}_EQ(expected, actual) is preferred to
+// {ASSERT|EXPECT}_TRUE(expected == actual), as the former tells you
+// what the actual value is when it fails, and similarly for the
+// other comparisons.
+//
+// 4. Do not depend on the order in which {ASSERT|EXPECT}_??()
+// evaluate their arguments, which is undefined.
+//
+// 5. These macros evaluate their arguments exactly once.
+//
+// Examples:
+//
+// EXPECT_NE(5, Foo());
+// EXPECT_EQ(NULL, a_pointer);
+// ASSERT_LT(i, array_size);
+// ASSERT_GT(records.size(), 0) << "There is no record left.";
+
+#define EXPECT_EQ(expected, actual) \
+ EXPECT_PRED_FORMAT2(::testing::internal:: \
+ EqHelper<GTEST_IS_NULL_LITERAL_(expected)>::Compare, \
+ expected, actual)
+#define EXPECT_NE(expected, actual) \
+ EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperNE, expected, actual)
+#define EXPECT_LE(val1, val2) \
+ EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperLE, val1, val2)
+#define EXPECT_LT(val1, val2) \
+ EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperLT, val1, val2)
+#define EXPECT_GE(val1, val2) \
+ EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperGE, val1, val2)
+#define EXPECT_GT(val1, val2) \
+ EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperGT, val1, val2)
+
+#define GTEST_ASSERT_EQ(expected, actual) \
+ ASSERT_PRED_FORMAT2(::testing::internal:: \
+ EqHelper<GTEST_IS_NULL_LITERAL_(expected)>::Compare, \
+ expected, actual)
+#define GTEST_ASSERT_NE(val1, val2) \
+ ASSERT_PRED_FORMAT2(::testing::internal::CmpHelperNE, val1, val2)
+#define GTEST_ASSERT_LE(val1, val2) \
+ ASSERT_PRED_FORMAT2(::testing::internal::CmpHelperLE, val1, val2)
+#define GTEST_ASSERT_LT(val1, val2) \
+ ASSERT_PRED_FORMAT2(::testing::internal::CmpHelperLT, val1, val2)
+#define GTEST_ASSERT_GE(val1, val2) \
+ ASSERT_PRED_FORMAT2(::testing::internal::CmpHelperGE, val1, val2)
+#define GTEST_ASSERT_GT(val1, val2) \
+ ASSERT_PRED_FORMAT2(::testing::internal::CmpHelperGT, val1, val2)
+
+// Define macro GTEST_DONT_DEFINE_ASSERT_XY to 1 to omit the definition of
+// ASSERT_XY(), which clashes with some users' own code.
+
+#if !GTEST_DONT_DEFINE_ASSERT_EQ
+# define ASSERT_EQ(val1, val2) GTEST_ASSERT_EQ(val1, val2)
+#endif
+
+#if !GTEST_DONT_DEFINE_ASSERT_NE
+# define ASSERT_NE(val1, val2) GTEST_ASSERT_NE(val1, val2)
+#endif
+
+#if !GTEST_DONT_DEFINE_ASSERT_LE
+# define ASSERT_LE(val1, val2) GTEST_ASSERT_LE(val1, val2)
+#endif
+
+#if !GTEST_DONT_DEFINE_ASSERT_LT
+# define ASSERT_LT(val1, val2) GTEST_ASSERT_LT(val1, val2)
+#endif
+
+#if !GTEST_DONT_DEFINE_ASSERT_GE
+# define ASSERT_GE(val1, val2) GTEST_ASSERT_GE(val1, val2)
+#endif
+
+#if !GTEST_DONT_DEFINE_ASSERT_GT
+# define ASSERT_GT(val1, val2) GTEST_ASSERT_GT(val1, val2)
+#endif
+
+// C-string Comparisons. All tests treat NULL and any non-NULL string
+// as different. Two NULLs are equal.
+//
+// * {ASSERT|EXPECT}_STREQ(s1, s2): Tests that s1 == s2
+// * {ASSERT|EXPECT}_STRNE(s1, s2): Tests that s1 != s2
+// * {ASSERT|EXPECT}_STRCASEEQ(s1, s2): Tests that s1 == s2, ignoring case
+// * {ASSERT|EXPECT}_STRCASENE(s1, s2): Tests that s1 != s2, ignoring case
+//
+// For wide or narrow string objects, you can use the
+// {ASSERT|EXPECT}_??() macros.
+//
+// Don't depend on the order in which the arguments are evaluated,
+// which is undefined.
+//
+// These macros evaluate their arguments exactly once.
+
+#define EXPECT_STREQ(expected, actual) \
+ EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperSTREQ, expected, actual)
+#define EXPECT_STRNE(s1, s2) \
+ EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperSTRNE, s1, s2)
+#define EXPECT_STRCASEEQ(expected, actual) \
+ EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperSTRCASEEQ, expected, actual)
+#define EXPECT_STRCASENE(s1, s2)\
+ EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperSTRCASENE, s1, s2)
+
+#define ASSERT_STREQ(expected, actual) \
+ ASSERT_PRED_FORMAT2(::testing::internal::CmpHelperSTREQ, expected, actual)
+#define ASSERT_STRNE(s1, s2) \
+ ASSERT_PRED_FORMAT2(::testing::internal::CmpHelperSTRNE, s1, s2)
+#define ASSERT_STRCASEEQ(expected, actual) \
+ ASSERT_PRED_FORMAT2(::testing::internal::CmpHelperSTRCASEEQ, expected, actual)
+#define ASSERT_STRCASENE(s1, s2)\
+ ASSERT_PRED_FORMAT2(::testing::internal::CmpHelperSTRCASENE, s1, s2)
+
+// Macros for comparing floating-point numbers.
+//
+// * {ASSERT|EXPECT}_FLOAT_EQ(expected, actual):
+// Tests that two float values are almost equal.
+// * {ASSERT|EXPECT}_DOUBLE_EQ(expected, actual):
+// Tests that two double values are almost equal.
+// * {ASSERT|EXPECT}_NEAR(v1, v2, abs_error):
+// Tests that v1 and v2 are within the given distance to each other.
+//
+// Google Test uses ULP-based comparison to automatically pick a default
+// error bound that is appropriate for the operands. See the
+// FloatingPoint template class in gtest-internal.h if you are
+// interested in the implementation details.
+
+#define EXPECT_FLOAT_EQ(expected, actual)\
+ EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperFloatingPointEQ<float>, \
+ expected, actual)
+
+#define EXPECT_DOUBLE_EQ(expected, actual)\
+ EXPECT_PRED_FORMAT2(::testing::internal::CmpHelperFloatingPointEQ<double>, \
+ expected, actual)
+
+#define ASSERT_FLOAT_EQ(expected, actual)\
+ ASSERT_PRED_FORMAT2(::testing::internal::CmpHelperFloatingPointEQ<float>, \
+ expected, actual)
+
+#define ASSERT_DOUBLE_EQ(expected, actual)\
+ ASSERT_PRED_FORMAT2(::testing::internal::CmpHelperFloatingPointEQ<double>, \
+ expected, actual)
+
+#define EXPECT_NEAR(val1, val2, abs_error)\
+ EXPECT_PRED_FORMAT3(::testing::internal::DoubleNearPredFormat, \
+ val1, val2, abs_error)
+
+#define ASSERT_NEAR(val1, val2, abs_error)\
+ ASSERT_PRED_FORMAT3(::testing::internal::DoubleNearPredFormat, \
+ val1, val2, abs_error)
+
+// These predicate format functions work on floating-point values, and
+// can be used in {ASSERT|EXPECT}_PRED_FORMAT2*(), e.g.
+//
+// EXPECT_PRED_FORMAT2(testing::DoubleLE, Foo(), 5.0);
+
+// Asserts that val1 is less than, or almost equal to, val2. Fails
+// otherwise. In particular, it fails if either val1 or val2 is NaN.
+GTEST_API_ AssertionResult FloatLE(const char* expr1, const char* expr2,
+ float val1, float val2);
+GTEST_API_ AssertionResult DoubleLE(const char* expr1, const char* expr2,
+ double val1, double val2);
+
+
+#if GTEST_OS_WINDOWS
+
+// Macros that test for HRESULT failure and success, these are only useful
+// on Windows, and rely on Windows SDK macros and APIs to compile.
+//
+// * {ASSERT|EXPECT}_HRESULT_{SUCCEEDED|FAILED}(expr)
+//
+// When expr unexpectedly fails or succeeds, Google Test prints the
+// expected result and the actual result with both a human-readable
+// string representation of the error, if available, as well as the
+// hex result code.
+# define EXPECT_HRESULT_SUCCEEDED(expr) \
+ EXPECT_PRED_FORMAT1(::testing::internal::IsHRESULTSuccess, (expr))
+
+# define ASSERT_HRESULT_SUCCEEDED(expr) \
+ ASSERT_PRED_FORMAT1(::testing::internal::IsHRESULTSuccess, (expr))
+
+# define EXPECT_HRESULT_FAILED(expr) \
+ EXPECT_PRED_FORMAT1(::testing::internal::IsHRESULTFailure, (expr))
+
+# define ASSERT_HRESULT_FAILED(expr) \
+ ASSERT_PRED_FORMAT1(::testing::internal::IsHRESULTFailure, (expr))
+
+#endif // GTEST_OS_WINDOWS
+
+// Macros that execute statement and check that it doesn't generate new fatal
+// failures in the current thread.
+//
+// * {ASSERT|EXPECT}_NO_FATAL_FAILURE(statement);
+//
+// Examples:
+//
+// EXPECT_NO_FATAL_FAILURE(Process());
+// ASSERT_NO_FATAL_FAILURE(Process()) << "Process() failed";
+//
+#define ASSERT_NO_FATAL_FAILURE(statement) \
+ GTEST_TEST_NO_FATAL_FAILURE_(statement, GTEST_FATAL_FAILURE_)
+#define EXPECT_NO_FATAL_FAILURE(statement) \
+ GTEST_TEST_NO_FATAL_FAILURE_(statement, GTEST_NONFATAL_FAILURE_)
+
+// Causes a trace (including the source file path, the current line
+// number, and the given message) to be included in every test failure
+// message generated by code in the current scope. The effect is
+// undone when the control leaves the current scope.
+//
+// The message argument can be anything streamable to std::ostream.
+//
+// In the implementation, we include the current line number as part
+// of the dummy variable name, thus allowing multiple SCOPED_TRACE()s
+// to appear in the same block - as long as they are on different
+// lines.
+#define SCOPED_TRACE(message) \
+ ::testing::internal::ScopedTrace GTEST_CONCAT_TOKEN_(gtest_trace_, __LINE__)(\
+ __FILE__, __LINE__, ::testing::Message() << (message))
+
+// Compile-time assertion for type equality.
+// StaticAssertTypeEq<type1, type2>() compiles iff type1 and type2 are
+// the same type. The value it returns is not interesting.
+//
+// Instead of making StaticAssertTypeEq a class template, we make it a
+// function template that invokes a helper class template. This
+// prevents a user from misusing StaticAssertTypeEq<T1, T2> by
+// defining objects of that type.
+//
+// CAVEAT:
+//
+// When used inside a method of a class template,
+// StaticAssertTypeEq<T1, T2>() is effective ONLY IF the method is
+// instantiated. For example, given:
+//
+// template <typename T> class Foo {
+// public:
+// void Bar() { testing::StaticAssertTypeEq<int, T>(); }
+// };
+//
+// the code:
+//
+// void Test1() { Foo<bool> foo; }
+//
+// will NOT generate a compiler error, as Foo<bool>::Bar() is never
+// actually instantiated. Instead, you need:
+//
+// void Test2() { Foo<bool> foo; foo.Bar(); }
+//
+// to cause a compiler error.
+template <typename T1, typename T2>
+bool StaticAssertTypeEq() {
+ (void)internal::StaticAssertTypeEqHelper<T1, T2>();
+ return true;
+}
+
+// Defines a test.
+//
+// The first parameter is the name of the test case, and the second
+// parameter is the name of the test within the test case.
+//
+// The convention is to end the test case name with "Test". For
+// example, a test case for the Foo class can be named FooTest.
+//
+// The user should put his test code between braces after using this
+// macro. Example:
+//
+// TEST(FooTest, InitializesCorrectly) {
+// Foo foo;
+// EXPECT_TRUE(foo.StatusIsOK());
+// }
+
+// Note that we call GetTestTypeId() instead of GetTypeId<
+// ::testing::Test>() here to get the type ID of testing::Test. This
+// is to work around a suspected linker bug when using Google Test as
+// a framework on Mac OS X. The bug causes GetTypeId<
+// ::testing::Test>() to return different values depending on whether
+// the call is from the Google Test framework itself or from user test
+// code. GetTestTypeId() is guaranteed to always return the same
+// value, as it always calls GetTypeId<>() from the Google Test
+// framework.
+#define GTEST_TEST(test_case_name, test_name)\
+ GTEST_TEST_(test_case_name, test_name, \
+ ::testing::Test, ::testing::internal::GetTestTypeId())
+
+// Define this macro to 1 to omit the definition of TEST(), which
+// is a generic name and clashes with some other libraries.
+#if !GTEST_DONT_DEFINE_TEST
+# define TEST(test_case_name, test_name) GTEST_TEST(test_case_name, test_name)
+#endif
+
+// Defines a test that uses a test fixture.
+//
+// The first parameter is the name of the test fixture class, which
+// also doubles as the test case name. The second parameter is the
+// name of the test within the test case.
+//
+// A test fixture class must be declared earlier. The user should put
+// his test code between braces after using this macro. Example:
+//
+// class FooTest : public testing::Test {
+// protected:
+// virtual void SetUp() { b_.AddElement(3); }
+//
+// Foo a_;
+// Foo b_;
+// };
+//
+// TEST_F(FooTest, InitializesCorrectly) {
+// EXPECT_TRUE(a_.StatusIsOK());
+// }
+//
+// TEST_F(FooTest, ReturnsElementCountCorrectly) {
+// EXPECT_EQ(0, a_.size());
+// EXPECT_EQ(1, b_.size());
+// }
+
+#define TEST_F(test_fixture, test_name)\
+ GTEST_TEST_(test_fixture, test_name, test_fixture, \
+ ::testing::internal::GetTypeId<test_fixture>())
+
+} // namespace testing
+
+// Use this function in main() to run all tests. It returns 0 if all
+// tests are successful, or 1 otherwise.
+//
+// RUN_ALL_TESTS() should be invoked after the command line has been
+// parsed by InitGoogleTest().
+//
+// This function was formerly a macro; thus, it is in the global
+// namespace and has an all-caps name.
+int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_;
+
+inline int RUN_ALL_TESTS() {
+ return ::testing::UnitTest::GetInstance()->Run();
+}
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/gtest_pred_impl.h b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest_pred_impl.h
new file mode 100644
index 0000000..30ae712
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest_pred_impl.h
@@ -0,0 +1,358 @@
+// Copyright 2006, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// This file is AUTOMATICALLY GENERATED on 10/31/2011 by command
+// 'gen_gtest_pred_impl.py 5'. DO NOT EDIT BY HAND!
+//
+// Implements a family of generic predicate assertion macros.
+
+#ifndef GTEST_INCLUDE_GTEST_GTEST_PRED_IMPL_H_
+#define GTEST_INCLUDE_GTEST_GTEST_PRED_IMPL_H_
+
+// Makes sure this header is not included before gtest.h.
+#ifndef GTEST_INCLUDE_GTEST_GTEST_H_
+# error Do not include gtest_pred_impl.h directly. Include gtest.h instead.
+#endif // GTEST_INCLUDE_GTEST_GTEST_H_
+
+// This header implements a family of generic predicate assertion
+// macros:
+//
+// ASSERT_PRED_FORMAT1(pred_format, v1)
+// ASSERT_PRED_FORMAT2(pred_format, v1, v2)
+// ...
+//
+// where pred_format is a function or functor that takes n (in the
+// case of ASSERT_PRED_FORMATn) values and their source expression
+// text, and returns a testing::AssertionResult. See the definition
+// of ASSERT_EQ in gtest.h for an example.
+//
+// If you don't care about formatting, you can use the more
+// restrictive version:
+//
+// ASSERT_PRED1(pred, v1)
+// ASSERT_PRED2(pred, v1, v2)
+// ...
+//
+// where pred is an n-ary function or functor that returns bool,
+// and the values v1, v2, ..., must support the << operator for
+// streaming to std::ostream.
+//
+// We also define the EXPECT_* variations.
+//
+// For now we only support predicates whose arity is at most 5.
+// Please email googletestframework at googlegroups.com if you need
+// support for higher arities.
+
+// GTEST_ASSERT_ is the basic statement to which all of the assertions
+// in this file reduce. Don't use this in your code.
+
+#define GTEST_ASSERT_(expression, on_failure) \
+ GTEST_AMBIGUOUS_ELSE_BLOCKER_ \
+ if (const ::testing::AssertionResult gtest_ar = (expression)) \
+ ; \
+ else \
+ on_failure(gtest_ar.failure_message())
+
+
+// Helper function for implementing {EXPECT|ASSERT}_PRED1. Don't use
+// this in your code.
+template <typename Pred,
+ typename T1>
+AssertionResult AssertPred1Helper(const char* pred_text,
+ const char* e1,
+ Pred pred,
+ const T1& v1) {
+ if (pred(v1)) return AssertionSuccess();
+
+ return AssertionFailure() << pred_text << "("
+ << e1 << ") evaluates to false, where"
+ << "\n" << e1 << " evaluates to " << v1;
+}
+
+// Internal macro for implementing {EXPECT|ASSERT}_PRED_FORMAT1.
+// Don't use this in your code.
+#define GTEST_PRED_FORMAT1_(pred_format, v1, on_failure)\
+ GTEST_ASSERT_(pred_format(#v1, v1), \
+ on_failure)
+
+// Internal macro for implementing {EXPECT|ASSERT}_PRED1. Don't use
+// this in your code.
+#define GTEST_PRED1_(pred, v1, on_failure)\
+ GTEST_ASSERT_(::testing::AssertPred1Helper(#pred, \
+ #v1, \
+ pred, \
+ v1), on_failure)
+
+// Unary predicate assertion macros.
+#define EXPECT_PRED_FORMAT1(pred_format, v1) \
+ GTEST_PRED_FORMAT1_(pred_format, v1, GTEST_NONFATAL_FAILURE_)
+#define EXPECT_PRED1(pred, v1) \
+ GTEST_PRED1_(pred, v1, GTEST_NONFATAL_FAILURE_)
+#define ASSERT_PRED_FORMAT1(pred_format, v1) \
+ GTEST_PRED_FORMAT1_(pred_format, v1, GTEST_FATAL_FAILURE_)
+#define ASSERT_PRED1(pred, v1) \
+ GTEST_PRED1_(pred, v1, GTEST_FATAL_FAILURE_)
+
+
+
+// Helper function for implementing {EXPECT|ASSERT}_PRED2. Don't use
+// this in your code.
+template <typename Pred,
+ typename T1,
+ typename T2>
+AssertionResult AssertPred2Helper(const char* pred_text,
+ const char* e1,
+ const char* e2,
+ Pred pred,
+ const T1& v1,
+ const T2& v2) {
+ if (pred(v1, v2)) return AssertionSuccess();
+
+ return AssertionFailure() << pred_text << "("
+ << e1 << ", "
+ << e2 << ") evaluates to false, where"
+ << "\n" << e1 << " evaluates to " << v1
+ << "\n" << e2 << " evaluates to " << v2;
+}
+
+// Internal macro for implementing {EXPECT|ASSERT}_PRED_FORMAT2.
+// Don't use this in your code.
+#define GTEST_PRED_FORMAT2_(pred_format, v1, v2, on_failure)\
+ GTEST_ASSERT_(pred_format(#v1, #v2, v1, v2), \
+ on_failure)
+
+// Internal macro for implementing {EXPECT|ASSERT}_PRED2. Don't use
+// this in your code.
+#define GTEST_PRED2_(pred, v1, v2, on_failure)\
+ GTEST_ASSERT_(::testing::AssertPred2Helper(#pred, \
+ #v1, \
+ #v2, \
+ pred, \
+ v1, \
+ v2), on_failure)
+
+// Binary predicate assertion macros.
+#define EXPECT_PRED_FORMAT2(pred_format, v1, v2) \
+ GTEST_PRED_FORMAT2_(pred_format, v1, v2, GTEST_NONFATAL_FAILURE_)
+#define EXPECT_PRED2(pred, v1, v2) \
+ GTEST_PRED2_(pred, v1, v2, GTEST_NONFATAL_FAILURE_)
+#define ASSERT_PRED_FORMAT2(pred_format, v1, v2) \
+ GTEST_PRED_FORMAT2_(pred_format, v1, v2, GTEST_FATAL_FAILURE_)
+#define ASSERT_PRED2(pred, v1, v2) \
+ GTEST_PRED2_(pred, v1, v2, GTEST_FATAL_FAILURE_)
+
+
+
+// Helper function for implementing {EXPECT|ASSERT}_PRED3. Don't use
+// this in your code.
+template <typename Pred,
+ typename T1,
+ typename T2,
+ typename T3>
+AssertionResult AssertPred3Helper(const char* pred_text,
+ const char* e1,
+ const char* e2,
+ const char* e3,
+ Pred pred,
+ const T1& v1,
+ const T2& v2,
+ const T3& v3) {
+ if (pred(v1, v2, v3)) return AssertionSuccess();
+
+ return AssertionFailure() << pred_text << "("
+ << e1 << ", "
+ << e2 << ", "
+ << e3 << ") evaluates to false, where"
+ << "\n" << e1 << " evaluates to " << v1
+ << "\n" << e2 << " evaluates to " << v2
+ << "\n" << e3 << " evaluates to " << v3;
+}
+
+// Internal macro for implementing {EXPECT|ASSERT}_PRED_FORMAT3.
+// Don't use this in your code.
+#define GTEST_PRED_FORMAT3_(pred_format, v1, v2, v3, on_failure)\
+ GTEST_ASSERT_(pred_format(#v1, #v2, #v3, v1, v2, v3), \
+ on_failure)
+
+// Internal macro for implementing {EXPECT|ASSERT}_PRED3. Don't use
+// this in your code.
+#define GTEST_PRED3_(pred, v1, v2, v3, on_failure)\
+ GTEST_ASSERT_(::testing::AssertPred3Helper(#pred, \
+ #v1, \
+ #v2, \
+ #v3, \
+ pred, \
+ v1, \
+ v2, \
+ v3), on_failure)
+
+// Ternary predicate assertion macros.
+#define EXPECT_PRED_FORMAT3(pred_format, v1, v2, v3) \
+ GTEST_PRED_FORMAT3_(pred_format, v1, v2, v3, GTEST_NONFATAL_FAILURE_)
+#define EXPECT_PRED3(pred, v1, v2, v3) \
+ GTEST_PRED3_(pred, v1, v2, v3, GTEST_NONFATAL_FAILURE_)
+#define ASSERT_PRED_FORMAT3(pred_format, v1, v2, v3) \
+ GTEST_PRED_FORMAT3_(pred_format, v1, v2, v3, GTEST_FATAL_FAILURE_)
+#define ASSERT_PRED3(pred, v1, v2, v3) \
+ GTEST_PRED3_(pred, v1, v2, v3, GTEST_FATAL_FAILURE_)
+
+
+
+// Helper function for implementing {EXPECT|ASSERT}_PRED4. Don't use
+// this in your code.
+template <typename Pred,
+ typename T1,
+ typename T2,
+ typename T3,
+ typename T4>
+AssertionResult AssertPred4Helper(const char* pred_text,
+ const char* e1,
+ const char* e2,
+ const char* e3,
+ const char* e4,
+ Pred pred,
+ const T1& v1,
+ const T2& v2,
+ const T3& v3,
+ const T4& v4) {
+ if (pred(v1, v2, v3, v4)) return AssertionSuccess();
+
+ return AssertionFailure() << pred_text << "("
+ << e1 << ", "
+ << e2 << ", "
+ << e3 << ", "
+ << e4 << ") evaluates to false, where"
+ << "\n" << e1 << " evaluates to " << v1
+ << "\n" << e2 << " evaluates to " << v2
+ << "\n" << e3 << " evaluates to " << v3
+ << "\n" << e4 << " evaluates to " << v4;
+}
+
+// Internal macro for implementing {EXPECT|ASSERT}_PRED_FORMAT4.
+// Don't use this in your code.
+#define GTEST_PRED_FORMAT4_(pred_format, v1, v2, v3, v4, on_failure)\
+ GTEST_ASSERT_(pred_format(#v1, #v2, #v3, #v4, v1, v2, v3, v4), \
+ on_failure)
+
+// Internal macro for implementing {EXPECT|ASSERT}_PRED4. Don't use
+// this in your code.
+#define GTEST_PRED4_(pred, v1, v2, v3, v4, on_failure)\
+ GTEST_ASSERT_(::testing::AssertPred4Helper(#pred, \
+ #v1, \
+ #v2, \
+ #v3, \
+ #v4, \
+ pred, \
+ v1, \
+ v2, \
+ v3, \
+ v4), on_failure)
+
+// 4-ary predicate assertion macros.
+#define EXPECT_PRED_FORMAT4(pred_format, v1, v2, v3, v4) \
+ GTEST_PRED_FORMAT4_(pred_format, v1, v2, v3, v4, GTEST_NONFATAL_FAILURE_)
+#define EXPECT_PRED4(pred, v1, v2, v3, v4) \
+ GTEST_PRED4_(pred, v1, v2, v3, v4, GTEST_NONFATAL_FAILURE_)
+#define ASSERT_PRED_FORMAT4(pred_format, v1, v2, v3, v4) \
+ GTEST_PRED_FORMAT4_(pred_format, v1, v2, v3, v4, GTEST_FATAL_FAILURE_)
+#define ASSERT_PRED4(pred, v1, v2, v3, v4) \
+ GTEST_PRED4_(pred, v1, v2, v3, v4, GTEST_FATAL_FAILURE_)
+
+
+
+// Helper function for implementing {EXPECT|ASSERT}_PRED5. Don't use
+// this in your code.
+template <typename Pred,
+ typename T1,
+ typename T2,
+ typename T3,
+ typename T4,
+ typename T5>
+AssertionResult AssertPred5Helper(const char* pred_text,
+ const char* e1,
+ const char* e2,
+ const char* e3,
+ const char* e4,
+ const char* e5,
+ Pred pred,
+ const T1& v1,
+ const T2& v2,
+ const T3& v3,
+ const T4& v4,
+ const T5& v5) {
+ if (pred(v1, v2, v3, v4, v5)) return AssertionSuccess();
+
+ return AssertionFailure() << pred_text << "("
+ << e1 << ", "
+ << e2 << ", "
+ << e3 << ", "
+ << e4 << ", "
+ << e5 << ") evaluates to false, where"
+ << "\n" << e1 << " evaluates to " << v1
+ << "\n" << e2 << " evaluates to " << v2
+ << "\n" << e3 << " evaluates to " << v3
+ << "\n" << e4 << " evaluates to " << v4
+ << "\n" << e5 << " evaluates to " << v5;
+}
+
+// Internal macro for implementing {EXPECT|ASSERT}_PRED_FORMAT5.
+// Don't use this in your code.
+#define GTEST_PRED_FORMAT5_(pred_format, v1, v2, v3, v4, v5, on_failure)\
+ GTEST_ASSERT_(pred_format(#v1, #v2, #v3, #v4, #v5, v1, v2, v3, v4, v5), \
+ on_failure)
+
+// Internal macro for implementing {EXPECT|ASSERT}_PRED5. Don't use
+// this in your code.
+#define GTEST_PRED5_(pred, v1, v2, v3, v4, v5, on_failure)\
+ GTEST_ASSERT_(::testing::AssertPred5Helper(#pred, \
+ #v1, \
+ #v2, \
+ #v3, \
+ #v4, \
+ #v5, \
+ pred, \
+ v1, \
+ v2, \
+ v3, \
+ v4, \
+ v5), on_failure)
+
+// 5-ary predicate assertion macros.
+#define EXPECT_PRED_FORMAT5(pred_format, v1, v2, v3, v4, v5) \
+ GTEST_PRED_FORMAT5_(pred_format, v1, v2, v3, v4, v5, GTEST_NONFATAL_FAILURE_)
+#define EXPECT_PRED5(pred, v1, v2, v3, v4, v5) \
+ GTEST_PRED5_(pred, v1, v2, v3, v4, v5, GTEST_NONFATAL_FAILURE_)
+#define ASSERT_PRED_FORMAT5(pred_format, v1, v2, v3, v4, v5) \
+ GTEST_PRED_FORMAT5_(pred_format, v1, v2, v3, v4, v5, GTEST_FATAL_FAILURE_)
+#define ASSERT_PRED5(pred, v1, v2, v3, v4, v5) \
+ GTEST_PRED5_(pred, v1, v2, v3, v4, v5, GTEST_FATAL_FAILURE_)
+
+
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_PRED_IMPL_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/gtest_prod.h b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest_prod.h
new file mode 100644
index 0000000..da80ddc
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/gtest_prod.h
@@ -0,0 +1,58 @@
+// Copyright 2006, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+//
+// Google C++ Testing Framework definitions useful in production code.
+
+#ifndef GTEST_INCLUDE_GTEST_GTEST_PROD_H_
+#define GTEST_INCLUDE_GTEST_GTEST_PROD_H_
+
+// When you need to test the private or protected members of a class,
+// use the FRIEND_TEST macro to declare your tests as friends of the
+// class. For example:
+//
+// class MyClass {
+// private:
+// void MyMethod();
+// FRIEND_TEST(MyClassTest, MyMethod);
+// };
+//
+// class MyClassTest : public testing::Test {
+// // ...
+// };
+//
+// TEST_F(MyClassTest, MyMethod) {
+// // Can call MyClass::MyMethod() here.
+// }
+
+#define FRIEND_TEST(test_case_name, test_name)\
+friend class test_case_name##_##test_name##_Test
+
+#endif // GTEST_INCLUDE_GTEST_GTEST_PROD_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-death-test-internal.h b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-death-test-internal.h
new file mode 100644
index 0000000..2b3a78f
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-death-test-internal.h
@@ -0,0 +1,319 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Authors: wan at google.com (Zhanyong Wan), eefacm at gmail.com (Sean Mcafee)
+//
+// The Google C++ Testing Framework (Google Test)
+//
+// This header file defines internal utilities needed for implementing
+// death tests. They are subject to change without notice.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_DEATH_TEST_INTERNAL_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_DEATH_TEST_INTERNAL_H_
+
+#include "gtest/internal/gtest-internal.h"
+
+#include <stdio.h>
+
+namespace testing {
+namespace internal {
+
+GTEST_DECLARE_string_(internal_run_death_test);
+
+// Names of the flags (needed for parsing Google Test flags).
+const char kDeathTestStyleFlag[] = "death_test_style";
+const char kDeathTestUseFork[] = "death_test_use_fork";
+const char kInternalRunDeathTestFlag[] = "internal_run_death_test";
+
+#if GTEST_HAS_DEATH_TEST
+
+// DeathTest is a class that hides much of the complexity of the
+// GTEST_DEATH_TEST_ macro. It is abstract; its static Create method
+// returns a concrete class that depends on the prevailing death test
+// style, as defined by the --gtest_death_test_style and/or
+// --gtest_internal_run_death_test flags.
+
+// In describing the results of death tests, these terms are used with
+// the corresponding definitions:
+//
+// exit status: The integer exit information in the format specified
+// by wait(2)
+// exit code: The integer code passed to exit(3), _exit(2), or
+// returned from main()
+class GTEST_API_ DeathTest {
+ public:
+ // Create returns false if there was an error determining the
+ // appropriate action to take for the current death test; for example,
+ // if the gtest_death_test_style flag is set to an invalid value.
+ // The LastMessage method will return a more detailed message in that
+ // case. Otherwise, the DeathTest pointer pointed to by the "test"
+ // argument is set. If the death test should be skipped, the pointer
+ // is set to NULL; otherwise, it is set to the address of a new concrete
+ // DeathTest object that controls the execution of the current test.
+ static bool Create(const char* statement, const RE* regex,
+ const char* file, int line, DeathTest** test);
+ DeathTest();
+ virtual ~DeathTest() { }
+
+ // A helper class that aborts a death test when it's deleted.
+ class ReturnSentinel {
+ public:
+ explicit ReturnSentinel(DeathTest* test) : test_(test) { }
+ ~ReturnSentinel() { test_->Abort(TEST_ENCOUNTERED_RETURN_STATEMENT); }
+ private:
+ DeathTest* const test_;
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ReturnSentinel);
+ } GTEST_ATTRIBUTE_UNUSED_;
+
+ // An enumeration of possible roles that may be taken when a death
+ // test is encountered. EXECUTE means that the death test logic should
+ // be executed immediately. OVERSEE means that the program should prepare
+ // the appropriate environment for a child process to execute the death
+ // test, then wait for it to complete.
+ enum TestRole { OVERSEE_TEST, EXECUTE_TEST };
+
+ // An enumeration of the three reasons that a test might be aborted.
+ enum AbortReason {
+ TEST_ENCOUNTERED_RETURN_STATEMENT,
+ TEST_THREW_EXCEPTION,
+ TEST_DID_NOT_DIE
+ };
+
+ // Assumes one of the above roles.
+ virtual TestRole AssumeRole() = 0;
+
+ // Waits for the death test to finish and returns its status.
+ virtual int Wait() = 0;
+
+ // Returns true if the death test passed; that is, the test process
+ // exited during the test, its exit status matches a user-supplied
+ // predicate, and its stderr output matches a user-supplied regular
+ // expression.
+ // The user-supplied predicate may be a macro expression rather
+ // than a function pointer or functor, or else Wait and Passed could
+ // be combined.
+ virtual bool Passed(bool exit_status_ok) = 0;
+
+ // Signals that the death test did not die as expected.
+ virtual void Abort(AbortReason reason) = 0;
+
+ // Returns a human-readable outcome message regarding the outcome of
+ // the last death test.
+ static const char* LastMessage();
+
+ static void set_last_death_test_message(const std::string& message);
+
+ private:
+ // A string containing a description of the outcome of the last death test.
+ static std::string last_death_test_message_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(DeathTest);
+};
+
+// Factory interface for death tests. May be mocked out for testing.
+class DeathTestFactory {
+ public:
+ virtual ~DeathTestFactory() { }
+ virtual bool Create(const char* statement, const RE* regex,
+ const char* file, int line, DeathTest** test) = 0;
+};
+
+// A concrete DeathTestFactory implementation for normal use.
+class DefaultDeathTestFactory : public DeathTestFactory {
+ public:
+ virtual bool Create(const char* statement, const RE* regex,
+ const char* file, int line, DeathTest** test);
+};
+
+// Returns true if exit_status describes a process that was terminated
+// by a signal, or exited normally with a nonzero exit code.
+GTEST_API_ bool ExitedUnsuccessfully(int exit_status);
+
+// Traps C++ exceptions escaping statement and reports them as test
+// failures. Note that trapping SEH exceptions is not implemented here.
+# if GTEST_HAS_EXCEPTIONS
+# define GTEST_EXECUTE_DEATH_TEST_STATEMENT_(statement, death_test) \
+ try { \
+ GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement); \
+ } catch (const ::std::exception& gtest_exception) { \
+ fprintf(\
+ stderr, \
+ "\n%s: Caught std::exception-derived exception escaping the " \
+ "death test statement. Exception message: %s\n", \
+ ::testing::internal::FormatFileLocation(__FILE__, __LINE__).c_str(), \
+ gtest_exception.what()); \
+ fflush(stderr); \
+ death_test->Abort(::testing::internal::DeathTest::TEST_THREW_EXCEPTION); \
+ } catch (...) { \
+ death_test->Abort(::testing::internal::DeathTest::TEST_THREW_EXCEPTION); \
+ }
+
+# else
+# define GTEST_EXECUTE_DEATH_TEST_STATEMENT_(statement, death_test) \
+ GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement)
+
+# endif
+
+// This macro is for implementing ASSERT_DEATH*, EXPECT_DEATH*,
+// ASSERT_EXIT*, and EXPECT_EXIT*.
+# define GTEST_DEATH_TEST_(statement, predicate, regex, fail) \
+ GTEST_AMBIGUOUS_ELSE_BLOCKER_ \
+ if (::testing::internal::AlwaysTrue()) { \
+ const ::testing::internal::RE& gtest_regex = (regex); \
+ ::testing::internal::DeathTest* gtest_dt; \
+ if (!::testing::internal::DeathTest::Create(#statement, >est_regex, \
+ __FILE__, __LINE__, >est_dt)) { \
+ goto GTEST_CONCAT_TOKEN_(gtest_label_, __LINE__); \
+ } \
+ if (gtest_dt != NULL) { \
+ ::testing::internal::scoped_ptr< ::testing::internal::DeathTest> \
+ gtest_dt_ptr(gtest_dt); \
+ switch (gtest_dt->AssumeRole()) { \
+ case ::testing::internal::DeathTest::OVERSEE_TEST: \
+ if (!gtest_dt->Passed(predicate(gtest_dt->Wait()))) { \
+ goto GTEST_CONCAT_TOKEN_(gtest_label_, __LINE__); \
+ } \
+ break; \
+ case ::testing::internal::DeathTest::EXECUTE_TEST: { \
+ ::testing::internal::DeathTest::ReturnSentinel \
+ gtest_sentinel(gtest_dt); \
+ GTEST_EXECUTE_DEATH_TEST_STATEMENT_(statement, gtest_dt); \
+ gtest_dt->Abort(::testing::internal::DeathTest::TEST_DID_NOT_DIE); \
+ break; \
+ } \
+ default: \
+ break; \
+ } \
+ } \
+ } else \
+ GTEST_CONCAT_TOKEN_(gtest_label_, __LINE__): \
+ fail(::testing::internal::DeathTest::LastMessage())
+// The symbol "fail" here expands to something into which a message
+// can be streamed.
+
+// This macro is for implementing ASSERT/EXPECT_DEBUG_DEATH when compiled in
+// NDEBUG mode. In this case we need the statements to be executed, the regex is
+// ignored, and the macro must accept a streamed message even though the message
+// is never printed.
+# define GTEST_EXECUTE_STATEMENT_(statement, regex) \
+ GTEST_AMBIGUOUS_ELSE_BLOCKER_ \
+ if (::testing::internal::AlwaysTrue()) { \
+ GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement); \
+ } else \
+ ::testing::Message()
+
+// A class representing the parsed contents of the
+// --gtest_internal_run_death_test flag, as it existed when
+// RUN_ALL_TESTS was called.
+class InternalRunDeathTestFlag {
+ public:
+ InternalRunDeathTestFlag(const std::string& a_file,
+ int a_line,
+ int an_index,
+ int a_write_fd)
+ : file_(a_file), line_(a_line), index_(an_index),
+ write_fd_(a_write_fd) {}
+
+ ~InternalRunDeathTestFlag() {
+ if (write_fd_ >= 0)
+ posix::Close(write_fd_);
+ }
+
+ const std::string& file() const { return file_; }
+ int line() const { return line_; }
+ int index() const { return index_; }
+ int write_fd() const { return write_fd_; }
+
+ private:
+ std::string file_;
+ int line_;
+ int index_;
+ int write_fd_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(InternalRunDeathTestFlag);
+};
+
+// Returns a newly created InternalRunDeathTestFlag object with fields
+// initialized from the GTEST_FLAG(internal_run_death_test) flag if
+// the flag is specified; otherwise returns NULL.
+InternalRunDeathTestFlag* ParseInternalRunDeathTestFlag();
+
+#else // GTEST_HAS_DEATH_TEST
+
+// This macro is used for implementing macros such as
+// EXPECT_DEATH_IF_SUPPORTED and ASSERT_DEATH_IF_SUPPORTED on systems where
+// death tests are not supported. Those macros must compile on such systems
+// iff EXPECT_DEATH and ASSERT_DEATH compile with the same parameters on
+// systems that support death tests. This allows one to write such a macro
+// on a system that does not support death tests and be sure that it will
+// compile on a death-test supporting system.
+//
+// Parameters:
+// statement - A statement that a macro such as EXPECT_DEATH would test
+// for program termination. This macro has to make sure this
+// statement is compiled but not executed, to ensure that
+// EXPECT_DEATH_IF_SUPPORTED compiles with a certain
+// parameter iff EXPECT_DEATH compiles with it.
+// regex - A regex that a macro such as EXPECT_DEATH would use to test
+// the output of statement. This parameter has to be
+// compiled but not evaluated by this macro, to ensure that
+// this macro only accepts expressions that a macro such as
+// EXPECT_DEATH would accept.
+// terminator - Must be an empty statement for EXPECT_DEATH_IF_SUPPORTED
+// and a return statement for ASSERT_DEATH_IF_SUPPORTED.
+// This ensures that ASSERT_DEATH_IF_SUPPORTED will not
+// compile inside functions where ASSERT_DEATH doesn't
+// compile.
+//
+// The branch that has an always false condition is used to ensure that
+// statement and regex are compiled (and thus syntactically correct) but
+// never executed. The unreachable code macro protects the terminator
+// statement from generating an 'unreachable code' warning in case
+// statement unconditionally returns or throws. The Message constructor at
+// the end allows the syntax of streaming additional messages into the
+// macro, for compilational compatibility with EXPECT_DEATH/ASSERT_DEATH.
+# define GTEST_UNSUPPORTED_DEATH_TEST_(statement, regex, terminator) \
+ GTEST_AMBIGUOUS_ELSE_BLOCKER_ \
+ if (::testing::internal::AlwaysTrue()) { \
+ GTEST_LOG_(WARNING) \
+ << "Death tests are not supported on this platform.\n" \
+ << "Statement '" #statement "' cannot be verified."; \
+ } else if (::testing::internal::AlwaysFalse()) { \
+ ::testing::internal::RE::PartialMatch(".*", (regex)); \
+ GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement); \
+ terminator; \
+ } else \
+ ::testing::Message()
+
+#endif // GTEST_HAS_DEATH_TEST
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_DEATH_TEST_INTERNAL_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-filepath.h b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-filepath.h
new file mode 100644
index 0000000..7a13b4b
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-filepath.h
@@ -0,0 +1,206 @@
+// Copyright 2008, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: keith.ray at gmail.com (Keith Ray)
+//
+// Google Test filepath utilities
+//
+// This header file declares classes and functions used internally by
+// Google Test. They are subject to change without notice.
+//
+// This file is #included in <gtest/internal/gtest-internal.h>.
+// Do not include this header file separately!
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_FILEPATH_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_FILEPATH_H_
+
+#include "gtest/internal/gtest-string.h"
+
+namespace testing {
+namespace internal {
+
+// FilePath - a class for file and directory pathname manipulation which
+// handles platform-specific conventions (like the pathname separator).
+// Used for helper functions for naming files in a directory for xml output.
+// Except for Set methods, all methods are const or static, which provides an
+// "immutable value object" -- useful for peace of mind.
+// A FilePath with a value ending in a path separator ("like/this/") represents
+// a directory, otherwise it is assumed to represent a file. In either case,
+// it may or may not represent an actual file or directory in the file system.
+// Names are NOT checked for syntax correctness -- no checking for illegal
+// characters, malformed paths, etc.
+
+class GTEST_API_ FilePath {
+ public:
+ FilePath() : pathname_("") { }
+ FilePath(const FilePath& rhs) : pathname_(rhs.pathname_) { }
+
+ explicit FilePath(const std::string& pathname) : pathname_(pathname) {
+ Normalize();
+ }
+
+ FilePath& operator=(const FilePath& rhs) {
+ Set(rhs);
+ return *this;
+ }
+
+ void Set(const FilePath& rhs) {
+ pathname_ = rhs.pathname_;
+ }
+
+ const std::string& string() const { return pathname_; }
+ const char* c_str() const { return pathname_.c_str(); }
+
+ // Returns the current working directory, or "" if unsuccessful.
+ static FilePath GetCurrentDir();
+
+ // Given directory = "dir", base_name = "test", number = 0,
+ // extension = "xml", returns "dir/test.xml". If number is greater
+ // than zero (e.g., 12), returns "dir/test_12.xml".
+ // On Windows platform, uses \ as the separator rather than /.
+ static FilePath MakeFileName(const FilePath& directory,
+ const FilePath& base_name,
+ int number,
+ const char* extension);
+
+ // Given directory = "dir", relative_path = "test.xml",
+ // returns "dir/test.xml".
+ // On Windows, uses \ as the separator rather than /.
+ static FilePath ConcatPaths(const FilePath& directory,
+ const FilePath& relative_path);
+
+ // Returns a pathname for a file that does not currently exist. The pathname
+ // will be directory/base_name.extension or
+ // directory/base_name_<number>.extension if directory/base_name.extension
+ // already exists. The number will be incremented until a pathname is found
+ // that does not already exist.
+ // Examples: 'dir/foo_test.xml' or 'dir/foo_test_1.xml'.
+ // There could be a race condition if two or more processes are calling this
+ // function at the same time -- they could both pick the same filename.
+ static FilePath GenerateUniqueFileName(const FilePath& directory,
+ const FilePath& base_name,
+ const char* extension);
+
+ // Returns true iff the path is "".
+ bool IsEmpty() const { return pathname_.empty(); }
+
+ // If input name has a trailing separator character, removes it and returns
+ // the name, otherwise return the name string unmodified.
+ // On Windows platform, uses \ as the separator, other platforms use /.
+ FilePath RemoveTrailingPathSeparator() const;
+
+ // Returns a copy of the FilePath with the directory part removed.
+ // Example: FilePath("path/to/file").RemoveDirectoryName() returns
+ // FilePath("file"). If there is no directory part ("just_a_file"), it returns
+ // the FilePath unmodified. If there is no file part ("just_a_dir/") it
+ // returns an empty FilePath ("").
+ // On Windows platform, '\' is the path separator, otherwise it is '/'.
+ FilePath RemoveDirectoryName() const;
+
+ // RemoveFileName returns the directory path with the filename removed.
+ // Example: FilePath("path/to/file").RemoveFileName() returns "path/to/".
+ // If the FilePath is "a_file" or "/a_file", RemoveFileName returns
+ // FilePath("./") or, on Windows, FilePath(".\\"). If the filepath does
+ // not have a file, like "just/a/dir/", it returns the FilePath unmodified.
+ // On Windows platform, '\' is the path separator, otherwise it is '/'.
+ FilePath RemoveFileName() const;
+
+ // Returns a copy of the FilePath with the case-insensitive extension removed.
+ // Example: FilePath("dir/file.exe").RemoveExtension("EXE") returns
+ // FilePath("dir/file"). If a case-insensitive extension is not
+ // found, returns a copy of the original FilePath.
+ FilePath RemoveExtension(const char* extension) const;
+
+ // Creates directories so that path exists. Returns true if successful or if
+ // the directories already exist; returns false if unable to create
+ // directories for any reason. Will also return false if the FilePath does
+ // not represent a directory (that is, it doesn't end with a path separator).
+ bool CreateDirectoriesRecursively() const;
+
+ // Create the directory so that path exists. Returns true if successful or
+ // if the directory already exists; returns false if unable to create the
+ // directory for any reason, including if the parent directory does not
+ // exist. Not named "CreateDirectory" because that's a macro on Windows.
+ bool CreateFolder() const;
+
+ // Returns true if FilePath describes something in the file-system,
+ // either a file, directory, or whatever, and that something exists.
+ bool FileOrDirectoryExists() const;
+
+ // Returns true if pathname describes a directory in the file-system
+ // that exists.
+ bool DirectoryExists() const;
+
+ // Returns true if FilePath ends with a path separator, which indicates that
+ // it is intended to represent a directory. Returns false otherwise.
+ // This does NOT check that a directory (or file) actually exists.
+ bool IsDirectory() const;
+
+ // Returns true if pathname describes a root directory. (Windows has one
+ // root directory per disk drive.)
+ bool IsRootDirectory() const;
+
+ // Returns true if pathname describes an absolute path.
+ bool IsAbsolutePath() const;
+
+ private:
+ // Replaces multiple consecutive separators with a single separator.
+ // For example, "bar///foo" becomes "bar/foo". Does not eliminate other
+ // redundancies that might be in a pathname involving "." or "..".
+ //
+ // A pathname with multiple consecutive separators may occur either through
+ // user error or as a result of some scripts or APIs that generate a pathname
+ // with a trailing separator. On other platforms the same API or script
+ // may NOT generate a pathname with a trailing "/". Then elsewhere that
+ // pathname may have another "/" and pathname components added to it,
+ // without checking for the separator already being there.
+ // The script language and operating system may allow paths like "foo//bar"
+ // but some of the functions in FilePath will not handle that correctly. In
+ // particular, RemoveTrailingPathSeparator() only removes one separator, and
+ // it is called in CreateDirectoriesRecursively() assuming that it will change
+ // a pathname from directory syntax (trailing separator) to filename syntax.
+ //
+ // On Windows this method also replaces the alternate path separator '/' with
+ // the primary path separator '\\', so that for example "bar\\/\\foo" becomes
+ // "bar\\foo".
+
+ void Normalize();
+
+ // Returns a pointer to the last occurence of a valid path separator in
+ // the FilePath. On Windows, for example, both '/' and '\' are valid path
+ // separators. Returns NULL if no path separator was found.
+ const char* FindLastPathSeparator() const;
+
+ std::string pathname_;
+}; // class FilePath
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_FILEPATH_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-internal.h b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-internal.h
new file mode 100644
index 0000000..0dcc3a3
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-internal.h
@@ -0,0 +1,1158 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Authors: wan at google.com (Zhanyong Wan), eefacm at gmail.com (Sean Mcafee)
+//
+// The Google C++ Testing Framework (Google Test)
+//
+// This header file declares functions and macros used internally by
+// Google Test. They are subject to change without notice.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_INTERNAL_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_INTERNAL_H_
+
+#include "gtest/internal/gtest-port.h"
+
+#if GTEST_OS_LINUX
+# include <stdlib.h>
+# include <sys/types.h>
+# include <sys/wait.h>
+# include <unistd.h>
+#endif // GTEST_OS_LINUX
+
+#if GTEST_HAS_EXCEPTIONS
+# include <stdexcept>
+#endif
+
+#include <ctype.h>
+#include <float.h>
+#include <string.h>
+#include <iomanip>
+#include <limits>
+#include <set>
+
+#include "gtest/gtest-message.h"
+#include "gtest/internal/gtest-string.h"
+#include "gtest/internal/gtest-filepath.h"
+#include "gtest/internal/gtest-type-util.h"
+
+// Due to C++ preprocessor weirdness, we need double indirection to
+// concatenate two tokens when one of them is __LINE__. Writing
+//
+// foo ## __LINE__
+//
+// will result in the token foo__LINE__, instead of foo followed by
+// the current line number. For more details, see
+// http://www.parashift.com/c++-faq-lite/misc-technical-issues.html#faq-39.6
+#define GTEST_CONCAT_TOKEN_(foo, bar) GTEST_CONCAT_TOKEN_IMPL_(foo, bar)
+#define GTEST_CONCAT_TOKEN_IMPL_(foo, bar) foo ## bar
+
+class ProtocolMessage;
+namespace proto2 { class Message; }
+
+namespace testing {
+
+// Forward declarations.
+
+class AssertionResult; // Result of an assertion.
+class Message; // Represents a failure message.
+class Test; // Represents a test.
+class TestInfo; // Information about a test.
+class TestPartResult; // Result of a test part.
+class UnitTest; // A collection of test cases.
+
+template <typename T>
+::std::string PrintToString(const T& value);
+
+namespace internal {
+
+struct TraceInfo; // Information about a trace point.
+class ScopedTrace; // Implements scoped trace.
+class TestInfoImpl; // Opaque implementation of TestInfo
+class UnitTestImpl; // Opaque implementation of UnitTest
+
+// How many times InitGoogleTest() has been called.
+GTEST_API_ extern int g_init_gtest_count;
+
+// The text used in failure messages to indicate the start of the
+// stack trace.
+GTEST_API_ extern const char kStackTraceMarker[];
+
+// Two overloaded helpers for checking at compile time whether an
+// expression is a null pointer literal (i.e. NULL or any 0-valued
+// compile-time integral constant). Their return values have
+// different sizes, so we can use sizeof() to test which version is
+// picked by the compiler. These helpers have no implementations, as
+// we only need their signatures.
+//
+// Given IsNullLiteralHelper(x), the compiler will pick the first
+// version if x can be implicitly converted to Secret*, and pick the
+// second version otherwise. Since Secret is a secret and incomplete
+// type, the only expression a user can write that has type Secret* is
+// a null pointer literal. Therefore, we know that x is a null
+// pointer literal if and only if the first version is picked by the
+// compiler.
+char IsNullLiteralHelper(Secret* p);
+char (&IsNullLiteralHelper(...))[2]; // NOLINT
+
+// A compile-time bool constant that is true if and only if x is a
+// null pointer literal (i.e. NULL or any 0-valued compile-time
+// integral constant).
+#ifdef GTEST_ELLIPSIS_NEEDS_POD_
+// We lose support for NULL detection where the compiler doesn't like
+// passing non-POD classes through ellipsis (...).
+# define GTEST_IS_NULL_LITERAL_(x) false
+#else
+# define GTEST_IS_NULL_LITERAL_(x) \
+ (sizeof(::testing::internal::IsNullLiteralHelper(x)) == 1)
+#endif // GTEST_ELLIPSIS_NEEDS_POD_
+
+// Appends the user-supplied message to the Google-Test-generated message.
+GTEST_API_ std::string AppendUserMessage(
+ const std::string& gtest_msg, const Message& user_msg);
+
+#if GTEST_HAS_EXCEPTIONS
+
+// This exception is thrown by (and only by) a failed Google Test
+// assertion when GTEST_FLAG(throw_on_failure) is true (if exceptions
+// are enabled). We derive it from std::runtime_error, which is for
+// errors presumably detectable only at run time. Since
+// std::runtime_error inherits from std::exception, many testing
+// frameworks know how to extract and print the message inside it.
+class GTEST_API_ GoogleTestFailureException : public ::std::runtime_error {
+ public:
+ explicit GoogleTestFailureException(const TestPartResult& failure);
+};
+
+#endif // GTEST_HAS_EXCEPTIONS
+
+// A helper class for creating scoped traces in user programs.
+class GTEST_API_ ScopedTrace {
+ public:
+ // The c'tor pushes the given source file location and message onto
+ // a trace stack maintained by Google Test.
+ ScopedTrace(const char* file, int line, const Message& message);
+
+ // The d'tor pops the info pushed by the c'tor.
+ //
+ // Note that the d'tor is not virtual in order to be efficient.
+ // Don't inherit from ScopedTrace!
+ ~ScopedTrace();
+
+ private:
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ScopedTrace);
+} GTEST_ATTRIBUTE_UNUSED_; // A ScopedTrace object does its job in its
+ // c'tor and d'tor. Therefore it doesn't
+ // need to be used otherwise.
+
+// Constructs and returns the message for an equality assertion
+// (e.g. ASSERT_EQ, EXPECT_STREQ, etc) failure.
+//
+// The first four parameters are the expressions used in the assertion
+// and their values, as strings. For example, for ASSERT_EQ(foo, bar)
+// where foo is 5 and bar is 6, we have:
+//
+// expected_expression: "foo"
+// actual_expression: "bar"
+// expected_value: "5"
+// actual_value: "6"
+//
+// The ignoring_case parameter is true iff the assertion is a
+// *_STRCASEEQ*. When it's true, the string " (ignoring case)" will
+// be inserted into the message.
+GTEST_API_ AssertionResult EqFailure(const char* expected_expression,
+ const char* actual_expression,
+ const std::string& expected_value,
+ const std::string& actual_value,
+ bool ignoring_case);
+
+// Constructs a failure message for Boolean assertions such as EXPECT_TRUE.
+GTEST_API_ std::string GetBoolAssertionFailureMessage(
+ const AssertionResult& assertion_result,
+ const char* expression_text,
+ const char* actual_predicate_value,
+ const char* expected_predicate_value);
+
+// This template class represents an IEEE floating-point number
+// (either single-precision or double-precision, depending on the
+// template parameters).
+//
+// The purpose of this class is to do more sophisticated number
+// comparison. (Due to round-off error, etc, it's very unlikely that
+// two floating-points will be equal exactly. Hence a naive
+// comparison by the == operation often doesn't work.)
+//
+// Format of IEEE floating-point:
+//
+// The most-significant bit being the leftmost, an IEEE
+// floating-point looks like
+//
+// sign_bit exponent_bits fraction_bits
+//
+// Here, sign_bit is a single bit that designates the sign of the
+// number.
+//
+// For float, there are 8 exponent bits and 23 fraction bits.
+//
+// For double, there are 11 exponent bits and 52 fraction bits.
+//
+// More details can be found at
+// http://en.wikipedia.org/wiki/IEEE_floating-point_standard.
+//
+// Template parameter:
+//
+// RawType: the raw floating-point type (either float or double)
+template <typename RawType>
+class FloatingPoint {
+ public:
+ // Defines the unsigned integer type that has the same size as the
+ // floating point number.
+ typedef typename TypeWithSize<sizeof(RawType)>::UInt Bits;
+
+ // Constants.
+
+ // # of bits in a number.
+ static const size_t kBitCount = 8*sizeof(RawType);
+
+ // # of fraction bits in a number.
+ static const size_t kFractionBitCount =
+ std::numeric_limits<RawType>::digits - 1;
+
+ // # of exponent bits in a number.
+ static const size_t kExponentBitCount = kBitCount - 1 - kFractionBitCount;
+
+ // The mask for the sign bit.
+ static const Bits kSignBitMask = static_cast<Bits>(1) << (kBitCount - 1);
+
+ // The mask for the fraction bits.
+ static const Bits kFractionBitMask =
+ ~static_cast<Bits>(0) >> (kExponentBitCount + 1);
+
+ // The mask for the exponent bits.
+ static const Bits kExponentBitMask = ~(kSignBitMask | kFractionBitMask);
+
+ // How many ULP's (Units in the Last Place) we want to tolerate when
+ // comparing two numbers. The larger the value, the more error we
+ // allow. A 0 value means that two numbers must be exactly the same
+ // to be considered equal.
+ //
+ // The maximum error of a single floating-point operation is 0.5
+ // units in the last place. On Intel CPU's, all floating-point
+ // calculations are done with 80-bit precision, while double has 64
+ // bits. Therefore, 4 should be enough for ordinary use.
+ //
+ // See the following article for more details on ULP:
+ // http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
+ static const size_t kMaxUlps = 4;
+
+ // Constructs a FloatingPoint from a raw floating-point number.
+ //
+ // On an Intel CPU, passing a non-normalized NAN (Not a Number)
+ // around may change its bits, although the new value is guaranteed
+ // to be also a NAN. Therefore, don't expect this constructor to
+ // preserve the bits in x when x is a NAN.
+ explicit FloatingPoint(const RawType& x) { u_.value_ = x; }
+
+ // Static methods
+
+ // Reinterprets a bit pattern as a floating-point number.
+ //
+ // This function is needed to test the AlmostEquals() method.
+ static RawType ReinterpretBits(const Bits bits) {
+ FloatingPoint fp(0);
+ fp.u_.bits_ = bits;
+ return fp.u_.value_;
+ }
+
+ // Returns the floating-point number that represent positive infinity.
+ static RawType Infinity() {
+ return ReinterpretBits(kExponentBitMask);
+ }
+
+ // Returns the maximum representable finite floating-point number.
+ static RawType Max();
+
+ // Non-static methods
+
+ // Returns the bits that represents this number.
+ const Bits &bits() const { return u_.bits_; }
+
+ // Returns the exponent bits of this number.
+ Bits exponent_bits() const { return kExponentBitMask & u_.bits_; }
+
+ // Returns the fraction bits of this number.
+ Bits fraction_bits() const { return kFractionBitMask & u_.bits_; }
+
+ // Returns the sign bit of this number.
+ Bits sign_bit() const { return kSignBitMask & u_.bits_; }
+
+ // Returns true iff this is NAN (not a number).
+ bool is_nan() const {
+ // It's a NAN if the exponent bits are all ones and the fraction
+ // bits are not entirely zeros.
+ return (exponent_bits() == kExponentBitMask) && (fraction_bits() != 0);
+ }
+
+ // Returns true iff this number is at most kMaxUlps ULP's away from
+ // rhs. In particular, this function:
+ //
+ // - returns false if either number is (or both are) NAN.
+ // - treats really large numbers as almost equal to infinity.
+ // - thinks +0.0 and -0.0 are 0 DLP's apart.
+ bool AlmostEquals(const FloatingPoint& rhs) const {
+ // The IEEE standard says that any comparison operation involving
+ // a NAN must return false.
+ if (is_nan() || rhs.is_nan()) return false;
+
+ return DistanceBetweenSignAndMagnitudeNumbers(u_.bits_, rhs.u_.bits_)
+ <= kMaxUlps;
+ }
+
+ private:
+ // The data type used to store the actual floating-point number.
+ union FloatingPointUnion {
+ RawType value_; // The raw floating-point number.
+ Bits bits_; // The bits that represent the number.
+ };
+
+ // Converts an integer from the sign-and-magnitude representation to
+ // the biased representation. More precisely, let N be 2 to the
+ // power of (kBitCount - 1), an integer x is represented by the
+ // unsigned number x + N.
+ //
+ // For instance,
+ //
+ // -N + 1 (the most negative number representable using
+ // sign-and-magnitude) is represented by 1;
+ // 0 is represented by N; and
+ // N - 1 (the biggest number representable using
+ // sign-and-magnitude) is represented by 2N - 1.
+ //
+ // Read http://en.wikipedia.org/wiki/Signed_number_representations
+ // for more details on signed number representations.
+ static Bits SignAndMagnitudeToBiased(const Bits &sam) {
+ if (kSignBitMask & sam) {
+ // sam represents a negative number.
+ return ~sam + 1;
+ } else {
+ // sam represents a positive number.
+ return kSignBitMask | sam;
+ }
+ }
+
+ // Given two numbers in the sign-and-magnitude representation,
+ // returns the distance between them as an unsigned number.
+ static Bits DistanceBetweenSignAndMagnitudeNumbers(const Bits &sam1,
+ const Bits &sam2) {
+ const Bits biased1 = SignAndMagnitudeToBiased(sam1);
+ const Bits biased2 = SignAndMagnitudeToBiased(sam2);
+ return (biased1 >= biased2) ? (biased1 - biased2) : (biased2 - biased1);
+ }
+
+ FloatingPointUnion u_;
+};
+
+// We cannot use std::numeric_limits<T>::max() as it clashes with the max()
+// macro defined by <windows.h>.
+template <>
+inline float FloatingPoint<float>::Max() { return FLT_MAX; }
+template <>
+inline double FloatingPoint<double>::Max() { return DBL_MAX; }
+
+// Typedefs the instances of the FloatingPoint template class that we
+// care to use.
+typedef FloatingPoint<float> Float;
+typedef FloatingPoint<double> Double;
+
+// In order to catch the mistake of putting tests that use different
+// test fixture classes in the same test case, we need to assign
+// unique IDs to fixture classes and compare them. The TypeId type is
+// used to hold such IDs. The user should treat TypeId as an opaque
+// type: the only operation allowed on TypeId values is to compare
+// them for equality using the == operator.
+typedef const void* TypeId;
+
+template <typename T>
+class TypeIdHelper {
+ public:
+ // dummy_ must not have a const type. Otherwise an overly eager
+ // compiler (e.g. MSVC 7.1 & 8.0) may try to merge
+ // TypeIdHelper<T>::dummy_ for different Ts as an "optimization".
+ static bool dummy_;
+};
+
+template <typename T>
+bool TypeIdHelper<T>::dummy_ = false;
+
+// GetTypeId<T>() returns the ID of type T. Different values will be
+// returned for different types. Calling the function twice with the
+// same type argument is guaranteed to return the same ID.
+template <typename T>
+TypeId GetTypeId() {
+ // The compiler is required to allocate a different
+ // TypeIdHelper<T>::dummy_ variable for each T used to instantiate
+ // the template. Therefore, the address of dummy_ is guaranteed to
+ // be unique.
+ return &(TypeIdHelper<T>::dummy_);
+}
+
+// Returns the type ID of ::testing::Test. Always call this instead
+// of GetTypeId< ::testing::Test>() to get the type ID of
+// ::testing::Test, as the latter may give the wrong result due to a
+// suspected linker bug when compiling Google Test as a Mac OS X
+// framework.
+GTEST_API_ TypeId GetTestTypeId();
+
+// Defines the abstract factory interface that creates instances
+// of a Test object.
+class TestFactoryBase {
+ public:
+ virtual ~TestFactoryBase() {}
+
+ // Creates a test instance to run. The instance is both created and destroyed
+ // within TestInfoImpl::Run()
+ virtual Test* CreateTest() = 0;
+
+ protected:
+ TestFactoryBase() {}
+
+ private:
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(TestFactoryBase);
+};
+
+// This class provides implementation of TeastFactoryBase interface.
+// It is used in TEST and TEST_F macros.
+template <class TestClass>
+class TestFactoryImpl : public TestFactoryBase {
+ public:
+ virtual Test* CreateTest() { return new TestClass; }
+};
+
+#if GTEST_OS_WINDOWS
+
+// Predicate-formatters for implementing the HRESULT checking macros
+// {ASSERT|EXPECT}_HRESULT_{SUCCEEDED|FAILED}
+// We pass a long instead of HRESULT to avoid causing an
+// include dependency for the HRESULT type.
+GTEST_API_ AssertionResult IsHRESULTSuccess(const char* expr,
+ long hr); // NOLINT
+GTEST_API_ AssertionResult IsHRESULTFailure(const char* expr,
+ long hr); // NOLINT
+
+#endif // GTEST_OS_WINDOWS
+
+// Types of SetUpTestCase() and TearDownTestCase() functions.
+typedef void (*SetUpTestCaseFunc)();
+typedef void (*TearDownTestCaseFunc)();
+
+// Creates a new TestInfo object and registers it with Google Test;
+// returns the created object.
+//
+// Arguments:
+//
+// test_case_name: name of the test case
+// name: name of the test
+// type_param the name of the test's type parameter, or NULL if
+// this is not a typed or a type-parameterized test.
+// value_param text representation of the test's value parameter,
+// or NULL if this is not a type-parameterized test.
+// fixture_class_id: ID of the test fixture class
+// set_up_tc: pointer to the function that sets up the test case
+// tear_down_tc: pointer to the function that tears down the test case
+// factory: pointer to the factory that creates a test object.
+// The newly created TestInfo instance will assume
+// ownership of the factory object.
+GTEST_API_ TestInfo* MakeAndRegisterTestInfo(
+ const char* test_case_name,
+ const char* name,
+ const char* type_param,
+ const char* value_param,
+ TypeId fixture_class_id,
+ SetUpTestCaseFunc set_up_tc,
+ TearDownTestCaseFunc tear_down_tc,
+ TestFactoryBase* factory);
+
+// If *pstr starts with the given prefix, modifies *pstr to be right
+// past the prefix and returns true; otherwise leaves *pstr unchanged
+// and returns false. None of pstr, *pstr, and prefix can be NULL.
+GTEST_API_ bool SkipPrefix(const char* prefix, const char** pstr);
+
+#if GTEST_HAS_TYPED_TEST || GTEST_HAS_TYPED_TEST_P
+
+// State of the definition of a type-parameterized test case.
+class GTEST_API_ TypedTestCasePState {
+ public:
+ TypedTestCasePState() : registered_(false) {}
+
+ // Adds the given test name to defined_test_names_ and return true
+ // if the test case hasn't been registered; otherwise aborts the
+ // program.
+ bool AddTestName(const char* file, int line, const char* case_name,
+ const char* test_name) {
+ if (registered_) {
+ fprintf(stderr, "%s Test %s must be defined before "
+ "REGISTER_TYPED_TEST_CASE_P(%s, ...).\n",
+ FormatFileLocation(file, line).c_str(), test_name, case_name);
+ fflush(stderr);
+ posix::Abort();
+ }
+ defined_test_names_.insert(test_name);
+ return true;
+ }
+
+ // Verifies that registered_tests match the test names in
+ // defined_test_names_; returns registered_tests if successful, or
+ // aborts the program otherwise.
+ const char* VerifyRegisteredTestNames(
+ const char* file, int line, const char* registered_tests);
+
+ private:
+ bool registered_;
+ ::std::set<const char*> defined_test_names_;
+};
+
+// Skips to the first non-space char after the first comma in 'str';
+// returns NULL if no comma is found in 'str'.
+inline const char* SkipComma(const char* str) {
+ const char* comma = strchr(str, ',');
+ if (comma == NULL) {
+ return NULL;
+ }
+ while (IsSpace(*(++comma))) {}
+ return comma;
+}
+
+// Returns the prefix of 'str' before the first comma in it; returns
+// the entire string if it contains no comma.
+inline std::string GetPrefixUntilComma(const char* str) {
+ const char* comma = strchr(str, ',');
+ return comma == NULL ? str : std::string(str, comma);
+}
+
+// TypeParameterizedTest<Fixture, TestSel, Types>::Register()
+// registers a list of type-parameterized tests with Google Test. The
+// return value is insignificant - we just need to return something
+// such that we can call this function in a namespace scope.
+//
+// Implementation note: The GTEST_TEMPLATE_ macro declares a template
+// template parameter. It's defined in gtest-type-util.h.
+template <GTEST_TEMPLATE_ Fixture, class TestSel, typename Types>
+class TypeParameterizedTest {
+ public:
+ // 'index' is the index of the test in the type list 'Types'
+ // specified in INSTANTIATE_TYPED_TEST_CASE_P(Prefix, TestCase,
+ // Types). Valid values for 'index' are [0, N - 1] where N is the
+ // length of Types.
+ static bool Register(const char* prefix, const char* case_name,
+ const char* test_names, int index) {
+ typedef typename Types::Head Type;
+ typedef Fixture<Type> FixtureClass;
+ typedef typename GTEST_BIND_(TestSel, Type) TestClass;
+
+ // First, registers the first type-parameterized test in the type
+ // list.
+ MakeAndRegisterTestInfo(
+ (std::string(prefix) + (prefix[0] == '\0' ? "" : "/") + case_name + "/"
+ + StreamableToString(index)).c_str(),
+ GetPrefixUntilComma(test_names).c_str(),
+ GetTypeName<Type>().c_str(),
+ NULL, // No value parameter.
+ GetTypeId<FixtureClass>(),
+ TestClass::SetUpTestCase,
+ TestClass::TearDownTestCase,
+ new TestFactoryImpl<TestClass>);
+
+ // Next, recurses (at compile time) with the tail of the type list.
+ return TypeParameterizedTest<Fixture, TestSel, typename Types::Tail>
+ ::Register(prefix, case_name, test_names, index + 1);
+ }
+};
+
+// The base case for the compile time recursion.
+template <GTEST_TEMPLATE_ Fixture, class TestSel>
+class TypeParameterizedTest<Fixture, TestSel, Types0> {
+ public:
+ static bool Register(const char* /*prefix*/, const char* /*case_name*/,
+ const char* /*test_names*/, int /*index*/) {
+ return true;
+ }
+};
+
+// TypeParameterizedTestCase<Fixture, Tests, Types>::Register()
+// registers *all combinations* of 'Tests' and 'Types' with Google
+// Test. The return value is insignificant - we just need to return
+// something such that we can call this function in a namespace scope.
+template <GTEST_TEMPLATE_ Fixture, typename Tests, typename Types>
+class TypeParameterizedTestCase {
+ public:
+ static bool Register(const char* prefix, const char* case_name,
+ const char* test_names) {
+ typedef typename Tests::Head Head;
+
+ // First, register the first test in 'Test' for each type in 'Types'.
+ TypeParameterizedTest<Fixture, Head, Types>::Register(
+ prefix, case_name, test_names, 0);
+
+ // Next, recurses (at compile time) with the tail of the test list.
+ return TypeParameterizedTestCase<Fixture, typename Tests::Tail, Types>
+ ::Register(prefix, case_name, SkipComma(test_names));
+ }
+};
+
+// The base case for the compile time recursion.
+template <GTEST_TEMPLATE_ Fixture, typename Types>
+class TypeParameterizedTestCase<Fixture, Templates0, Types> {
+ public:
+ static bool Register(const char* /*prefix*/, const char* /*case_name*/,
+ const char* /*test_names*/) {
+ return true;
+ }
+};
+
+#endif // GTEST_HAS_TYPED_TEST || GTEST_HAS_TYPED_TEST_P
+
+// Returns the current OS stack trace as an std::string.
+//
+// The maximum number of stack frames to be included is specified by
+// the gtest_stack_trace_depth flag. The skip_count parameter
+// specifies the number of top frames to be skipped, which doesn't
+// count against the number of frames to be included.
+//
+// For example, if Foo() calls Bar(), which in turn calls
+// GetCurrentOsStackTraceExceptTop(..., 1), Foo() will be included in
+// the trace but Bar() and GetCurrentOsStackTraceExceptTop() won't.
+GTEST_API_ std::string GetCurrentOsStackTraceExceptTop(
+ UnitTest* unit_test, int skip_count);
+
+// Helpers for suppressing warnings on unreachable code or constant
+// condition.
+
+// Always returns true.
+GTEST_API_ bool AlwaysTrue();
+
+// Always returns false.
+inline bool AlwaysFalse() { return !AlwaysTrue(); }
+
+// Helper for suppressing false warning from Clang on a const char*
+// variable declared in a conditional expression always being NULL in
+// the else branch.
+struct GTEST_API_ ConstCharPtr {
+ ConstCharPtr(const char* str) : value(str) {}
+ operator bool() const { return true; }
+ const char* value;
+};
+
+// A simple Linear Congruential Generator for generating random
+// numbers with a uniform distribution. Unlike rand() and srand(), it
+// doesn't use global state (and therefore can't interfere with user
+// code). Unlike rand_r(), it's portable. An LCG isn't very random,
+// but it's good enough for our purposes.
+class GTEST_API_ Random {
+ public:
+ static const UInt32 kMaxRange = 1u << 31;
+
+ explicit Random(UInt32 seed) : state_(seed) {}
+
+ void Reseed(UInt32 seed) { state_ = seed; }
+
+ // Generates a random number from [0, range). Crashes if 'range' is
+ // 0 or greater than kMaxRange.
+ UInt32 Generate(UInt32 range);
+
+ private:
+ UInt32 state_;
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(Random);
+};
+
+// Defining a variable of type CompileAssertTypesEqual<T1, T2> will cause a
+// compiler error iff T1 and T2 are different types.
+template <typename T1, typename T2>
+struct CompileAssertTypesEqual;
+
+template <typename T>
+struct CompileAssertTypesEqual<T, T> {
+};
+
+// Removes the reference from a type if it is a reference type,
+// otherwise leaves it unchanged. This is the same as
+// tr1::remove_reference, which is not widely available yet.
+template <typename T>
+struct RemoveReference { typedef T type; }; // NOLINT
+template <typename T>
+struct RemoveReference<T&> { typedef T type; }; // NOLINT
+
+// A handy wrapper around RemoveReference that works when the argument
+// T depends on template parameters.
+#define GTEST_REMOVE_REFERENCE_(T) \
+ typename ::testing::internal::RemoveReference<T>::type
+
+// Removes const from a type if it is a const type, otherwise leaves
+// it unchanged. This is the same as tr1::remove_const, which is not
+// widely available yet.
+template <typename T>
+struct RemoveConst { typedef T type; }; // NOLINT
+template <typename T>
+struct RemoveConst<const T> { typedef T type; }; // NOLINT
+
+// MSVC 8.0, Sun C++, and IBM XL C++ have a bug which causes the above
+// definition to fail to remove the const in 'const int[3]' and 'const
+// char[3][4]'. The following specialization works around the bug.
+template <typename T, size_t N>
+struct RemoveConst<const T[N]> {
+ typedef typename RemoveConst<T>::type type[N];
+};
+
+#if defined(_MSC_VER) && _MSC_VER < 1400
+// This is the only specialization that allows VC++ 7.1 to remove const in
+// 'const int[3] and 'const int[3][4]'. However, it causes trouble with GCC
+// and thus needs to be conditionally compiled.
+template <typename T, size_t N>
+struct RemoveConst<T[N]> {
+ typedef typename RemoveConst<T>::type type[N];
+};
+#endif
+
+// A handy wrapper around RemoveConst that works when the argument
+// T depends on template parameters.
+#define GTEST_REMOVE_CONST_(T) \
+ typename ::testing::internal::RemoveConst<T>::type
+
+// Turns const U&, U&, const U, and U all into U.
+#define GTEST_REMOVE_REFERENCE_AND_CONST_(T) \
+ GTEST_REMOVE_CONST_(GTEST_REMOVE_REFERENCE_(T))
+
+// Adds reference to a type if it is not a reference type,
+// otherwise leaves it unchanged. This is the same as
+// tr1::add_reference, which is not widely available yet.
+template <typename T>
+struct AddReference { typedef T& type; }; // NOLINT
+template <typename T>
+struct AddReference<T&> { typedef T& type; }; // NOLINT
+
+// A handy wrapper around AddReference that works when the argument T
+// depends on template parameters.
+#define GTEST_ADD_REFERENCE_(T) \
+ typename ::testing::internal::AddReference<T>::type
+
+// Adds a reference to const on top of T as necessary. For example,
+// it transforms
+//
+// char ==> const char&
+// const char ==> const char&
+// char& ==> const char&
+// const char& ==> const char&
+//
+// The argument T must depend on some template parameters.
+#define GTEST_REFERENCE_TO_CONST_(T) \
+ GTEST_ADD_REFERENCE_(const GTEST_REMOVE_REFERENCE_(T))
+
+// ImplicitlyConvertible<From, To>::value is a compile-time bool
+// constant that's true iff type From can be implicitly converted to
+// type To.
+template <typename From, typename To>
+class ImplicitlyConvertible {
+ private:
+ // We need the following helper functions only for their types.
+ // They have no implementations.
+
+ // MakeFrom() is an expression whose type is From. We cannot simply
+ // use From(), as the type From may not have a public default
+ // constructor.
+ static From MakeFrom();
+
+ // These two functions are overloaded. Given an expression
+ // Helper(x), the compiler will pick the first version if x can be
+ // implicitly converted to type To; otherwise it will pick the
+ // second version.
+ //
+ // The first version returns a value of size 1, and the second
+ // version returns a value of size 2. Therefore, by checking the
+ // size of Helper(x), which can be done at compile time, we can tell
+ // which version of Helper() is used, and hence whether x can be
+ // implicitly converted to type To.
+ static char Helper(To);
+ static char (&Helper(...))[2]; // NOLINT
+
+ // We have to put the 'public' section after the 'private' section,
+ // or MSVC refuses to compile the code.
+ public:
+ // MSVC warns about implicitly converting from double to int for
+ // possible loss of data, so we need to temporarily disable the
+ // warning.
+#ifdef _MSC_VER
+# pragma warning(push) // Saves the current warning state.
+# pragma warning(disable:4244) // Temporarily disables warning 4244.
+
+ static const bool value =
+ sizeof(Helper(ImplicitlyConvertible::MakeFrom())) == 1;
+# pragma warning(pop) // Restores the warning state.
+#elif defined(__BORLANDC__)
+ // C++Builder cannot use member overload resolution during template
+ // instantiation. The simplest workaround is to use its C++0x type traits
+ // functions (C++Builder 2009 and above only).
+ static const bool value = __is_convertible(From, To);
+#else
+ static const bool value =
+ sizeof(Helper(ImplicitlyConvertible::MakeFrom())) == 1;
+#endif // _MSV_VER
+};
+template <typename From, typename To>
+const bool ImplicitlyConvertible<From, To>::value;
+
+// IsAProtocolMessage<T>::value is a compile-time bool constant that's
+// true iff T is type ProtocolMessage, proto2::Message, or a subclass
+// of those.
+template <typename T>
+struct IsAProtocolMessage
+ : public bool_constant<
+ ImplicitlyConvertible<const T*, const ::ProtocolMessage*>::value ||
+ ImplicitlyConvertible<const T*, const ::proto2::Message*>::value> {
+};
+
+// When the compiler sees expression IsContainerTest<C>(0), if C is an
+// STL-style container class, the first overload of IsContainerTest
+// will be viable (since both C::iterator* and C::const_iterator* are
+// valid types and NULL can be implicitly converted to them). It will
+// be picked over the second overload as 'int' is a perfect match for
+// the type of argument 0. If C::iterator or C::const_iterator is not
+// a valid type, the first overload is not viable, and the second
+// overload will be picked. Therefore, we can determine whether C is
+// a container class by checking the type of IsContainerTest<C>(0).
+// The value of the expression is insignificant.
+//
+// Note that we look for both C::iterator and C::const_iterator. The
+// reason is that C++ injects the name of a class as a member of the
+// class itself (e.g. you can refer to class iterator as either
+// 'iterator' or 'iterator::iterator'). If we look for C::iterator
+// only, for example, we would mistakenly think that a class named
+// iterator is an STL container.
+//
+// Also note that the simpler approach of overloading
+// IsContainerTest(typename C::const_iterator*) and
+// IsContainerTest(...) doesn't work with Visual Age C++ and Sun C++.
+typedef int IsContainer;
+template <class C>
+IsContainer IsContainerTest(int /* dummy */,
+ typename C::iterator* /* it */ = NULL,
+ typename C::const_iterator* /* const_it */ = NULL) {
+ return 0;
+}
+
+typedef char IsNotContainer;
+template <class C>
+IsNotContainer IsContainerTest(long /* dummy */) { return '\0'; }
+
+// EnableIf<condition>::type is void when 'Cond' is true, and
+// undefined when 'Cond' is false. To use SFINAE to make a function
+// overload only apply when a particular expression is true, add
+// "typename EnableIf<expression>::type* = 0" as the last parameter.
+template<bool> struct EnableIf;
+template<> struct EnableIf<true> { typedef void type; }; // NOLINT
+
+// Utilities for native arrays.
+
+// ArrayEq() compares two k-dimensional native arrays using the
+// elements' operator==, where k can be any integer >= 0. When k is
+// 0, ArrayEq() degenerates into comparing a single pair of values.
+
+template <typename T, typename U>
+bool ArrayEq(const T* lhs, size_t size, const U* rhs);
+
+// This generic version is used when k is 0.
+template <typename T, typename U>
+inline bool ArrayEq(const T& lhs, const U& rhs) { return lhs == rhs; }
+
+// This overload is used when k >= 1.
+template <typename T, typename U, size_t N>
+inline bool ArrayEq(const T(&lhs)[N], const U(&rhs)[N]) {
+ return internal::ArrayEq(lhs, N, rhs);
+}
+
+// This helper reduces code bloat. If we instead put its logic inside
+// the previous ArrayEq() function, arrays with different sizes would
+// lead to different copies of the template code.
+template <typename T, typename U>
+bool ArrayEq(const T* lhs, size_t size, const U* rhs) {
+ for (size_t i = 0; i != size; i++) {
+ if (!internal::ArrayEq(lhs[i], rhs[i]))
+ return false;
+ }
+ return true;
+}
+
+// Finds the first element in the iterator range [begin, end) that
+// equals elem. Element may be a native array type itself.
+template <typename Iter, typename Element>
+Iter ArrayAwareFind(Iter begin, Iter end, const Element& elem) {
+ for (Iter it = begin; it != end; ++it) {
+ if (internal::ArrayEq(*it, elem))
+ return it;
+ }
+ return end;
+}
+
+// CopyArray() copies a k-dimensional native array using the elements'
+// operator=, where k can be any integer >= 0. When k is 0,
+// CopyArray() degenerates into copying a single value.
+
+template <typename T, typename U>
+void CopyArray(const T* from, size_t size, U* to);
+
+// This generic version is used when k is 0.
+template <typename T, typename U>
+inline void CopyArray(const T& from, U* to) { *to = from; }
+
+// This overload is used when k >= 1.
+template <typename T, typename U, size_t N>
+inline void CopyArray(const T(&from)[N], U(*to)[N]) {
+ internal::CopyArray(from, N, *to);
+}
+
+// This helper reduces code bloat. If we instead put its logic inside
+// the previous CopyArray() function, arrays with different sizes
+// would lead to different copies of the template code.
+template <typename T, typename U>
+void CopyArray(const T* from, size_t size, U* to) {
+ for (size_t i = 0; i != size; i++) {
+ internal::CopyArray(from[i], to + i);
+ }
+}
+
+// The relation between an NativeArray object (see below) and the
+// native array it represents.
+enum RelationToSource {
+ kReference, // The NativeArray references the native array.
+ kCopy // The NativeArray makes a copy of the native array and
+ // owns the copy.
+};
+
+// Adapts a native array to a read-only STL-style container. Instead
+// of the complete STL container concept, this adaptor only implements
+// members useful for Google Mock's container matchers. New members
+// should be added as needed. To simplify the implementation, we only
+// support Element being a raw type (i.e. having no top-level const or
+// reference modifier). It's the client's responsibility to satisfy
+// this requirement. Element can be an array type itself (hence
+// multi-dimensional arrays are supported).
+template <typename Element>
+class NativeArray {
+ public:
+ // STL-style container typedefs.
+ typedef Element value_type;
+ typedef Element* iterator;
+ typedef const Element* const_iterator;
+
+ // Constructs from a native array.
+ NativeArray(const Element* array, size_t count, RelationToSource relation) {
+ Init(array, count, relation);
+ }
+
+ // Copy constructor.
+ NativeArray(const NativeArray& rhs) {
+ Init(rhs.array_, rhs.size_, rhs.relation_to_source_);
+ }
+
+ ~NativeArray() {
+ // Ensures that the user doesn't instantiate NativeArray with a
+ // const or reference type.
+ static_cast<void>(StaticAssertTypeEqHelper<Element,
+ GTEST_REMOVE_REFERENCE_AND_CONST_(Element)>());
+ if (relation_to_source_ == kCopy)
+ delete[] array_;
+ }
+
+ // STL-style container methods.
+ size_t size() const { return size_; }
+ const_iterator begin() const { return array_; }
+ const_iterator end() const { return array_ + size_; }
+ bool operator==(const NativeArray& rhs) const {
+ return size() == rhs.size() &&
+ ArrayEq(begin(), size(), rhs.begin());
+ }
+
+ private:
+ // Initializes this object; makes a copy of the input array if
+ // 'relation' is kCopy.
+ void Init(const Element* array, size_t a_size, RelationToSource relation) {
+ if (relation == kReference) {
+ array_ = array;
+ } else {
+ Element* const copy = new Element[a_size];
+ CopyArray(array, a_size, copy);
+ array_ = copy;
+ }
+ size_ = a_size;
+ relation_to_source_ = relation;
+ }
+
+ const Element* array_;
+ size_t size_;
+ RelationToSource relation_to_source_;
+
+ GTEST_DISALLOW_ASSIGN_(NativeArray);
+};
+
+} // namespace internal
+} // namespace testing
+
+#define GTEST_MESSAGE_AT_(file, line, message, result_type) \
+ ::testing::internal::AssertHelper(result_type, file, line, message) \
+ = ::testing::Message()
+
+#define GTEST_MESSAGE_(message, result_type) \
+ GTEST_MESSAGE_AT_(__FILE__, __LINE__, message, result_type)
+
+#define GTEST_FATAL_FAILURE_(message) \
+ return GTEST_MESSAGE_(message, ::testing::TestPartResult::kFatalFailure)
+
+#define GTEST_NONFATAL_FAILURE_(message) \
+ GTEST_MESSAGE_(message, ::testing::TestPartResult::kNonFatalFailure)
+
+#define GTEST_SUCCESS_(message) \
+ GTEST_MESSAGE_(message, ::testing::TestPartResult::kSuccess)
+
+// Suppresses MSVC warnings 4072 (unreachable code) for the code following
+// statement if it returns or throws (or doesn't return or throw in some
+// situations).
+#define GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement) \
+ if (::testing::internal::AlwaysTrue()) { statement; }
+
+#define GTEST_TEST_THROW_(statement, expected_exception, fail) \
+ GTEST_AMBIGUOUS_ELSE_BLOCKER_ \
+ if (::testing::internal::ConstCharPtr gtest_msg = "") { \
+ bool gtest_caught_expected = false; \
+ try { \
+ GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement); \
+ } \
+ catch (expected_exception const&) { \
+ gtest_caught_expected = true; \
+ } \
+ catch (...) { \
+ gtest_msg.value = \
+ "Expected: " #statement " throws an exception of type " \
+ #expected_exception ".\n Actual: it throws a different type."; \
+ goto GTEST_CONCAT_TOKEN_(gtest_label_testthrow_, __LINE__); \
+ } \
+ if (!gtest_caught_expected) { \
+ gtest_msg.value = \
+ "Expected: " #statement " throws an exception of type " \
+ #expected_exception ".\n Actual: it throws nothing."; \
+ goto GTEST_CONCAT_TOKEN_(gtest_label_testthrow_, __LINE__); \
+ } \
+ } else \
+ GTEST_CONCAT_TOKEN_(gtest_label_testthrow_, __LINE__): \
+ fail(gtest_msg.value)
+
+#define GTEST_TEST_NO_THROW_(statement, fail) \
+ GTEST_AMBIGUOUS_ELSE_BLOCKER_ \
+ if (::testing::internal::AlwaysTrue()) { \
+ try { \
+ GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement); \
+ } \
+ catch (...) { \
+ goto GTEST_CONCAT_TOKEN_(gtest_label_testnothrow_, __LINE__); \
+ } \
+ } else \
+ GTEST_CONCAT_TOKEN_(gtest_label_testnothrow_, __LINE__): \
+ fail("Expected: " #statement " doesn't throw an exception.\n" \
+ " Actual: it throws.")
+
+#define GTEST_TEST_ANY_THROW_(statement, fail) \
+ GTEST_AMBIGUOUS_ELSE_BLOCKER_ \
+ if (::testing::internal::AlwaysTrue()) { \
+ bool gtest_caught_any = false; \
+ try { \
+ GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement); \
+ } \
+ catch (...) { \
+ gtest_caught_any = true; \
+ } \
+ if (!gtest_caught_any) { \
+ goto GTEST_CONCAT_TOKEN_(gtest_label_testanythrow_, __LINE__); \
+ } \
+ } else \
+ GTEST_CONCAT_TOKEN_(gtest_label_testanythrow_, __LINE__): \
+ fail("Expected: " #statement " throws an exception.\n" \
+ " Actual: it doesn't.")
+
+
+// Implements Boolean test assertions such as EXPECT_TRUE. expression can be
+// either a boolean expression or an AssertionResult. text is a textual
+// represenation of expression as it was passed into the EXPECT_TRUE.
+#define GTEST_TEST_BOOLEAN_(expression, text, actual, expected, fail) \
+ GTEST_AMBIGUOUS_ELSE_BLOCKER_ \
+ if (const ::testing::AssertionResult gtest_ar_ = \
+ ::testing::AssertionResult(expression)) \
+ ; \
+ else \
+ fail(::testing::internal::GetBoolAssertionFailureMessage(\
+ gtest_ar_, text, #actual, #expected).c_str())
+
+#define GTEST_TEST_NO_FATAL_FAILURE_(statement, fail) \
+ GTEST_AMBIGUOUS_ELSE_BLOCKER_ \
+ if (::testing::internal::AlwaysTrue()) { \
+ ::testing::internal::HasNewFatalFailureHelper gtest_fatal_failure_checker; \
+ GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_(statement); \
+ if (gtest_fatal_failure_checker.has_new_fatal_failure()) { \
+ goto GTEST_CONCAT_TOKEN_(gtest_label_testnofatal_, __LINE__); \
+ } \
+ } else \
+ GTEST_CONCAT_TOKEN_(gtest_label_testnofatal_, __LINE__): \
+ fail("Expected: " #statement " doesn't generate new fatal " \
+ "failures in the current thread.\n" \
+ " Actual: it does.")
+
+// Expands to the name of the class that implements the given test.
+#define GTEST_TEST_CLASS_NAME_(test_case_name, test_name) \
+ test_case_name##_##test_name##_Test
+
+// Helper macro for defining tests.
+#define GTEST_TEST_(test_case_name, test_name, parent_class, parent_id)\
+class GTEST_TEST_CLASS_NAME_(test_case_name, test_name) : public parent_class {\
+ public:\
+ GTEST_TEST_CLASS_NAME_(test_case_name, test_name)() {}\
+ private:\
+ virtual void TestBody();\
+ static ::testing::TestInfo* const test_info_ GTEST_ATTRIBUTE_UNUSED_;\
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(\
+ GTEST_TEST_CLASS_NAME_(test_case_name, test_name));\
+};\
+\
+::testing::TestInfo* const GTEST_TEST_CLASS_NAME_(test_case_name, test_name)\
+ ::test_info_ =\
+ ::testing::internal::MakeAndRegisterTestInfo(\
+ #test_case_name, #test_name, NULL, NULL, \
+ (parent_id), \
+ parent_class::SetUpTestCase, \
+ parent_class::TearDownTestCase, \
+ new ::testing::internal::TestFactoryImpl<\
+ GTEST_TEST_CLASS_NAME_(test_case_name, test_name)>);\
+void GTEST_TEST_CLASS_NAME_(test_case_name, test_name)::TestBody()
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_INTERNAL_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-linked_ptr.h b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-linked_ptr.h
new file mode 100644
index 0000000..b1362cd
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-linked_ptr.h
@@ -0,0 +1,233 @@
+// Copyright 2003 Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Authors: Dan Egnor (egnor at google.com)
+//
+// A "smart" pointer type with reference tracking. Every pointer to a
+// particular object is kept on a circular linked list. When the last pointer
+// to an object is destroyed or reassigned, the object is deleted.
+//
+// Used properly, this deletes the object when the last reference goes away.
+// There are several caveats:
+// - Like all reference counting schemes, cycles lead to leaks.
+// - Each smart pointer is actually two pointers (8 bytes instead of 4).
+// - Every time a pointer is assigned, the entire list of pointers to that
+// object is traversed. This class is therefore NOT SUITABLE when there
+// will often be more than two or three pointers to a particular object.
+// - References are only tracked as long as linked_ptr<> objects are copied.
+// If a linked_ptr<> is converted to a raw pointer and back, BAD THINGS
+// will happen (double deletion).
+//
+// A good use of this class is storing object references in STL containers.
+// You can safely put linked_ptr<> in a vector<>.
+// Other uses may not be as good.
+//
+// Note: If you use an incomplete type with linked_ptr<>, the class
+// *containing* linked_ptr<> must have a constructor and destructor (even
+// if they do nothing!).
+//
+// Bill Gibbons suggested we use something like this.
+//
+// Thread Safety:
+// Unlike other linked_ptr implementations, in this implementation
+// a linked_ptr object is thread-safe in the sense that:
+// - it's safe to copy linked_ptr objects concurrently,
+// - it's safe to copy *from* a linked_ptr and read its underlying
+// raw pointer (e.g. via get()) concurrently, and
+// - it's safe to write to two linked_ptrs that point to the same
+// shared object concurrently.
+// TODO(wan at google.com): rename this to safe_linked_ptr to avoid
+// confusion with normal linked_ptr.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_LINKED_PTR_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_LINKED_PTR_H_
+
+#include <stdlib.h>
+#include <assert.h>
+
+#include "gtest/internal/gtest-port.h"
+
+namespace testing {
+namespace internal {
+
+// Protects copying of all linked_ptr objects.
+GTEST_API_ GTEST_DECLARE_STATIC_MUTEX_(g_linked_ptr_mutex);
+
+// This is used internally by all instances of linked_ptr<>. It needs to be
+// a non-template class because different types of linked_ptr<> can refer to
+// the same object (linked_ptr<Superclass>(obj) vs linked_ptr<Subclass>(obj)).
+// So, it needs to be possible for different types of linked_ptr to participate
+// in the same circular linked list, so we need a single class type here.
+//
+// DO NOT USE THIS CLASS DIRECTLY YOURSELF. Use linked_ptr<T>.
+class linked_ptr_internal {
+ public:
+ // Create a new circle that includes only this instance.
+ void join_new() {
+ next_ = this;
+ }
+
+ // Many linked_ptr operations may change p.link_ for some linked_ptr
+ // variable p in the same circle as this object. Therefore we need
+ // to prevent two such operations from occurring concurrently.
+ //
+ // Note that different types of linked_ptr objects can coexist in a
+ // circle (e.g. linked_ptr<Base>, linked_ptr<Derived1>, and
+ // linked_ptr<Derived2>). Therefore we must use a single mutex to
+ // protect all linked_ptr objects. This can create serious
+ // contention in production code, but is acceptable in a testing
+ // framework.
+
+ // Join an existing circle.
+ void join(linked_ptr_internal const* ptr)
+ GTEST_LOCK_EXCLUDED_(g_linked_ptr_mutex) {
+ MutexLock lock(&g_linked_ptr_mutex);
+
+ linked_ptr_internal const* p = ptr;
+ while (p->next_ != ptr) p = p->next_;
+ p->next_ = this;
+ next_ = ptr;
+ }
+
+ // Leave whatever circle we're part of. Returns true if we were the
+ // last member of the circle. Once this is done, you can join() another.
+ bool depart()
+ GTEST_LOCK_EXCLUDED_(g_linked_ptr_mutex) {
+ MutexLock lock(&g_linked_ptr_mutex);
+
+ if (next_ == this) return true;
+ linked_ptr_internal const* p = next_;
+ while (p->next_ != this) p = p->next_;
+ p->next_ = next_;
+ return false;
+ }
+
+ private:
+ mutable linked_ptr_internal const* next_;
+};
+
+template <typename T>
+class linked_ptr {
+ public:
+ typedef T element_type;
+
+ // Take over ownership of a raw pointer. This should happen as soon as
+ // possible after the object is created.
+ explicit linked_ptr(T* ptr = NULL) { capture(ptr); }
+ ~linked_ptr() { depart(); }
+
+ // Copy an existing linked_ptr<>, adding ourselves to the list of references.
+ template <typename U> linked_ptr(linked_ptr<U> const& ptr) { copy(&ptr); }
+ linked_ptr(linked_ptr const& ptr) { // NOLINT
+ assert(&ptr != this);
+ copy(&ptr);
+ }
+
+ // Assignment releases the old value and acquires the new.
+ template <typename U> linked_ptr& operator=(linked_ptr<U> const& ptr) {
+ depart();
+ copy(&ptr);
+ return *this;
+ }
+
+ linked_ptr& operator=(linked_ptr const& ptr) {
+ if (&ptr != this) {
+ depart();
+ copy(&ptr);
+ }
+ return *this;
+ }
+
+ // Smart pointer members.
+ void reset(T* ptr = NULL) {
+ depart();
+ capture(ptr);
+ }
+ T* get() const { return value_; }
+ T* operator->() const { return value_; }
+ T& operator*() const { return *value_; }
+
+ bool operator==(T* p) const { return value_ == p; }
+ bool operator!=(T* p) const { return value_ != p; }
+ template <typename U>
+ bool operator==(linked_ptr<U> const& ptr) const {
+ return value_ == ptr.get();
+ }
+ template <typename U>
+ bool operator!=(linked_ptr<U> const& ptr) const {
+ return value_ != ptr.get();
+ }
+
+ private:
+ template <typename U>
+ friend class linked_ptr;
+
+ T* value_;
+ linked_ptr_internal link_;
+
+ void depart() {
+ if (link_.depart()) delete value_;
+ }
+
+ void capture(T* ptr) {
+ value_ = ptr;
+ link_.join_new();
+ }
+
+ template <typename U> void copy(linked_ptr<U> const* ptr) {
+ value_ = ptr->get();
+ if (value_)
+ link_.join(&ptr->link_);
+ else
+ link_.join_new();
+ }
+};
+
+template<typename T> inline
+bool operator==(T* ptr, const linked_ptr<T>& x) {
+ return ptr == x.get();
+}
+
+template<typename T> inline
+bool operator!=(T* ptr, const linked_ptr<T>& x) {
+ return ptr != x.get();
+}
+
+// A function to convert T* into linked_ptr<T>
+// Doing e.g. make_linked_ptr(new FooBarBaz<type>(arg)) is a shorter notation
+// for linked_ptr<FooBarBaz<type> >(new FooBarBaz<type>(arg))
+template <typename T>
+linked_ptr<T> make_linked_ptr(T* ptr) {
+ return linked_ptr<T>(ptr);
+}
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_LINKED_PTR_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-param-util-generated.h b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-param-util-generated.h
new file mode 100644
index 0000000..e805485
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-param-util-generated.h
@@ -0,0 +1,5143 @@
+// This file was GENERATED by command:
+// pump.py gtest-param-util-generated.h.pump
+// DO NOT EDIT BY HAND!!!
+
+// Copyright 2008 Google Inc.
+// All Rights Reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vladl at google.com (Vlad Losev)
+
+// Type and function utilities for implementing parameterized tests.
+// This file is generated by a SCRIPT. DO NOT EDIT BY HAND!
+//
+// Currently Google Test supports at most 50 arguments in Values,
+// and at most 10 arguments in Combine. Please contact
+// googletestframework at googlegroups.com if you need more.
+// Please note that the number of arguments to Combine is limited
+// by the maximum arity of the implementation of tr1::tuple which is
+// currently set at 10.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PARAM_UTIL_GENERATED_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PARAM_UTIL_GENERATED_H_
+
+// scripts/fuse_gtest.py depends on gtest's own header being #included
+// *unconditionally*. Therefore these #includes cannot be moved
+// inside #if GTEST_HAS_PARAM_TEST.
+#include "gtest/internal/gtest-param-util.h"
+#include "gtest/internal/gtest-port.h"
+
+#if GTEST_HAS_PARAM_TEST
+
+namespace testing {
+
+// Forward declarations of ValuesIn(), which is implemented in
+// include/gtest/gtest-param-test.h.
+template <typename ForwardIterator>
+internal::ParamGenerator<
+ typename ::testing::internal::IteratorTraits<ForwardIterator>::value_type>
+ValuesIn(ForwardIterator begin, ForwardIterator end);
+
+template <typename T, size_t N>
+internal::ParamGenerator<T> ValuesIn(const T (&array)[N]);
+
+template <class Container>
+internal::ParamGenerator<typename Container::value_type> ValuesIn(
+ const Container& container);
+
+namespace internal {
+
+// Used in the Values() function to provide polymorphic capabilities.
+template <typename T1>
+class ValueArray1 {
+ public:
+ explicit ValueArray1(T1 v1) : v1_(v1) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const { return ValuesIn(&v1_, &v1_ + 1); }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray1& other);
+
+ const T1 v1_;
+};
+
+template <typename T1, typename T2>
+class ValueArray2 {
+ public:
+ ValueArray2(T1 v1, T2 v2) : v1_(v1), v2_(v2) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray2& other);
+
+ const T1 v1_;
+ const T2 v2_;
+};
+
+template <typename T1, typename T2, typename T3>
+class ValueArray3 {
+ public:
+ ValueArray3(T1 v1, T2 v2, T3 v3) : v1_(v1), v2_(v2), v3_(v3) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray3& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4>
+class ValueArray4 {
+ public:
+ ValueArray4(T1 v1, T2 v2, T3 v3, T4 v4) : v1_(v1), v2_(v2), v3_(v3),
+ v4_(v4) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray4& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5>
+class ValueArray5 {
+ public:
+ ValueArray5(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5) : v1_(v1), v2_(v2), v3_(v3),
+ v4_(v4), v5_(v5) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray5& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6>
+class ValueArray6 {
+ public:
+ ValueArray6(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6) : v1_(v1), v2_(v2),
+ v3_(v3), v4_(v4), v5_(v5), v6_(v6) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray6& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7>
+class ValueArray7 {
+ public:
+ ValueArray7(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7) : v1_(v1),
+ v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray7& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8>
+class ValueArray8 {
+ public:
+ ValueArray8(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7,
+ T8 v8) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray8& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9>
+class ValueArray9 {
+ public:
+ ValueArray9(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8,
+ T9 v9) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray9& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10>
+class ValueArray10 {
+ public:
+ ValueArray10(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9), v10_(v10) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray10& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11>
+class ValueArray11 {
+ public:
+ ValueArray11(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6),
+ v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray11& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12>
+class ValueArray12 {
+ public:
+ ValueArray12(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5),
+ v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray12& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13>
+class ValueArray13 {
+ public:
+ ValueArray13(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13) : v1_(v1), v2_(v2), v3_(v3), v4_(v4),
+ v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11),
+ v12_(v12), v13_(v13) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray13& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14>
+class ValueArray14 {
+ public:
+ ValueArray14(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14) : v1_(v1), v2_(v2), v3_(v3),
+ v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10),
+ v11_(v11), v12_(v12), v13_(v13), v14_(v14) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray14& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15>
+class ValueArray15 {
+ public:
+ ValueArray15(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15) : v1_(v1), v2_(v2),
+ v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10),
+ v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray15& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16>
+class ValueArray16 {
+ public:
+ ValueArray16(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16) : v1_(v1),
+ v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9),
+ v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15),
+ v16_(v16) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray16& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17>
+class ValueArray17 {
+ public:
+ ValueArray17(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16,
+ T17 v17) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14),
+ v15_(v15), v16_(v16), v17_(v17) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray17& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18>
+class ValueArray18 {
+ public:
+ ValueArray18(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14),
+ v15_(v15), v16_(v16), v17_(v17), v18_(v18) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray18& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19>
+class ValueArray19 {
+ public:
+ ValueArray19(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6),
+ v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13),
+ v14_(v14), v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray19& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20>
+class ValueArray20 {
+ public:
+ ValueArray20(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5),
+ v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12),
+ v13_(v13), v14_(v14), v15_(v15), v16_(v16), v17_(v17), v18_(v18),
+ v19_(v19), v20_(v20) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray20& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21>
+class ValueArray21 {
+ public:
+ ValueArray21(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21) : v1_(v1), v2_(v2), v3_(v3), v4_(v4),
+ v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11),
+ v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16), v17_(v17),
+ v18_(v18), v19_(v19), v20_(v20), v21_(v21) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray21& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22>
+class ValueArray22 {
+ public:
+ ValueArray22(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22) : v1_(v1), v2_(v2), v3_(v3),
+ v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10),
+ v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16),
+ v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21), v22_(v22) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray22& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23>
+class ValueArray23 {
+ public:
+ ValueArray23(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23) : v1_(v1), v2_(v2),
+ v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10),
+ v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16),
+ v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21), v22_(v22),
+ v23_(v23) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray23& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24>
+class ValueArray24 {
+ public:
+ ValueArray24(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24) : v1_(v1),
+ v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9),
+ v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15),
+ v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21),
+ v22_(v22), v23_(v23), v24_(v24) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray24& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25>
+class ValueArray25 {
+ public:
+ ValueArray25(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24,
+ T25 v25) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14),
+ v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20),
+ v21_(v21), v22_(v22), v23_(v23), v24_(v24), v25_(v25) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray25& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26>
+class ValueArray26 {
+ public:
+ ValueArray26(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14),
+ v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20),
+ v21_(v21), v22_(v22), v23_(v23), v24_(v24), v25_(v25), v26_(v26) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray26& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27>
+class ValueArray27 {
+ public:
+ ValueArray27(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6),
+ v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13),
+ v14_(v14), v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19),
+ v20_(v20), v21_(v21), v22_(v22), v23_(v23), v24_(v24), v25_(v25),
+ v26_(v26), v27_(v27) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray27& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28>
+class ValueArray28 {
+ public:
+ ValueArray28(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5),
+ v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12),
+ v13_(v13), v14_(v14), v15_(v15), v16_(v16), v17_(v17), v18_(v18),
+ v19_(v19), v20_(v20), v21_(v21), v22_(v22), v23_(v23), v24_(v24),
+ v25_(v25), v26_(v26), v27_(v27), v28_(v28) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray28& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29>
+class ValueArray29 {
+ public:
+ ValueArray29(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29) : v1_(v1), v2_(v2), v3_(v3), v4_(v4),
+ v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11),
+ v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16), v17_(v17),
+ v18_(v18), v19_(v19), v20_(v20), v21_(v21), v22_(v22), v23_(v23),
+ v24_(v24), v25_(v25), v26_(v26), v27_(v27), v28_(v28), v29_(v29) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray29& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30>
+class ValueArray30 {
+ public:
+ ValueArray30(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30) : v1_(v1), v2_(v2), v3_(v3),
+ v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10),
+ v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16),
+ v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21), v22_(v22),
+ v23_(v23), v24_(v24), v25_(v25), v26_(v26), v27_(v27), v28_(v28),
+ v29_(v29), v30_(v30) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray30& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31>
+class ValueArray31 {
+ public:
+ ValueArray31(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31) : v1_(v1), v2_(v2),
+ v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10),
+ v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16),
+ v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21), v22_(v22),
+ v23_(v23), v24_(v24), v25_(v25), v26_(v26), v27_(v27), v28_(v28),
+ v29_(v29), v30_(v30), v31_(v31) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray31& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32>
+class ValueArray32 {
+ public:
+ ValueArray32(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32) : v1_(v1),
+ v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9),
+ v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15),
+ v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21),
+ v22_(v22), v23_(v23), v24_(v24), v25_(v25), v26_(v26), v27_(v27),
+ v28_(v28), v29_(v29), v30_(v30), v31_(v31), v32_(v32) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray32& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33>
+class ValueArray33 {
+ public:
+ ValueArray33(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32,
+ T33 v33) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14),
+ v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20),
+ v21_(v21), v22_(v22), v23_(v23), v24_(v24), v25_(v25), v26_(v26),
+ v27_(v27), v28_(v28), v29_(v29), v30_(v30), v31_(v31), v32_(v32),
+ v33_(v33) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray33& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34>
+class ValueArray34 {
+ public:
+ ValueArray34(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14),
+ v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20),
+ v21_(v21), v22_(v22), v23_(v23), v24_(v24), v25_(v25), v26_(v26),
+ v27_(v27), v28_(v28), v29_(v29), v30_(v30), v31_(v31), v32_(v32),
+ v33_(v33), v34_(v34) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray34& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35>
+class ValueArray35 {
+ public:
+ ValueArray35(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6),
+ v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13),
+ v14_(v14), v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19),
+ v20_(v20), v21_(v21), v22_(v22), v23_(v23), v24_(v24), v25_(v25),
+ v26_(v26), v27_(v27), v28_(v28), v29_(v29), v30_(v30), v31_(v31),
+ v32_(v32), v33_(v33), v34_(v34), v35_(v35) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray35& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36>
+class ValueArray36 {
+ public:
+ ValueArray36(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5),
+ v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12),
+ v13_(v13), v14_(v14), v15_(v15), v16_(v16), v17_(v17), v18_(v18),
+ v19_(v19), v20_(v20), v21_(v21), v22_(v22), v23_(v23), v24_(v24),
+ v25_(v25), v26_(v26), v27_(v27), v28_(v28), v29_(v29), v30_(v30),
+ v31_(v31), v32_(v32), v33_(v33), v34_(v34), v35_(v35), v36_(v36) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray36& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37>
+class ValueArray37 {
+ public:
+ ValueArray37(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37) : v1_(v1), v2_(v2), v3_(v3), v4_(v4),
+ v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11),
+ v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16), v17_(v17),
+ v18_(v18), v19_(v19), v20_(v20), v21_(v21), v22_(v22), v23_(v23),
+ v24_(v24), v25_(v25), v26_(v26), v27_(v27), v28_(v28), v29_(v29),
+ v30_(v30), v31_(v31), v32_(v32), v33_(v33), v34_(v34), v35_(v35),
+ v36_(v36), v37_(v37) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray37& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38>
+class ValueArray38 {
+ public:
+ ValueArray38(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38) : v1_(v1), v2_(v2), v3_(v3),
+ v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10),
+ v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16),
+ v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21), v22_(v22),
+ v23_(v23), v24_(v24), v25_(v25), v26_(v26), v27_(v27), v28_(v28),
+ v29_(v29), v30_(v30), v31_(v31), v32_(v32), v33_(v33), v34_(v34),
+ v35_(v35), v36_(v36), v37_(v37), v38_(v38) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray38& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39>
+class ValueArray39 {
+ public:
+ ValueArray39(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39) : v1_(v1), v2_(v2),
+ v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10),
+ v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16),
+ v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21), v22_(v22),
+ v23_(v23), v24_(v24), v25_(v25), v26_(v26), v27_(v27), v28_(v28),
+ v29_(v29), v30_(v30), v31_(v31), v32_(v32), v33_(v33), v34_(v34),
+ v35_(v35), v36_(v36), v37_(v37), v38_(v38), v39_(v39) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray39& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40>
+class ValueArray40 {
+ public:
+ ValueArray40(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40) : v1_(v1),
+ v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9),
+ v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15),
+ v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21),
+ v22_(v22), v23_(v23), v24_(v24), v25_(v25), v26_(v26), v27_(v27),
+ v28_(v28), v29_(v29), v30_(v30), v31_(v31), v32_(v32), v33_(v33),
+ v34_(v34), v35_(v35), v36_(v36), v37_(v37), v38_(v38), v39_(v39),
+ v40_(v40) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_), static_cast<T>(v40_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray40& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+ const T40 v40_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41>
+class ValueArray41 {
+ public:
+ ValueArray41(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40,
+ T41 v41) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14),
+ v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20),
+ v21_(v21), v22_(v22), v23_(v23), v24_(v24), v25_(v25), v26_(v26),
+ v27_(v27), v28_(v28), v29_(v29), v30_(v30), v31_(v31), v32_(v32),
+ v33_(v33), v34_(v34), v35_(v35), v36_(v36), v37_(v37), v38_(v38),
+ v39_(v39), v40_(v40), v41_(v41) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_), static_cast<T>(v40_), static_cast<T>(v41_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray41& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+ const T40 v40_;
+ const T41 v41_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42>
+class ValueArray42 {
+ public:
+ ValueArray42(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14),
+ v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20),
+ v21_(v21), v22_(v22), v23_(v23), v24_(v24), v25_(v25), v26_(v26),
+ v27_(v27), v28_(v28), v29_(v29), v30_(v30), v31_(v31), v32_(v32),
+ v33_(v33), v34_(v34), v35_(v35), v36_(v36), v37_(v37), v38_(v38),
+ v39_(v39), v40_(v40), v41_(v41), v42_(v42) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_), static_cast<T>(v40_), static_cast<T>(v41_),
+ static_cast<T>(v42_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray42& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+ const T40 v40_;
+ const T41 v41_;
+ const T42 v42_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43>
+class ValueArray43 {
+ public:
+ ValueArray43(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42, T43 v43) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6),
+ v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13),
+ v14_(v14), v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19),
+ v20_(v20), v21_(v21), v22_(v22), v23_(v23), v24_(v24), v25_(v25),
+ v26_(v26), v27_(v27), v28_(v28), v29_(v29), v30_(v30), v31_(v31),
+ v32_(v32), v33_(v33), v34_(v34), v35_(v35), v36_(v36), v37_(v37),
+ v38_(v38), v39_(v39), v40_(v40), v41_(v41), v42_(v42), v43_(v43) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_), static_cast<T>(v40_), static_cast<T>(v41_),
+ static_cast<T>(v42_), static_cast<T>(v43_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray43& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+ const T40 v40_;
+ const T41 v41_;
+ const T42 v42_;
+ const T43 v43_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44>
+class ValueArray44 {
+ public:
+ ValueArray44(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42, T43 v43, T44 v44) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5),
+ v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12),
+ v13_(v13), v14_(v14), v15_(v15), v16_(v16), v17_(v17), v18_(v18),
+ v19_(v19), v20_(v20), v21_(v21), v22_(v22), v23_(v23), v24_(v24),
+ v25_(v25), v26_(v26), v27_(v27), v28_(v28), v29_(v29), v30_(v30),
+ v31_(v31), v32_(v32), v33_(v33), v34_(v34), v35_(v35), v36_(v36),
+ v37_(v37), v38_(v38), v39_(v39), v40_(v40), v41_(v41), v42_(v42),
+ v43_(v43), v44_(v44) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_), static_cast<T>(v40_), static_cast<T>(v41_),
+ static_cast<T>(v42_), static_cast<T>(v43_), static_cast<T>(v44_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray44& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+ const T40 v40_;
+ const T41 v41_;
+ const T42 v42_;
+ const T43 v43_;
+ const T44 v44_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45>
+class ValueArray45 {
+ public:
+ ValueArray45(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42, T43 v43, T44 v44, T45 v45) : v1_(v1), v2_(v2), v3_(v3), v4_(v4),
+ v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10), v11_(v11),
+ v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16), v17_(v17),
+ v18_(v18), v19_(v19), v20_(v20), v21_(v21), v22_(v22), v23_(v23),
+ v24_(v24), v25_(v25), v26_(v26), v27_(v27), v28_(v28), v29_(v29),
+ v30_(v30), v31_(v31), v32_(v32), v33_(v33), v34_(v34), v35_(v35),
+ v36_(v36), v37_(v37), v38_(v38), v39_(v39), v40_(v40), v41_(v41),
+ v42_(v42), v43_(v43), v44_(v44), v45_(v45) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_), static_cast<T>(v40_), static_cast<T>(v41_),
+ static_cast<T>(v42_), static_cast<T>(v43_), static_cast<T>(v44_),
+ static_cast<T>(v45_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray45& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+ const T40 v40_;
+ const T41 v41_;
+ const T42 v42_;
+ const T43 v43_;
+ const T44 v44_;
+ const T45 v45_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46>
+class ValueArray46 {
+ public:
+ ValueArray46(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42, T43 v43, T44 v44, T45 v45, T46 v46) : v1_(v1), v2_(v2), v3_(v3),
+ v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10),
+ v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16),
+ v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21), v22_(v22),
+ v23_(v23), v24_(v24), v25_(v25), v26_(v26), v27_(v27), v28_(v28),
+ v29_(v29), v30_(v30), v31_(v31), v32_(v32), v33_(v33), v34_(v34),
+ v35_(v35), v36_(v36), v37_(v37), v38_(v38), v39_(v39), v40_(v40),
+ v41_(v41), v42_(v42), v43_(v43), v44_(v44), v45_(v45), v46_(v46) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_), static_cast<T>(v40_), static_cast<T>(v41_),
+ static_cast<T>(v42_), static_cast<T>(v43_), static_cast<T>(v44_),
+ static_cast<T>(v45_), static_cast<T>(v46_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray46& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+ const T40 v40_;
+ const T41 v41_;
+ const T42 v42_;
+ const T43 v43_;
+ const T44 v44_;
+ const T45 v45_;
+ const T46 v46_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47>
+class ValueArray47 {
+ public:
+ ValueArray47(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42, T43 v43, T44 v44, T45 v45, T46 v46, T47 v47) : v1_(v1), v2_(v2),
+ v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9), v10_(v10),
+ v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15), v16_(v16),
+ v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21), v22_(v22),
+ v23_(v23), v24_(v24), v25_(v25), v26_(v26), v27_(v27), v28_(v28),
+ v29_(v29), v30_(v30), v31_(v31), v32_(v32), v33_(v33), v34_(v34),
+ v35_(v35), v36_(v36), v37_(v37), v38_(v38), v39_(v39), v40_(v40),
+ v41_(v41), v42_(v42), v43_(v43), v44_(v44), v45_(v45), v46_(v46),
+ v47_(v47) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_), static_cast<T>(v40_), static_cast<T>(v41_),
+ static_cast<T>(v42_), static_cast<T>(v43_), static_cast<T>(v44_),
+ static_cast<T>(v45_), static_cast<T>(v46_), static_cast<T>(v47_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray47& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+ const T40 v40_;
+ const T41 v41_;
+ const T42 v42_;
+ const T43 v43_;
+ const T44 v44_;
+ const T45 v45_;
+ const T46 v46_;
+ const T47 v47_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48>
+class ValueArray48 {
+ public:
+ ValueArray48(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42, T43 v43, T44 v44, T45 v45, T46 v46, T47 v47, T48 v48) : v1_(v1),
+ v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7), v8_(v8), v9_(v9),
+ v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14), v15_(v15),
+ v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20), v21_(v21),
+ v22_(v22), v23_(v23), v24_(v24), v25_(v25), v26_(v26), v27_(v27),
+ v28_(v28), v29_(v29), v30_(v30), v31_(v31), v32_(v32), v33_(v33),
+ v34_(v34), v35_(v35), v36_(v36), v37_(v37), v38_(v38), v39_(v39),
+ v40_(v40), v41_(v41), v42_(v42), v43_(v43), v44_(v44), v45_(v45),
+ v46_(v46), v47_(v47), v48_(v48) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_), static_cast<T>(v40_), static_cast<T>(v41_),
+ static_cast<T>(v42_), static_cast<T>(v43_), static_cast<T>(v44_),
+ static_cast<T>(v45_), static_cast<T>(v46_), static_cast<T>(v47_),
+ static_cast<T>(v48_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray48& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+ const T40 v40_;
+ const T41 v41_;
+ const T42 v42_;
+ const T43 v43_;
+ const T44 v44_;
+ const T45 v45_;
+ const T46 v46_;
+ const T47 v47_;
+ const T48 v48_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48, typename T49>
+class ValueArray49 {
+ public:
+ ValueArray49(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42, T43 v43, T44 v44, T45 v45, T46 v46, T47 v47, T48 v48,
+ T49 v49) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14),
+ v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20),
+ v21_(v21), v22_(v22), v23_(v23), v24_(v24), v25_(v25), v26_(v26),
+ v27_(v27), v28_(v28), v29_(v29), v30_(v30), v31_(v31), v32_(v32),
+ v33_(v33), v34_(v34), v35_(v35), v36_(v36), v37_(v37), v38_(v38),
+ v39_(v39), v40_(v40), v41_(v41), v42_(v42), v43_(v43), v44_(v44),
+ v45_(v45), v46_(v46), v47_(v47), v48_(v48), v49_(v49) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_), static_cast<T>(v40_), static_cast<T>(v41_),
+ static_cast<T>(v42_), static_cast<T>(v43_), static_cast<T>(v44_),
+ static_cast<T>(v45_), static_cast<T>(v46_), static_cast<T>(v47_),
+ static_cast<T>(v48_), static_cast<T>(v49_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray49& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+ const T40 v40_;
+ const T41 v41_;
+ const T42 v42_;
+ const T43 v43_;
+ const T44 v44_;
+ const T45 v45_;
+ const T46 v46_;
+ const T47 v47_;
+ const T48 v48_;
+ const T49 v49_;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48, typename T49, typename T50>
+class ValueArray50 {
+ public:
+ ValueArray50(T1 v1, T2 v2, T3 v3, T4 v4, T5 v5, T6 v6, T7 v7, T8 v8, T9 v9,
+ T10 v10, T11 v11, T12 v12, T13 v13, T14 v14, T15 v15, T16 v16, T17 v17,
+ T18 v18, T19 v19, T20 v20, T21 v21, T22 v22, T23 v23, T24 v24, T25 v25,
+ T26 v26, T27 v27, T28 v28, T29 v29, T30 v30, T31 v31, T32 v32, T33 v33,
+ T34 v34, T35 v35, T36 v36, T37 v37, T38 v38, T39 v39, T40 v40, T41 v41,
+ T42 v42, T43 v43, T44 v44, T45 v45, T46 v46, T47 v47, T48 v48, T49 v49,
+ T50 v50) : v1_(v1), v2_(v2), v3_(v3), v4_(v4), v5_(v5), v6_(v6), v7_(v7),
+ v8_(v8), v9_(v9), v10_(v10), v11_(v11), v12_(v12), v13_(v13), v14_(v14),
+ v15_(v15), v16_(v16), v17_(v17), v18_(v18), v19_(v19), v20_(v20),
+ v21_(v21), v22_(v22), v23_(v23), v24_(v24), v25_(v25), v26_(v26),
+ v27_(v27), v28_(v28), v29_(v29), v30_(v30), v31_(v31), v32_(v32),
+ v33_(v33), v34_(v34), v35_(v35), v36_(v36), v37_(v37), v38_(v38),
+ v39_(v39), v40_(v40), v41_(v41), v42_(v42), v43_(v43), v44_(v44),
+ v45_(v45), v46_(v46), v47_(v47), v48_(v48), v49_(v49), v50_(v50) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {static_cast<T>(v1_), static_cast<T>(v2_),
+ static_cast<T>(v3_), static_cast<T>(v4_), static_cast<T>(v5_),
+ static_cast<T>(v6_), static_cast<T>(v7_), static_cast<T>(v8_),
+ static_cast<T>(v9_), static_cast<T>(v10_), static_cast<T>(v11_),
+ static_cast<T>(v12_), static_cast<T>(v13_), static_cast<T>(v14_),
+ static_cast<T>(v15_), static_cast<T>(v16_), static_cast<T>(v17_),
+ static_cast<T>(v18_), static_cast<T>(v19_), static_cast<T>(v20_),
+ static_cast<T>(v21_), static_cast<T>(v22_), static_cast<T>(v23_),
+ static_cast<T>(v24_), static_cast<T>(v25_), static_cast<T>(v26_),
+ static_cast<T>(v27_), static_cast<T>(v28_), static_cast<T>(v29_),
+ static_cast<T>(v30_), static_cast<T>(v31_), static_cast<T>(v32_),
+ static_cast<T>(v33_), static_cast<T>(v34_), static_cast<T>(v35_),
+ static_cast<T>(v36_), static_cast<T>(v37_), static_cast<T>(v38_),
+ static_cast<T>(v39_), static_cast<T>(v40_), static_cast<T>(v41_),
+ static_cast<T>(v42_), static_cast<T>(v43_), static_cast<T>(v44_),
+ static_cast<T>(v45_), static_cast<T>(v46_), static_cast<T>(v47_),
+ static_cast<T>(v48_), static_cast<T>(v49_), static_cast<T>(v50_)};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray50& other);
+
+ const T1 v1_;
+ const T2 v2_;
+ const T3 v3_;
+ const T4 v4_;
+ const T5 v5_;
+ const T6 v6_;
+ const T7 v7_;
+ const T8 v8_;
+ const T9 v9_;
+ const T10 v10_;
+ const T11 v11_;
+ const T12 v12_;
+ const T13 v13_;
+ const T14 v14_;
+ const T15 v15_;
+ const T16 v16_;
+ const T17 v17_;
+ const T18 v18_;
+ const T19 v19_;
+ const T20 v20_;
+ const T21 v21_;
+ const T22 v22_;
+ const T23 v23_;
+ const T24 v24_;
+ const T25 v25_;
+ const T26 v26_;
+ const T27 v27_;
+ const T28 v28_;
+ const T29 v29_;
+ const T30 v30_;
+ const T31 v31_;
+ const T32 v32_;
+ const T33 v33_;
+ const T34 v34_;
+ const T35 v35_;
+ const T36 v36_;
+ const T37 v37_;
+ const T38 v38_;
+ const T39 v39_;
+ const T40 v40_;
+ const T41 v41_;
+ const T42 v42_;
+ const T43 v43_;
+ const T44 v44_;
+ const T45 v45_;
+ const T46 v46_;
+ const T47 v47_;
+ const T48 v48_;
+ const T49 v49_;
+ const T50 v50_;
+};
+
+# if GTEST_HAS_COMBINE
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// Generates values from the Cartesian product of values produced
+// by the argument generators.
+//
+template <typename T1, typename T2>
+class CartesianProductGenerator2
+ : public ParamGeneratorInterface< ::std::tr1::tuple<T1, T2> > {
+ public:
+ typedef ::std::tr1::tuple<T1, T2> ParamType;
+
+ CartesianProductGenerator2(const ParamGenerator<T1>& g1,
+ const ParamGenerator<T2>& g2)
+ : g1_(g1), g2_(g2) {}
+ virtual ~CartesianProductGenerator2() {}
+
+ virtual ParamIteratorInterface<ParamType>* Begin() const {
+ return new Iterator(this, g1_, g1_.begin(), g2_, g2_.begin());
+ }
+ virtual ParamIteratorInterface<ParamType>* End() const {
+ return new Iterator(this, g1_, g1_.end(), g2_, g2_.end());
+ }
+
+ private:
+ class Iterator : public ParamIteratorInterface<ParamType> {
+ public:
+ Iterator(const ParamGeneratorInterface<ParamType>* base,
+ const ParamGenerator<T1>& g1,
+ const typename ParamGenerator<T1>::iterator& current1,
+ const ParamGenerator<T2>& g2,
+ const typename ParamGenerator<T2>::iterator& current2)
+ : base_(base),
+ begin1_(g1.begin()), end1_(g1.end()), current1_(current1),
+ begin2_(g2.begin()), end2_(g2.end()), current2_(current2) {
+ ComputeCurrentValue();
+ }
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<ParamType>* BaseGenerator() const {
+ return base_;
+ }
+ // Advance should not be called on beyond-of-range iterators
+ // so no component iterators must be beyond end of range, either.
+ virtual void Advance() {
+ assert(!AtEnd());
+ ++current2_;
+ if (current2_ == end2_) {
+ current2_ = begin2_;
+ ++current1_;
+ }
+ ComputeCurrentValue();
+ }
+ virtual ParamIteratorInterface<ParamType>* Clone() const {
+ return new Iterator(*this);
+ }
+ virtual const ParamType* Current() const { return ¤t_value_; }
+ virtual bool Equals(const ParamIteratorInterface<ParamType>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ const Iterator* typed_other =
+ CheckedDowncastToActualType<const Iterator>(&other);
+ // We must report iterators equal if they both point beyond their
+ // respective ranges. That can happen in a variety of fashions,
+ // so we have to consult AtEnd().
+ return (AtEnd() && typed_other->AtEnd()) ||
+ (
+ current1_ == typed_other->current1_ &&
+ current2_ == typed_other->current2_);
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ : base_(other.base_),
+ begin1_(other.begin1_),
+ end1_(other.end1_),
+ current1_(other.current1_),
+ begin2_(other.begin2_),
+ end2_(other.end2_),
+ current2_(other.current2_) {
+ ComputeCurrentValue();
+ }
+
+ void ComputeCurrentValue() {
+ if (!AtEnd())
+ current_value_ = ParamType(*current1_, *current2_);
+ }
+ bool AtEnd() const {
+ // We must report iterator past the end of the range when either of the
+ // component iterators has reached the end of its range.
+ return
+ current1_ == end1_ ||
+ current2_ == end2_;
+ }
+
+ // No implementation - assignment is unsupported.
+ void operator=(const Iterator& other);
+
+ const ParamGeneratorInterface<ParamType>* const base_;
+ // begin[i]_ and end[i]_ define the i-th range that Iterator traverses.
+ // current[i]_ is the actual traversing iterator.
+ const typename ParamGenerator<T1>::iterator begin1_;
+ const typename ParamGenerator<T1>::iterator end1_;
+ typename ParamGenerator<T1>::iterator current1_;
+ const typename ParamGenerator<T2>::iterator begin2_;
+ const typename ParamGenerator<T2>::iterator end2_;
+ typename ParamGenerator<T2>::iterator current2_;
+ ParamType current_value_;
+ }; // class CartesianProductGenerator2::Iterator
+
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductGenerator2& other);
+
+ const ParamGenerator<T1> g1_;
+ const ParamGenerator<T2> g2_;
+}; // class CartesianProductGenerator2
+
+
+template <typename T1, typename T2, typename T3>
+class CartesianProductGenerator3
+ : public ParamGeneratorInterface< ::std::tr1::tuple<T1, T2, T3> > {
+ public:
+ typedef ::std::tr1::tuple<T1, T2, T3> ParamType;
+
+ CartesianProductGenerator3(const ParamGenerator<T1>& g1,
+ const ParamGenerator<T2>& g2, const ParamGenerator<T3>& g3)
+ : g1_(g1), g2_(g2), g3_(g3) {}
+ virtual ~CartesianProductGenerator3() {}
+
+ virtual ParamIteratorInterface<ParamType>* Begin() const {
+ return new Iterator(this, g1_, g1_.begin(), g2_, g2_.begin(), g3_,
+ g3_.begin());
+ }
+ virtual ParamIteratorInterface<ParamType>* End() const {
+ return new Iterator(this, g1_, g1_.end(), g2_, g2_.end(), g3_, g3_.end());
+ }
+
+ private:
+ class Iterator : public ParamIteratorInterface<ParamType> {
+ public:
+ Iterator(const ParamGeneratorInterface<ParamType>* base,
+ const ParamGenerator<T1>& g1,
+ const typename ParamGenerator<T1>::iterator& current1,
+ const ParamGenerator<T2>& g2,
+ const typename ParamGenerator<T2>::iterator& current2,
+ const ParamGenerator<T3>& g3,
+ const typename ParamGenerator<T3>::iterator& current3)
+ : base_(base),
+ begin1_(g1.begin()), end1_(g1.end()), current1_(current1),
+ begin2_(g2.begin()), end2_(g2.end()), current2_(current2),
+ begin3_(g3.begin()), end3_(g3.end()), current3_(current3) {
+ ComputeCurrentValue();
+ }
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<ParamType>* BaseGenerator() const {
+ return base_;
+ }
+ // Advance should not be called on beyond-of-range iterators
+ // so no component iterators must be beyond end of range, either.
+ virtual void Advance() {
+ assert(!AtEnd());
+ ++current3_;
+ if (current3_ == end3_) {
+ current3_ = begin3_;
+ ++current2_;
+ }
+ if (current2_ == end2_) {
+ current2_ = begin2_;
+ ++current1_;
+ }
+ ComputeCurrentValue();
+ }
+ virtual ParamIteratorInterface<ParamType>* Clone() const {
+ return new Iterator(*this);
+ }
+ virtual const ParamType* Current() const { return ¤t_value_; }
+ virtual bool Equals(const ParamIteratorInterface<ParamType>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ const Iterator* typed_other =
+ CheckedDowncastToActualType<const Iterator>(&other);
+ // We must report iterators equal if they both point beyond their
+ // respective ranges. That can happen in a variety of fashions,
+ // so we have to consult AtEnd().
+ return (AtEnd() && typed_other->AtEnd()) ||
+ (
+ current1_ == typed_other->current1_ &&
+ current2_ == typed_other->current2_ &&
+ current3_ == typed_other->current3_);
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ : base_(other.base_),
+ begin1_(other.begin1_),
+ end1_(other.end1_),
+ current1_(other.current1_),
+ begin2_(other.begin2_),
+ end2_(other.end2_),
+ current2_(other.current2_),
+ begin3_(other.begin3_),
+ end3_(other.end3_),
+ current3_(other.current3_) {
+ ComputeCurrentValue();
+ }
+
+ void ComputeCurrentValue() {
+ if (!AtEnd())
+ current_value_ = ParamType(*current1_, *current2_, *current3_);
+ }
+ bool AtEnd() const {
+ // We must report iterator past the end of the range when either of the
+ // component iterators has reached the end of its range.
+ return
+ current1_ == end1_ ||
+ current2_ == end2_ ||
+ current3_ == end3_;
+ }
+
+ // No implementation - assignment is unsupported.
+ void operator=(const Iterator& other);
+
+ const ParamGeneratorInterface<ParamType>* const base_;
+ // begin[i]_ and end[i]_ define the i-th range that Iterator traverses.
+ // current[i]_ is the actual traversing iterator.
+ const typename ParamGenerator<T1>::iterator begin1_;
+ const typename ParamGenerator<T1>::iterator end1_;
+ typename ParamGenerator<T1>::iterator current1_;
+ const typename ParamGenerator<T2>::iterator begin2_;
+ const typename ParamGenerator<T2>::iterator end2_;
+ typename ParamGenerator<T2>::iterator current2_;
+ const typename ParamGenerator<T3>::iterator begin3_;
+ const typename ParamGenerator<T3>::iterator end3_;
+ typename ParamGenerator<T3>::iterator current3_;
+ ParamType current_value_;
+ }; // class CartesianProductGenerator3::Iterator
+
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductGenerator3& other);
+
+ const ParamGenerator<T1> g1_;
+ const ParamGenerator<T2> g2_;
+ const ParamGenerator<T3> g3_;
+}; // class CartesianProductGenerator3
+
+
+template <typename T1, typename T2, typename T3, typename T4>
+class CartesianProductGenerator4
+ : public ParamGeneratorInterface< ::std::tr1::tuple<T1, T2, T3, T4> > {
+ public:
+ typedef ::std::tr1::tuple<T1, T2, T3, T4> ParamType;
+
+ CartesianProductGenerator4(const ParamGenerator<T1>& g1,
+ const ParamGenerator<T2>& g2, const ParamGenerator<T3>& g3,
+ const ParamGenerator<T4>& g4)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4) {}
+ virtual ~CartesianProductGenerator4() {}
+
+ virtual ParamIteratorInterface<ParamType>* Begin() const {
+ return new Iterator(this, g1_, g1_.begin(), g2_, g2_.begin(), g3_,
+ g3_.begin(), g4_, g4_.begin());
+ }
+ virtual ParamIteratorInterface<ParamType>* End() const {
+ return new Iterator(this, g1_, g1_.end(), g2_, g2_.end(), g3_, g3_.end(),
+ g4_, g4_.end());
+ }
+
+ private:
+ class Iterator : public ParamIteratorInterface<ParamType> {
+ public:
+ Iterator(const ParamGeneratorInterface<ParamType>* base,
+ const ParamGenerator<T1>& g1,
+ const typename ParamGenerator<T1>::iterator& current1,
+ const ParamGenerator<T2>& g2,
+ const typename ParamGenerator<T2>::iterator& current2,
+ const ParamGenerator<T3>& g3,
+ const typename ParamGenerator<T3>::iterator& current3,
+ const ParamGenerator<T4>& g4,
+ const typename ParamGenerator<T4>::iterator& current4)
+ : base_(base),
+ begin1_(g1.begin()), end1_(g1.end()), current1_(current1),
+ begin2_(g2.begin()), end2_(g2.end()), current2_(current2),
+ begin3_(g3.begin()), end3_(g3.end()), current3_(current3),
+ begin4_(g4.begin()), end4_(g4.end()), current4_(current4) {
+ ComputeCurrentValue();
+ }
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<ParamType>* BaseGenerator() const {
+ return base_;
+ }
+ // Advance should not be called on beyond-of-range iterators
+ // so no component iterators must be beyond end of range, either.
+ virtual void Advance() {
+ assert(!AtEnd());
+ ++current4_;
+ if (current4_ == end4_) {
+ current4_ = begin4_;
+ ++current3_;
+ }
+ if (current3_ == end3_) {
+ current3_ = begin3_;
+ ++current2_;
+ }
+ if (current2_ == end2_) {
+ current2_ = begin2_;
+ ++current1_;
+ }
+ ComputeCurrentValue();
+ }
+ virtual ParamIteratorInterface<ParamType>* Clone() const {
+ return new Iterator(*this);
+ }
+ virtual const ParamType* Current() const { return ¤t_value_; }
+ virtual bool Equals(const ParamIteratorInterface<ParamType>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ const Iterator* typed_other =
+ CheckedDowncastToActualType<const Iterator>(&other);
+ // We must report iterators equal if they both point beyond their
+ // respective ranges. That can happen in a variety of fashions,
+ // so we have to consult AtEnd().
+ return (AtEnd() && typed_other->AtEnd()) ||
+ (
+ current1_ == typed_other->current1_ &&
+ current2_ == typed_other->current2_ &&
+ current3_ == typed_other->current3_ &&
+ current4_ == typed_other->current4_);
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ : base_(other.base_),
+ begin1_(other.begin1_),
+ end1_(other.end1_),
+ current1_(other.current1_),
+ begin2_(other.begin2_),
+ end2_(other.end2_),
+ current2_(other.current2_),
+ begin3_(other.begin3_),
+ end3_(other.end3_),
+ current3_(other.current3_),
+ begin4_(other.begin4_),
+ end4_(other.end4_),
+ current4_(other.current4_) {
+ ComputeCurrentValue();
+ }
+
+ void ComputeCurrentValue() {
+ if (!AtEnd())
+ current_value_ = ParamType(*current1_, *current2_, *current3_,
+ *current4_);
+ }
+ bool AtEnd() const {
+ // We must report iterator past the end of the range when either of the
+ // component iterators has reached the end of its range.
+ return
+ current1_ == end1_ ||
+ current2_ == end2_ ||
+ current3_ == end3_ ||
+ current4_ == end4_;
+ }
+
+ // No implementation - assignment is unsupported.
+ void operator=(const Iterator& other);
+
+ const ParamGeneratorInterface<ParamType>* const base_;
+ // begin[i]_ and end[i]_ define the i-th range that Iterator traverses.
+ // current[i]_ is the actual traversing iterator.
+ const typename ParamGenerator<T1>::iterator begin1_;
+ const typename ParamGenerator<T1>::iterator end1_;
+ typename ParamGenerator<T1>::iterator current1_;
+ const typename ParamGenerator<T2>::iterator begin2_;
+ const typename ParamGenerator<T2>::iterator end2_;
+ typename ParamGenerator<T2>::iterator current2_;
+ const typename ParamGenerator<T3>::iterator begin3_;
+ const typename ParamGenerator<T3>::iterator end3_;
+ typename ParamGenerator<T3>::iterator current3_;
+ const typename ParamGenerator<T4>::iterator begin4_;
+ const typename ParamGenerator<T4>::iterator end4_;
+ typename ParamGenerator<T4>::iterator current4_;
+ ParamType current_value_;
+ }; // class CartesianProductGenerator4::Iterator
+
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductGenerator4& other);
+
+ const ParamGenerator<T1> g1_;
+ const ParamGenerator<T2> g2_;
+ const ParamGenerator<T3> g3_;
+ const ParamGenerator<T4> g4_;
+}; // class CartesianProductGenerator4
+
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5>
+class CartesianProductGenerator5
+ : public ParamGeneratorInterface< ::std::tr1::tuple<T1, T2, T3, T4, T5> > {
+ public:
+ typedef ::std::tr1::tuple<T1, T2, T3, T4, T5> ParamType;
+
+ CartesianProductGenerator5(const ParamGenerator<T1>& g1,
+ const ParamGenerator<T2>& g2, const ParamGenerator<T3>& g3,
+ const ParamGenerator<T4>& g4, const ParamGenerator<T5>& g5)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5) {}
+ virtual ~CartesianProductGenerator5() {}
+
+ virtual ParamIteratorInterface<ParamType>* Begin() const {
+ return new Iterator(this, g1_, g1_.begin(), g2_, g2_.begin(), g3_,
+ g3_.begin(), g4_, g4_.begin(), g5_, g5_.begin());
+ }
+ virtual ParamIteratorInterface<ParamType>* End() const {
+ return new Iterator(this, g1_, g1_.end(), g2_, g2_.end(), g3_, g3_.end(),
+ g4_, g4_.end(), g5_, g5_.end());
+ }
+
+ private:
+ class Iterator : public ParamIteratorInterface<ParamType> {
+ public:
+ Iterator(const ParamGeneratorInterface<ParamType>* base,
+ const ParamGenerator<T1>& g1,
+ const typename ParamGenerator<T1>::iterator& current1,
+ const ParamGenerator<T2>& g2,
+ const typename ParamGenerator<T2>::iterator& current2,
+ const ParamGenerator<T3>& g3,
+ const typename ParamGenerator<T3>::iterator& current3,
+ const ParamGenerator<T4>& g4,
+ const typename ParamGenerator<T4>::iterator& current4,
+ const ParamGenerator<T5>& g5,
+ const typename ParamGenerator<T5>::iterator& current5)
+ : base_(base),
+ begin1_(g1.begin()), end1_(g1.end()), current1_(current1),
+ begin2_(g2.begin()), end2_(g2.end()), current2_(current2),
+ begin3_(g3.begin()), end3_(g3.end()), current3_(current3),
+ begin4_(g4.begin()), end4_(g4.end()), current4_(current4),
+ begin5_(g5.begin()), end5_(g5.end()), current5_(current5) {
+ ComputeCurrentValue();
+ }
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<ParamType>* BaseGenerator() const {
+ return base_;
+ }
+ // Advance should not be called on beyond-of-range iterators
+ // so no component iterators must be beyond end of range, either.
+ virtual void Advance() {
+ assert(!AtEnd());
+ ++current5_;
+ if (current5_ == end5_) {
+ current5_ = begin5_;
+ ++current4_;
+ }
+ if (current4_ == end4_) {
+ current4_ = begin4_;
+ ++current3_;
+ }
+ if (current3_ == end3_) {
+ current3_ = begin3_;
+ ++current2_;
+ }
+ if (current2_ == end2_) {
+ current2_ = begin2_;
+ ++current1_;
+ }
+ ComputeCurrentValue();
+ }
+ virtual ParamIteratorInterface<ParamType>* Clone() const {
+ return new Iterator(*this);
+ }
+ virtual const ParamType* Current() const { return ¤t_value_; }
+ virtual bool Equals(const ParamIteratorInterface<ParamType>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ const Iterator* typed_other =
+ CheckedDowncastToActualType<const Iterator>(&other);
+ // We must report iterators equal if they both point beyond their
+ // respective ranges. That can happen in a variety of fashions,
+ // so we have to consult AtEnd().
+ return (AtEnd() && typed_other->AtEnd()) ||
+ (
+ current1_ == typed_other->current1_ &&
+ current2_ == typed_other->current2_ &&
+ current3_ == typed_other->current3_ &&
+ current4_ == typed_other->current4_ &&
+ current5_ == typed_other->current5_);
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ : base_(other.base_),
+ begin1_(other.begin1_),
+ end1_(other.end1_),
+ current1_(other.current1_),
+ begin2_(other.begin2_),
+ end2_(other.end2_),
+ current2_(other.current2_),
+ begin3_(other.begin3_),
+ end3_(other.end3_),
+ current3_(other.current3_),
+ begin4_(other.begin4_),
+ end4_(other.end4_),
+ current4_(other.current4_),
+ begin5_(other.begin5_),
+ end5_(other.end5_),
+ current5_(other.current5_) {
+ ComputeCurrentValue();
+ }
+
+ void ComputeCurrentValue() {
+ if (!AtEnd())
+ current_value_ = ParamType(*current1_, *current2_, *current3_,
+ *current4_, *current5_);
+ }
+ bool AtEnd() const {
+ // We must report iterator past the end of the range when either of the
+ // component iterators has reached the end of its range.
+ return
+ current1_ == end1_ ||
+ current2_ == end2_ ||
+ current3_ == end3_ ||
+ current4_ == end4_ ||
+ current5_ == end5_;
+ }
+
+ // No implementation - assignment is unsupported.
+ void operator=(const Iterator& other);
+
+ const ParamGeneratorInterface<ParamType>* const base_;
+ // begin[i]_ and end[i]_ define the i-th range that Iterator traverses.
+ // current[i]_ is the actual traversing iterator.
+ const typename ParamGenerator<T1>::iterator begin1_;
+ const typename ParamGenerator<T1>::iterator end1_;
+ typename ParamGenerator<T1>::iterator current1_;
+ const typename ParamGenerator<T2>::iterator begin2_;
+ const typename ParamGenerator<T2>::iterator end2_;
+ typename ParamGenerator<T2>::iterator current2_;
+ const typename ParamGenerator<T3>::iterator begin3_;
+ const typename ParamGenerator<T3>::iterator end3_;
+ typename ParamGenerator<T3>::iterator current3_;
+ const typename ParamGenerator<T4>::iterator begin4_;
+ const typename ParamGenerator<T4>::iterator end4_;
+ typename ParamGenerator<T4>::iterator current4_;
+ const typename ParamGenerator<T5>::iterator begin5_;
+ const typename ParamGenerator<T5>::iterator end5_;
+ typename ParamGenerator<T5>::iterator current5_;
+ ParamType current_value_;
+ }; // class CartesianProductGenerator5::Iterator
+
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductGenerator5& other);
+
+ const ParamGenerator<T1> g1_;
+ const ParamGenerator<T2> g2_;
+ const ParamGenerator<T3> g3_;
+ const ParamGenerator<T4> g4_;
+ const ParamGenerator<T5> g5_;
+}; // class CartesianProductGenerator5
+
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6>
+class CartesianProductGenerator6
+ : public ParamGeneratorInterface< ::std::tr1::tuple<T1, T2, T3, T4, T5,
+ T6> > {
+ public:
+ typedef ::std::tr1::tuple<T1, T2, T3, T4, T5, T6> ParamType;
+
+ CartesianProductGenerator6(const ParamGenerator<T1>& g1,
+ const ParamGenerator<T2>& g2, const ParamGenerator<T3>& g3,
+ const ParamGenerator<T4>& g4, const ParamGenerator<T5>& g5,
+ const ParamGenerator<T6>& g6)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5), g6_(g6) {}
+ virtual ~CartesianProductGenerator6() {}
+
+ virtual ParamIteratorInterface<ParamType>* Begin() const {
+ return new Iterator(this, g1_, g1_.begin(), g2_, g2_.begin(), g3_,
+ g3_.begin(), g4_, g4_.begin(), g5_, g5_.begin(), g6_, g6_.begin());
+ }
+ virtual ParamIteratorInterface<ParamType>* End() const {
+ return new Iterator(this, g1_, g1_.end(), g2_, g2_.end(), g3_, g3_.end(),
+ g4_, g4_.end(), g5_, g5_.end(), g6_, g6_.end());
+ }
+
+ private:
+ class Iterator : public ParamIteratorInterface<ParamType> {
+ public:
+ Iterator(const ParamGeneratorInterface<ParamType>* base,
+ const ParamGenerator<T1>& g1,
+ const typename ParamGenerator<T1>::iterator& current1,
+ const ParamGenerator<T2>& g2,
+ const typename ParamGenerator<T2>::iterator& current2,
+ const ParamGenerator<T3>& g3,
+ const typename ParamGenerator<T3>::iterator& current3,
+ const ParamGenerator<T4>& g4,
+ const typename ParamGenerator<T4>::iterator& current4,
+ const ParamGenerator<T5>& g5,
+ const typename ParamGenerator<T5>::iterator& current5,
+ const ParamGenerator<T6>& g6,
+ const typename ParamGenerator<T6>::iterator& current6)
+ : base_(base),
+ begin1_(g1.begin()), end1_(g1.end()), current1_(current1),
+ begin2_(g2.begin()), end2_(g2.end()), current2_(current2),
+ begin3_(g3.begin()), end3_(g3.end()), current3_(current3),
+ begin4_(g4.begin()), end4_(g4.end()), current4_(current4),
+ begin5_(g5.begin()), end5_(g5.end()), current5_(current5),
+ begin6_(g6.begin()), end6_(g6.end()), current6_(current6) {
+ ComputeCurrentValue();
+ }
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<ParamType>* BaseGenerator() const {
+ return base_;
+ }
+ // Advance should not be called on beyond-of-range iterators
+ // so no component iterators must be beyond end of range, either.
+ virtual void Advance() {
+ assert(!AtEnd());
+ ++current6_;
+ if (current6_ == end6_) {
+ current6_ = begin6_;
+ ++current5_;
+ }
+ if (current5_ == end5_) {
+ current5_ = begin5_;
+ ++current4_;
+ }
+ if (current4_ == end4_) {
+ current4_ = begin4_;
+ ++current3_;
+ }
+ if (current3_ == end3_) {
+ current3_ = begin3_;
+ ++current2_;
+ }
+ if (current2_ == end2_) {
+ current2_ = begin2_;
+ ++current1_;
+ }
+ ComputeCurrentValue();
+ }
+ virtual ParamIteratorInterface<ParamType>* Clone() const {
+ return new Iterator(*this);
+ }
+ virtual const ParamType* Current() const { return ¤t_value_; }
+ virtual bool Equals(const ParamIteratorInterface<ParamType>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ const Iterator* typed_other =
+ CheckedDowncastToActualType<const Iterator>(&other);
+ // We must report iterators equal if they both point beyond their
+ // respective ranges. That can happen in a variety of fashions,
+ // so we have to consult AtEnd().
+ return (AtEnd() && typed_other->AtEnd()) ||
+ (
+ current1_ == typed_other->current1_ &&
+ current2_ == typed_other->current2_ &&
+ current3_ == typed_other->current3_ &&
+ current4_ == typed_other->current4_ &&
+ current5_ == typed_other->current5_ &&
+ current6_ == typed_other->current6_);
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ : base_(other.base_),
+ begin1_(other.begin1_),
+ end1_(other.end1_),
+ current1_(other.current1_),
+ begin2_(other.begin2_),
+ end2_(other.end2_),
+ current2_(other.current2_),
+ begin3_(other.begin3_),
+ end3_(other.end3_),
+ current3_(other.current3_),
+ begin4_(other.begin4_),
+ end4_(other.end4_),
+ current4_(other.current4_),
+ begin5_(other.begin5_),
+ end5_(other.end5_),
+ current5_(other.current5_),
+ begin6_(other.begin6_),
+ end6_(other.end6_),
+ current6_(other.current6_) {
+ ComputeCurrentValue();
+ }
+
+ void ComputeCurrentValue() {
+ if (!AtEnd())
+ current_value_ = ParamType(*current1_, *current2_, *current3_,
+ *current4_, *current5_, *current6_);
+ }
+ bool AtEnd() const {
+ // We must report iterator past the end of the range when either of the
+ // component iterators has reached the end of its range.
+ return
+ current1_ == end1_ ||
+ current2_ == end2_ ||
+ current3_ == end3_ ||
+ current4_ == end4_ ||
+ current5_ == end5_ ||
+ current6_ == end6_;
+ }
+
+ // No implementation - assignment is unsupported.
+ void operator=(const Iterator& other);
+
+ const ParamGeneratorInterface<ParamType>* const base_;
+ // begin[i]_ and end[i]_ define the i-th range that Iterator traverses.
+ // current[i]_ is the actual traversing iterator.
+ const typename ParamGenerator<T1>::iterator begin1_;
+ const typename ParamGenerator<T1>::iterator end1_;
+ typename ParamGenerator<T1>::iterator current1_;
+ const typename ParamGenerator<T2>::iterator begin2_;
+ const typename ParamGenerator<T2>::iterator end2_;
+ typename ParamGenerator<T2>::iterator current2_;
+ const typename ParamGenerator<T3>::iterator begin3_;
+ const typename ParamGenerator<T3>::iterator end3_;
+ typename ParamGenerator<T3>::iterator current3_;
+ const typename ParamGenerator<T4>::iterator begin4_;
+ const typename ParamGenerator<T4>::iterator end4_;
+ typename ParamGenerator<T4>::iterator current4_;
+ const typename ParamGenerator<T5>::iterator begin5_;
+ const typename ParamGenerator<T5>::iterator end5_;
+ typename ParamGenerator<T5>::iterator current5_;
+ const typename ParamGenerator<T6>::iterator begin6_;
+ const typename ParamGenerator<T6>::iterator end6_;
+ typename ParamGenerator<T6>::iterator current6_;
+ ParamType current_value_;
+ }; // class CartesianProductGenerator6::Iterator
+
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductGenerator6& other);
+
+ const ParamGenerator<T1> g1_;
+ const ParamGenerator<T2> g2_;
+ const ParamGenerator<T3> g3_;
+ const ParamGenerator<T4> g4_;
+ const ParamGenerator<T5> g5_;
+ const ParamGenerator<T6> g6_;
+}; // class CartesianProductGenerator6
+
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7>
+class CartesianProductGenerator7
+ : public ParamGeneratorInterface< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6,
+ T7> > {
+ public:
+ typedef ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7> ParamType;
+
+ CartesianProductGenerator7(const ParamGenerator<T1>& g1,
+ const ParamGenerator<T2>& g2, const ParamGenerator<T3>& g3,
+ const ParamGenerator<T4>& g4, const ParamGenerator<T5>& g5,
+ const ParamGenerator<T6>& g6, const ParamGenerator<T7>& g7)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5), g6_(g6), g7_(g7) {}
+ virtual ~CartesianProductGenerator7() {}
+
+ virtual ParamIteratorInterface<ParamType>* Begin() const {
+ return new Iterator(this, g1_, g1_.begin(), g2_, g2_.begin(), g3_,
+ g3_.begin(), g4_, g4_.begin(), g5_, g5_.begin(), g6_, g6_.begin(), g7_,
+ g7_.begin());
+ }
+ virtual ParamIteratorInterface<ParamType>* End() const {
+ return new Iterator(this, g1_, g1_.end(), g2_, g2_.end(), g3_, g3_.end(),
+ g4_, g4_.end(), g5_, g5_.end(), g6_, g6_.end(), g7_, g7_.end());
+ }
+
+ private:
+ class Iterator : public ParamIteratorInterface<ParamType> {
+ public:
+ Iterator(const ParamGeneratorInterface<ParamType>* base,
+ const ParamGenerator<T1>& g1,
+ const typename ParamGenerator<T1>::iterator& current1,
+ const ParamGenerator<T2>& g2,
+ const typename ParamGenerator<T2>::iterator& current2,
+ const ParamGenerator<T3>& g3,
+ const typename ParamGenerator<T3>::iterator& current3,
+ const ParamGenerator<T4>& g4,
+ const typename ParamGenerator<T4>::iterator& current4,
+ const ParamGenerator<T5>& g5,
+ const typename ParamGenerator<T5>::iterator& current5,
+ const ParamGenerator<T6>& g6,
+ const typename ParamGenerator<T6>::iterator& current6,
+ const ParamGenerator<T7>& g7,
+ const typename ParamGenerator<T7>::iterator& current7)
+ : base_(base),
+ begin1_(g1.begin()), end1_(g1.end()), current1_(current1),
+ begin2_(g2.begin()), end2_(g2.end()), current2_(current2),
+ begin3_(g3.begin()), end3_(g3.end()), current3_(current3),
+ begin4_(g4.begin()), end4_(g4.end()), current4_(current4),
+ begin5_(g5.begin()), end5_(g5.end()), current5_(current5),
+ begin6_(g6.begin()), end6_(g6.end()), current6_(current6),
+ begin7_(g7.begin()), end7_(g7.end()), current7_(current7) {
+ ComputeCurrentValue();
+ }
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<ParamType>* BaseGenerator() const {
+ return base_;
+ }
+ // Advance should not be called on beyond-of-range iterators
+ // so no component iterators must be beyond end of range, either.
+ virtual void Advance() {
+ assert(!AtEnd());
+ ++current7_;
+ if (current7_ == end7_) {
+ current7_ = begin7_;
+ ++current6_;
+ }
+ if (current6_ == end6_) {
+ current6_ = begin6_;
+ ++current5_;
+ }
+ if (current5_ == end5_) {
+ current5_ = begin5_;
+ ++current4_;
+ }
+ if (current4_ == end4_) {
+ current4_ = begin4_;
+ ++current3_;
+ }
+ if (current3_ == end3_) {
+ current3_ = begin3_;
+ ++current2_;
+ }
+ if (current2_ == end2_) {
+ current2_ = begin2_;
+ ++current1_;
+ }
+ ComputeCurrentValue();
+ }
+ virtual ParamIteratorInterface<ParamType>* Clone() const {
+ return new Iterator(*this);
+ }
+ virtual const ParamType* Current() const { return ¤t_value_; }
+ virtual bool Equals(const ParamIteratorInterface<ParamType>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ const Iterator* typed_other =
+ CheckedDowncastToActualType<const Iterator>(&other);
+ // We must report iterators equal if they both point beyond their
+ // respective ranges. That can happen in a variety of fashions,
+ // so we have to consult AtEnd().
+ return (AtEnd() && typed_other->AtEnd()) ||
+ (
+ current1_ == typed_other->current1_ &&
+ current2_ == typed_other->current2_ &&
+ current3_ == typed_other->current3_ &&
+ current4_ == typed_other->current4_ &&
+ current5_ == typed_other->current5_ &&
+ current6_ == typed_other->current6_ &&
+ current7_ == typed_other->current7_);
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ : base_(other.base_),
+ begin1_(other.begin1_),
+ end1_(other.end1_),
+ current1_(other.current1_),
+ begin2_(other.begin2_),
+ end2_(other.end2_),
+ current2_(other.current2_),
+ begin3_(other.begin3_),
+ end3_(other.end3_),
+ current3_(other.current3_),
+ begin4_(other.begin4_),
+ end4_(other.end4_),
+ current4_(other.current4_),
+ begin5_(other.begin5_),
+ end5_(other.end5_),
+ current5_(other.current5_),
+ begin6_(other.begin6_),
+ end6_(other.end6_),
+ current6_(other.current6_),
+ begin7_(other.begin7_),
+ end7_(other.end7_),
+ current7_(other.current7_) {
+ ComputeCurrentValue();
+ }
+
+ void ComputeCurrentValue() {
+ if (!AtEnd())
+ current_value_ = ParamType(*current1_, *current2_, *current3_,
+ *current4_, *current5_, *current6_, *current7_);
+ }
+ bool AtEnd() const {
+ // We must report iterator past the end of the range when either of the
+ // component iterators has reached the end of its range.
+ return
+ current1_ == end1_ ||
+ current2_ == end2_ ||
+ current3_ == end3_ ||
+ current4_ == end4_ ||
+ current5_ == end5_ ||
+ current6_ == end6_ ||
+ current7_ == end7_;
+ }
+
+ // No implementation - assignment is unsupported.
+ void operator=(const Iterator& other);
+
+ const ParamGeneratorInterface<ParamType>* const base_;
+ // begin[i]_ and end[i]_ define the i-th range that Iterator traverses.
+ // current[i]_ is the actual traversing iterator.
+ const typename ParamGenerator<T1>::iterator begin1_;
+ const typename ParamGenerator<T1>::iterator end1_;
+ typename ParamGenerator<T1>::iterator current1_;
+ const typename ParamGenerator<T2>::iterator begin2_;
+ const typename ParamGenerator<T2>::iterator end2_;
+ typename ParamGenerator<T2>::iterator current2_;
+ const typename ParamGenerator<T3>::iterator begin3_;
+ const typename ParamGenerator<T3>::iterator end3_;
+ typename ParamGenerator<T3>::iterator current3_;
+ const typename ParamGenerator<T4>::iterator begin4_;
+ const typename ParamGenerator<T4>::iterator end4_;
+ typename ParamGenerator<T4>::iterator current4_;
+ const typename ParamGenerator<T5>::iterator begin5_;
+ const typename ParamGenerator<T5>::iterator end5_;
+ typename ParamGenerator<T5>::iterator current5_;
+ const typename ParamGenerator<T6>::iterator begin6_;
+ const typename ParamGenerator<T6>::iterator end6_;
+ typename ParamGenerator<T6>::iterator current6_;
+ const typename ParamGenerator<T7>::iterator begin7_;
+ const typename ParamGenerator<T7>::iterator end7_;
+ typename ParamGenerator<T7>::iterator current7_;
+ ParamType current_value_;
+ }; // class CartesianProductGenerator7::Iterator
+
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductGenerator7& other);
+
+ const ParamGenerator<T1> g1_;
+ const ParamGenerator<T2> g2_;
+ const ParamGenerator<T3> g3_;
+ const ParamGenerator<T4> g4_;
+ const ParamGenerator<T5> g5_;
+ const ParamGenerator<T6> g6_;
+ const ParamGenerator<T7> g7_;
+}; // class CartesianProductGenerator7
+
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8>
+class CartesianProductGenerator8
+ : public ParamGeneratorInterface< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6,
+ T7, T8> > {
+ public:
+ typedef ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7, T8> ParamType;
+
+ CartesianProductGenerator8(const ParamGenerator<T1>& g1,
+ const ParamGenerator<T2>& g2, const ParamGenerator<T3>& g3,
+ const ParamGenerator<T4>& g4, const ParamGenerator<T5>& g5,
+ const ParamGenerator<T6>& g6, const ParamGenerator<T7>& g7,
+ const ParamGenerator<T8>& g8)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5), g6_(g6), g7_(g7),
+ g8_(g8) {}
+ virtual ~CartesianProductGenerator8() {}
+
+ virtual ParamIteratorInterface<ParamType>* Begin() const {
+ return new Iterator(this, g1_, g1_.begin(), g2_, g2_.begin(), g3_,
+ g3_.begin(), g4_, g4_.begin(), g5_, g5_.begin(), g6_, g6_.begin(), g7_,
+ g7_.begin(), g8_, g8_.begin());
+ }
+ virtual ParamIteratorInterface<ParamType>* End() const {
+ return new Iterator(this, g1_, g1_.end(), g2_, g2_.end(), g3_, g3_.end(),
+ g4_, g4_.end(), g5_, g5_.end(), g6_, g6_.end(), g7_, g7_.end(), g8_,
+ g8_.end());
+ }
+
+ private:
+ class Iterator : public ParamIteratorInterface<ParamType> {
+ public:
+ Iterator(const ParamGeneratorInterface<ParamType>* base,
+ const ParamGenerator<T1>& g1,
+ const typename ParamGenerator<T1>::iterator& current1,
+ const ParamGenerator<T2>& g2,
+ const typename ParamGenerator<T2>::iterator& current2,
+ const ParamGenerator<T3>& g3,
+ const typename ParamGenerator<T3>::iterator& current3,
+ const ParamGenerator<T4>& g4,
+ const typename ParamGenerator<T4>::iterator& current4,
+ const ParamGenerator<T5>& g5,
+ const typename ParamGenerator<T5>::iterator& current5,
+ const ParamGenerator<T6>& g6,
+ const typename ParamGenerator<T6>::iterator& current6,
+ const ParamGenerator<T7>& g7,
+ const typename ParamGenerator<T7>::iterator& current7,
+ const ParamGenerator<T8>& g8,
+ const typename ParamGenerator<T8>::iterator& current8)
+ : base_(base),
+ begin1_(g1.begin()), end1_(g1.end()), current1_(current1),
+ begin2_(g2.begin()), end2_(g2.end()), current2_(current2),
+ begin3_(g3.begin()), end3_(g3.end()), current3_(current3),
+ begin4_(g4.begin()), end4_(g4.end()), current4_(current4),
+ begin5_(g5.begin()), end5_(g5.end()), current5_(current5),
+ begin6_(g6.begin()), end6_(g6.end()), current6_(current6),
+ begin7_(g7.begin()), end7_(g7.end()), current7_(current7),
+ begin8_(g8.begin()), end8_(g8.end()), current8_(current8) {
+ ComputeCurrentValue();
+ }
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<ParamType>* BaseGenerator() const {
+ return base_;
+ }
+ // Advance should not be called on beyond-of-range iterators
+ // so no component iterators must be beyond end of range, either.
+ virtual void Advance() {
+ assert(!AtEnd());
+ ++current8_;
+ if (current8_ == end8_) {
+ current8_ = begin8_;
+ ++current7_;
+ }
+ if (current7_ == end7_) {
+ current7_ = begin7_;
+ ++current6_;
+ }
+ if (current6_ == end6_) {
+ current6_ = begin6_;
+ ++current5_;
+ }
+ if (current5_ == end5_) {
+ current5_ = begin5_;
+ ++current4_;
+ }
+ if (current4_ == end4_) {
+ current4_ = begin4_;
+ ++current3_;
+ }
+ if (current3_ == end3_) {
+ current3_ = begin3_;
+ ++current2_;
+ }
+ if (current2_ == end2_) {
+ current2_ = begin2_;
+ ++current1_;
+ }
+ ComputeCurrentValue();
+ }
+ virtual ParamIteratorInterface<ParamType>* Clone() const {
+ return new Iterator(*this);
+ }
+ virtual const ParamType* Current() const { return ¤t_value_; }
+ virtual bool Equals(const ParamIteratorInterface<ParamType>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ const Iterator* typed_other =
+ CheckedDowncastToActualType<const Iterator>(&other);
+ // We must report iterators equal if they both point beyond their
+ // respective ranges. That can happen in a variety of fashions,
+ // so we have to consult AtEnd().
+ return (AtEnd() && typed_other->AtEnd()) ||
+ (
+ current1_ == typed_other->current1_ &&
+ current2_ == typed_other->current2_ &&
+ current3_ == typed_other->current3_ &&
+ current4_ == typed_other->current4_ &&
+ current5_ == typed_other->current5_ &&
+ current6_ == typed_other->current6_ &&
+ current7_ == typed_other->current7_ &&
+ current8_ == typed_other->current8_);
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ : base_(other.base_),
+ begin1_(other.begin1_),
+ end1_(other.end1_),
+ current1_(other.current1_),
+ begin2_(other.begin2_),
+ end2_(other.end2_),
+ current2_(other.current2_),
+ begin3_(other.begin3_),
+ end3_(other.end3_),
+ current3_(other.current3_),
+ begin4_(other.begin4_),
+ end4_(other.end4_),
+ current4_(other.current4_),
+ begin5_(other.begin5_),
+ end5_(other.end5_),
+ current5_(other.current5_),
+ begin6_(other.begin6_),
+ end6_(other.end6_),
+ current6_(other.current6_),
+ begin7_(other.begin7_),
+ end7_(other.end7_),
+ current7_(other.current7_),
+ begin8_(other.begin8_),
+ end8_(other.end8_),
+ current8_(other.current8_) {
+ ComputeCurrentValue();
+ }
+
+ void ComputeCurrentValue() {
+ if (!AtEnd())
+ current_value_ = ParamType(*current1_, *current2_, *current3_,
+ *current4_, *current5_, *current6_, *current7_, *current8_);
+ }
+ bool AtEnd() const {
+ // We must report iterator past the end of the range when either of the
+ // component iterators has reached the end of its range.
+ return
+ current1_ == end1_ ||
+ current2_ == end2_ ||
+ current3_ == end3_ ||
+ current4_ == end4_ ||
+ current5_ == end5_ ||
+ current6_ == end6_ ||
+ current7_ == end7_ ||
+ current8_ == end8_;
+ }
+
+ // No implementation - assignment is unsupported.
+ void operator=(const Iterator& other);
+
+ const ParamGeneratorInterface<ParamType>* const base_;
+ // begin[i]_ and end[i]_ define the i-th range that Iterator traverses.
+ // current[i]_ is the actual traversing iterator.
+ const typename ParamGenerator<T1>::iterator begin1_;
+ const typename ParamGenerator<T1>::iterator end1_;
+ typename ParamGenerator<T1>::iterator current1_;
+ const typename ParamGenerator<T2>::iterator begin2_;
+ const typename ParamGenerator<T2>::iterator end2_;
+ typename ParamGenerator<T2>::iterator current2_;
+ const typename ParamGenerator<T3>::iterator begin3_;
+ const typename ParamGenerator<T3>::iterator end3_;
+ typename ParamGenerator<T3>::iterator current3_;
+ const typename ParamGenerator<T4>::iterator begin4_;
+ const typename ParamGenerator<T4>::iterator end4_;
+ typename ParamGenerator<T4>::iterator current4_;
+ const typename ParamGenerator<T5>::iterator begin5_;
+ const typename ParamGenerator<T5>::iterator end5_;
+ typename ParamGenerator<T5>::iterator current5_;
+ const typename ParamGenerator<T6>::iterator begin6_;
+ const typename ParamGenerator<T6>::iterator end6_;
+ typename ParamGenerator<T6>::iterator current6_;
+ const typename ParamGenerator<T7>::iterator begin7_;
+ const typename ParamGenerator<T7>::iterator end7_;
+ typename ParamGenerator<T7>::iterator current7_;
+ const typename ParamGenerator<T8>::iterator begin8_;
+ const typename ParamGenerator<T8>::iterator end8_;
+ typename ParamGenerator<T8>::iterator current8_;
+ ParamType current_value_;
+ }; // class CartesianProductGenerator8::Iterator
+
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductGenerator8& other);
+
+ const ParamGenerator<T1> g1_;
+ const ParamGenerator<T2> g2_;
+ const ParamGenerator<T3> g3_;
+ const ParamGenerator<T4> g4_;
+ const ParamGenerator<T5> g5_;
+ const ParamGenerator<T6> g6_;
+ const ParamGenerator<T7> g7_;
+ const ParamGenerator<T8> g8_;
+}; // class CartesianProductGenerator8
+
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9>
+class CartesianProductGenerator9
+ : public ParamGeneratorInterface< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6,
+ T7, T8, T9> > {
+ public:
+ typedef ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7, T8, T9> ParamType;
+
+ CartesianProductGenerator9(const ParamGenerator<T1>& g1,
+ const ParamGenerator<T2>& g2, const ParamGenerator<T3>& g3,
+ const ParamGenerator<T4>& g4, const ParamGenerator<T5>& g5,
+ const ParamGenerator<T6>& g6, const ParamGenerator<T7>& g7,
+ const ParamGenerator<T8>& g8, const ParamGenerator<T9>& g9)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5), g6_(g6), g7_(g7), g8_(g8),
+ g9_(g9) {}
+ virtual ~CartesianProductGenerator9() {}
+
+ virtual ParamIteratorInterface<ParamType>* Begin() const {
+ return new Iterator(this, g1_, g1_.begin(), g2_, g2_.begin(), g3_,
+ g3_.begin(), g4_, g4_.begin(), g5_, g5_.begin(), g6_, g6_.begin(), g7_,
+ g7_.begin(), g8_, g8_.begin(), g9_, g9_.begin());
+ }
+ virtual ParamIteratorInterface<ParamType>* End() const {
+ return new Iterator(this, g1_, g1_.end(), g2_, g2_.end(), g3_, g3_.end(),
+ g4_, g4_.end(), g5_, g5_.end(), g6_, g6_.end(), g7_, g7_.end(), g8_,
+ g8_.end(), g9_, g9_.end());
+ }
+
+ private:
+ class Iterator : public ParamIteratorInterface<ParamType> {
+ public:
+ Iterator(const ParamGeneratorInterface<ParamType>* base,
+ const ParamGenerator<T1>& g1,
+ const typename ParamGenerator<T1>::iterator& current1,
+ const ParamGenerator<T2>& g2,
+ const typename ParamGenerator<T2>::iterator& current2,
+ const ParamGenerator<T3>& g3,
+ const typename ParamGenerator<T3>::iterator& current3,
+ const ParamGenerator<T4>& g4,
+ const typename ParamGenerator<T4>::iterator& current4,
+ const ParamGenerator<T5>& g5,
+ const typename ParamGenerator<T5>::iterator& current5,
+ const ParamGenerator<T6>& g6,
+ const typename ParamGenerator<T6>::iterator& current6,
+ const ParamGenerator<T7>& g7,
+ const typename ParamGenerator<T7>::iterator& current7,
+ const ParamGenerator<T8>& g8,
+ const typename ParamGenerator<T8>::iterator& current8,
+ const ParamGenerator<T9>& g9,
+ const typename ParamGenerator<T9>::iterator& current9)
+ : base_(base),
+ begin1_(g1.begin()), end1_(g1.end()), current1_(current1),
+ begin2_(g2.begin()), end2_(g2.end()), current2_(current2),
+ begin3_(g3.begin()), end3_(g3.end()), current3_(current3),
+ begin4_(g4.begin()), end4_(g4.end()), current4_(current4),
+ begin5_(g5.begin()), end5_(g5.end()), current5_(current5),
+ begin6_(g6.begin()), end6_(g6.end()), current6_(current6),
+ begin7_(g7.begin()), end7_(g7.end()), current7_(current7),
+ begin8_(g8.begin()), end8_(g8.end()), current8_(current8),
+ begin9_(g9.begin()), end9_(g9.end()), current9_(current9) {
+ ComputeCurrentValue();
+ }
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<ParamType>* BaseGenerator() const {
+ return base_;
+ }
+ // Advance should not be called on beyond-of-range iterators
+ // so no component iterators must be beyond end of range, either.
+ virtual void Advance() {
+ assert(!AtEnd());
+ ++current9_;
+ if (current9_ == end9_) {
+ current9_ = begin9_;
+ ++current8_;
+ }
+ if (current8_ == end8_) {
+ current8_ = begin8_;
+ ++current7_;
+ }
+ if (current7_ == end7_) {
+ current7_ = begin7_;
+ ++current6_;
+ }
+ if (current6_ == end6_) {
+ current6_ = begin6_;
+ ++current5_;
+ }
+ if (current5_ == end5_) {
+ current5_ = begin5_;
+ ++current4_;
+ }
+ if (current4_ == end4_) {
+ current4_ = begin4_;
+ ++current3_;
+ }
+ if (current3_ == end3_) {
+ current3_ = begin3_;
+ ++current2_;
+ }
+ if (current2_ == end2_) {
+ current2_ = begin2_;
+ ++current1_;
+ }
+ ComputeCurrentValue();
+ }
+ virtual ParamIteratorInterface<ParamType>* Clone() const {
+ return new Iterator(*this);
+ }
+ virtual const ParamType* Current() const { return ¤t_value_; }
+ virtual bool Equals(const ParamIteratorInterface<ParamType>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ const Iterator* typed_other =
+ CheckedDowncastToActualType<const Iterator>(&other);
+ // We must report iterators equal if they both point beyond their
+ // respective ranges. That can happen in a variety of fashions,
+ // so we have to consult AtEnd().
+ return (AtEnd() && typed_other->AtEnd()) ||
+ (
+ current1_ == typed_other->current1_ &&
+ current2_ == typed_other->current2_ &&
+ current3_ == typed_other->current3_ &&
+ current4_ == typed_other->current4_ &&
+ current5_ == typed_other->current5_ &&
+ current6_ == typed_other->current6_ &&
+ current7_ == typed_other->current7_ &&
+ current8_ == typed_other->current8_ &&
+ current9_ == typed_other->current9_);
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ : base_(other.base_),
+ begin1_(other.begin1_),
+ end1_(other.end1_),
+ current1_(other.current1_),
+ begin2_(other.begin2_),
+ end2_(other.end2_),
+ current2_(other.current2_),
+ begin3_(other.begin3_),
+ end3_(other.end3_),
+ current3_(other.current3_),
+ begin4_(other.begin4_),
+ end4_(other.end4_),
+ current4_(other.current4_),
+ begin5_(other.begin5_),
+ end5_(other.end5_),
+ current5_(other.current5_),
+ begin6_(other.begin6_),
+ end6_(other.end6_),
+ current6_(other.current6_),
+ begin7_(other.begin7_),
+ end7_(other.end7_),
+ current7_(other.current7_),
+ begin8_(other.begin8_),
+ end8_(other.end8_),
+ current8_(other.current8_),
+ begin9_(other.begin9_),
+ end9_(other.end9_),
+ current9_(other.current9_) {
+ ComputeCurrentValue();
+ }
+
+ void ComputeCurrentValue() {
+ if (!AtEnd())
+ current_value_ = ParamType(*current1_, *current2_, *current3_,
+ *current4_, *current5_, *current6_, *current7_, *current8_,
+ *current9_);
+ }
+ bool AtEnd() const {
+ // We must report iterator past the end of the range when either of the
+ // component iterators has reached the end of its range.
+ return
+ current1_ == end1_ ||
+ current2_ == end2_ ||
+ current3_ == end3_ ||
+ current4_ == end4_ ||
+ current5_ == end5_ ||
+ current6_ == end6_ ||
+ current7_ == end7_ ||
+ current8_ == end8_ ||
+ current9_ == end9_;
+ }
+
+ // No implementation - assignment is unsupported.
+ void operator=(const Iterator& other);
+
+ const ParamGeneratorInterface<ParamType>* const base_;
+ // begin[i]_ and end[i]_ define the i-th range that Iterator traverses.
+ // current[i]_ is the actual traversing iterator.
+ const typename ParamGenerator<T1>::iterator begin1_;
+ const typename ParamGenerator<T1>::iterator end1_;
+ typename ParamGenerator<T1>::iterator current1_;
+ const typename ParamGenerator<T2>::iterator begin2_;
+ const typename ParamGenerator<T2>::iterator end2_;
+ typename ParamGenerator<T2>::iterator current2_;
+ const typename ParamGenerator<T3>::iterator begin3_;
+ const typename ParamGenerator<T3>::iterator end3_;
+ typename ParamGenerator<T3>::iterator current3_;
+ const typename ParamGenerator<T4>::iterator begin4_;
+ const typename ParamGenerator<T4>::iterator end4_;
+ typename ParamGenerator<T4>::iterator current4_;
+ const typename ParamGenerator<T5>::iterator begin5_;
+ const typename ParamGenerator<T5>::iterator end5_;
+ typename ParamGenerator<T5>::iterator current5_;
+ const typename ParamGenerator<T6>::iterator begin6_;
+ const typename ParamGenerator<T6>::iterator end6_;
+ typename ParamGenerator<T6>::iterator current6_;
+ const typename ParamGenerator<T7>::iterator begin7_;
+ const typename ParamGenerator<T7>::iterator end7_;
+ typename ParamGenerator<T7>::iterator current7_;
+ const typename ParamGenerator<T8>::iterator begin8_;
+ const typename ParamGenerator<T8>::iterator end8_;
+ typename ParamGenerator<T8>::iterator current8_;
+ const typename ParamGenerator<T9>::iterator begin9_;
+ const typename ParamGenerator<T9>::iterator end9_;
+ typename ParamGenerator<T9>::iterator current9_;
+ ParamType current_value_;
+ }; // class CartesianProductGenerator9::Iterator
+
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductGenerator9& other);
+
+ const ParamGenerator<T1> g1_;
+ const ParamGenerator<T2> g2_;
+ const ParamGenerator<T3> g3_;
+ const ParamGenerator<T4> g4_;
+ const ParamGenerator<T5> g5_;
+ const ParamGenerator<T6> g6_;
+ const ParamGenerator<T7> g7_;
+ const ParamGenerator<T8> g8_;
+ const ParamGenerator<T9> g9_;
+}; // class CartesianProductGenerator9
+
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10>
+class CartesianProductGenerator10
+ : public ParamGeneratorInterface< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6,
+ T7, T8, T9, T10> > {
+ public:
+ typedef ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10> ParamType;
+
+ CartesianProductGenerator10(const ParamGenerator<T1>& g1,
+ const ParamGenerator<T2>& g2, const ParamGenerator<T3>& g3,
+ const ParamGenerator<T4>& g4, const ParamGenerator<T5>& g5,
+ const ParamGenerator<T6>& g6, const ParamGenerator<T7>& g7,
+ const ParamGenerator<T8>& g8, const ParamGenerator<T9>& g9,
+ const ParamGenerator<T10>& g10)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5), g6_(g6), g7_(g7), g8_(g8),
+ g9_(g9), g10_(g10) {}
+ virtual ~CartesianProductGenerator10() {}
+
+ virtual ParamIteratorInterface<ParamType>* Begin() const {
+ return new Iterator(this, g1_, g1_.begin(), g2_, g2_.begin(), g3_,
+ g3_.begin(), g4_, g4_.begin(), g5_, g5_.begin(), g6_, g6_.begin(), g7_,
+ g7_.begin(), g8_, g8_.begin(), g9_, g9_.begin(), g10_, g10_.begin());
+ }
+ virtual ParamIteratorInterface<ParamType>* End() const {
+ return new Iterator(this, g1_, g1_.end(), g2_, g2_.end(), g3_, g3_.end(),
+ g4_, g4_.end(), g5_, g5_.end(), g6_, g6_.end(), g7_, g7_.end(), g8_,
+ g8_.end(), g9_, g9_.end(), g10_, g10_.end());
+ }
+
+ private:
+ class Iterator : public ParamIteratorInterface<ParamType> {
+ public:
+ Iterator(const ParamGeneratorInterface<ParamType>* base,
+ const ParamGenerator<T1>& g1,
+ const typename ParamGenerator<T1>::iterator& current1,
+ const ParamGenerator<T2>& g2,
+ const typename ParamGenerator<T2>::iterator& current2,
+ const ParamGenerator<T3>& g3,
+ const typename ParamGenerator<T3>::iterator& current3,
+ const ParamGenerator<T4>& g4,
+ const typename ParamGenerator<T4>::iterator& current4,
+ const ParamGenerator<T5>& g5,
+ const typename ParamGenerator<T5>::iterator& current5,
+ const ParamGenerator<T6>& g6,
+ const typename ParamGenerator<T6>::iterator& current6,
+ const ParamGenerator<T7>& g7,
+ const typename ParamGenerator<T7>::iterator& current7,
+ const ParamGenerator<T8>& g8,
+ const typename ParamGenerator<T8>::iterator& current8,
+ const ParamGenerator<T9>& g9,
+ const typename ParamGenerator<T9>::iterator& current9,
+ const ParamGenerator<T10>& g10,
+ const typename ParamGenerator<T10>::iterator& current10)
+ : base_(base),
+ begin1_(g1.begin()), end1_(g1.end()), current1_(current1),
+ begin2_(g2.begin()), end2_(g2.end()), current2_(current2),
+ begin3_(g3.begin()), end3_(g3.end()), current3_(current3),
+ begin4_(g4.begin()), end4_(g4.end()), current4_(current4),
+ begin5_(g5.begin()), end5_(g5.end()), current5_(current5),
+ begin6_(g6.begin()), end6_(g6.end()), current6_(current6),
+ begin7_(g7.begin()), end7_(g7.end()), current7_(current7),
+ begin8_(g8.begin()), end8_(g8.end()), current8_(current8),
+ begin9_(g9.begin()), end9_(g9.end()), current9_(current9),
+ begin10_(g10.begin()), end10_(g10.end()), current10_(current10) {
+ ComputeCurrentValue();
+ }
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<ParamType>* BaseGenerator() const {
+ return base_;
+ }
+ // Advance should not be called on beyond-of-range iterators
+ // so no component iterators must be beyond end of range, either.
+ virtual void Advance() {
+ assert(!AtEnd());
+ ++current10_;
+ if (current10_ == end10_) {
+ current10_ = begin10_;
+ ++current9_;
+ }
+ if (current9_ == end9_) {
+ current9_ = begin9_;
+ ++current8_;
+ }
+ if (current8_ == end8_) {
+ current8_ = begin8_;
+ ++current7_;
+ }
+ if (current7_ == end7_) {
+ current7_ = begin7_;
+ ++current6_;
+ }
+ if (current6_ == end6_) {
+ current6_ = begin6_;
+ ++current5_;
+ }
+ if (current5_ == end5_) {
+ current5_ = begin5_;
+ ++current4_;
+ }
+ if (current4_ == end4_) {
+ current4_ = begin4_;
+ ++current3_;
+ }
+ if (current3_ == end3_) {
+ current3_ = begin3_;
+ ++current2_;
+ }
+ if (current2_ == end2_) {
+ current2_ = begin2_;
+ ++current1_;
+ }
+ ComputeCurrentValue();
+ }
+ virtual ParamIteratorInterface<ParamType>* Clone() const {
+ return new Iterator(*this);
+ }
+ virtual const ParamType* Current() const { return ¤t_value_; }
+ virtual bool Equals(const ParamIteratorInterface<ParamType>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ const Iterator* typed_other =
+ CheckedDowncastToActualType<const Iterator>(&other);
+ // We must report iterators equal if they both point beyond their
+ // respective ranges. That can happen in a variety of fashions,
+ // so we have to consult AtEnd().
+ return (AtEnd() && typed_other->AtEnd()) ||
+ (
+ current1_ == typed_other->current1_ &&
+ current2_ == typed_other->current2_ &&
+ current3_ == typed_other->current3_ &&
+ current4_ == typed_other->current4_ &&
+ current5_ == typed_other->current5_ &&
+ current6_ == typed_other->current6_ &&
+ current7_ == typed_other->current7_ &&
+ current8_ == typed_other->current8_ &&
+ current9_ == typed_other->current9_ &&
+ current10_ == typed_other->current10_);
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ : base_(other.base_),
+ begin1_(other.begin1_),
+ end1_(other.end1_),
+ current1_(other.current1_),
+ begin2_(other.begin2_),
+ end2_(other.end2_),
+ current2_(other.current2_),
+ begin3_(other.begin3_),
+ end3_(other.end3_),
+ current3_(other.current3_),
+ begin4_(other.begin4_),
+ end4_(other.end4_),
+ current4_(other.current4_),
+ begin5_(other.begin5_),
+ end5_(other.end5_),
+ current5_(other.current5_),
+ begin6_(other.begin6_),
+ end6_(other.end6_),
+ current6_(other.current6_),
+ begin7_(other.begin7_),
+ end7_(other.end7_),
+ current7_(other.current7_),
+ begin8_(other.begin8_),
+ end8_(other.end8_),
+ current8_(other.current8_),
+ begin9_(other.begin9_),
+ end9_(other.end9_),
+ current9_(other.current9_),
+ begin10_(other.begin10_),
+ end10_(other.end10_),
+ current10_(other.current10_) {
+ ComputeCurrentValue();
+ }
+
+ void ComputeCurrentValue() {
+ if (!AtEnd())
+ current_value_ = ParamType(*current1_, *current2_, *current3_,
+ *current4_, *current5_, *current6_, *current7_, *current8_,
+ *current9_, *current10_);
+ }
+ bool AtEnd() const {
+ // We must report iterator past the end of the range when either of the
+ // component iterators has reached the end of its range.
+ return
+ current1_ == end1_ ||
+ current2_ == end2_ ||
+ current3_ == end3_ ||
+ current4_ == end4_ ||
+ current5_ == end5_ ||
+ current6_ == end6_ ||
+ current7_ == end7_ ||
+ current8_ == end8_ ||
+ current9_ == end9_ ||
+ current10_ == end10_;
+ }
+
+ // No implementation - assignment is unsupported.
+ void operator=(const Iterator& other);
+
+ const ParamGeneratorInterface<ParamType>* const base_;
+ // begin[i]_ and end[i]_ define the i-th range that Iterator traverses.
+ // current[i]_ is the actual traversing iterator.
+ const typename ParamGenerator<T1>::iterator begin1_;
+ const typename ParamGenerator<T1>::iterator end1_;
+ typename ParamGenerator<T1>::iterator current1_;
+ const typename ParamGenerator<T2>::iterator begin2_;
+ const typename ParamGenerator<T2>::iterator end2_;
+ typename ParamGenerator<T2>::iterator current2_;
+ const typename ParamGenerator<T3>::iterator begin3_;
+ const typename ParamGenerator<T3>::iterator end3_;
+ typename ParamGenerator<T3>::iterator current3_;
+ const typename ParamGenerator<T4>::iterator begin4_;
+ const typename ParamGenerator<T4>::iterator end4_;
+ typename ParamGenerator<T4>::iterator current4_;
+ const typename ParamGenerator<T5>::iterator begin5_;
+ const typename ParamGenerator<T5>::iterator end5_;
+ typename ParamGenerator<T5>::iterator current5_;
+ const typename ParamGenerator<T6>::iterator begin6_;
+ const typename ParamGenerator<T6>::iterator end6_;
+ typename ParamGenerator<T6>::iterator current6_;
+ const typename ParamGenerator<T7>::iterator begin7_;
+ const typename ParamGenerator<T7>::iterator end7_;
+ typename ParamGenerator<T7>::iterator current7_;
+ const typename ParamGenerator<T8>::iterator begin8_;
+ const typename ParamGenerator<T8>::iterator end8_;
+ typename ParamGenerator<T8>::iterator current8_;
+ const typename ParamGenerator<T9>::iterator begin9_;
+ const typename ParamGenerator<T9>::iterator end9_;
+ typename ParamGenerator<T9>::iterator current9_;
+ const typename ParamGenerator<T10>::iterator begin10_;
+ const typename ParamGenerator<T10>::iterator end10_;
+ typename ParamGenerator<T10>::iterator current10_;
+ ParamType current_value_;
+ }; // class CartesianProductGenerator10::Iterator
+
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductGenerator10& other);
+
+ const ParamGenerator<T1> g1_;
+ const ParamGenerator<T2> g2_;
+ const ParamGenerator<T3> g3_;
+ const ParamGenerator<T4> g4_;
+ const ParamGenerator<T5> g5_;
+ const ParamGenerator<T6> g6_;
+ const ParamGenerator<T7> g7_;
+ const ParamGenerator<T8> g8_;
+ const ParamGenerator<T9> g9_;
+ const ParamGenerator<T10> g10_;
+}; // class CartesianProductGenerator10
+
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// Helper classes providing Combine() with polymorphic features. They allow
+// casting CartesianProductGeneratorN<T> to ParamGenerator<U> if T is
+// convertible to U.
+//
+template <class Generator1, class Generator2>
+class CartesianProductHolder2 {
+ public:
+CartesianProductHolder2(const Generator1& g1, const Generator2& g2)
+ : g1_(g1), g2_(g2) {}
+ template <typename T1, typename T2>
+ operator ParamGenerator< ::std::tr1::tuple<T1, T2> >() const {
+ return ParamGenerator< ::std::tr1::tuple<T1, T2> >(
+ new CartesianProductGenerator2<T1, T2>(
+ static_cast<ParamGenerator<T1> >(g1_),
+ static_cast<ParamGenerator<T2> >(g2_)));
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductHolder2& other);
+
+ const Generator1 g1_;
+ const Generator2 g2_;
+}; // class CartesianProductHolder2
+
+template <class Generator1, class Generator2, class Generator3>
+class CartesianProductHolder3 {
+ public:
+CartesianProductHolder3(const Generator1& g1, const Generator2& g2,
+ const Generator3& g3)
+ : g1_(g1), g2_(g2), g3_(g3) {}
+ template <typename T1, typename T2, typename T3>
+ operator ParamGenerator< ::std::tr1::tuple<T1, T2, T3> >() const {
+ return ParamGenerator< ::std::tr1::tuple<T1, T2, T3> >(
+ new CartesianProductGenerator3<T1, T2, T3>(
+ static_cast<ParamGenerator<T1> >(g1_),
+ static_cast<ParamGenerator<T2> >(g2_),
+ static_cast<ParamGenerator<T3> >(g3_)));
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductHolder3& other);
+
+ const Generator1 g1_;
+ const Generator2 g2_;
+ const Generator3 g3_;
+}; // class CartesianProductHolder3
+
+template <class Generator1, class Generator2, class Generator3,
+ class Generator4>
+class CartesianProductHolder4 {
+ public:
+CartesianProductHolder4(const Generator1& g1, const Generator2& g2,
+ const Generator3& g3, const Generator4& g4)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4) {}
+ template <typename T1, typename T2, typename T3, typename T4>
+ operator ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4> >() const {
+ return ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4> >(
+ new CartesianProductGenerator4<T1, T2, T3, T4>(
+ static_cast<ParamGenerator<T1> >(g1_),
+ static_cast<ParamGenerator<T2> >(g2_),
+ static_cast<ParamGenerator<T3> >(g3_),
+ static_cast<ParamGenerator<T4> >(g4_)));
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductHolder4& other);
+
+ const Generator1 g1_;
+ const Generator2 g2_;
+ const Generator3 g3_;
+ const Generator4 g4_;
+}; // class CartesianProductHolder4
+
+template <class Generator1, class Generator2, class Generator3,
+ class Generator4, class Generator5>
+class CartesianProductHolder5 {
+ public:
+CartesianProductHolder5(const Generator1& g1, const Generator2& g2,
+ const Generator3& g3, const Generator4& g4, const Generator5& g5)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5) {}
+ template <typename T1, typename T2, typename T3, typename T4, typename T5>
+ operator ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5> >() const {
+ return ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5> >(
+ new CartesianProductGenerator5<T1, T2, T3, T4, T5>(
+ static_cast<ParamGenerator<T1> >(g1_),
+ static_cast<ParamGenerator<T2> >(g2_),
+ static_cast<ParamGenerator<T3> >(g3_),
+ static_cast<ParamGenerator<T4> >(g4_),
+ static_cast<ParamGenerator<T5> >(g5_)));
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductHolder5& other);
+
+ const Generator1 g1_;
+ const Generator2 g2_;
+ const Generator3 g3_;
+ const Generator4 g4_;
+ const Generator5 g5_;
+}; // class CartesianProductHolder5
+
+template <class Generator1, class Generator2, class Generator3,
+ class Generator4, class Generator5, class Generator6>
+class CartesianProductHolder6 {
+ public:
+CartesianProductHolder6(const Generator1& g1, const Generator2& g2,
+ const Generator3& g3, const Generator4& g4, const Generator5& g5,
+ const Generator6& g6)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5), g6_(g6) {}
+ template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6>
+ operator ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6> >() const {
+ return ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6> >(
+ new CartesianProductGenerator6<T1, T2, T3, T4, T5, T6>(
+ static_cast<ParamGenerator<T1> >(g1_),
+ static_cast<ParamGenerator<T2> >(g2_),
+ static_cast<ParamGenerator<T3> >(g3_),
+ static_cast<ParamGenerator<T4> >(g4_),
+ static_cast<ParamGenerator<T5> >(g5_),
+ static_cast<ParamGenerator<T6> >(g6_)));
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductHolder6& other);
+
+ const Generator1 g1_;
+ const Generator2 g2_;
+ const Generator3 g3_;
+ const Generator4 g4_;
+ const Generator5 g5_;
+ const Generator6 g6_;
+}; // class CartesianProductHolder6
+
+template <class Generator1, class Generator2, class Generator3,
+ class Generator4, class Generator5, class Generator6, class Generator7>
+class CartesianProductHolder7 {
+ public:
+CartesianProductHolder7(const Generator1& g1, const Generator2& g2,
+ const Generator3& g3, const Generator4& g4, const Generator5& g5,
+ const Generator6& g6, const Generator7& g7)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5), g6_(g6), g7_(g7) {}
+ template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7>
+ operator ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6,
+ T7> >() const {
+ return ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7> >(
+ new CartesianProductGenerator7<T1, T2, T3, T4, T5, T6, T7>(
+ static_cast<ParamGenerator<T1> >(g1_),
+ static_cast<ParamGenerator<T2> >(g2_),
+ static_cast<ParamGenerator<T3> >(g3_),
+ static_cast<ParamGenerator<T4> >(g4_),
+ static_cast<ParamGenerator<T5> >(g5_),
+ static_cast<ParamGenerator<T6> >(g6_),
+ static_cast<ParamGenerator<T7> >(g7_)));
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductHolder7& other);
+
+ const Generator1 g1_;
+ const Generator2 g2_;
+ const Generator3 g3_;
+ const Generator4 g4_;
+ const Generator5 g5_;
+ const Generator6 g6_;
+ const Generator7 g7_;
+}; // class CartesianProductHolder7
+
+template <class Generator1, class Generator2, class Generator3,
+ class Generator4, class Generator5, class Generator6, class Generator7,
+ class Generator8>
+class CartesianProductHolder8 {
+ public:
+CartesianProductHolder8(const Generator1& g1, const Generator2& g2,
+ const Generator3& g3, const Generator4& g4, const Generator5& g5,
+ const Generator6& g6, const Generator7& g7, const Generator8& g8)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5), g6_(g6), g7_(g7),
+ g8_(g8) {}
+ template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8>
+ operator ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7,
+ T8> >() const {
+ return ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7, T8> >(
+ new CartesianProductGenerator8<T1, T2, T3, T4, T5, T6, T7, T8>(
+ static_cast<ParamGenerator<T1> >(g1_),
+ static_cast<ParamGenerator<T2> >(g2_),
+ static_cast<ParamGenerator<T3> >(g3_),
+ static_cast<ParamGenerator<T4> >(g4_),
+ static_cast<ParamGenerator<T5> >(g5_),
+ static_cast<ParamGenerator<T6> >(g6_),
+ static_cast<ParamGenerator<T7> >(g7_),
+ static_cast<ParamGenerator<T8> >(g8_)));
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductHolder8& other);
+
+ const Generator1 g1_;
+ const Generator2 g2_;
+ const Generator3 g3_;
+ const Generator4 g4_;
+ const Generator5 g5_;
+ const Generator6 g6_;
+ const Generator7 g7_;
+ const Generator8 g8_;
+}; // class CartesianProductHolder8
+
+template <class Generator1, class Generator2, class Generator3,
+ class Generator4, class Generator5, class Generator6, class Generator7,
+ class Generator8, class Generator9>
+class CartesianProductHolder9 {
+ public:
+CartesianProductHolder9(const Generator1& g1, const Generator2& g2,
+ const Generator3& g3, const Generator4& g4, const Generator5& g5,
+ const Generator6& g6, const Generator7& g7, const Generator8& g8,
+ const Generator9& g9)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5), g6_(g6), g7_(g7), g8_(g8),
+ g9_(g9) {}
+ template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9>
+ operator ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7, T8,
+ T9> >() const {
+ return ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7, T8,
+ T9> >(
+ new CartesianProductGenerator9<T1, T2, T3, T4, T5, T6, T7, T8, T9>(
+ static_cast<ParamGenerator<T1> >(g1_),
+ static_cast<ParamGenerator<T2> >(g2_),
+ static_cast<ParamGenerator<T3> >(g3_),
+ static_cast<ParamGenerator<T4> >(g4_),
+ static_cast<ParamGenerator<T5> >(g5_),
+ static_cast<ParamGenerator<T6> >(g6_),
+ static_cast<ParamGenerator<T7> >(g7_),
+ static_cast<ParamGenerator<T8> >(g8_),
+ static_cast<ParamGenerator<T9> >(g9_)));
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductHolder9& other);
+
+ const Generator1 g1_;
+ const Generator2 g2_;
+ const Generator3 g3_;
+ const Generator4 g4_;
+ const Generator5 g5_;
+ const Generator6 g6_;
+ const Generator7 g7_;
+ const Generator8 g8_;
+ const Generator9 g9_;
+}; // class CartesianProductHolder9
+
+template <class Generator1, class Generator2, class Generator3,
+ class Generator4, class Generator5, class Generator6, class Generator7,
+ class Generator8, class Generator9, class Generator10>
+class CartesianProductHolder10 {
+ public:
+CartesianProductHolder10(const Generator1& g1, const Generator2& g2,
+ const Generator3& g3, const Generator4& g4, const Generator5& g5,
+ const Generator6& g6, const Generator7& g7, const Generator8& g8,
+ const Generator9& g9, const Generator10& g10)
+ : g1_(g1), g2_(g2), g3_(g3), g4_(g4), g5_(g5), g6_(g6), g7_(g7), g8_(g8),
+ g9_(g9), g10_(g10) {}
+ template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10>
+ operator ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7, T8,
+ T9, T10> >() const {
+ return ParamGenerator< ::std::tr1::tuple<T1, T2, T3, T4, T5, T6, T7, T8,
+ T9, T10> >(
+ new CartesianProductGenerator10<T1, T2, T3, T4, T5, T6, T7, T8, T9,
+ T10>(
+ static_cast<ParamGenerator<T1> >(g1_),
+ static_cast<ParamGenerator<T2> >(g2_),
+ static_cast<ParamGenerator<T3> >(g3_),
+ static_cast<ParamGenerator<T4> >(g4_),
+ static_cast<ParamGenerator<T5> >(g5_),
+ static_cast<ParamGenerator<T6> >(g6_),
+ static_cast<ParamGenerator<T7> >(g7_),
+ static_cast<ParamGenerator<T8> >(g8_),
+ static_cast<ParamGenerator<T9> >(g9_),
+ static_cast<ParamGenerator<T10> >(g10_)));
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductHolder10& other);
+
+ const Generator1 g1_;
+ const Generator2 g2_;
+ const Generator3 g3_;
+ const Generator4 g4_;
+ const Generator5 g5_;
+ const Generator6 g6_;
+ const Generator7 g7_;
+ const Generator8 g8_;
+ const Generator9 g9_;
+ const Generator10 g10_;
+}; // class CartesianProductHolder10
+
+# endif // GTEST_HAS_COMBINE
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_HAS_PARAM_TEST
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PARAM_UTIL_GENERATED_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-param-util-generated.h.pump b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-param-util-generated.h.pump
new file mode 100644
index 0000000..009206f
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-param-util-generated.h.pump
@@ -0,0 +1,301 @@
+$$ -*- mode: c++; -*-
+$var n = 50 $$ Maximum length of Values arguments we want to support.
+$var maxtuple = 10 $$ Maximum number of Combine arguments we want to support.
+// Copyright 2008 Google Inc.
+// All Rights Reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vladl at google.com (Vlad Losev)
+
+// Type and function utilities for implementing parameterized tests.
+// This file is generated by a SCRIPT. DO NOT EDIT BY HAND!
+//
+// Currently Google Test supports at most $n arguments in Values,
+// and at most $maxtuple arguments in Combine. Please contact
+// googletestframework at googlegroups.com if you need more.
+// Please note that the number of arguments to Combine is limited
+// by the maximum arity of the implementation of tr1::tuple which is
+// currently set at $maxtuple.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PARAM_UTIL_GENERATED_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PARAM_UTIL_GENERATED_H_
+
+// scripts/fuse_gtest.py depends on gtest's own header being #included
+// *unconditionally*. Therefore these #includes cannot be moved
+// inside #if GTEST_HAS_PARAM_TEST.
+#include "gtest/internal/gtest-param-util.h"
+#include "gtest/internal/gtest-port.h"
+
+#if GTEST_HAS_PARAM_TEST
+
+namespace testing {
+
+// Forward declarations of ValuesIn(), which is implemented in
+// include/gtest/gtest-param-test.h.
+template <typename ForwardIterator>
+internal::ParamGenerator<
+ typename ::testing::internal::IteratorTraits<ForwardIterator>::value_type>
+ValuesIn(ForwardIterator begin, ForwardIterator end);
+
+template <typename T, size_t N>
+internal::ParamGenerator<T> ValuesIn(const T (&array)[N]);
+
+template <class Container>
+internal::ParamGenerator<typename Container::value_type> ValuesIn(
+ const Container& container);
+
+namespace internal {
+
+// Used in the Values() function to provide polymorphic capabilities.
+template <typename T1>
+class ValueArray1 {
+ public:
+ explicit ValueArray1(T1 v1) : v1_(v1) {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const { return ValuesIn(&v1_, &v1_ + 1); }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray1& other);
+
+ const T1 v1_;
+};
+
+$range i 2..n
+$for i [[
+$range j 1..i
+
+template <$for j, [[typename T$j]]>
+class ValueArray$i {
+ public:
+ ValueArray$i($for j, [[T$j v$j]]) : $for j, [[v$(j)_(v$j)]] {}
+
+ template <typename T>
+ operator ParamGenerator<T>() const {
+ const T array[] = {$for j, [[static_cast<T>(v$(j)_)]]};
+ return ValuesIn(array);
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const ValueArray$i& other);
+
+$for j [[
+
+ const T$j v$(j)_;
+]]
+
+};
+
+]]
+
+# if GTEST_HAS_COMBINE
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// Generates values from the Cartesian product of values produced
+// by the argument generators.
+//
+$range i 2..maxtuple
+$for i [[
+$range j 1..i
+$range k 2..i
+
+template <$for j, [[typename T$j]]>
+class CartesianProductGenerator$i
+ : public ParamGeneratorInterface< ::std::tr1::tuple<$for j, [[T$j]]> > {
+ public:
+ typedef ::std::tr1::tuple<$for j, [[T$j]]> ParamType;
+
+ CartesianProductGenerator$i($for j, [[const ParamGenerator<T$j>& g$j]])
+ : $for j, [[g$(j)_(g$j)]] {}
+ virtual ~CartesianProductGenerator$i() {}
+
+ virtual ParamIteratorInterface<ParamType>* Begin() const {
+ return new Iterator(this, $for j, [[g$(j)_, g$(j)_.begin()]]);
+ }
+ virtual ParamIteratorInterface<ParamType>* End() const {
+ return new Iterator(this, $for j, [[g$(j)_, g$(j)_.end()]]);
+ }
+
+ private:
+ class Iterator : public ParamIteratorInterface<ParamType> {
+ public:
+ Iterator(const ParamGeneratorInterface<ParamType>* base, $for j, [[
+
+ const ParamGenerator<T$j>& g$j,
+ const typename ParamGenerator<T$j>::iterator& current$(j)]])
+ : base_(base),
+$for j, [[
+
+ begin$(j)_(g$j.begin()), end$(j)_(g$j.end()), current$(j)_(current$j)
+]] {
+ ComputeCurrentValue();
+ }
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<ParamType>* BaseGenerator() const {
+ return base_;
+ }
+ // Advance should not be called on beyond-of-range iterators
+ // so no component iterators must be beyond end of range, either.
+ virtual void Advance() {
+ assert(!AtEnd());
+ ++current$(i)_;
+
+$for k [[
+ if (current$(i+2-k)_ == end$(i+2-k)_) {
+ current$(i+2-k)_ = begin$(i+2-k)_;
+ ++current$(i+2-k-1)_;
+ }
+
+]]
+ ComputeCurrentValue();
+ }
+ virtual ParamIteratorInterface<ParamType>* Clone() const {
+ return new Iterator(*this);
+ }
+ virtual const ParamType* Current() const { return ¤t_value_; }
+ virtual bool Equals(const ParamIteratorInterface<ParamType>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ const Iterator* typed_other =
+ CheckedDowncastToActualType<const Iterator>(&other);
+ // We must report iterators equal if they both point beyond their
+ // respective ranges. That can happen in a variety of fashions,
+ // so we have to consult AtEnd().
+ return (AtEnd() && typed_other->AtEnd()) ||
+ ($for j && [[
+
+ current$(j)_ == typed_other->current$(j)_
+]]);
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ : base_(other.base_), $for j, [[
+
+ begin$(j)_(other.begin$(j)_),
+ end$(j)_(other.end$(j)_),
+ current$(j)_(other.current$(j)_)
+]] {
+ ComputeCurrentValue();
+ }
+
+ void ComputeCurrentValue() {
+ if (!AtEnd())
+ current_value_ = ParamType($for j, [[*current$(j)_]]);
+ }
+ bool AtEnd() const {
+ // We must report iterator past the end of the range when either of the
+ // component iterators has reached the end of its range.
+ return
+$for j || [[
+
+ current$(j)_ == end$(j)_
+]];
+ }
+
+ // No implementation - assignment is unsupported.
+ void operator=(const Iterator& other);
+
+ const ParamGeneratorInterface<ParamType>* const base_;
+ // begin[i]_ and end[i]_ define the i-th range that Iterator traverses.
+ // current[i]_ is the actual traversing iterator.
+$for j [[
+
+ const typename ParamGenerator<T$j>::iterator begin$(j)_;
+ const typename ParamGenerator<T$j>::iterator end$(j)_;
+ typename ParamGenerator<T$j>::iterator current$(j)_;
+]]
+
+ ParamType current_value_;
+ }; // class CartesianProductGenerator$i::Iterator
+
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductGenerator$i& other);
+
+
+$for j [[
+ const ParamGenerator<T$j> g$(j)_;
+
+]]
+}; // class CartesianProductGenerator$i
+
+
+]]
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// Helper classes providing Combine() with polymorphic features. They allow
+// casting CartesianProductGeneratorN<T> to ParamGenerator<U> if T is
+// convertible to U.
+//
+$range i 2..maxtuple
+$for i [[
+$range j 1..i
+
+template <$for j, [[class Generator$j]]>
+class CartesianProductHolder$i {
+ public:
+CartesianProductHolder$i($for j, [[const Generator$j& g$j]])
+ : $for j, [[g$(j)_(g$j)]] {}
+ template <$for j, [[typename T$j]]>
+ operator ParamGenerator< ::std::tr1::tuple<$for j, [[T$j]]> >() const {
+ return ParamGenerator< ::std::tr1::tuple<$for j, [[T$j]]> >(
+ new CartesianProductGenerator$i<$for j, [[T$j]]>(
+$for j,[[
+
+ static_cast<ParamGenerator<T$j> >(g$(j)_)
+]]));
+ }
+
+ private:
+ // No implementation - assignment is unsupported.
+ void operator=(const CartesianProductHolder$i& other);
+
+
+$for j [[
+ const Generator$j g$(j)_;
+
+]]
+}; // class CartesianProductHolder$i
+
+]]
+
+# endif // GTEST_HAS_COMBINE
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_HAS_PARAM_TEST
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PARAM_UTIL_GENERATED_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-param-util.h b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-param-util.h
new file mode 100644
index 0000000..d5e1028
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-param-util.h
@@ -0,0 +1,619 @@
+// Copyright 2008 Google Inc.
+// All Rights Reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vladl at google.com (Vlad Losev)
+
+// Type and function utilities for implementing parameterized tests.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PARAM_UTIL_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PARAM_UTIL_H_
+
+#include <iterator>
+#include <utility>
+#include <vector>
+
+// scripts/fuse_gtest.py depends on gtest's own header being #included
+// *unconditionally*. Therefore these #includes cannot be moved
+// inside #if GTEST_HAS_PARAM_TEST.
+#include "gtest/internal/gtest-internal.h"
+#include "gtest/internal/gtest-linked_ptr.h"
+#include "gtest/internal/gtest-port.h"
+#include "gtest/gtest-printers.h"
+
+#if GTEST_HAS_PARAM_TEST
+
+namespace testing {
+namespace internal {
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// Outputs a message explaining invalid registration of different
+// fixture class for the same test case. This may happen when
+// TEST_P macro is used to define two tests with the same name
+// but in different namespaces.
+GTEST_API_ void ReportInvalidTestCaseType(const char* test_case_name,
+ const char* file, int line);
+
+template <typename> class ParamGeneratorInterface;
+template <typename> class ParamGenerator;
+
+// Interface for iterating over elements provided by an implementation
+// of ParamGeneratorInterface<T>.
+template <typename T>
+class ParamIteratorInterface {
+ public:
+ virtual ~ParamIteratorInterface() {}
+ // A pointer to the base generator instance.
+ // Used only for the purposes of iterator comparison
+ // to make sure that two iterators belong to the same generator.
+ virtual const ParamGeneratorInterface<T>* BaseGenerator() const = 0;
+ // Advances iterator to point to the next element
+ // provided by the generator. The caller is responsible
+ // for not calling Advance() on an iterator equal to
+ // BaseGenerator()->End().
+ virtual void Advance() = 0;
+ // Clones the iterator object. Used for implementing copy semantics
+ // of ParamIterator<T>.
+ virtual ParamIteratorInterface* Clone() const = 0;
+ // Dereferences the current iterator and provides (read-only) access
+ // to the pointed value. It is the caller's responsibility not to call
+ // Current() on an iterator equal to BaseGenerator()->End().
+ // Used for implementing ParamGenerator<T>::operator*().
+ virtual const T* Current() const = 0;
+ // Determines whether the given iterator and other point to the same
+ // element in the sequence generated by the generator.
+ // Used for implementing ParamGenerator<T>::operator==().
+ virtual bool Equals(const ParamIteratorInterface& other) const = 0;
+};
+
+// Class iterating over elements provided by an implementation of
+// ParamGeneratorInterface<T>. It wraps ParamIteratorInterface<T>
+// and implements the const forward iterator concept.
+template <typename T>
+class ParamIterator {
+ public:
+ typedef T value_type;
+ typedef const T& reference;
+ typedef ptrdiff_t difference_type;
+
+ // ParamIterator assumes ownership of the impl_ pointer.
+ ParamIterator(const ParamIterator& other) : impl_(other.impl_->Clone()) {}
+ ParamIterator& operator=(const ParamIterator& other) {
+ if (this != &other)
+ impl_.reset(other.impl_->Clone());
+ return *this;
+ }
+
+ const T& operator*() const { return *impl_->Current(); }
+ const T* operator->() const { return impl_->Current(); }
+ // Prefix version of operator++.
+ ParamIterator& operator++() {
+ impl_->Advance();
+ return *this;
+ }
+ // Postfix version of operator++.
+ ParamIterator operator++(int /*unused*/) {
+ ParamIteratorInterface<T>* clone = impl_->Clone();
+ impl_->Advance();
+ return ParamIterator(clone);
+ }
+ bool operator==(const ParamIterator& other) const {
+ return impl_.get() == other.impl_.get() || impl_->Equals(*other.impl_);
+ }
+ bool operator!=(const ParamIterator& other) const {
+ return !(*this == other);
+ }
+
+ private:
+ friend class ParamGenerator<T>;
+ explicit ParamIterator(ParamIteratorInterface<T>* impl) : impl_(impl) {}
+ scoped_ptr<ParamIteratorInterface<T> > impl_;
+};
+
+// ParamGeneratorInterface<T> is the binary interface to access generators
+// defined in other translation units.
+template <typename T>
+class ParamGeneratorInterface {
+ public:
+ typedef T ParamType;
+
+ virtual ~ParamGeneratorInterface() {}
+
+ // Generator interface definition
+ virtual ParamIteratorInterface<T>* Begin() const = 0;
+ virtual ParamIteratorInterface<T>* End() const = 0;
+};
+
+// Wraps ParamGeneratorInterface<T> and provides general generator syntax
+// compatible with the STL Container concept.
+// This class implements copy initialization semantics and the contained
+// ParamGeneratorInterface<T> instance is shared among all copies
+// of the original object. This is possible because that instance is immutable.
+template<typename T>
+class ParamGenerator {
+ public:
+ typedef ParamIterator<T> iterator;
+
+ explicit ParamGenerator(ParamGeneratorInterface<T>* impl) : impl_(impl) {}
+ ParamGenerator(const ParamGenerator& other) : impl_(other.impl_) {}
+
+ ParamGenerator& operator=(const ParamGenerator& other) {
+ impl_ = other.impl_;
+ return *this;
+ }
+
+ iterator begin() const { return iterator(impl_->Begin()); }
+ iterator end() const { return iterator(impl_->End()); }
+
+ private:
+ linked_ptr<const ParamGeneratorInterface<T> > impl_;
+};
+
+// Generates values from a range of two comparable values. Can be used to
+// generate sequences of user-defined types that implement operator+() and
+// operator<().
+// This class is used in the Range() function.
+template <typename T, typename IncrementT>
+class RangeGenerator : public ParamGeneratorInterface<T> {
+ public:
+ RangeGenerator(T begin, T end, IncrementT step)
+ : begin_(begin), end_(end),
+ step_(step), end_index_(CalculateEndIndex(begin, end, step)) {}
+ virtual ~RangeGenerator() {}
+
+ virtual ParamIteratorInterface<T>* Begin() const {
+ return new Iterator(this, begin_, 0, step_);
+ }
+ virtual ParamIteratorInterface<T>* End() const {
+ return new Iterator(this, end_, end_index_, step_);
+ }
+
+ private:
+ class Iterator : public ParamIteratorInterface<T> {
+ public:
+ Iterator(const ParamGeneratorInterface<T>* base, T value, int index,
+ IncrementT step)
+ : base_(base), value_(value), index_(index), step_(step) {}
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<T>* BaseGenerator() const {
+ return base_;
+ }
+ virtual void Advance() {
+ value_ = value_ + step_;
+ index_++;
+ }
+ virtual ParamIteratorInterface<T>* Clone() const {
+ return new Iterator(*this);
+ }
+ virtual const T* Current() const { return &value_; }
+ virtual bool Equals(const ParamIteratorInterface<T>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ const int other_index =
+ CheckedDowncastToActualType<const Iterator>(&other)->index_;
+ return index_ == other_index;
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ : ParamIteratorInterface<T>(),
+ base_(other.base_), value_(other.value_), index_(other.index_),
+ step_(other.step_) {}
+
+ // No implementation - assignment is unsupported.
+ void operator=(const Iterator& other);
+
+ const ParamGeneratorInterface<T>* const base_;
+ T value_;
+ int index_;
+ const IncrementT step_;
+ }; // class RangeGenerator::Iterator
+
+ static int CalculateEndIndex(const T& begin,
+ const T& end,
+ const IncrementT& step) {
+ int end_index = 0;
+ for (T i = begin; i < end; i = i + step)
+ end_index++;
+ return end_index;
+ }
+
+ // No implementation - assignment is unsupported.
+ void operator=(const RangeGenerator& other);
+
+ const T begin_;
+ const T end_;
+ const IncrementT step_;
+ // The index for the end() iterator. All the elements in the generated
+ // sequence are indexed (0-based) to aid iterator comparison.
+ const int end_index_;
+}; // class RangeGenerator
+
+
+// Generates values from a pair of STL-style iterators. Used in the
+// ValuesIn() function. The elements are copied from the source range
+// since the source can be located on the stack, and the generator
+// is likely to persist beyond that stack frame.
+template <typename T>
+class ValuesInIteratorRangeGenerator : public ParamGeneratorInterface<T> {
+ public:
+ template <typename ForwardIterator>
+ ValuesInIteratorRangeGenerator(ForwardIterator begin, ForwardIterator end)
+ : container_(begin, end) {}
+ virtual ~ValuesInIteratorRangeGenerator() {}
+
+ virtual ParamIteratorInterface<T>* Begin() const {
+ return new Iterator(this, container_.begin());
+ }
+ virtual ParamIteratorInterface<T>* End() const {
+ return new Iterator(this, container_.end());
+ }
+
+ private:
+ typedef typename ::std::vector<T> ContainerType;
+
+ class Iterator : public ParamIteratorInterface<T> {
+ public:
+ Iterator(const ParamGeneratorInterface<T>* base,
+ typename ContainerType::const_iterator iterator)
+ : base_(base), iterator_(iterator) {}
+ virtual ~Iterator() {}
+
+ virtual const ParamGeneratorInterface<T>* BaseGenerator() const {
+ return base_;
+ }
+ virtual void Advance() {
+ ++iterator_;
+ value_.reset();
+ }
+ virtual ParamIteratorInterface<T>* Clone() const {
+ return new Iterator(*this);
+ }
+ // We need to use cached value referenced by iterator_ because *iterator_
+ // can return a temporary object (and of type other then T), so just
+ // having "return &*iterator_;" doesn't work.
+ // value_ is updated here and not in Advance() because Advance()
+ // can advance iterator_ beyond the end of the range, and we cannot
+ // detect that fact. The client code, on the other hand, is
+ // responsible for not calling Current() on an out-of-range iterator.
+ virtual const T* Current() const {
+ if (value_.get() == NULL)
+ value_.reset(new T(*iterator_));
+ return value_.get();
+ }
+ virtual bool Equals(const ParamIteratorInterface<T>& other) const {
+ // Having the same base generator guarantees that the other
+ // iterator is of the same type and we can downcast.
+ GTEST_CHECK_(BaseGenerator() == other.BaseGenerator())
+ << "The program attempted to compare iterators "
+ << "from different generators." << std::endl;
+ return iterator_ ==
+ CheckedDowncastToActualType<const Iterator>(&other)->iterator_;
+ }
+
+ private:
+ Iterator(const Iterator& other)
+ // The explicit constructor call suppresses a false warning
+ // emitted by gcc when supplied with the -Wextra option.
+ : ParamIteratorInterface<T>(),
+ base_(other.base_),
+ iterator_(other.iterator_) {}
+
+ const ParamGeneratorInterface<T>* const base_;
+ typename ContainerType::const_iterator iterator_;
+ // A cached value of *iterator_. We keep it here to allow access by
+ // pointer in the wrapping iterator's operator->().
+ // value_ needs to be mutable to be accessed in Current().
+ // Use of scoped_ptr helps manage cached value's lifetime,
+ // which is bound by the lifespan of the iterator itself.
+ mutable scoped_ptr<const T> value_;
+ }; // class ValuesInIteratorRangeGenerator::Iterator
+
+ // No implementation - assignment is unsupported.
+ void operator=(const ValuesInIteratorRangeGenerator& other);
+
+ const ContainerType container_;
+}; // class ValuesInIteratorRangeGenerator
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// Stores a parameter value and later creates tests parameterized with that
+// value.
+template <class TestClass>
+class ParameterizedTestFactory : public TestFactoryBase {
+ public:
+ typedef typename TestClass::ParamType ParamType;
+ explicit ParameterizedTestFactory(ParamType parameter) :
+ parameter_(parameter) {}
+ virtual Test* CreateTest() {
+ TestClass::SetParam(¶meter_);
+ return new TestClass();
+ }
+
+ private:
+ const ParamType parameter_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ParameterizedTestFactory);
+};
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// TestMetaFactoryBase is a base class for meta-factories that create
+// test factories for passing into MakeAndRegisterTestInfo function.
+template <class ParamType>
+class TestMetaFactoryBase {
+ public:
+ virtual ~TestMetaFactoryBase() {}
+
+ virtual TestFactoryBase* CreateTestFactory(ParamType parameter) = 0;
+};
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// TestMetaFactory creates test factories for passing into
+// MakeAndRegisterTestInfo function. Since MakeAndRegisterTestInfo receives
+// ownership of test factory pointer, same factory object cannot be passed
+// into that method twice. But ParameterizedTestCaseInfo is going to call
+// it for each Test/Parameter value combination. Thus it needs meta factory
+// creator class.
+template <class TestCase>
+class TestMetaFactory
+ : public TestMetaFactoryBase<typename TestCase::ParamType> {
+ public:
+ typedef typename TestCase::ParamType ParamType;
+
+ TestMetaFactory() {}
+
+ virtual TestFactoryBase* CreateTestFactory(ParamType parameter) {
+ return new ParameterizedTestFactory<TestCase>(parameter);
+ }
+
+ private:
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(TestMetaFactory);
+};
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// ParameterizedTestCaseInfoBase is a generic interface
+// to ParameterizedTestCaseInfo classes. ParameterizedTestCaseInfoBase
+// accumulates test information provided by TEST_P macro invocations
+// and generators provided by INSTANTIATE_TEST_CASE_P macro invocations
+// and uses that information to register all resulting test instances
+// in RegisterTests method. The ParameterizeTestCaseRegistry class holds
+// a collection of pointers to the ParameterizedTestCaseInfo objects
+// and calls RegisterTests() on each of them when asked.
+class ParameterizedTestCaseInfoBase {
+ public:
+ virtual ~ParameterizedTestCaseInfoBase() {}
+
+ // Base part of test case name for display purposes.
+ virtual const string& GetTestCaseName() const = 0;
+ // Test case id to verify identity.
+ virtual TypeId GetTestCaseTypeId() const = 0;
+ // UnitTest class invokes this method to register tests in this
+ // test case right before running them in RUN_ALL_TESTS macro.
+ // This method should not be called more then once on any single
+ // instance of a ParameterizedTestCaseInfoBase derived class.
+ virtual void RegisterTests() = 0;
+
+ protected:
+ ParameterizedTestCaseInfoBase() {}
+
+ private:
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ParameterizedTestCaseInfoBase);
+};
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// ParameterizedTestCaseInfo accumulates tests obtained from TEST_P
+// macro invocations for a particular test case and generators
+// obtained from INSTANTIATE_TEST_CASE_P macro invocations for that
+// test case. It registers tests with all values generated by all
+// generators when asked.
+template <class TestCase>
+class ParameterizedTestCaseInfo : public ParameterizedTestCaseInfoBase {
+ public:
+ // ParamType and GeneratorCreationFunc are private types but are required
+ // for declarations of public methods AddTestPattern() and
+ // AddTestCaseInstantiation().
+ typedef typename TestCase::ParamType ParamType;
+ // A function that returns an instance of appropriate generator type.
+ typedef ParamGenerator<ParamType>(GeneratorCreationFunc)();
+
+ explicit ParameterizedTestCaseInfo(const char* name)
+ : test_case_name_(name) {}
+
+ // Test case base name for display purposes.
+ virtual const string& GetTestCaseName() const { return test_case_name_; }
+ // Test case id to verify identity.
+ virtual TypeId GetTestCaseTypeId() const { return GetTypeId<TestCase>(); }
+ // TEST_P macro uses AddTestPattern() to record information
+ // about a single test in a LocalTestInfo structure.
+ // test_case_name is the base name of the test case (without invocation
+ // prefix). test_base_name is the name of an individual test without
+ // parameter index. For the test SequenceA/FooTest.DoBar/1 FooTest is
+ // test case base name and DoBar is test base name.
+ void AddTestPattern(const char* test_case_name,
+ const char* test_base_name,
+ TestMetaFactoryBase<ParamType>* meta_factory) {
+ tests_.push_back(linked_ptr<TestInfo>(new TestInfo(test_case_name,
+ test_base_name,
+ meta_factory)));
+ }
+ // INSTANTIATE_TEST_CASE_P macro uses AddGenerator() to record information
+ // about a generator.
+ int AddTestCaseInstantiation(const string& instantiation_name,
+ GeneratorCreationFunc* func,
+ const char* /* file */,
+ int /* line */) {
+ instantiations_.push_back(::std::make_pair(instantiation_name, func));
+ return 0; // Return value used only to run this method in namespace scope.
+ }
+ // UnitTest class invokes this method to register tests in this test case
+ // test cases right before running tests in RUN_ALL_TESTS macro.
+ // This method should not be called more then once on any single
+ // instance of a ParameterizedTestCaseInfoBase derived class.
+ // UnitTest has a guard to prevent from calling this method more then once.
+ virtual void RegisterTests() {
+ for (typename TestInfoContainer::iterator test_it = tests_.begin();
+ test_it != tests_.end(); ++test_it) {
+ linked_ptr<TestInfo> test_info = *test_it;
+ for (typename InstantiationContainer::iterator gen_it =
+ instantiations_.begin(); gen_it != instantiations_.end();
+ ++gen_it) {
+ const string& instantiation_name = gen_it->first;
+ ParamGenerator<ParamType> generator((*gen_it->second)());
+
+ string test_case_name;
+ if ( !instantiation_name.empty() )
+ test_case_name = instantiation_name + "/";
+ test_case_name += test_info->test_case_base_name;
+
+ int i = 0;
+ for (typename ParamGenerator<ParamType>::iterator param_it =
+ generator.begin();
+ param_it != generator.end(); ++param_it, ++i) {
+ Message test_name_stream;
+ test_name_stream << test_info->test_base_name << "/" << i;
+ MakeAndRegisterTestInfo(
+ test_case_name.c_str(),
+ test_name_stream.GetString().c_str(),
+ NULL, // No type parameter.
+ PrintToString(*param_it).c_str(),
+ GetTestCaseTypeId(),
+ TestCase::SetUpTestCase,
+ TestCase::TearDownTestCase,
+ test_info->test_meta_factory->CreateTestFactory(*param_it));
+ } // for param_it
+ } // for gen_it
+ } // for test_it
+ } // RegisterTests
+
+ private:
+ // LocalTestInfo structure keeps information about a single test registered
+ // with TEST_P macro.
+ struct TestInfo {
+ TestInfo(const char* a_test_case_base_name,
+ const char* a_test_base_name,
+ TestMetaFactoryBase<ParamType>* a_test_meta_factory) :
+ test_case_base_name(a_test_case_base_name),
+ test_base_name(a_test_base_name),
+ test_meta_factory(a_test_meta_factory) {}
+
+ const string test_case_base_name;
+ const string test_base_name;
+ const scoped_ptr<TestMetaFactoryBase<ParamType> > test_meta_factory;
+ };
+ typedef ::std::vector<linked_ptr<TestInfo> > TestInfoContainer;
+ // Keeps pairs of <Instantiation name, Sequence generator creation function>
+ // received from INSTANTIATE_TEST_CASE_P macros.
+ typedef ::std::vector<std::pair<string, GeneratorCreationFunc*> >
+ InstantiationContainer;
+
+ const string test_case_name_;
+ TestInfoContainer tests_;
+ InstantiationContainer instantiations_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ParameterizedTestCaseInfo);
+}; // class ParameterizedTestCaseInfo
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// ParameterizedTestCaseRegistry contains a map of ParameterizedTestCaseInfoBase
+// classes accessed by test case names. TEST_P and INSTANTIATE_TEST_CASE_P
+// macros use it to locate their corresponding ParameterizedTestCaseInfo
+// descriptors.
+class ParameterizedTestCaseRegistry {
+ public:
+ ParameterizedTestCaseRegistry() {}
+ ~ParameterizedTestCaseRegistry() {
+ for (TestCaseInfoContainer::iterator it = test_case_infos_.begin();
+ it != test_case_infos_.end(); ++it) {
+ delete *it;
+ }
+ }
+
+ // Looks up or creates and returns a structure containing information about
+ // tests and instantiations of a particular test case.
+ template <class TestCase>
+ ParameterizedTestCaseInfo<TestCase>* GetTestCasePatternHolder(
+ const char* test_case_name,
+ const char* file,
+ int line) {
+ ParameterizedTestCaseInfo<TestCase>* typed_test_info = NULL;
+ for (TestCaseInfoContainer::iterator it = test_case_infos_.begin();
+ it != test_case_infos_.end(); ++it) {
+ if ((*it)->GetTestCaseName() == test_case_name) {
+ if ((*it)->GetTestCaseTypeId() != GetTypeId<TestCase>()) {
+ // Complain about incorrect usage of Google Test facilities
+ // and terminate the program since we cannot guaranty correct
+ // test case setup and tear-down in this case.
+ ReportInvalidTestCaseType(test_case_name, file, line);
+ posix::Abort();
+ } else {
+ // At this point we are sure that the object we found is of the same
+ // type we are looking for, so we downcast it to that type
+ // without further checks.
+ typed_test_info = CheckedDowncastToActualType<
+ ParameterizedTestCaseInfo<TestCase> >(*it);
+ }
+ break;
+ }
+ }
+ if (typed_test_info == NULL) {
+ typed_test_info = new ParameterizedTestCaseInfo<TestCase>(test_case_name);
+ test_case_infos_.push_back(typed_test_info);
+ }
+ return typed_test_info;
+ }
+ void RegisterTests() {
+ for (TestCaseInfoContainer::iterator it = test_case_infos_.begin();
+ it != test_case_infos_.end(); ++it) {
+ (*it)->RegisterTests();
+ }
+ }
+
+ private:
+ typedef ::std::vector<ParameterizedTestCaseInfoBase*> TestCaseInfoContainer;
+
+ TestCaseInfoContainer test_case_infos_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ParameterizedTestCaseRegistry);
+};
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_HAS_PARAM_TEST
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PARAM_UTIL_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-port.h b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-port.h
new file mode 100644
index 0000000..ab40df1
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-port.h
@@ -0,0 +1,1948 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Authors: wan at google.com (Zhanyong Wan)
+//
+// Low-level types and utilities for porting Google Test to various
+// platforms. They are subject to change without notice. DO NOT USE
+// THEM IN USER CODE.
+//
+// This file is fundamental to Google Test. All other Google Test source
+// files are expected to #include this. Therefore, it cannot #include
+// any other Google Test header.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PORT_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PORT_H_
+#pragma GCC system_header
+
+// The user can define the following macros in the build script to
+// control Google Test's behavior. If the user doesn't define a macro
+// in this list, Google Test will define it.
+//
+// GTEST_HAS_CLONE - Define it to 1/0 to indicate that clone(2)
+// is/isn't available.
+// GTEST_HAS_EXCEPTIONS - Define it to 1/0 to indicate that exceptions
+// are enabled.
+// GTEST_HAS_GLOBAL_STRING - Define it to 1/0 to indicate that ::string
+// is/isn't available (some systems define
+// ::string, which is different to std::string).
+// GTEST_HAS_GLOBAL_WSTRING - Define it to 1/0 to indicate that ::string
+// is/isn't available (some systems define
+// ::wstring, which is different to std::wstring).
+// GTEST_HAS_POSIX_RE - Define it to 1/0 to indicate that POSIX regular
+// expressions are/aren't available.
+// GTEST_HAS_PTHREAD - Define it to 1/0 to indicate that <pthread.h>
+// is/isn't available.
+// GTEST_HAS_RTTI - Define it to 1/0 to indicate that RTTI is/isn't
+// enabled.
+// GTEST_HAS_STD_WSTRING - Define it to 1/0 to indicate that
+// std::wstring does/doesn't work (Google Test can
+// be used where std::wstring is unavailable).
+// GTEST_HAS_TR1_TUPLE - Define it to 1/0 to indicate tr1::tuple
+// is/isn't available.
+// GTEST_HAS_SEH - Define it to 1/0 to indicate whether the
+// compiler supports Microsoft's "Structured
+// Exception Handling".
+// GTEST_HAS_STREAM_REDIRECTION
+// - Define it to 1/0 to indicate whether the
+// platform supports I/O stream redirection using
+// dup() and dup2().
+// GTEST_USE_OWN_TR1_TUPLE - Define it to 1/0 to indicate whether Google
+// Test's own tr1 tuple implementation should be
+// used. Unused when the user sets
+// GTEST_HAS_TR1_TUPLE to 0.
+// GTEST_LANG_CXX11 - Define it to 1/0 to indicate that Google Test
+// is building in C++11/C++98 mode.
+// GTEST_LINKED_AS_SHARED_LIBRARY
+// - Define to 1 when compiling tests that use
+// Google Test as a shared library (known as
+// DLL on Windows).
+// GTEST_CREATE_SHARED_LIBRARY
+// - Define to 1 when compiling Google Test itself
+// as a shared library.
+
+// This header defines the following utilities:
+//
+// Macros indicating the current platform (defined to 1 if compiled on
+// the given platform; otherwise undefined):
+// GTEST_OS_AIX - IBM AIX
+// GTEST_OS_CYGWIN - Cygwin
+// GTEST_OS_HPUX - HP-UX
+// GTEST_OS_LINUX - Linux
+// GTEST_OS_LINUX_ANDROID - Google Android
+// GTEST_OS_MAC - Mac OS X
+// GTEST_OS_IOS - iOS
+// GTEST_OS_IOS_SIMULATOR - iOS simulator
+// GTEST_OS_NACL - Google Native Client (NaCl)
+// GTEST_OS_OPENBSD - OpenBSD
+// GTEST_OS_QNX - QNX
+// GTEST_OS_SOLARIS - Sun Solaris
+// GTEST_OS_SYMBIAN - Symbian
+// GTEST_OS_WINDOWS - Windows (Desktop, MinGW, or Mobile)
+// GTEST_OS_WINDOWS_DESKTOP - Windows Desktop
+// GTEST_OS_WINDOWS_MINGW - MinGW
+// GTEST_OS_WINDOWS_MOBILE - Windows Mobile
+// GTEST_OS_ZOS - z/OS
+//
+// Among the platforms, Cygwin, Linux, Max OS X, and Windows have the
+// most stable support. Since core members of the Google Test project
+// don't have access to other platforms, support for them may be less
+// stable. If you notice any problems on your platform, please notify
+// googletestframework at googlegroups.com (patches for fixing them are
+// even more welcome!).
+//
+// Note that it is possible that none of the GTEST_OS_* macros are defined.
+//
+// Macros indicating available Google Test features (defined to 1 if
+// the corresponding feature is supported; otherwise undefined):
+// GTEST_HAS_COMBINE - the Combine() function (for value-parameterized
+// tests)
+// GTEST_HAS_DEATH_TEST - death tests
+// GTEST_HAS_PARAM_TEST - value-parameterized tests
+// GTEST_HAS_TYPED_TEST - typed tests
+// GTEST_HAS_TYPED_TEST_P - type-parameterized tests
+// GTEST_USES_POSIX_RE - enhanced POSIX regex is used. Do not confuse with
+// GTEST_HAS_POSIX_RE (see above) which users can
+// define themselves.
+// GTEST_USES_SIMPLE_RE - our own simple regex is used;
+// the above two are mutually exclusive.
+// GTEST_CAN_COMPARE_NULL - accepts untyped NULL in EXPECT_EQ().
+//
+// Macros for basic C++ coding:
+// GTEST_AMBIGUOUS_ELSE_BLOCKER_ - for disabling a gcc warning.
+// GTEST_ATTRIBUTE_UNUSED_ - declares that a class' instances or a
+// variable don't have to be used.
+// GTEST_DISALLOW_ASSIGN_ - disables operator=.
+// GTEST_DISALLOW_COPY_AND_ASSIGN_ - disables copy ctor and operator=.
+// GTEST_MUST_USE_RESULT_ - declares that a function's result must be used.
+//
+// Synchronization:
+// Mutex, MutexLock, ThreadLocal, GetThreadCount()
+// - synchronization primitives.
+// GTEST_IS_THREADSAFE - defined to 1 to indicate that the above
+// synchronization primitives have real implementations
+// and Google Test is thread-safe; or 0 otherwise.
+//
+// Template meta programming:
+// is_pointer - as in TR1; needed on Symbian and IBM XL C/C++ only.
+// IteratorTraits - partial implementation of std::iterator_traits, which
+// is not available in libCstd when compiled with Sun C++.
+//
+// Smart pointers:
+// scoped_ptr - as in TR2.
+//
+// Regular expressions:
+// RE - a simple regular expression class using the POSIX
+// Extended Regular Expression syntax on UNIX-like
+// platforms, or a reduced regular exception syntax on
+// other platforms, including Windows.
+//
+// Logging:
+// GTEST_LOG_() - logs messages at the specified severity level.
+// LogToStderr() - directs all log messages to stderr.
+// FlushInfoLog() - flushes informational log messages.
+//
+// Stdout and stderr capturing:
+// CaptureStdout() - starts capturing stdout.
+// GetCapturedStdout() - stops capturing stdout and returns the captured
+// string.
+// CaptureStderr() - starts capturing stderr.
+// GetCapturedStderr() - stops capturing stderr and returns the captured
+// string.
+//
+// Integer types:
+// TypeWithSize - maps an integer to a int type.
+// Int32, UInt32, Int64, UInt64, TimeInMillis
+// - integers of known sizes.
+// BiggestInt - the biggest signed integer type.
+//
+// Command-line utilities:
+// GTEST_FLAG() - references a flag.
+// GTEST_DECLARE_*() - declares a flag.
+// GTEST_DEFINE_*() - defines a flag.
+// GetInjectableArgvs() - returns the command line as a vector of strings.
+//
+// Environment variable utilities:
+// GetEnv() - gets the value of an environment variable.
+// BoolFromGTestEnv() - parses a bool environment variable.
+// Int32FromGTestEnv() - parses an Int32 environment variable.
+// StringFromGTestEnv() - parses a string environment variable.
+
+#include <ctype.h> // for isspace, etc
+#include <stddef.h> // for ptrdiff_t
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#ifndef _WIN32_WCE
+# include <sys/types.h>
+# include <sys/stat.h>
+#endif // !_WIN32_WCE
+
+#if defined __APPLE__
+# include <AvailabilityMacros.h>
+# include <TargetConditionals.h>
+#endif
+
+#include <iostream> // NOLINT
+#include <sstream> // NOLINT
+#include <string> // NOLINT
+
+#define GTEST_DEV_EMAIL_ "googletestframework@@googlegroups.com"
+#define GTEST_FLAG_PREFIX_ "gtest_"
+#define GTEST_FLAG_PREFIX_DASH_ "gtest-"
+#define GTEST_FLAG_PREFIX_UPPER_ "GTEST_"
+#define GTEST_NAME_ "Google Test"
+#define GTEST_PROJECT_URL_ "http://code.google.com/p/googletest/"
+
+// Determines the version of gcc that is used to compile this.
+#ifdef __GNUC__
+// 40302 means version 4.3.2.
+# define GTEST_GCC_VER_ \
+ (__GNUC__*10000 + __GNUC_MINOR__*100 + __GNUC_PATCHLEVEL__)
+#endif // __GNUC__
+
+// Determines the platform on which Google Test is compiled.
+#ifdef __CYGWIN__
+# define GTEST_OS_CYGWIN 1
+#elif defined __SYMBIAN32__
+# define GTEST_OS_SYMBIAN 1
+#elif defined _WIN32
+# define GTEST_OS_WINDOWS 1
+# ifdef _WIN32_WCE
+# define GTEST_OS_WINDOWS_MOBILE 1
+# elif defined(__MINGW__) || defined(__MINGW32__)
+# define GTEST_OS_WINDOWS_MINGW 1
+# else
+# define GTEST_OS_WINDOWS_DESKTOP 1
+# endif // _WIN32_WCE
+#elif defined __APPLE__
+# define GTEST_OS_MAC 1
+# if TARGET_OS_IPHONE
+# define GTEST_OS_IOS 1
+# if TARGET_IPHONE_SIMULATOR
+# define GTEST_OS_IOS_SIMULATOR 1
+# endif
+# endif
+#elif defined __linux__
+# define GTEST_OS_LINUX 1
+# if defined __ANDROID__
+# define GTEST_OS_LINUX_ANDROID 1
+# endif
+#elif defined __MVS__
+# define GTEST_OS_ZOS 1
+#elif defined(__sun) && defined(__SVR4)
+# define GTEST_OS_SOLARIS 1
+#elif defined(_AIX)
+# define GTEST_OS_AIX 1
+#elif defined(__hpux)
+# define GTEST_OS_HPUX 1
+#elif defined __native_client__
+# define GTEST_OS_NACL 1
+#elif defined __OpenBSD__
+# define GTEST_OS_OPENBSD 1
+#elif defined __QNX__
+# define GTEST_OS_QNX 1
+#endif // __CYGWIN__
+
+#ifndef GTEST_LANG_CXX11
+// gcc and clang define __GXX_EXPERIMENTAL_CXX0X__ when
+// -std={c,gnu}++{0x,11} is passed. The C++11 standard specifies a
+// value for __cplusplus, and recent versions of clang, gcc, and
+// probably other compilers set that too in C++11 mode.
+# if __GXX_EXPERIMENTAL_CXX0X__ || __cplusplus >= 201103L
+// Compiling in at least C++11 mode.
+# define GTEST_LANG_CXX11 1
+# else
+# define GTEST_LANG_CXX11 0
+# endif
+#endif
+
+// Brings in definitions for functions used in the testing::internal::posix
+// namespace (read, write, close, chdir, isatty, stat). We do not currently
+// use them on Windows Mobile.
+#if !GTEST_OS_WINDOWS
+// This assumes that non-Windows OSes provide unistd.h. For OSes where this
+// is not the case, we need to include headers that provide the functions
+// mentioned above.
+# include <unistd.h>
+# include <strings.h>
+#elif !GTEST_OS_WINDOWS_MOBILE
+# include <direct.h>
+# include <io.h>
+#endif
+
+#if GTEST_OS_LINUX_ANDROID
+// Used to define __ANDROID_API__ matching the target NDK API level.
+# include <android/api-level.h> // NOLINT
+#endif
+
+// Defines this to true iff Google Test can use POSIX regular expressions.
+#ifndef GTEST_HAS_POSIX_RE
+# if GTEST_OS_LINUX_ANDROID
+// On Android, <regex.h> is only available starting with Gingerbread.
+# define GTEST_HAS_POSIX_RE (__ANDROID_API__ >= 9)
+# else
+# define GTEST_HAS_POSIX_RE (!GTEST_OS_WINDOWS)
+# endif
+#endif
+
+#if GTEST_HAS_POSIX_RE
+
+// On some platforms, <regex.h> needs someone to define size_t, and
+// won't compile otherwise. We can #include it here as we already
+// included <stdlib.h>, which is guaranteed to define size_t through
+// <stddef.h>.
+# include <regex.h> // NOLINT
+
+# define GTEST_USES_POSIX_RE 1
+
+#elif GTEST_OS_WINDOWS
+
+// <regex.h> is not available on Windows. Use our own simple regex
+// implementation instead.
+# define GTEST_USES_SIMPLE_RE 1
+
+#else
+
+// <regex.h> may not be available on this platform. Use our own
+// simple regex implementation instead.
+# define GTEST_USES_SIMPLE_RE 1
+
+#endif // GTEST_HAS_POSIX_RE
+
+#ifndef GTEST_HAS_EXCEPTIONS
+// The user didn't tell us whether exceptions are enabled, so we need
+// to figure it out.
+# if defined(_MSC_VER) || defined(__BORLANDC__)
+// MSVC's and C++Builder's implementations of the STL use the _HAS_EXCEPTIONS
+// macro to enable exceptions, so we'll do the same.
+// Assumes that exceptions are enabled by default.
+# ifndef _HAS_EXCEPTIONS
+# define _HAS_EXCEPTIONS 1
+# endif // _HAS_EXCEPTIONS
+# define GTEST_HAS_EXCEPTIONS _HAS_EXCEPTIONS
+# elif defined(__GNUC__) && __EXCEPTIONS
+// gcc defines __EXCEPTIONS to 1 iff exceptions are enabled.
+# define GTEST_HAS_EXCEPTIONS 1
+# elif defined(__SUNPRO_CC)
+// Sun Pro CC supports exceptions. However, there is no compile-time way of
+// detecting whether they are enabled or not. Therefore, we assume that
+// they are enabled unless the user tells us otherwise.
+# define GTEST_HAS_EXCEPTIONS 1
+# elif defined(__IBMCPP__) && __EXCEPTIONS
+// xlC defines __EXCEPTIONS to 1 iff exceptions are enabled.
+# define GTEST_HAS_EXCEPTIONS 1
+# elif defined(__HP_aCC)
+// Exception handling is in effect by default in HP aCC compiler. It has to
+// be turned of by +noeh compiler option if desired.
+# define GTEST_HAS_EXCEPTIONS 1
+# else
+// For other compilers, we assume exceptions are disabled to be
+// conservative.
+# define GTEST_HAS_EXCEPTIONS 0
+# endif // defined(_MSC_VER) || defined(__BORLANDC__)
+#endif // GTEST_HAS_EXCEPTIONS
+
+#if !defined(GTEST_HAS_STD_STRING)
+// Even though we don't use this macro any longer, we keep it in case
+// some clients still depend on it.
+# define GTEST_HAS_STD_STRING 1
+#elif !GTEST_HAS_STD_STRING
+// The user told us that ::std::string isn't available.
+# error "Google Test cannot be used where ::std::string isn't available."
+#endif // !defined(GTEST_HAS_STD_STRING)
+
+#ifndef GTEST_HAS_GLOBAL_STRING
+// The user didn't tell us whether ::string is available, so we need
+// to figure it out.
+
+# define GTEST_HAS_GLOBAL_STRING 0
+
+#endif // GTEST_HAS_GLOBAL_STRING
+
+#ifndef GTEST_HAS_STD_WSTRING
+// The user didn't tell us whether ::std::wstring is available, so we need
+// to figure it out.
+// TODO(wan at google.com): uses autoconf to detect whether ::std::wstring
+// is available.
+
+// Cygwin 1.7 and below doesn't support ::std::wstring.
+// Solaris' libc++ doesn't support it either. Android has
+// no support for it at least as recent as Froyo (2.2).
+# define GTEST_HAS_STD_WSTRING \
+ (!(GTEST_OS_LINUX_ANDROID || GTEST_OS_CYGWIN || GTEST_OS_SOLARIS))
+
+#endif // GTEST_HAS_STD_WSTRING
+
+#ifndef GTEST_HAS_GLOBAL_WSTRING
+// The user didn't tell us whether ::wstring is available, so we need
+// to figure it out.
+# define GTEST_HAS_GLOBAL_WSTRING \
+ (GTEST_HAS_STD_WSTRING && GTEST_HAS_GLOBAL_STRING)
+#endif // GTEST_HAS_GLOBAL_WSTRING
+
+// Determines whether RTTI is available.
+#ifndef GTEST_HAS_RTTI
+// The user didn't tell us whether RTTI is enabled, so we need to
+// figure it out.
+
+# ifdef _MSC_VER
+
+# ifdef _CPPRTTI // MSVC defines this macro iff RTTI is enabled.
+# define GTEST_HAS_RTTI 1
+# else
+# define GTEST_HAS_RTTI 0
+# endif
+
+// Starting with version 4.3.2, gcc defines __GXX_RTTI iff RTTI is enabled.
+# elif defined(__GNUC__) && (GTEST_GCC_VER_ >= 40302)
+
+# ifdef __GXX_RTTI
+// When building against STLport with the Android NDK and with
+// -frtti -fno-exceptions, the build fails at link time with undefined
+// references to __cxa_bad_typeid. Note sure if STL or toolchain bug,
+// so disable RTTI when detected.
+# if GTEST_OS_LINUX_ANDROID && defined(_STLPORT_MAJOR) && \
+ !defined(__EXCEPTIONS)
+# define GTEST_HAS_RTTI 0
+# else
+# define GTEST_HAS_RTTI 1
+# endif // GTEST_OS_LINUX_ANDROID && __STLPORT_MAJOR && !__EXCEPTIONS
+# else
+# define GTEST_HAS_RTTI 0
+# endif // __GXX_RTTI
+
+// Clang defines __GXX_RTTI starting with version 3.0, but its manual recommends
+// using has_feature instead. has_feature(cxx_rtti) is supported since 2.7, the
+// first version with C++ support.
+# elif defined(__clang__)
+
+# define GTEST_HAS_RTTI __has_feature(cxx_rtti)
+
+// Starting with version 9.0 IBM Visual Age defines __RTTI_ALL__ to 1 if
+// both the typeid and dynamic_cast features are present.
+# elif defined(__IBMCPP__) && (__IBMCPP__ >= 900)
+
+# ifdef __RTTI_ALL__
+# define GTEST_HAS_RTTI 1
+# else
+# define GTEST_HAS_RTTI 0
+# endif
+
+# else
+
+// For all other compilers, we assume RTTI is enabled.
+# define GTEST_HAS_RTTI 1
+
+# endif // _MSC_VER
+
+#endif // GTEST_HAS_RTTI
+
+// It's this header's responsibility to #include <typeinfo> when RTTI
+// is enabled.
+#if GTEST_HAS_RTTI
+# include <typeinfo>
+#endif
+
+// Determines whether Google Test can use the pthreads library.
+#ifndef GTEST_HAS_PTHREAD
+// The user didn't tell us explicitly, so we assume pthreads support is
+// available on Linux and Mac.
+//
+// To disable threading support in Google Test, add -DGTEST_HAS_PTHREAD=0
+// to your compiler flags.
+# define GTEST_HAS_PTHREAD (GTEST_OS_LINUX || GTEST_OS_MAC || GTEST_OS_HPUX \
+ || GTEST_OS_QNX)
+#endif // GTEST_HAS_PTHREAD
+
+#if GTEST_HAS_PTHREAD
+// gtest-port.h guarantees to #include <pthread.h> when GTEST_HAS_PTHREAD is
+// true.
+# include <pthread.h> // NOLINT
+
+// For timespec and nanosleep, used below.
+# include <time.h> // NOLINT
+#endif
+
+// Determines whether Google Test can use tr1/tuple. You can define
+// this macro to 0 to prevent Google Test from using tuple (any
+// feature depending on tuple with be disabled in this mode).
+#ifndef GTEST_HAS_TR1_TUPLE
+# if GTEST_OS_LINUX_ANDROID && defined(_STLPORT_MAJOR)
+// STLport, provided with the Android NDK, has neither <tr1/tuple> or <tuple>.
+# define GTEST_HAS_TR1_TUPLE 0
+# else
+// The user didn't tell us not to do it, so we assume it's OK.
+# define GTEST_HAS_TR1_TUPLE 1
+# endif
+#endif // GTEST_HAS_TR1_TUPLE
+
+// Determines whether Google Test's own tr1 tuple implementation
+// should be used.
+#ifndef GTEST_USE_OWN_TR1_TUPLE
+// The user didn't tell us, so we need to figure it out.
+
+// We use our own TR1 tuple if we aren't sure the user has an
+// implementation of it already. At this time, libstdc++ 4.0.0+ and
+// MSVC 2010 are the only mainstream standard libraries that come
+// with a TR1 tuple implementation. NVIDIA's CUDA NVCC compiler
+// pretends to be GCC by defining __GNUC__ and friends, but cannot
+// compile GCC's tuple implementation. MSVC 2008 (9.0) provides TR1
+// tuple in a 323 MB Feature Pack download, which we cannot assume the
+// user has. QNX's QCC compiler is a modified GCC but it doesn't
+// support TR1 tuple. libc++ only provides std::tuple, in C++11 mode,
+// and it can be used with some compilers that define __GNUC__.
+# if (defined(__GNUC__) && !defined(__CUDACC__) && (GTEST_GCC_VER_ >= 40000) \
+ && !GTEST_OS_QNX && !defined(_LIBCPP_VERSION)) || _MSC_VER >= 1600
+# define GTEST_ENV_HAS_TR1_TUPLE_ 1
+# endif
+
+// C++11 specifies that <tuple> provides std::tuple. Use that if gtest is used
+// in C++11 mode and libstdc++ isn't very old (binaries targeting OS X 10.6
+// can build with clang but need to use gcc4.2's libstdc++).
+# if GTEST_LANG_CXX11 && (!defined(__GLIBCXX__) || __GLIBCXX__ > 20110325)
+# define GTEST_ENV_HAS_STD_TUPLE_ 1
+# endif
+
+# if GTEST_ENV_HAS_TR1_TUPLE_ || GTEST_ENV_HAS_STD_TUPLE_
+# define GTEST_USE_OWN_TR1_TUPLE 0
+# else
+# define GTEST_USE_OWN_TR1_TUPLE 1
+# endif
+
+#endif // GTEST_USE_OWN_TR1_TUPLE
+
+// To avoid conditional compilation everywhere, we make it
+// gtest-port.h's responsibility to #include the header implementing
+// tr1/tuple.
+#if GTEST_HAS_TR1_TUPLE
+
+# if GTEST_USE_OWN_TR1_TUPLE
+# include "gtest/internal/gtest-tuple.h"
+# elif GTEST_ENV_HAS_STD_TUPLE_
+# include <tuple>
+// C++11 puts its tuple into the ::std namespace rather than
+// ::std::tr1. gtest expects tuple to live in ::std::tr1, so put it there.
+// This causes undefined behavior, but supported compilers react in
+// the way we intend.
+namespace std {
+namespace tr1 {
+using ::std::get;
+using ::std::make_tuple;
+using ::std::tuple;
+using ::std::tuple_element;
+using ::std::tuple_size;
+}
+}
+
+# elif GTEST_OS_SYMBIAN
+
+// On Symbian, BOOST_HAS_TR1_TUPLE causes Boost's TR1 tuple library to
+// use STLport's tuple implementation, which unfortunately doesn't
+// work as the copy of STLport distributed with Symbian is incomplete.
+// By making sure BOOST_HAS_TR1_TUPLE is undefined, we force Boost to
+// use its own tuple implementation.
+# ifdef BOOST_HAS_TR1_TUPLE
+# undef BOOST_HAS_TR1_TUPLE
+# endif // BOOST_HAS_TR1_TUPLE
+
+// This prevents <boost/tr1/detail/config.hpp>, which defines
+// BOOST_HAS_TR1_TUPLE, from being #included by Boost's <tuple>.
+# define BOOST_TR1_DETAIL_CONFIG_HPP_INCLUDED
+# include <tuple>
+
+# elif defined(__GNUC__) && (GTEST_GCC_VER_ >= 40000)
+// GCC 4.0+ implements tr1/tuple in the <tr1/tuple> header. This does
+// not conform to the TR1 spec, which requires the header to be <tuple>.
+
+# if !GTEST_HAS_RTTI && GTEST_GCC_VER_ < 40302
+// Until version 4.3.2, gcc has a bug that causes <tr1/functional>,
+// which is #included by <tr1/tuple>, to not compile when RTTI is
+// disabled. _TR1_FUNCTIONAL is the header guard for
+// <tr1/functional>. Hence the following #define is a hack to prevent
+// <tr1/functional> from being included.
+# define _TR1_FUNCTIONAL 1
+# include <tr1/tuple>
+# undef _TR1_FUNCTIONAL // Allows the user to #include
+ // <tr1/functional> if he chooses to.
+# else
+# include <tr1/tuple> // NOLINT
+# endif // !GTEST_HAS_RTTI && GTEST_GCC_VER_ < 40302
+
+# else
+// If the compiler is not GCC 4.0+, we assume the user is using a
+// spec-conforming TR1 implementation.
+# include <tuple> // NOLINT
+# endif // GTEST_USE_OWN_TR1_TUPLE
+
+#endif // GTEST_HAS_TR1_TUPLE
+
+// Determines whether clone(2) is supported.
+// Usually it will only be available on Linux, excluding
+// Linux on the Itanium architecture.
+// Also see http://linux.die.net/man/2/clone.
+#ifndef GTEST_HAS_CLONE
+// The user didn't tell us, so we need to figure it out.
+
+# if GTEST_OS_LINUX && !defined(__ia64__)
+# if GTEST_OS_LINUX_ANDROID
+// On Android, clone() is only available on ARM starting with Gingerbread.
+# if defined(__arm__) && __ANDROID_API__ >= 9
+# define GTEST_HAS_CLONE 1
+# else
+# define GTEST_HAS_CLONE 0
+# endif
+# else
+# define GTEST_HAS_CLONE 1
+# endif
+# else
+# define GTEST_HAS_CLONE 0
+# endif // GTEST_OS_LINUX && !defined(__ia64__)
+
+#endif // GTEST_HAS_CLONE
+
+// Determines whether to support stream redirection. This is used to test
+// output correctness and to implement death tests.
+#ifndef GTEST_HAS_STREAM_REDIRECTION
+// By default, we assume that stream redirection is supported on all
+// platforms except known mobile ones.
+# if GTEST_OS_WINDOWS_MOBILE || GTEST_OS_SYMBIAN
+# define GTEST_HAS_STREAM_REDIRECTION 0
+# else
+# define GTEST_HAS_STREAM_REDIRECTION 1
+# endif // !GTEST_OS_WINDOWS_MOBILE && !GTEST_OS_SYMBIAN
+#endif // GTEST_HAS_STREAM_REDIRECTION
+
+// Determines whether to support death tests.
+// Google Test does not support death tests for VC 7.1 and earlier as
+// abort() in a VC 7.1 application compiled as GUI in debug config
+// pops up a dialog window that cannot be suppressed programmatically.
+#if (GTEST_OS_LINUX || GTEST_OS_CYGWIN || GTEST_OS_SOLARIS || \
+ (GTEST_OS_MAC && !GTEST_OS_IOS) || GTEST_OS_IOS_SIMULATOR || \
+ (GTEST_OS_WINDOWS_DESKTOP && _MSC_VER >= 1400) || \
+ GTEST_OS_WINDOWS_MINGW || GTEST_OS_AIX || GTEST_OS_HPUX || \
+ GTEST_OS_OPENBSD || GTEST_OS_QNX)
+# define GTEST_HAS_DEATH_TEST 1
+# include <vector> // NOLINT
+#endif
+
+// We don't support MSVC 7.1 with exceptions disabled now. Therefore
+// all the compilers we care about are adequate for supporting
+// value-parameterized tests.
+#define GTEST_HAS_PARAM_TEST 1
+
+// Determines whether to support type-driven tests.
+
+// Typed tests need <typeinfo> and variadic macros, which GCC, VC++ 8.0,
+// Sun Pro CC, IBM Visual Age, and HP aCC support.
+#if defined(__GNUC__) || (_MSC_VER >= 1400) || defined(__SUNPRO_CC) || \
+ defined(__IBMCPP__) || defined(__HP_aCC)
+# define GTEST_HAS_TYPED_TEST 1
+# define GTEST_HAS_TYPED_TEST_P 1
+#endif
+
+// Determines whether to support Combine(). This only makes sense when
+// value-parameterized tests are enabled. The implementation doesn't
+// work on Sun Studio since it doesn't understand templated conversion
+// operators.
+#if GTEST_HAS_PARAM_TEST && GTEST_HAS_TR1_TUPLE && !defined(__SUNPRO_CC)
+# define GTEST_HAS_COMBINE 1
+#endif
+
+// Determines whether the system compiler uses UTF-16 for encoding wide strings.
+#define GTEST_WIDE_STRING_USES_UTF16_ \
+ (GTEST_OS_WINDOWS || GTEST_OS_CYGWIN || GTEST_OS_SYMBIAN || GTEST_OS_AIX)
+
+// Determines whether test results can be streamed to a socket.
+#if GTEST_OS_LINUX
+# define GTEST_CAN_STREAM_RESULTS_ 1
+#endif
+
+// Defines some utility macros.
+
+// The GNU compiler emits a warning if nested "if" statements are followed by
+// an "else" statement and braces are not used to explicitly disambiguate the
+// "else" binding. This leads to problems with code like:
+//
+// if (gate)
+// ASSERT_*(condition) << "Some message";
+//
+// The "switch (0) case 0:" idiom is used to suppress this.
+#ifdef __INTEL_COMPILER
+# define GTEST_AMBIGUOUS_ELSE_BLOCKER_
+#else
+# define GTEST_AMBIGUOUS_ELSE_BLOCKER_ switch (0) case 0: default: // NOLINT
+#endif
+
+// Use this annotation at the end of a struct/class definition to
+// prevent the compiler from optimizing away instances that are never
+// used. This is useful when all interesting logic happens inside the
+// c'tor and / or d'tor. Example:
+//
+// struct Foo {
+// Foo() { ... }
+// } GTEST_ATTRIBUTE_UNUSED_;
+//
+// Also use it after a variable or parameter declaration to tell the
+// compiler the variable/parameter does not have to be used.
+#if defined(__GNUC__) && !defined(COMPILER_ICC)
+# define GTEST_ATTRIBUTE_UNUSED_ __attribute__ ((unused))
+#else
+# define GTEST_ATTRIBUTE_UNUSED_
+#endif
+
+// A macro to disallow operator=
+// This should be used in the private: declarations for a class.
+#define GTEST_DISALLOW_ASSIGN_(type)\
+ void operator=(type const &)
+
+// A macro to disallow copy constructor and operator=
+// This should be used in the private: declarations for a class.
+#define GTEST_DISALLOW_COPY_AND_ASSIGN_(type)\
+ type(type const &);\
+ GTEST_DISALLOW_ASSIGN_(type)
+
+// Tell the compiler to warn about unused return values for functions declared
+// with this macro. The macro should be used on function declarations
+// following the argument list:
+//
+// Sprocket* AllocateSprocket() GTEST_MUST_USE_RESULT_;
+#if defined(__GNUC__) && (GTEST_GCC_VER_ >= 30400) && !defined(COMPILER_ICC)
+# define GTEST_MUST_USE_RESULT_ __attribute__ ((warn_unused_result))
+#else
+# define GTEST_MUST_USE_RESULT_
+#endif // __GNUC__ && (GTEST_GCC_VER_ >= 30400) && !COMPILER_ICC
+
+// Determine whether the compiler supports Microsoft's Structured Exception
+// Handling. This is supported by several Windows compilers but generally
+// does not exist on any other system.
+#ifndef GTEST_HAS_SEH
+// The user didn't tell us, so we need to figure it out.
+
+# if defined(_MSC_VER) || defined(__BORLANDC__)
+// These two compilers are known to support SEH.
+# define GTEST_HAS_SEH 1
+# else
+// Assume no SEH.
+# define GTEST_HAS_SEH 0
+# endif
+
+#endif // GTEST_HAS_SEH
+
+#ifdef _MSC_VER
+
+# if GTEST_LINKED_AS_SHARED_LIBRARY
+# define GTEST_API_ __declspec(dllimport)
+# elif GTEST_CREATE_SHARED_LIBRARY
+# define GTEST_API_ __declspec(dllexport)
+# endif
+
+#endif // _MSC_VER
+
+#ifndef GTEST_API_
+# define GTEST_API_
+#endif
+
+#ifdef __GNUC__
+// Ask the compiler to never inline a given function.
+# define GTEST_NO_INLINE_ __attribute__((noinline))
+#else
+# define GTEST_NO_INLINE_
+#endif
+
+// _LIBCPP_VERSION is defined by the libc++ library from the LLVM project.
+#if defined(__GLIBCXX__) || defined(_LIBCPP_VERSION)
+# define GTEST_HAS_CXXABI_H_ 1
+#else
+# define GTEST_HAS_CXXABI_H_ 0
+#endif
+
+namespace testing {
+
+class Message;
+
+namespace internal {
+
+// A secret type that Google Test users don't know about. It has no
+// definition on purpose. Therefore it's impossible to create a
+// Secret object, which is what we want.
+class Secret;
+
+// The GTEST_COMPILE_ASSERT_ macro can be used to verify that a compile time
+// expression is true. For example, you could use it to verify the
+// size of a static array:
+//
+// GTEST_COMPILE_ASSERT_(ARRAYSIZE(content_type_names) == CONTENT_NUM_TYPES,
+// content_type_names_incorrect_size);
+//
+// or to make sure a struct is smaller than a certain size:
+//
+// GTEST_COMPILE_ASSERT_(sizeof(foo) < 128, foo_too_large);
+//
+// The second argument to the macro is the name of the variable. If
+// the expression is false, most compilers will issue a warning/error
+// containing the name of the variable.
+
+template <bool>
+struct CompileAssert {
+};
+
+#define GTEST_COMPILE_ASSERT_(expr, msg) \
+ typedef ::testing::internal::CompileAssert<(static_cast<bool>(expr))> \
+ msg[static_cast<bool>(expr) ? 1 : -1] GTEST_ATTRIBUTE_UNUSED_
+
+// Implementation details of GTEST_COMPILE_ASSERT_:
+//
+// - GTEST_COMPILE_ASSERT_ works by defining an array type that has -1
+// elements (and thus is invalid) when the expression is false.
+//
+// - The simpler definition
+//
+// #define GTEST_COMPILE_ASSERT_(expr, msg) typedef char msg[(expr) ? 1 : -1]
+//
+// does not work, as gcc supports variable-length arrays whose sizes
+// are determined at run-time (this is gcc's extension and not part
+// of the C++ standard). As a result, gcc fails to reject the
+// following code with the simple definition:
+//
+// int foo;
+// GTEST_COMPILE_ASSERT_(foo, msg); // not supposed to compile as foo is
+// // not a compile-time constant.
+//
+// - By using the type CompileAssert<(bool(expr))>, we ensures that
+// expr is a compile-time constant. (Template arguments must be
+// determined at compile-time.)
+//
+// - The outter parentheses in CompileAssert<(bool(expr))> are necessary
+// to work around a bug in gcc 3.4.4 and 4.0.1. If we had written
+//
+// CompileAssert<bool(expr)>
+//
+// instead, these compilers will refuse to compile
+//
+// GTEST_COMPILE_ASSERT_(5 > 0, some_message);
+//
+// (They seem to think the ">" in "5 > 0" marks the end of the
+// template argument list.)
+//
+// - The array size is (bool(expr) ? 1 : -1), instead of simply
+//
+// ((expr) ? 1 : -1).
+//
+// This is to avoid running into a bug in MS VC 7.1, which
+// causes ((0.0) ? 1 : -1) to incorrectly evaluate to 1.
+
+// StaticAssertTypeEqHelper is used by StaticAssertTypeEq defined in gtest.h.
+//
+// This template is declared, but intentionally undefined.
+template <typename T1, typename T2>
+struct StaticAssertTypeEqHelper;
+
+template <typename T>
+struct StaticAssertTypeEqHelper<T, T> {};
+
+#if GTEST_HAS_GLOBAL_STRING
+typedef ::string string;
+#else
+typedef ::std::string string;
+#endif // GTEST_HAS_GLOBAL_STRING
+
+#if GTEST_HAS_GLOBAL_WSTRING
+typedef ::wstring wstring;
+#elif GTEST_HAS_STD_WSTRING
+typedef ::std::wstring wstring;
+#endif // GTEST_HAS_GLOBAL_WSTRING
+
+// A helper for suppressing warnings on constant condition. It just
+// returns 'condition'.
+GTEST_API_ bool IsTrue(bool condition);
+
+// Defines scoped_ptr.
+
+// This implementation of scoped_ptr is PARTIAL - it only contains
+// enough stuff to satisfy Google Test's need.
+template <typename T>
+class scoped_ptr {
+ public:
+ typedef T element_type;
+
+ explicit scoped_ptr(T* p = NULL) : ptr_(p) {}
+ ~scoped_ptr() { reset(); }
+
+ T& operator*() const { return *ptr_; }
+ T* operator->() const { return ptr_; }
+ T* get() const { return ptr_; }
+
+ T* release() {
+ T* const ptr = ptr_;
+ ptr_ = NULL;
+ return ptr;
+ }
+
+ void reset(T* p = NULL) {
+ if (p != ptr_) {
+ if (IsTrue(sizeof(T) > 0)) { // Makes sure T is a complete type.
+ delete ptr_;
+ }
+ ptr_ = p;
+ }
+ }
+
+ private:
+ T* ptr_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(scoped_ptr);
+};
+
+// Defines RE.
+
+// A simple C++ wrapper for <regex.h>. It uses the POSIX Extended
+// Regular Expression syntax.
+class GTEST_API_ RE {
+ public:
+ // A copy constructor is required by the Standard to initialize object
+ // references from r-values.
+ RE(const RE& other) { Init(other.pattern()); }
+
+ // Constructs an RE from a string.
+ RE(const ::std::string& regex) { Init(regex.c_str()); } // NOLINT
+
+#if GTEST_HAS_GLOBAL_STRING
+
+ RE(const ::string& regex) { Init(regex.c_str()); } // NOLINT
+
+#endif // GTEST_HAS_GLOBAL_STRING
+
+ RE(const char* regex) { Init(regex); } // NOLINT
+ ~RE();
+
+ // Returns the string representation of the regex.
+ const char* pattern() const { return pattern_; }
+
+ // FullMatch(str, re) returns true iff regular expression re matches
+ // the entire str.
+ // PartialMatch(str, re) returns true iff regular expression re
+ // matches a substring of str (including str itself).
+ //
+ // TODO(wan at google.com): make FullMatch() and PartialMatch() work
+ // when str contains NUL characters.
+ static bool FullMatch(const ::std::string& str, const RE& re) {
+ return FullMatch(str.c_str(), re);
+ }
+ static bool PartialMatch(const ::std::string& str, const RE& re) {
+ return PartialMatch(str.c_str(), re);
+ }
+
+#if GTEST_HAS_GLOBAL_STRING
+
+ static bool FullMatch(const ::string& str, const RE& re) {
+ return FullMatch(str.c_str(), re);
+ }
+ static bool PartialMatch(const ::string& str, const RE& re) {
+ return PartialMatch(str.c_str(), re);
+ }
+
+#endif // GTEST_HAS_GLOBAL_STRING
+
+ static bool FullMatch(const char* str, const RE& re);
+ static bool PartialMatch(const char* str, const RE& re);
+
+ private:
+ void Init(const char* regex);
+
+ // We use a const char* instead of an std::string, as Google Test used to be
+ // used where std::string is not available. TODO(wan at google.com): change to
+ // std::string.
+ const char* pattern_;
+ bool is_valid_;
+
+#if GTEST_USES_POSIX_RE
+
+ regex_t full_regex_; // For FullMatch().
+ regex_t partial_regex_; // For PartialMatch().
+
+#else // GTEST_USES_SIMPLE_RE
+
+ const char* full_pattern_; // For FullMatch();
+
+#endif
+
+ GTEST_DISALLOW_ASSIGN_(RE);
+};
+
+// Formats a source file path and a line number as they would appear
+// in an error message from the compiler used to compile this code.
+GTEST_API_ ::std::string FormatFileLocation(const char* file, int line);
+
+// Formats a file location for compiler-independent XML output.
+// Although this function is not platform dependent, we put it next to
+// FormatFileLocation in order to contrast the two functions.
+GTEST_API_ ::std::string FormatCompilerIndependentFileLocation(const char* file,
+ int line);
+
+// Defines logging utilities:
+// GTEST_LOG_(severity) - logs messages at the specified severity level. The
+// message itself is streamed into the macro.
+// LogToStderr() - directs all log messages to stderr.
+// FlushInfoLog() - flushes informational log messages.
+
+enum GTestLogSeverity {
+ GTEST_INFO,
+ GTEST_WARNING,
+ GTEST_ERROR,
+ GTEST_FATAL
+};
+
+// Formats log entry severity, provides a stream object for streaming the
+// log message, and terminates the message with a newline when going out of
+// scope.
+class GTEST_API_ GTestLog {
+ public:
+ GTestLog(GTestLogSeverity severity, const char* file, int line);
+
+ // Flushes the buffers and, if severity is GTEST_FATAL, aborts the program.
+ ~GTestLog();
+
+ ::std::ostream& GetStream() { return ::std::cerr; }
+
+ private:
+ const GTestLogSeverity severity_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(GTestLog);
+};
+
+#define GTEST_LOG_(severity) \
+ ::testing::internal::GTestLog(::testing::internal::GTEST_##severity, \
+ __FILE__, __LINE__).GetStream()
+
+inline void LogToStderr() {}
+inline void FlushInfoLog() { fflush(NULL); }
+
+// INTERNAL IMPLEMENTATION - DO NOT USE.
+//
+// GTEST_CHECK_ is an all-mode assert. It aborts the program if the condition
+// is not satisfied.
+// Synopsys:
+// GTEST_CHECK_(boolean_condition);
+// or
+// GTEST_CHECK_(boolean_condition) << "Additional message";
+//
+// This checks the condition and if the condition is not satisfied
+// it prints message about the condition violation, including the
+// condition itself, plus additional message streamed into it, if any,
+// and then it aborts the program. It aborts the program irrespective of
+// whether it is built in the debug mode or not.
+#define GTEST_CHECK_(condition) \
+ GTEST_AMBIGUOUS_ELSE_BLOCKER_ \
+ if (::testing::internal::IsTrue(condition)) \
+ ; \
+ else \
+ GTEST_LOG_(FATAL) << "Condition " #condition " failed. "
+
+// An all-mode assert to verify that the given POSIX-style function
+// call returns 0 (indicating success). Known limitation: this
+// doesn't expand to a balanced 'if' statement, so enclose the macro
+// in {} if you need to use it as the only statement in an 'if'
+// branch.
+#define GTEST_CHECK_POSIX_SUCCESS_(posix_call) \
+ if (const int gtest_error = (posix_call)) \
+ GTEST_LOG_(FATAL) << #posix_call << "failed with error " \
+ << gtest_error
+
+// INTERNAL IMPLEMENTATION - DO NOT USE IN USER CODE.
+//
+// Use ImplicitCast_ as a safe version of static_cast for upcasting in
+// the type hierarchy (e.g. casting a Foo* to a SuperclassOfFoo* or a
+// const Foo*). When you use ImplicitCast_, the compiler checks that
+// the cast is safe. Such explicit ImplicitCast_s are necessary in
+// surprisingly many situations where C++ demands an exact type match
+// instead of an argument type convertable to a target type.
+//
+// The syntax for using ImplicitCast_ is the same as for static_cast:
+//
+// ImplicitCast_<ToType>(expr)
+//
+// ImplicitCast_ would have been part of the C++ standard library,
+// but the proposal was submitted too late. It will probably make
+// its way into the language in the future.
+//
+// This relatively ugly name is intentional. It prevents clashes with
+// similar functions users may have (e.g., implicit_cast). The internal
+// namespace alone is not enough because the function can be found by ADL.
+template<typename To>
+inline To ImplicitCast_(To x) { return x; }
+
+// When you upcast (that is, cast a pointer from type Foo to type
+// SuperclassOfFoo), it's fine to use ImplicitCast_<>, since upcasts
+// always succeed. When you downcast (that is, cast a pointer from
+// type Foo to type SubclassOfFoo), static_cast<> isn't safe, because
+// how do you know the pointer is really of type SubclassOfFoo? It
+// could be a bare Foo, or of type DifferentSubclassOfFoo. Thus,
+// when you downcast, you should use this macro. In debug mode, we
+// use dynamic_cast<> to double-check the downcast is legal (we die
+// if it's not). In normal mode, we do the efficient static_cast<>
+// instead. Thus, it's important to test in debug mode to make sure
+// the cast is legal!
+// This is the only place in the code we should use dynamic_cast<>.
+// In particular, you SHOULDN'T be using dynamic_cast<> in order to
+// do RTTI (eg code like this:
+// if (dynamic_cast<Subclass1>(foo)) HandleASubclass1Object(foo);
+// if (dynamic_cast<Subclass2>(foo)) HandleASubclass2Object(foo);
+// You should design the code some other way not to need this.
+//
+// This relatively ugly name is intentional. It prevents clashes with
+// similar functions users may have (e.g., down_cast). The internal
+// namespace alone is not enough because the function can be found by ADL.
+template<typename To, typename From> // use like this: DownCast_<T*>(foo);
+inline To DownCast_(From* f) { // so we only accept pointers
+ // Ensures that To is a sub-type of From *. This test is here only
+ // for compile-time type checking, and has no overhead in an
+ // optimized build at run-time, as it will be optimized away
+ // completely.
+ if (false) {
+ const To to = NULL;
+ ::testing::internal::ImplicitCast_<From*>(to);
+ }
+
+#if GTEST_HAS_RTTI
+ // RTTI: debug mode only!
+ GTEST_CHECK_(f == NULL || dynamic_cast<To>(f) != NULL);
+#endif
+ return static_cast<To>(f);
+}
+
+// Downcasts the pointer of type Base to Derived.
+// Derived must be a subclass of Base. The parameter MUST
+// point to a class of type Derived, not any subclass of it.
+// When RTTI is available, the function performs a runtime
+// check to enforce this.
+template <class Derived, class Base>
+Derived* CheckedDowncastToActualType(Base* base) {
+#if GTEST_HAS_RTTI
+ GTEST_CHECK_(typeid(*base) == typeid(Derived));
+ return dynamic_cast<Derived*>(base); // NOLINT
+#else
+ return static_cast<Derived*>(base); // Poor man's downcast.
+#endif
+}
+
+#if GTEST_HAS_STREAM_REDIRECTION
+
+// Defines the stderr capturer:
+// CaptureStdout - starts capturing stdout.
+// GetCapturedStdout - stops capturing stdout and returns the captured string.
+// CaptureStderr - starts capturing stderr.
+// GetCapturedStderr - stops capturing stderr and returns the captured string.
+//
+GTEST_API_ void CaptureStdout();
+GTEST_API_ std::string GetCapturedStdout();
+GTEST_API_ void CaptureStderr();
+GTEST_API_ std::string GetCapturedStderr();
+
+#endif // GTEST_HAS_STREAM_REDIRECTION
+
+
+#if GTEST_HAS_DEATH_TEST
+
+const ::std::vector<testing::internal::string>& GetInjectableArgvs();
+void SetInjectableArgvs(const ::std::vector<testing::internal::string>*
+ new_argvs);
+
+// A copy of all command line arguments. Set by InitGoogleTest().
+extern ::std::vector<testing::internal::string> g_argvs;
+
+#endif // GTEST_HAS_DEATH_TEST
+
+// Defines synchronization primitives.
+
+#if GTEST_HAS_PTHREAD
+
+// Sleeps for (roughly) n milli-seconds. This function is only for
+// testing Google Test's own constructs. Don't use it in user tests,
+// either directly or indirectly.
+inline void SleepMilliseconds(int n) {
+ const timespec time = {
+ 0, // 0 seconds.
+ n * 1000L * 1000L, // And n ms.
+ };
+ nanosleep(&time, NULL);
+}
+
+// Allows a controller thread to pause execution of newly created
+// threads until notified. Instances of this class must be created
+// and destroyed in the controller thread.
+//
+// This class is only for testing Google Test's own constructs. Do not
+// use it in user tests, either directly or indirectly.
+class Notification {
+ public:
+ Notification() : notified_(false) {
+ GTEST_CHECK_POSIX_SUCCESS_(pthread_mutex_init(&mutex_, NULL));
+ }
+ ~Notification() {
+ pthread_mutex_destroy(&mutex_);
+ }
+
+ // Notifies all threads created with this notification to start. Must
+ // be called from the controller thread.
+ void Notify() {
+ pthread_mutex_lock(&mutex_);
+ notified_ = true;
+ pthread_mutex_unlock(&mutex_);
+ }
+
+ // Blocks until the controller thread notifies. Must be called from a test
+ // thread.
+ void WaitForNotification() {
+ for (;;) {
+ pthread_mutex_lock(&mutex_);
+ const bool notified = notified_;
+ pthread_mutex_unlock(&mutex_);
+ if (notified)
+ break;
+ SleepMilliseconds(10);
+ }
+ }
+
+ private:
+ pthread_mutex_t mutex_;
+ bool notified_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(Notification);
+};
+
+// As a C-function, ThreadFuncWithCLinkage cannot be templated itself.
+// Consequently, it cannot select a correct instantiation of ThreadWithParam
+// in order to call its Run(). Introducing ThreadWithParamBase as a
+// non-templated base class for ThreadWithParam allows us to bypass this
+// problem.
+class ThreadWithParamBase {
+ public:
+ virtual ~ThreadWithParamBase() {}
+ virtual void Run() = 0;
+};
+
+// pthread_create() accepts a pointer to a function type with the C linkage.
+// According to the Standard (7.5/1), function types with different linkages
+// are different even if they are otherwise identical. Some compilers (for
+// example, SunStudio) treat them as different types. Since class methods
+// cannot be defined with C-linkage we need to define a free C-function to
+// pass into pthread_create().
+extern "C" inline void* ThreadFuncWithCLinkage(void* thread) {
+ static_cast<ThreadWithParamBase*>(thread)->Run();
+ return NULL;
+}
+
+// Helper class for testing Google Test's multi-threading constructs.
+// To use it, write:
+//
+// void ThreadFunc(int param) { /* Do things with param */ }
+// Notification thread_can_start;
+// ...
+// // The thread_can_start parameter is optional; you can supply NULL.
+// ThreadWithParam<int> thread(&ThreadFunc, 5, &thread_can_start);
+// thread_can_start.Notify();
+//
+// These classes are only for testing Google Test's own constructs. Do
+// not use them in user tests, either directly or indirectly.
+template <typename T>
+class ThreadWithParam : public ThreadWithParamBase {
+ public:
+ typedef void (*UserThreadFunc)(T);
+
+ ThreadWithParam(
+ UserThreadFunc func, T param, Notification* thread_can_start)
+ : func_(func),
+ param_(param),
+ thread_can_start_(thread_can_start),
+ finished_(false) {
+ ThreadWithParamBase* const base = this;
+ // The thread can be created only after all fields except thread_
+ // have been initialized.
+ GTEST_CHECK_POSIX_SUCCESS_(
+ pthread_create(&thread_, 0, &ThreadFuncWithCLinkage, base));
+ }
+ ~ThreadWithParam() { Join(); }
+
+ void Join() {
+ if (!finished_) {
+ GTEST_CHECK_POSIX_SUCCESS_(pthread_join(thread_, 0));
+ finished_ = true;
+ }
+ }
+
+ virtual void Run() {
+ if (thread_can_start_ != NULL)
+ thread_can_start_->WaitForNotification();
+ func_(param_);
+ }
+
+ private:
+ const UserThreadFunc func_; // User-supplied thread function.
+ const T param_; // User-supplied parameter to the thread function.
+ // When non-NULL, used to block execution until the controller thread
+ // notifies.
+ Notification* const thread_can_start_;
+ bool finished_; // true iff we know that the thread function has finished.
+ pthread_t thread_; // The native thread object.
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ThreadWithParam);
+};
+
+// MutexBase and Mutex implement mutex on pthreads-based platforms. They
+// are used in conjunction with class MutexLock:
+//
+// Mutex mutex;
+// ...
+// MutexLock lock(&mutex); // Acquires the mutex and releases it at the end
+// // of the current scope.
+//
+// MutexBase implements behavior for both statically and dynamically
+// allocated mutexes. Do not use MutexBase directly. Instead, write
+// the following to define a static mutex:
+//
+// GTEST_DEFINE_STATIC_MUTEX_(g_some_mutex);
+//
+// You can forward declare a static mutex like this:
+//
+// GTEST_DECLARE_STATIC_MUTEX_(g_some_mutex);
+//
+// To create a dynamic mutex, just define an object of type Mutex.
+class MutexBase {
+ public:
+ // Acquires this mutex.
+ void Lock() {
+ GTEST_CHECK_POSIX_SUCCESS_(pthread_mutex_lock(&mutex_));
+ owner_ = pthread_self();
+ has_owner_ = true;
+ }
+
+ // Releases this mutex.
+ void Unlock() {
+ // Since the lock is being released the owner_ field should no longer be
+ // considered valid. We don't protect writing to has_owner_ here, as it's
+ // the caller's responsibility to ensure that the current thread holds the
+ // mutex when this is called.
+ has_owner_ = false;
+ GTEST_CHECK_POSIX_SUCCESS_(pthread_mutex_unlock(&mutex_));
+ }
+
+ // Does nothing if the current thread holds the mutex. Otherwise, crashes
+ // with high probability.
+ void AssertHeld() const {
+ GTEST_CHECK_(has_owner_ && pthread_equal(owner_, pthread_self()))
+ << "The current thread is not holding the mutex @" << this;
+ }
+
+ // A static mutex may be used before main() is entered. It may even
+ // be used before the dynamic initialization stage. Therefore we
+ // must be able to initialize a static mutex object at link time.
+ // This means MutexBase has to be a POD and its member variables
+ // have to be public.
+ public:
+ pthread_mutex_t mutex_; // The underlying pthread mutex.
+ // has_owner_ indicates whether the owner_ field below contains a valid thread
+ // ID and is therefore safe to inspect (e.g., to use in pthread_equal()). All
+ // accesses to the owner_ field should be protected by a check of this field.
+ // An alternative might be to memset() owner_ to all zeros, but there's no
+ // guarantee that a zero'd pthread_t is necessarily invalid or even different
+ // from pthread_self().
+ bool has_owner_;
+ pthread_t owner_; // The thread holding the mutex.
+};
+
+// Forward-declares a static mutex.
+# define GTEST_DECLARE_STATIC_MUTEX_(mutex) \
+ extern ::testing::internal::MutexBase mutex
+
+// Defines and statically (i.e. at link time) initializes a static mutex.
+// The initialization list here does not explicitly initialize each field,
+// instead relying on default initialization for the unspecified fields. In
+// particular, the owner_ field (a pthread_t) is not explicitly initialized.
+// This allows initialization to work whether pthread_t is a scalar or struct.
+// The flag -Wmissing-field-initializers must not be specified for this to work.
+# define GTEST_DEFINE_STATIC_MUTEX_(mutex) \
+ ::testing::internal::MutexBase mutex = { PTHREAD_MUTEX_INITIALIZER, false }
+
+// The Mutex class can only be used for mutexes created at runtime. It
+// shares its API with MutexBase otherwise.
+class Mutex : public MutexBase {
+ public:
+ Mutex() {
+ GTEST_CHECK_POSIX_SUCCESS_(pthread_mutex_init(&mutex_, NULL));
+ has_owner_ = false;
+ }
+ ~Mutex() {
+ GTEST_CHECK_POSIX_SUCCESS_(pthread_mutex_destroy(&mutex_));
+ }
+
+ private:
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(Mutex);
+};
+
+// We cannot name this class MutexLock as the ctor declaration would
+// conflict with a macro named MutexLock, which is defined on some
+// platforms. Hence the typedef trick below.
+class GTestMutexLock {
+ public:
+ explicit GTestMutexLock(MutexBase* mutex)
+ : mutex_(mutex) { mutex_->Lock(); }
+
+ ~GTestMutexLock() { mutex_->Unlock(); }
+
+ private:
+ MutexBase* const mutex_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(GTestMutexLock);
+};
+
+typedef GTestMutexLock MutexLock;
+
+// Helpers for ThreadLocal.
+
+// pthread_key_create() requires DeleteThreadLocalValue() to have
+// C-linkage. Therefore it cannot be templatized to access
+// ThreadLocal<T>. Hence the need for class
+// ThreadLocalValueHolderBase.
+class ThreadLocalValueHolderBase {
+ public:
+ virtual ~ThreadLocalValueHolderBase() {}
+};
+
+// Called by pthread to delete thread-local data stored by
+// pthread_setspecific().
+extern "C" inline void DeleteThreadLocalValue(void* value_holder) {
+ delete static_cast<ThreadLocalValueHolderBase*>(value_holder);
+}
+
+// Implements thread-local storage on pthreads-based systems.
+//
+// // Thread 1
+// ThreadLocal<int> tl(100); // 100 is the default value for each thread.
+//
+// // Thread 2
+// tl.set(150); // Changes the value for thread 2 only.
+// EXPECT_EQ(150, tl.get());
+//
+// // Thread 1
+// EXPECT_EQ(100, tl.get()); // In thread 1, tl has the original value.
+// tl.set(200);
+// EXPECT_EQ(200, tl.get());
+//
+// The template type argument T must have a public copy constructor.
+// In addition, the default ThreadLocal constructor requires T to have
+// a public default constructor.
+//
+// An object managed for a thread by a ThreadLocal instance is deleted
+// when the thread exits. Or, if the ThreadLocal instance dies in
+// that thread, when the ThreadLocal dies. It's the user's
+// responsibility to ensure that all other threads using a ThreadLocal
+// have exited when it dies, or the per-thread objects for those
+// threads will not be deleted.
+//
+// Google Test only uses global ThreadLocal objects. That means they
+// will die after main() has returned. Therefore, no per-thread
+// object managed by Google Test will be leaked as long as all threads
+// using Google Test have exited when main() returns.
+template <typename T>
+class ThreadLocal {
+ public:
+ ThreadLocal() : key_(CreateKey()),
+ default_() {}
+ explicit ThreadLocal(const T& value) : key_(CreateKey()),
+ default_(value) {}
+
+ ~ThreadLocal() {
+ // Destroys the managed object for the current thread, if any.
+ DeleteThreadLocalValue(pthread_getspecific(key_));
+
+ // Releases resources associated with the key. This will *not*
+ // delete managed objects for other threads.
+ GTEST_CHECK_POSIX_SUCCESS_(pthread_key_delete(key_));
+ }
+
+ T* pointer() { return GetOrCreateValue(); }
+ const T* pointer() const { return GetOrCreateValue(); }
+ const T& get() const { return *pointer(); }
+ void set(const T& value) { *pointer() = value; }
+
+ private:
+ // Holds a value of type T.
+ class ValueHolder : public ThreadLocalValueHolderBase {
+ public:
+ explicit ValueHolder(const T& value) : value_(value) {}
+
+ T* pointer() { return &value_; }
+
+ private:
+ T value_;
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ValueHolder);
+ };
+
+ static pthread_key_t CreateKey() {
+ pthread_key_t key;
+ // When a thread exits, DeleteThreadLocalValue() will be called on
+ // the object managed for that thread.
+ GTEST_CHECK_POSIX_SUCCESS_(
+ pthread_key_create(&key, &DeleteThreadLocalValue));
+ return key;
+ }
+
+ T* GetOrCreateValue() const {
+ ThreadLocalValueHolderBase* const holder =
+ static_cast<ThreadLocalValueHolderBase*>(pthread_getspecific(key_));
+ if (holder != NULL) {
+ return CheckedDowncastToActualType<ValueHolder>(holder)->pointer();
+ }
+
+ ValueHolder* const new_holder = new ValueHolder(default_);
+ ThreadLocalValueHolderBase* const holder_base = new_holder;
+ GTEST_CHECK_POSIX_SUCCESS_(pthread_setspecific(key_, holder_base));
+ return new_holder->pointer();
+ }
+
+ // A key pthreads uses for looking up per-thread values.
+ const pthread_key_t key_;
+ const T default_; // The default value for each thread.
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ThreadLocal);
+};
+
+# define GTEST_IS_THREADSAFE 1
+
+#else // GTEST_HAS_PTHREAD
+
+// A dummy implementation of synchronization primitives (mutex, lock,
+// and thread-local variable). Necessary for compiling Google Test where
+// mutex is not supported - using Google Test in multiple threads is not
+// supported on such platforms.
+
+class Mutex {
+ public:
+ Mutex() {}
+ void Lock() {}
+ void Unlock() {}
+ void AssertHeld() const {}
+};
+
+# define GTEST_DECLARE_STATIC_MUTEX_(mutex) \
+ extern ::testing::internal::Mutex mutex
+
+# define GTEST_DEFINE_STATIC_MUTEX_(mutex) ::testing::internal::Mutex mutex
+
+class GTestMutexLock {
+ public:
+ explicit GTestMutexLock(Mutex*) {} // NOLINT
+};
+
+typedef GTestMutexLock MutexLock;
+
+template <typename T>
+class ThreadLocal {
+ public:
+ ThreadLocal() : value_() {}
+ explicit ThreadLocal(const T& value) : value_(value) {}
+ T* pointer() { return &value_; }
+ const T* pointer() const { return &value_; }
+ const T& get() const { return value_; }
+ void set(const T& value) { value_ = value; }
+ private:
+ T value_;
+};
+
+// The above synchronization primitives have dummy implementations.
+// Therefore Google Test is not thread-safe.
+# define GTEST_IS_THREADSAFE 0
+
+#endif // GTEST_HAS_PTHREAD
+
+// Returns the number of threads running in the process, or 0 to indicate that
+// we cannot detect it.
+GTEST_API_ size_t GetThreadCount();
+
+// Passing non-POD classes through ellipsis (...) crashes the ARM
+// compiler and generates a warning in Sun Studio. The Nokia Symbian
+// and the IBM XL C/C++ compiler try to instantiate a copy constructor
+// for objects passed through ellipsis (...), failing for uncopyable
+// objects. We define this to ensure that only POD is passed through
+// ellipsis on these systems.
+#if defined(__SYMBIAN32__) || defined(__IBMCPP__) || defined(__SUNPRO_CC)
+// We lose support for NULL detection where the compiler doesn't like
+// passing non-POD classes through ellipsis (...).
+# define GTEST_ELLIPSIS_NEEDS_POD_ 1
+#else
+# define GTEST_CAN_COMPARE_NULL 1
+#endif
+
+// The Nokia Symbian and IBM XL C/C++ compilers cannot decide between
+// const T& and const T* in a function template. These compilers
+// _can_ decide between class template specializations for T and T*,
+// so a tr1::type_traits-like is_pointer works.
+#if defined(__SYMBIAN32__) || defined(__IBMCPP__)
+# define GTEST_NEEDS_IS_POINTER_ 1
+#endif
+
+template <bool bool_value>
+struct bool_constant {
+ typedef bool_constant<bool_value> type;
+ static const bool value = bool_value;
+};
+template <bool bool_value> const bool bool_constant<bool_value>::value;
+
+typedef bool_constant<false> false_type;
+typedef bool_constant<true> true_type;
+
+template <typename T>
+struct is_pointer : public false_type {};
+
+template <typename T>
+struct is_pointer<T*> : public true_type {};
+
+template <typename Iterator>
+struct IteratorTraits {
+ typedef typename Iterator::value_type value_type;
+};
+
+template <typename T>
+struct IteratorTraits<T*> {
+ typedef T value_type;
+};
+
+template <typename T>
+struct IteratorTraits<const T*> {
+ typedef T value_type;
+};
+
+#if GTEST_OS_WINDOWS
+# define GTEST_PATH_SEP_ "\\"
+# define GTEST_HAS_ALT_PATH_SEP_ 1
+// The biggest signed integer type the compiler supports.
+typedef __int64 BiggestInt;
+#else
+# define GTEST_PATH_SEP_ "/"
+# define GTEST_HAS_ALT_PATH_SEP_ 0
+typedef long long BiggestInt; // NOLINT
+#endif // GTEST_OS_WINDOWS
+
+// Utilities for char.
+
+// isspace(int ch) and friends accept an unsigned char or EOF. char
+// may be signed, depending on the compiler (or compiler flags).
+// Therefore we need to cast a char to unsigned char before calling
+// isspace(), etc.
+
+inline bool IsAlpha(char ch) {
+ return isalpha(static_cast<unsigned char>(ch)) != 0;
+}
+inline bool IsAlNum(char ch) {
+ return isalnum(static_cast<unsigned char>(ch)) != 0;
+}
+inline bool IsDigit(char ch) {
+ return isdigit(static_cast<unsigned char>(ch)) != 0;
+}
+inline bool IsLower(char ch) {
+ return islower(static_cast<unsigned char>(ch)) != 0;
+}
+inline bool IsSpace(char ch) {
+ return isspace(static_cast<unsigned char>(ch)) != 0;
+}
+inline bool IsUpper(char ch) {
+ return isupper(static_cast<unsigned char>(ch)) != 0;
+}
+inline bool IsXDigit(char ch) {
+ return isxdigit(static_cast<unsigned char>(ch)) != 0;
+}
+inline bool IsXDigit(wchar_t ch) {
+ const unsigned char low_byte = static_cast<unsigned char>(ch);
+ return ch == low_byte && isxdigit(low_byte) != 0;
+}
+
+inline char ToLower(char ch) {
+ return static_cast<char>(tolower(static_cast<unsigned char>(ch)));
+}
+inline char ToUpper(char ch) {
+ return static_cast<char>(toupper(static_cast<unsigned char>(ch)));
+}
+
+// The testing::internal::posix namespace holds wrappers for common
+// POSIX functions. These wrappers hide the differences between
+// Windows/MSVC and POSIX systems. Since some compilers define these
+// standard functions as macros, the wrapper cannot have the same name
+// as the wrapped function.
+
+namespace posix {
+
+// Functions with a different name on Windows.
+
+#if GTEST_OS_WINDOWS
+
+typedef struct _stat StatStruct;
+
+# ifdef __BORLANDC__
+inline int IsATTY(int fd) { return isatty(fd); }
+inline int StrCaseCmp(const char* s1, const char* s2) {
+ return stricmp(s1, s2);
+}
+inline char* StrDup(const char* src) { return strdup(src); }
+# else // !__BORLANDC__
+# if GTEST_OS_WINDOWS_MOBILE
+inline int IsATTY(int /* fd */) { return 0; }
+# else
+inline int IsATTY(int fd) { return _isatty(fd); }
+# endif // GTEST_OS_WINDOWS_MOBILE
+inline int StrCaseCmp(const char* s1, const char* s2) {
+ return _stricmp(s1, s2);
+}
+inline char* StrDup(const char* src) { return _strdup(src); }
+# endif // __BORLANDC__
+
+# if GTEST_OS_WINDOWS_MOBILE
+inline int FileNo(FILE* file) { return reinterpret_cast<int>(_fileno(file)); }
+// Stat(), RmDir(), and IsDir() are not needed on Windows CE at this
+// time and thus not defined there.
+# else
+inline int FileNo(FILE* file) { return _fileno(file); }
+inline int Stat(const char* path, StatStruct* buf) { return _stat(path, buf); }
+inline int RmDir(const char* dir) { return _rmdir(dir); }
+inline bool IsDir(const StatStruct& st) {
+ return (_S_IFDIR & st.st_mode) != 0;
+}
+# endif // GTEST_OS_WINDOWS_MOBILE
+
+#else
+
+typedef struct stat StatStruct;
+
+inline int FileNo(FILE* file) { return fileno(file); }
+inline int IsATTY(int fd) { return isatty(fd); }
+inline int Stat(const char* path, StatStruct* buf) { return stat(path, buf); }
+inline int StrCaseCmp(const char* s1, const char* s2) {
+ return strcasecmp(s1, s2);
+}
+inline char* StrDup(const char* src) { return strdup(src); }
+inline int RmDir(const char* dir) { return rmdir(dir); }
+inline bool IsDir(const StatStruct& st) { return S_ISDIR(st.st_mode); }
+
+#endif // GTEST_OS_WINDOWS
+
+// Functions deprecated by MSVC 8.0.
+
+#ifdef _MSC_VER
+// Temporarily disable warning 4996 (deprecated function).
+# pragma warning(push)
+# pragma warning(disable:4996)
+#endif
+
+inline const char* StrNCpy(char* dest, const char* src, size_t n) {
+ return strncpy(dest, src, n);
+}
+
+// ChDir(), FReopen(), FDOpen(), Read(), Write(), Close(), and
+// StrError() aren't needed on Windows CE at this time and thus not
+// defined there.
+
+#if !GTEST_OS_WINDOWS_MOBILE
+inline int ChDir(const char* dir) { return chdir(dir); }
+#endif
+inline FILE* FOpen(const char* path, const char* mode) {
+ return fopen(path, mode);
+}
+#if !GTEST_OS_WINDOWS_MOBILE
+inline FILE *FReopen(const char* path, const char* mode, FILE* stream) {
+ return freopen(path, mode, stream);
+}
+inline FILE* FDOpen(int fd, const char* mode) { return fdopen(fd, mode); }
+#endif
+inline int FClose(FILE* fp) { return fclose(fp); }
+#if !GTEST_OS_WINDOWS_MOBILE
+inline int Read(int fd, void* buf, unsigned int count) {
+ return static_cast<int>(read(fd, buf, count));
+}
+inline int Write(int fd, const void* buf, unsigned int count) {
+ return static_cast<int>(write(fd, buf, count));
+}
+inline int Close(int fd) { return close(fd); }
+inline const char* StrError(int errnum) { return strerror(errnum); }
+#endif
+inline const char* GetEnv(const char* name) {
+#if GTEST_OS_WINDOWS_MOBILE
+ // We are on Windows CE, which has no environment variables.
+ return NULL;
+#elif defined(__BORLANDC__) || defined(__SunOS_5_8) || defined(__SunOS_5_9)
+ // Environment variables which we programmatically clear will be set to the
+ // empty string rather than unset (NULL). Handle that case.
+ const char* const env = getenv(name);
+ return (env != NULL && env[0] != '\0') ? env : NULL;
+#else
+ return getenv(name);
+#endif
+}
+
+#ifdef _MSC_VER
+# pragma warning(pop) // Restores the warning state.
+#endif
+
+#if GTEST_OS_WINDOWS_MOBILE
+// Windows CE has no C library. The abort() function is used in
+// several places in Google Test. This implementation provides a reasonable
+// imitation of standard behaviour.
+void Abort();
+#else
+inline void Abort() { abort(); }
+#endif // GTEST_OS_WINDOWS_MOBILE
+
+} // namespace posix
+
+// MSVC "deprecates" snprintf and issues warnings wherever it is used. In
+// order to avoid these warnings, we need to use _snprintf or _snprintf_s on
+// MSVC-based platforms. We map the GTEST_SNPRINTF_ macro to the appropriate
+// function in order to achieve that. We use macro definition here because
+// snprintf is a variadic function.
+#if _MSC_VER >= 1400 && !GTEST_OS_WINDOWS_MOBILE
+// MSVC 2005 and above support variadic macros.
+# define GTEST_SNPRINTF_(buffer, size, format, ...) \
+ _snprintf_s(buffer, size, size, format, __VA_ARGS__)
+#elif defined(_MSC_VER)
+// Windows CE does not define _snprintf_s and MSVC prior to 2005 doesn't
+// complain about _snprintf.
+# define GTEST_SNPRINTF_ _snprintf
+#else
+# define GTEST_SNPRINTF_ snprintf
+#endif
+
+// The maximum number a BiggestInt can represent. This definition
+// works no matter BiggestInt is represented in one's complement or
+// two's complement.
+//
+// We cannot rely on numeric_limits in STL, as __int64 and long long
+// are not part of standard C++ and numeric_limits doesn't need to be
+// defined for them.
+const BiggestInt kMaxBiggestInt =
+ ~(static_cast<BiggestInt>(1) << (8*sizeof(BiggestInt) - 1));
+
+// This template class serves as a compile-time function from size to
+// type. It maps a size in bytes to a primitive type with that
+// size. e.g.
+//
+// TypeWithSize<4>::UInt
+//
+// is typedef-ed to be unsigned int (unsigned integer made up of 4
+// bytes).
+//
+// Such functionality should belong to STL, but I cannot find it
+// there.
+//
+// Google Test uses this class in the implementation of floating-point
+// comparison.
+//
+// For now it only handles UInt (unsigned int) as that's all Google Test
+// needs. Other types can be easily added in the future if need
+// arises.
+template <size_t size>
+class TypeWithSize {
+ public:
+ // This prevents the user from using TypeWithSize<N> with incorrect
+ // values of N.
+ typedef void UInt;
+};
+
+// The specialization for size 4.
+template <>
+class TypeWithSize<4> {
+ public:
+ // unsigned int has size 4 in both gcc and MSVC.
+ //
+ // As base/basictypes.h doesn't compile on Windows, we cannot use
+ // uint32, uint64, and etc here.
+ typedef int Int;
+ typedef unsigned int UInt;
+};
+
+// The specialization for size 8.
+template <>
+class TypeWithSize<8> {
+ public:
+#if GTEST_OS_WINDOWS
+ typedef __int64 Int;
+ typedef unsigned __int64 UInt;
+#else
+ typedef long long Int; // NOLINT
+ typedef unsigned long long UInt; // NOLINT
+#endif // GTEST_OS_WINDOWS
+};
+
+// Integer types of known sizes.
+typedef TypeWithSize<4>::Int Int32;
+typedef TypeWithSize<4>::UInt UInt32;
+typedef TypeWithSize<8>::Int Int64;
+typedef TypeWithSize<8>::UInt UInt64;
+typedef TypeWithSize<8>::Int TimeInMillis; // Represents time in milliseconds.
+
+// Utilities for command line flags and environment variables.
+
+// Macro for referencing flags.
+#define GTEST_FLAG(name) FLAGS_gtest_##name
+
+// Macros for declaring flags.
+#define GTEST_DECLARE_bool_(name) GTEST_API_ extern bool GTEST_FLAG(name)
+#define GTEST_DECLARE_int32_(name) \
+ GTEST_API_ extern ::testing::internal::Int32 GTEST_FLAG(name)
+#define GTEST_DECLARE_string_(name) \
+ GTEST_API_ extern ::std::string GTEST_FLAG(name)
+
+// Macros for defining flags.
+#define GTEST_DEFINE_bool_(name, default_val, doc) \
+ GTEST_API_ bool GTEST_FLAG(name) = (default_val)
+#define GTEST_DEFINE_int32_(name, default_val, doc) \
+ GTEST_API_ ::testing::internal::Int32 GTEST_FLAG(name) = (default_val)
+#define GTEST_DEFINE_string_(name, default_val, doc) \
+ GTEST_API_ ::std::string GTEST_FLAG(name) = (default_val)
+
+// Thread annotations
+#define GTEST_EXCLUSIVE_LOCK_REQUIRED_(locks)
+#define GTEST_LOCK_EXCLUDED_(locks)
+
+// Parses 'str' for a 32-bit signed integer. If successful, writes the result
+// to *value and returns true; otherwise leaves *value unchanged and returns
+// false.
+// TODO(chandlerc): Find a better way to refactor flag and environment parsing
+// out of both gtest-port.cc and gtest.cc to avoid exporting this utility
+// function.
+bool ParseInt32(const Message& src_text, const char* str, Int32* value);
+
+// Parses a bool/Int32/string from the environment variable
+// corresponding to the given Google Test flag.
+bool BoolFromGTestEnv(const char* flag, bool default_val);
+GTEST_API_ Int32 Int32FromGTestEnv(const char* flag, Int32 default_val);
+const char* StringFromGTestEnv(const char* flag, const char* default_val);
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_PORT_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-string.h b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-string.h
new file mode 100644
index 0000000..97f1a7f
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-string.h
@@ -0,0 +1,167 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Authors: wan at google.com (Zhanyong Wan), eefacm at gmail.com (Sean Mcafee)
+//
+// The Google C++ Testing Framework (Google Test)
+//
+// This header file declares the String class and functions used internally by
+// Google Test. They are subject to change without notice. They should not used
+// by code external to Google Test.
+//
+// This header file is #included by <gtest/internal/gtest-internal.h>.
+// It should not be #included by other files.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_STRING_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_STRING_H_
+
+#ifdef __BORLANDC__
+// string.h is not guaranteed to provide strcpy on C++ Builder.
+# include <mem.h>
+#endif
+
+#include <string.h>
+#include <string>
+
+#include "gtest/internal/gtest-port.h"
+
+namespace testing {
+namespace internal {
+
+// String - an abstract class holding static string utilities.
+class GTEST_API_ String {
+ public:
+ // Static utility methods
+
+ // Clones a 0-terminated C string, allocating memory using new. The
+ // caller is responsible for deleting the return value using
+ // delete[]. Returns the cloned string, or NULL if the input is
+ // NULL.
+ //
+ // This is different from strdup() in string.h, which allocates
+ // memory using malloc().
+ static const char* CloneCString(const char* c_str);
+
+#if GTEST_OS_WINDOWS_MOBILE
+ // Windows CE does not have the 'ANSI' versions of Win32 APIs. To be
+ // able to pass strings to Win32 APIs on CE we need to convert them
+ // to 'Unicode', UTF-16.
+
+ // Creates a UTF-16 wide string from the given ANSI string, allocating
+ // memory using new. The caller is responsible for deleting the return
+ // value using delete[]. Returns the wide string, or NULL if the
+ // input is NULL.
+ //
+ // The wide string is created using the ANSI codepage (CP_ACP) to
+ // match the behaviour of the ANSI versions of Win32 calls and the
+ // C runtime.
+ static LPCWSTR AnsiToUtf16(const char* c_str);
+
+ // Creates an ANSI string from the given wide string, allocating
+ // memory using new. The caller is responsible for deleting the return
+ // value using delete[]. Returns the ANSI string, or NULL if the
+ // input is NULL.
+ //
+ // The returned string is created using the ANSI codepage (CP_ACP) to
+ // match the behaviour of the ANSI versions of Win32 calls and the
+ // C runtime.
+ static const char* Utf16ToAnsi(LPCWSTR utf16_str);
+#endif
+
+ // Compares two C strings. Returns true iff they have the same content.
+ //
+ // Unlike strcmp(), this function can handle NULL argument(s). A
+ // NULL C string is considered different to any non-NULL C string,
+ // including the empty string.
+ static bool CStringEquals(const char* lhs, const char* rhs);
+
+ // Converts a wide C string to a String using the UTF-8 encoding.
+ // NULL will be converted to "(null)". If an error occurred during
+ // the conversion, "(failed to convert from wide string)" is
+ // returned.
+ static std::string ShowWideCString(const wchar_t* wide_c_str);
+
+ // Compares two wide C strings. Returns true iff they have the same
+ // content.
+ //
+ // Unlike wcscmp(), this function can handle NULL argument(s). A
+ // NULL C string is considered different to any non-NULL C string,
+ // including the empty string.
+ static bool WideCStringEquals(const wchar_t* lhs, const wchar_t* rhs);
+
+ // Compares two C strings, ignoring case. Returns true iff they
+ // have the same content.
+ //
+ // Unlike strcasecmp(), this function can handle NULL argument(s).
+ // A NULL C string is considered different to any non-NULL C string,
+ // including the empty string.
+ static bool CaseInsensitiveCStringEquals(const char* lhs,
+ const char* rhs);
+
+ // Compares two wide C strings, ignoring case. Returns true iff they
+ // have the same content.
+ //
+ // Unlike wcscasecmp(), this function can handle NULL argument(s).
+ // A NULL C string is considered different to any non-NULL wide C string,
+ // including the empty string.
+ // NB: The implementations on different platforms slightly differ.
+ // On windows, this method uses _wcsicmp which compares according to LC_CTYPE
+ // environment variable. On GNU platform this method uses wcscasecmp
+ // which compares according to LC_CTYPE category of the current locale.
+ // On MacOS X, it uses towlower, which also uses LC_CTYPE category of the
+ // current locale.
+ static bool CaseInsensitiveWideCStringEquals(const wchar_t* lhs,
+ const wchar_t* rhs);
+
+ // Returns true iff the given string ends with the given suffix, ignoring
+ // case. Any string is considered to end with an empty suffix.
+ static bool EndsWithCaseInsensitive(
+ const std::string& str, const std::string& suffix);
+
+ // Formats an int value as "%02d".
+ static std::string FormatIntWidth2(int value); // "%02d" for width == 2
+
+ // Formats an int value as "%X".
+ static std::string FormatHexInt(int value);
+
+ // Formats a byte as "%02X".
+ static std::string FormatByte(unsigned char value);
+
+ private:
+ String(); // Not meant to be instantiated.
+}; // class String
+
+// Gets the content of the stringstream's buffer as an std::string. Each '\0'
+// character in the buffer is replaced with "\\0".
+GTEST_API_ std::string StringStreamToString(::std::stringstream* stream);
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_STRING_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-tuple.h b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-tuple.h
new file mode 100644
index 0000000..7b3dfc3
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-tuple.h
@@ -0,0 +1,1012 @@
+// This file was GENERATED by command:
+// pump.py gtest-tuple.h.pump
+// DO NOT EDIT BY HAND!!!
+
+// Copyright 2009 Google Inc.
+// All Rights Reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+
+// Implements a subset of TR1 tuple needed by Google Test and Google Mock.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TUPLE_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TUPLE_H_
+
+#include <utility> // For ::std::pair.
+
+// The compiler used in Symbian has a bug that prevents us from declaring the
+// tuple template as a friend (it complains that tuple is redefined). This
+// hack bypasses the bug by declaring the members that should otherwise be
+// private as public.
+// Sun Studio versions < 12 also have the above bug.
+#if defined(__SYMBIAN32__) || (defined(__SUNPRO_CC) && __SUNPRO_CC < 0x590)
+# define GTEST_DECLARE_TUPLE_AS_FRIEND_ public:
+#else
+# define GTEST_DECLARE_TUPLE_AS_FRIEND_ \
+ template <GTEST_10_TYPENAMES_(U)> friend class tuple; \
+ private:
+#endif
+
+// GTEST_n_TUPLE_(T) is the type of an n-tuple.
+#define GTEST_0_TUPLE_(T) tuple<>
+#define GTEST_1_TUPLE_(T) tuple<T##0, void, void, void, void, void, void, \
+ void, void, void>
+#define GTEST_2_TUPLE_(T) tuple<T##0, T##1, void, void, void, void, void, \
+ void, void, void>
+#define GTEST_3_TUPLE_(T) tuple<T##0, T##1, T##2, void, void, void, void, \
+ void, void, void>
+#define GTEST_4_TUPLE_(T) tuple<T##0, T##1, T##2, T##3, void, void, void, \
+ void, void, void>
+#define GTEST_5_TUPLE_(T) tuple<T##0, T##1, T##2, T##3, T##4, void, void, \
+ void, void, void>
+#define GTEST_6_TUPLE_(T) tuple<T##0, T##1, T##2, T##3, T##4, T##5, void, \
+ void, void, void>
+#define GTEST_7_TUPLE_(T) tuple<T##0, T##1, T##2, T##3, T##4, T##5, T##6, \
+ void, void, void>
+#define GTEST_8_TUPLE_(T) tuple<T##0, T##1, T##2, T##3, T##4, T##5, T##6, \
+ T##7, void, void>
+#define GTEST_9_TUPLE_(T) tuple<T##0, T##1, T##2, T##3, T##4, T##5, T##6, \
+ T##7, T##8, void>
+#define GTEST_10_TUPLE_(T) tuple<T##0, T##1, T##2, T##3, T##4, T##5, T##6, \
+ T##7, T##8, T##9>
+
+// GTEST_n_TYPENAMES_(T) declares a list of n typenames.
+#define GTEST_0_TYPENAMES_(T)
+#define GTEST_1_TYPENAMES_(T) typename T##0
+#define GTEST_2_TYPENAMES_(T) typename T##0, typename T##1
+#define GTEST_3_TYPENAMES_(T) typename T##0, typename T##1, typename T##2
+#define GTEST_4_TYPENAMES_(T) typename T##0, typename T##1, typename T##2, \
+ typename T##3
+#define GTEST_5_TYPENAMES_(T) typename T##0, typename T##1, typename T##2, \
+ typename T##3, typename T##4
+#define GTEST_6_TYPENAMES_(T) typename T##0, typename T##1, typename T##2, \
+ typename T##3, typename T##4, typename T##5
+#define GTEST_7_TYPENAMES_(T) typename T##0, typename T##1, typename T##2, \
+ typename T##3, typename T##4, typename T##5, typename T##6
+#define GTEST_8_TYPENAMES_(T) typename T##0, typename T##1, typename T##2, \
+ typename T##3, typename T##4, typename T##5, typename T##6, typename T##7
+#define GTEST_9_TYPENAMES_(T) typename T##0, typename T##1, typename T##2, \
+ typename T##3, typename T##4, typename T##5, typename T##6, \
+ typename T##7, typename T##8
+#define GTEST_10_TYPENAMES_(T) typename T##0, typename T##1, typename T##2, \
+ typename T##3, typename T##4, typename T##5, typename T##6, \
+ typename T##7, typename T##8, typename T##9
+
+// In theory, defining stuff in the ::std namespace is undefined
+// behavior. We can do this as we are playing the role of a standard
+// library vendor.
+namespace std {
+namespace tr1 {
+
+template <typename T0 = void, typename T1 = void, typename T2 = void,
+ typename T3 = void, typename T4 = void, typename T5 = void,
+ typename T6 = void, typename T7 = void, typename T8 = void,
+ typename T9 = void>
+class tuple;
+
+// Anything in namespace gtest_internal is Google Test's INTERNAL
+// IMPLEMENTATION DETAIL and MUST NOT BE USED DIRECTLY in user code.
+namespace gtest_internal {
+
+// ByRef<T>::type is T if T is a reference; otherwise it's const T&.
+template <typename T>
+struct ByRef { typedef const T& type; }; // NOLINT
+template <typename T>
+struct ByRef<T&> { typedef T& type; }; // NOLINT
+
+// A handy wrapper for ByRef.
+#define GTEST_BY_REF_(T) typename ::std::tr1::gtest_internal::ByRef<T>::type
+
+// AddRef<T>::type is T if T is a reference; otherwise it's T&. This
+// is the same as tr1::add_reference<T>::type.
+template <typename T>
+struct AddRef { typedef T& type; }; // NOLINT
+template <typename T>
+struct AddRef<T&> { typedef T& type; }; // NOLINT
+
+// A handy wrapper for AddRef.
+#define GTEST_ADD_REF_(T) typename ::std::tr1::gtest_internal::AddRef<T>::type
+
+// A helper for implementing get<k>().
+template <int k> class Get;
+
+// A helper for implementing tuple_element<k, T>. kIndexValid is true
+// iff k < the number of fields in tuple type T.
+template <bool kIndexValid, int kIndex, class Tuple>
+struct TupleElement;
+
+template <GTEST_10_TYPENAMES_(T)>
+struct TupleElement<true, 0, GTEST_10_TUPLE_(T) > {
+ typedef T0 type;
+};
+
+template <GTEST_10_TYPENAMES_(T)>
+struct TupleElement<true, 1, GTEST_10_TUPLE_(T) > {
+ typedef T1 type;
+};
+
+template <GTEST_10_TYPENAMES_(T)>
+struct TupleElement<true, 2, GTEST_10_TUPLE_(T) > {
+ typedef T2 type;
+};
+
+template <GTEST_10_TYPENAMES_(T)>
+struct TupleElement<true, 3, GTEST_10_TUPLE_(T) > {
+ typedef T3 type;
+};
+
+template <GTEST_10_TYPENAMES_(T)>
+struct TupleElement<true, 4, GTEST_10_TUPLE_(T) > {
+ typedef T4 type;
+};
+
+template <GTEST_10_TYPENAMES_(T)>
+struct TupleElement<true, 5, GTEST_10_TUPLE_(T) > {
+ typedef T5 type;
+};
+
+template <GTEST_10_TYPENAMES_(T)>
+struct TupleElement<true, 6, GTEST_10_TUPLE_(T) > {
+ typedef T6 type;
+};
+
+template <GTEST_10_TYPENAMES_(T)>
+struct TupleElement<true, 7, GTEST_10_TUPLE_(T) > {
+ typedef T7 type;
+};
+
+template <GTEST_10_TYPENAMES_(T)>
+struct TupleElement<true, 8, GTEST_10_TUPLE_(T) > {
+ typedef T8 type;
+};
+
+template <GTEST_10_TYPENAMES_(T)>
+struct TupleElement<true, 9, GTEST_10_TUPLE_(T) > {
+ typedef T9 type;
+};
+
+} // namespace gtest_internal
+
+template <>
+class tuple<> {
+ public:
+ tuple() {}
+ tuple(const tuple& /* t */) {}
+ tuple& operator=(const tuple& /* t */) { return *this; }
+};
+
+template <GTEST_1_TYPENAMES_(T)>
+class GTEST_1_TUPLE_(T) {
+ public:
+ template <int k> friend class gtest_internal::Get;
+
+ tuple() : f0_() {}
+
+ explicit tuple(GTEST_BY_REF_(T0) f0) : f0_(f0) {}
+
+ tuple(const tuple& t) : f0_(t.f0_) {}
+
+ template <GTEST_1_TYPENAMES_(U)>
+ tuple(const GTEST_1_TUPLE_(U)& t) : f0_(t.f0_) {}
+
+ tuple& operator=(const tuple& t) { return CopyFrom(t); }
+
+ template <GTEST_1_TYPENAMES_(U)>
+ tuple& operator=(const GTEST_1_TUPLE_(U)& t) {
+ return CopyFrom(t);
+ }
+
+ GTEST_DECLARE_TUPLE_AS_FRIEND_
+
+ template <GTEST_1_TYPENAMES_(U)>
+ tuple& CopyFrom(const GTEST_1_TUPLE_(U)& t) {
+ f0_ = t.f0_;
+ return *this;
+ }
+
+ T0 f0_;
+};
+
+template <GTEST_2_TYPENAMES_(T)>
+class GTEST_2_TUPLE_(T) {
+ public:
+ template <int k> friend class gtest_internal::Get;
+
+ tuple() : f0_(), f1_() {}
+
+ explicit tuple(GTEST_BY_REF_(T0) f0, GTEST_BY_REF_(T1) f1) : f0_(f0),
+ f1_(f1) {}
+
+ tuple(const tuple& t) : f0_(t.f0_), f1_(t.f1_) {}
+
+ template <GTEST_2_TYPENAMES_(U)>
+ tuple(const GTEST_2_TUPLE_(U)& t) : f0_(t.f0_), f1_(t.f1_) {}
+ template <typename U0, typename U1>
+ tuple(const ::std::pair<U0, U1>& p) : f0_(p.first), f1_(p.second) {}
+
+ tuple& operator=(const tuple& t) { return CopyFrom(t); }
+
+ template <GTEST_2_TYPENAMES_(U)>
+ tuple& operator=(const GTEST_2_TUPLE_(U)& t) {
+ return CopyFrom(t);
+ }
+ template <typename U0, typename U1>
+ tuple& operator=(const ::std::pair<U0, U1>& p) {
+ f0_ = p.first;
+ f1_ = p.second;
+ return *this;
+ }
+
+ GTEST_DECLARE_TUPLE_AS_FRIEND_
+
+ template <GTEST_2_TYPENAMES_(U)>
+ tuple& CopyFrom(const GTEST_2_TUPLE_(U)& t) {
+ f0_ = t.f0_;
+ f1_ = t.f1_;
+ return *this;
+ }
+
+ T0 f0_;
+ T1 f1_;
+};
+
+template <GTEST_3_TYPENAMES_(T)>
+class GTEST_3_TUPLE_(T) {
+ public:
+ template <int k> friend class gtest_internal::Get;
+
+ tuple() : f0_(), f1_(), f2_() {}
+
+ explicit tuple(GTEST_BY_REF_(T0) f0, GTEST_BY_REF_(T1) f1,
+ GTEST_BY_REF_(T2) f2) : f0_(f0), f1_(f1), f2_(f2) {}
+
+ tuple(const tuple& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_) {}
+
+ template <GTEST_3_TYPENAMES_(U)>
+ tuple(const GTEST_3_TUPLE_(U)& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_) {}
+
+ tuple& operator=(const tuple& t) { return CopyFrom(t); }
+
+ template <GTEST_3_TYPENAMES_(U)>
+ tuple& operator=(const GTEST_3_TUPLE_(U)& t) {
+ return CopyFrom(t);
+ }
+
+ GTEST_DECLARE_TUPLE_AS_FRIEND_
+
+ template <GTEST_3_TYPENAMES_(U)>
+ tuple& CopyFrom(const GTEST_3_TUPLE_(U)& t) {
+ f0_ = t.f0_;
+ f1_ = t.f1_;
+ f2_ = t.f2_;
+ return *this;
+ }
+
+ T0 f0_;
+ T1 f1_;
+ T2 f2_;
+};
+
+template <GTEST_4_TYPENAMES_(T)>
+class GTEST_4_TUPLE_(T) {
+ public:
+ template <int k> friend class gtest_internal::Get;
+
+ tuple() : f0_(), f1_(), f2_(), f3_() {}
+
+ explicit tuple(GTEST_BY_REF_(T0) f0, GTEST_BY_REF_(T1) f1,
+ GTEST_BY_REF_(T2) f2, GTEST_BY_REF_(T3) f3) : f0_(f0), f1_(f1), f2_(f2),
+ f3_(f3) {}
+
+ tuple(const tuple& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_), f3_(t.f3_) {}
+
+ template <GTEST_4_TYPENAMES_(U)>
+ tuple(const GTEST_4_TUPLE_(U)& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_),
+ f3_(t.f3_) {}
+
+ tuple& operator=(const tuple& t) { return CopyFrom(t); }
+
+ template <GTEST_4_TYPENAMES_(U)>
+ tuple& operator=(const GTEST_4_TUPLE_(U)& t) {
+ return CopyFrom(t);
+ }
+
+ GTEST_DECLARE_TUPLE_AS_FRIEND_
+
+ template <GTEST_4_TYPENAMES_(U)>
+ tuple& CopyFrom(const GTEST_4_TUPLE_(U)& t) {
+ f0_ = t.f0_;
+ f1_ = t.f1_;
+ f2_ = t.f2_;
+ f3_ = t.f3_;
+ return *this;
+ }
+
+ T0 f0_;
+ T1 f1_;
+ T2 f2_;
+ T3 f3_;
+};
+
+template <GTEST_5_TYPENAMES_(T)>
+class GTEST_5_TUPLE_(T) {
+ public:
+ template <int k> friend class gtest_internal::Get;
+
+ tuple() : f0_(), f1_(), f2_(), f3_(), f4_() {}
+
+ explicit tuple(GTEST_BY_REF_(T0) f0, GTEST_BY_REF_(T1) f1,
+ GTEST_BY_REF_(T2) f2, GTEST_BY_REF_(T3) f3,
+ GTEST_BY_REF_(T4) f4) : f0_(f0), f1_(f1), f2_(f2), f3_(f3), f4_(f4) {}
+
+ tuple(const tuple& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_), f3_(t.f3_),
+ f4_(t.f4_) {}
+
+ template <GTEST_5_TYPENAMES_(U)>
+ tuple(const GTEST_5_TUPLE_(U)& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_),
+ f3_(t.f3_), f4_(t.f4_) {}
+
+ tuple& operator=(const tuple& t) { return CopyFrom(t); }
+
+ template <GTEST_5_TYPENAMES_(U)>
+ tuple& operator=(const GTEST_5_TUPLE_(U)& t) {
+ return CopyFrom(t);
+ }
+
+ GTEST_DECLARE_TUPLE_AS_FRIEND_
+
+ template <GTEST_5_TYPENAMES_(U)>
+ tuple& CopyFrom(const GTEST_5_TUPLE_(U)& t) {
+ f0_ = t.f0_;
+ f1_ = t.f1_;
+ f2_ = t.f2_;
+ f3_ = t.f3_;
+ f4_ = t.f4_;
+ return *this;
+ }
+
+ T0 f0_;
+ T1 f1_;
+ T2 f2_;
+ T3 f3_;
+ T4 f4_;
+};
+
+template <GTEST_6_TYPENAMES_(T)>
+class GTEST_6_TUPLE_(T) {
+ public:
+ template <int k> friend class gtest_internal::Get;
+
+ tuple() : f0_(), f1_(), f2_(), f3_(), f4_(), f5_() {}
+
+ explicit tuple(GTEST_BY_REF_(T0) f0, GTEST_BY_REF_(T1) f1,
+ GTEST_BY_REF_(T2) f2, GTEST_BY_REF_(T3) f3, GTEST_BY_REF_(T4) f4,
+ GTEST_BY_REF_(T5) f5) : f0_(f0), f1_(f1), f2_(f2), f3_(f3), f4_(f4),
+ f5_(f5) {}
+
+ tuple(const tuple& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_), f3_(t.f3_),
+ f4_(t.f4_), f5_(t.f5_) {}
+
+ template <GTEST_6_TYPENAMES_(U)>
+ tuple(const GTEST_6_TUPLE_(U)& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_),
+ f3_(t.f3_), f4_(t.f4_), f5_(t.f5_) {}
+
+ tuple& operator=(const tuple& t) { return CopyFrom(t); }
+
+ template <GTEST_6_TYPENAMES_(U)>
+ tuple& operator=(const GTEST_6_TUPLE_(U)& t) {
+ return CopyFrom(t);
+ }
+
+ GTEST_DECLARE_TUPLE_AS_FRIEND_
+
+ template <GTEST_6_TYPENAMES_(U)>
+ tuple& CopyFrom(const GTEST_6_TUPLE_(U)& t) {
+ f0_ = t.f0_;
+ f1_ = t.f1_;
+ f2_ = t.f2_;
+ f3_ = t.f3_;
+ f4_ = t.f4_;
+ f5_ = t.f5_;
+ return *this;
+ }
+
+ T0 f0_;
+ T1 f1_;
+ T2 f2_;
+ T3 f3_;
+ T4 f4_;
+ T5 f5_;
+};
+
+template <GTEST_7_TYPENAMES_(T)>
+class GTEST_7_TUPLE_(T) {
+ public:
+ template <int k> friend class gtest_internal::Get;
+
+ tuple() : f0_(), f1_(), f2_(), f3_(), f4_(), f5_(), f6_() {}
+
+ explicit tuple(GTEST_BY_REF_(T0) f0, GTEST_BY_REF_(T1) f1,
+ GTEST_BY_REF_(T2) f2, GTEST_BY_REF_(T3) f3, GTEST_BY_REF_(T4) f4,
+ GTEST_BY_REF_(T5) f5, GTEST_BY_REF_(T6) f6) : f0_(f0), f1_(f1), f2_(f2),
+ f3_(f3), f4_(f4), f5_(f5), f6_(f6) {}
+
+ tuple(const tuple& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_), f3_(t.f3_),
+ f4_(t.f4_), f5_(t.f5_), f6_(t.f6_) {}
+
+ template <GTEST_7_TYPENAMES_(U)>
+ tuple(const GTEST_7_TUPLE_(U)& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_),
+ f3_(t.f3_), f4_(t.f4_), f5_(t.f5_), f6_(t.f6_) {}
+
+ tuple& operator=(const tuple& t) { return CopyFrom(t); }
+
+ template <GTEST_7_TYPENAMES_(U)>
+ tuple& operator=(const GTEST_7_TUPLE_(U)& t) {
+ return CopyFrom(t);
+ }
+
+ GTEST_DECLARE_TUPLE_AS_FRIEND_
+
+ template <GTEST_7_TYPENAMES_(U)>
+ tuple& CopyFrom(const GTEST_7_TUPLE_(U)& t) {
+ f0_ = t.f0_;
+ f1_ = t.f1_;
+ f2_ = t.f2_;
+ f3_ = t.f3_;
+ f4_ = t.f4_;
+ f5_ = t.f5_;
+ f6_ = t.f6_;
+ return *this;
+ }
+
+ T0 f0_;
+ T1 f1_;
+ T2 f2_;
+ T3 f3_;
+ T4 f4_;
+ T5 f5_;
+ T6 f6_;
+};
+
+template <GTEST_8_TYPENAMES_(T)>
+class GTEST_8_TUPLE_(T) {
+ public:
+ template <int k> friend class gtest_internal::Get;
+
+ tuple() : f0_(), f1_(), f2_(), f3_(), f4_(), f5_(), f6_(), f7_() {}
+
+ explicit tuple(GTEST_BY_REF_(T0) f0, GTEST_BY_REF_(T1) f1,
+ GTEST_BY_REF_(T2) f2, GTEST_BY_REF_(T3) f3, GTEST_BY_REF_(T4) f4,
+ GTEST_BY_REF_(T5) f5, GTEST_BY_REF_(T6) f6,
+ GTEST_BY_REF_(T7) f7) : f0_(f0), f1_(f1), f2_(f2), f3_(f3), f4_(f4),
+ f5_(f5), f6_(f6), f7_(f7) {}
+
+ tuple(const tuple& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_), f3_(t.f3_),
+ f4_(t.f4_), f5_(t.f5_), f6_(t.f6_), f7_(t.f7_) {}
+
+ template <GTEST_8_TYPENAMES_(U)>
+ tuple(const GTEST_8_TUPLE_(U)& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_),
+ f3_(t.f3_), f4_(t.f4_), f5_(t.f5_), f6_(t.f6_), f7_(t.f7_) {}
+
+ tuple& operator=(const tuple& t) { return CopyFrom(t); }
+
+ template <GTEST_8_TYPENAMES_(U)>
+ tuple& operator=(const GTEST_8_TUPLE_(U)& t) {
+ return CopyFrom(t);
+ }
+
+ GTEST_DECLARE_TUPLE_AS_FRIEND_
+
+ template <GTEST_8_TYPENAMES_(U)>
+ tuple& CopyFrom(const GTEST_8_TUPLE_(U)& t) {
+ f0_ = t.f0_;
+ f1_ = t.f1_;
+ f2_ = t.f2_;
+ f3_ = t.f3_;
+ f4_ = t.f4_;
+ f5_ = t.f5_;
+ f6_ = t.f6_;
+ f7_ = t.f7_;
+ return *this;
+ }
+
+ T0 f0_;
+ T1 f1_;
+ T2 f2_;
+ T3 f3_;
+ T4 f4_;
+ T5 f5_;
+ T6 f6_;
+ T7 f7_;
+};
+
+template <GTEST_9_TYPENAMES_(T)>
+class GTEST_9_TUPLE_(T) {
+ public:
+ template <int k> friend class gtest_internal::Get;
+
+ tuple() : f0_(), f1_(), f2_(), f3_(), f4_(), f5_(), f6_(), f7_(), f8_() {}
+
+ explicit tuple(GTEST_BY_REF_(T0) f0, GTEST_BY_REF_(T1) f1,
+ GTEST_BY_REF_(T2) f2, GTEST_BY_REF_(T3) f3, GTEST_BY_REF_(T4) f4,
+ GTEST_BY_REF_(T5) f5, GTEST_BY_REF_(T6) f6, GTEST_BY_REF_(T7) f7,
+ GTEST_BY_REF_(T8) f8) : f0_(f0), f1_(f1), f2_(f2), f3_(f3), f4_(f4),
+ f5_(f5), f6_(f6), f7_(f7), f8_(f8) {}
+
+ tuple(const tuple& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_), f3_(t.f3_),
+ f4_(t.f4_), f5_(t.f5_), f6_(t.f6_), f7_(t.f7_), f8_(t.f8_) {}
+
+ template <GTEST_9_TYPENAMES_(U)>
+ tuple(const GTEST_9_TUPLE_(U)& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_),
+ f3_(t.f3_), f4_(t.f4_), f5_(t.f5_), f6_(t.f6_), f7_(t.f7_), f8_(t.f8_) {}
+
+ tuple& operator=(const tuple& t) { return CopyFrom(t); }
+
+ template <GTEST_9_TYPENAMES_(U)>
+ tuple& operator=(const GTEST_9_TUPLE_(U)& t) {
+ return CopyFrom(t);
+ }
+
+ GTEST_DECLARE_TUPLE_AS_FRIEND_
+
+ template <GTEST_9_TYPENAMES_(U)>
+ tuple& CopyFrom(const GTEST_9_TUPLE_(U)& t) {
+ f0_ = t.f0_;
+ f1_ = t.f1_;
+ f2_ = t.f2_;
+ f3_ = t.f3_;
+ f4_ = t.f4_;
+ f5_ = t.f5_;
+ f6_ = t.f6_;
+ f7_ = t.f7_;
+ f8_ = t.f8_;
+ return *this;
+ }
+
+ T0 f0_;
+ T1 f1_;
+ T2 f2_;
+ T3 f3_;
+ T4 f4_;
+ T5 f5_;
+ T6 f6_;
+ T7 f7_;
+ T8 f8_;
+};
+
+template <GTEST_10_TYPENAMES_(T)>
+class tuple {
+ public:
+ template <int k> friend class gtest_internal::Get;
+
+ tuple() : f0_(), f1_(), f2_(), f3_(), f4_(), f5_(), f6_(), f7_(), f8_(),
+ f9_() {}
+
+ explicit tuple(GTEST_BY_REF_(T0) f0, GTEST_BY_REF_(T1) f1,
+ GTEST_BY_REF_(T2) f2, GTEST_BY_REF_(T3) f3, GTEST_BY_REF_(T4) f4,
+ GTEST_BY_REF_(T5) f5, GTEST_BY_REF_(T6) f6, GTEST_BY_REF_(T7) f7,
+ GTEST_BY_REF_(T8) f8, GTEST_BY_REF_(T9) f9) : f0_(f0), f1_(f1), f2_(f2),
+ f3_(f3), f4_(f4), f5_(f5), f6_(f6), f7_(f7), f8_(f8), f9_(f9) {}
+
+ tuple(const tuple& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_), f3_(t.f3_),
+ f4_(t.f4_), f5_(t.f5_), f6_(t.f6_), f7_(t.f7_), f8_(t.f8_), f9_(t.f9_) {}
+
+ template <GTEST_10_TYPENAMES_(U)>
+ tuple(const GTEST_10_TUPLE_(U)& t) : f0_(t.f0_), f1_(t.f1_), f2_(t.f2_),
+ f3_(t.f3_), f4_(t.f4_), f5_(t.f5_), f6_(t.f6_), f7_(t.f7_), f8_(t.f8_),
+ f9_(t.f9_) {}
+
+ tuple& operator=(const tuple& t) { return CopyFrom(t); }
+
+ template <GTEST_10_TYPENAMES_(U)>
+ tuple& operator=(const GTEST_10_TUPLE_(U)& t) {
+ return CopyFrom(t);
+ }
+
+ GTEST_DECLARE_TUPLE_AS_FRIEND_
+
+ template <GTEST_10_TYPENAMES_(U)>
+ tuple& CopyFrom(const GTEST_10_TUPLE_(U)& t) {
+ f0_ = t.f0_;
+ f1_ = t.f1_;
+ f2_ = t.f2_;
+ f3_ = t.f3_;
+ f4_ = t.f4_;
+ f5_ = t.f5_;
+ f6_ = t.f6_;
+ f7_ = t.f7_;
+ f8_ = t.f8_;
+ f9_ = t.f9_;
+ return *this;
+ }
+
+ T0 f0_;
+ T1 f1_;
+ T2 f2_;
+ T3 f3_;
+ T4 f4_;
+ T5 f5_;
+ T6 f6_;
+ T7 f7_;
+ T8 f8_;
+ T9 f9_;
+};
+
+// 6.1.3.2 Tuple creation functions.
+
+// Known limitations: we don't support passing an
+// std::tr1::reference_wrapper<T> to make_tuple(). And we don't
+// implement tie().
+
+inline tuple<> make_tuple() { return tuple<>(); }
+
+template <GTEST_1_TYPENAMES_(T)>
+inline GTEST_1_TUPLE_(T) make_tuple(const T0& f0) {
+ return GTEST_1_TUPLE_(T)(f0);
+}
+
+template <GTEST_2_TYPENAMES_(T)>
+inline GTEST_2_TUPLE_(T) make_tuple(const T0& f0, const T1& f1) {
+ return GTEST_2_TUPLE_(T)(f0, f1);
+}
+
+template <GTEST_3_TYPENAMES_(T)>
+inline GTEST_3_TUPLE_(T) make_tuple(const T0& f0, const T1& f1, const T2& f2) {
+ return GTEST_3_TUPLE_(T)(f0, f1, f2);
+}
+
+template <GTEST_4_TYPENAMES_(T)>
+inline GTEST_4_TUPLE_(T) make_tuple(const T0& f0, const T1& f1, const T2& f2,
+ const T3& f3) {
+ return GTEST_4_TUPLE_(T)(f0, f1, f2, f3);
+}
+
+template <GTEST_5_TYPENAMES_(T)>
+inline GTEST_5_TUPLE_(T) make_tuple(const T0& f0, const T1& f1, const T2& f2,
+ const T3& f3, const T4& f4) {
+ return GTEST_5_TUPLE_(T)(f0, f1, f2, f3, f4);
+}
+
+template <GTEST_6_TYPENAMES_(T)>
+inline GTEST_6_TUPLE_(T) make_tuple(const T0& f0, const T1& f1, const T2& f2,
+ const T3& f3, const T4& f4, const T5& f5) {
+ return GTEST_6_TUPLE_(T)(f0, f1, f2, f3, f4, f5);
+}
+
+template <GTEST_7_TYPENAMES_(T)>
+inline GTEST_7_TUPLE_(T) make_tuple(const T0& f0, const T1& f1, const T2& f2,
+ const T3& f3, const T4& f4, const T5& f5, const T6& f6) {
+ return GTEST_7_TUPLE_(T)(f0, f1, f2, f3, f4, f5, f6);
+}
+
+template <GTEST_8_TYPENAMES_(T)>
+inline GTEST_8_TUPLE_(T) make_tuple(const T0& f0, const T1& f1, const T2& f2,
+ const T3& f3, const T4& f4, const T5& f5, const T6& f6, const T7& f7) {
+ return GTEST_8_TUPLE_(T)(f0, f1, f2, f3, f4, f5, f6, f7);
+}
+
+template <GTEST_9_TYPENAMES_(T)>
+inline GTEST_9_TUPLE_(T) make_tuple(const T0& f0, const T1& f1, const T2& f2,
+ const T3& f3, const T4& f4, const T5& f5, const T6& f6, const T7& f7,
+ const T8& f8) {
+ return GTEST_9_TUPLE_(T)(f0, f1, f2, f3, f4, f5, f6, f7, f8);
+}
+
+template <GTEST_10_TYPENAMES_(T)>
+inline GTEST_10_TUPLE_(T) make_tuple(const T0& f0, const T1& f1, const T2& f2,
+ const T3& f3, const T4& f4, const T5& f5, const T6& f6, const T7& f7,
+ const T8& f8, const T9& f9) {
+ return GTEST_10_TUPLE_(T)(f0, f1, f2, f3, f4, f5, f6, f7, f8, f9);
+}
+
+// 6.1.3.3 Tuple helper classes.
+
+template <typename Tuple> struct tuple_size;
+
+template <GTEST_0_TYPENAMES_(T)>
+struct tuple_size<GTEST_0_TUPLE_(T) > {
+ static const int value = 0;
+};
+
+template <GTEST_1_TYPENAMES_(T)>
+struct tuple_size<GTEST_1_TUPLE_(T) > {
+ static const int value = 1;
+};
+
+template <GTEST_2_TYPENAMES_(T)>
+struct tuple_size<GTEST_2_TUPLE_(T) > {
+ static const int value = 2;
+};
+
+template <GTEST_3_TYPENAMES_(T)>
+struct tuple_size<GTEST_3_TUPLE_(T) > {
+ static const int value = 3;
+};
+
+template <GTEST_4_TYPENAMES_(T)>
+struct tuple_size<GTEST_4_TUPLE_(T) > {
+ static const int value = 4;
+};
+
+template <GTEST_5_TYPENAMES_(T)>
+struct tuple_size<GTEST_5_TUPLE_(T) > {
+ static const int value = 5;
+};
+
+template <GTEST_6_TYPENAMES_(T)>
+struct tuple_size<GTEST_6_TUPLE_(T) > {
+ static const int value = 6;
+};
+
+template <GTEST_7_TYPENAMES_(T)>
+struct tuple_size<GTEST_7_TUPLE_(T) > {
+ static const int value = 7;
+};
+
+template <GTEST_8_TYPENAMES_(T)>
+struct tuple_size<GTEST_8_TUPLE_(T) > {
+ static const int value = 8;
+};
+
+template <GTEST_9_TYPENAMES_(T)>
+struct tuple_size<GTEST_9_TUPLE_(T) > {
+ static const int value = 9;
+};
+
+template <GTEST_10_TYPENAMES_(T)>
+struct tuple_size<GTEST_10_TUPLE_(T) > {
+ static const int value = 10;
+};
+
+template <int k, class Tuple>
+struct tuple_element {
+ typedef typename gtest_internal::TupleElement<
+ k < (tuple_size<Tuple>::value), k, Tuple>::type type;
+};
+
+#define GTEST_TUPLE_ELEMENT_(k, Tuple) typename tuple_element<k, Tuple >::type
+
+// 6.1.3.4 Element access.
+
+namespace gtest_internal {
+
+template <>
+class Get<0> {
+ public:
+ template <class Tuple>
+ static GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(0, Tuple))
+ Field(Tuple& t) { return t.f0_; } // NOLINT
+
+ template <class Tuple>
+ static GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(0, Tuple))
+ ConstField(const Tuple& t) { return t.f0_; }
+};
+
+template <>
+class Get<1> {
+ public:
+ template <class Tuple>
+ static GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(1, Tuple))
+ Field(Tuple& t) { return t.f1_; } // NOLINT
+
+ template <class Tuple>
+ static GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(1, Tuple))
+ ConstField(const Tuple& t) { return t.f1_; }
+};
+
+template <>
+class Get<2> {
+ public:
+ template <class Tuple>
+ static GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(2, Tuple))
+ Field(Tuple& t) { return t.f2_; } // NOLINT
+
+ template <class Tuple>
+ static GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(2, Tuple))
+ ConstField(const Tuple& t) { return t.f2_; }
+};
+
+template <>
+class Get<3> {
+ public:
+ template <class Tuple>
+ static GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(3, Tuple))
+ Field(Tuple& t) { return t.f3_; } // NOLINT
+
+ template <class Tuple>
+ static GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(3, Tuple))
+ ConstField(const Tuple& t) { return t.f3_; }
+};
+
+template <>
+class Get<4> {
+ public:
+ template <class Tuple>
+ static GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(4, Tuple))
+ Field(Tuple& t) { return t.f4_; } // NOLINT
+
+ template <class Tuple>
+ static GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(4, Tuple))
+ ConstField(const Tuple& t) { return t.f4_; }
+};
+
+template <>
+class Get<5> {
+ public:
+ template <class Tuple>
+ static GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(5, Tuple))
+ Field(Tuple& t) { return t.f5_; } // NOLINT
+
+ template <class Tuple>
+ static GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(5, Tuple))
+ ConstField(const Tuple& t) { return t.f5_; }
+};
+
+template <>
+class Get<6> {
+ public:
+ template <class Tuple>
+ static GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(6, Tuple))
+ Field(Tuple& t) { return t.f6_; } // NOLINT
+
+ template <class Tuple>
+ static GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(6, Tuple))
+ ConstField(const Tuple& t) { return t.f6_; }
+};
+
+template <>
+class Get<7> {
+ public:
+ template <class Tuple>
+ static GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(7, Tuple))
+ Field(Tuple& t) { return t.f7_; } // NOLINT
+
+ template <class Tuple>
+ static GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(7, Tuple))
+ ConstField(const Tuple& t) { return t.f7_; }
+};
+
+template <>
+class Get<8> {
+ public:
+ template <class Tuple>
+ static GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(8, Tuple))
+ Field(Tuple& t) { return t.f8_; } // NOLINT
+
+ template <class Tuple>
+ static GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(8, Tuple))
+ ConstField(const Tuple& t) { return t.f8_; }
+};
+
+template <>
+class Get<9> {
+ public:
+ template <class Tuple>
+ static GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(9, Tuple))
+ Field(Tuple& t) { return t.f9_; } // NOLINT
+
+ template <class Tuple>
+ static GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(9, Tuple))
+ ConstField(const Tuple& t) { return t.f9_; }
+};
+
+} // namespace gtest_internal
+
+template <int k, GTEST_10_TYPENAMES_(T)>
+GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(k, GTEST_10_TUPLE_(T)))
+get(GTEST_10_TUPLE_(T)& t) {
+ return gtest_internal::Get<k>::Field(t);
+}
+
+template <int k, GTEST_10_TYPENAMES_(T)>
+GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(k, GTEST_10_TUPLE_(T)))
+get(const GTEST_10_TUPLE_(T)& t) {
+ return gtest_internal::Get<k>::ConstField(t);
+}
+
+// 6.1.3.5 Relational operators
+
+// We only implement == and !=, as we don't have a need for the rest yet.
+
+namespace gtest_internal {
+
+// SameSizeTuplePrefixComparator<k, k>::Eq(t1, t2) returns true if the
+// first k fields of t1 equals the first k fields of t2.
+// SameSizeTuplePrefixComparator(k1, k2) would be a compiler error if
+// k1 != k2.
+template <int kSize1, int kSize2>
+struct SameSizeTuplePrefixComparator;
+
+template <>
+struct SameSizeTuplePrefixComparator<0, 0> {
+ template <class Tuple1, class Tuple2>
+ static bool Eq(const Tuple1& /* t1 */, const Tuple2& /* t2 */) {
+ return true;
+ }
+};
+
+template <int k>
+struct SameSizeTuplePrefixComparator<k, k> {
+ template <class Tuple1, class Tuple2>
+ static bool Eq(const Tuple1& t1, const Tuple2& t2) {
+ return SameSizeTuplePrefixComparator<k - 1, k - 1>::Eq(t1, t2) &&
+ ::std::tr1::get<k - 1>(t1) == ::std::tr1::get<k - 1>(t2);
+ }
+};
+
+} // namespace gtest_internal
+
+template <GTEST_10_TYPENAMES_(T), GTEST_10_TYPENAMES_(U)>
+inline bool operator==(const GTEST_10_TUPLE_(T)& t,
+ const GTEST_10_TUPLE_(U)& u) {
+ return gtest_internal::SameSizeTuplePrefixComparator<
+ tuple_size<GTEST_10_TUPLE_(T) >::value,
+ tuple_size<GTEST_10_TUPLE_(U) >::value>::Eq(t, u);
+}
+
+template <GTEST_10_TYPENAMES_(T), GTEST_10_TYPENAMES_(U)>
+inline bool operator!=(const GTEST_10_TUPLE_(T)& t,
+ const GTEST_10_TUPLE_(U)& u) { return !(t == u); }
+
+// 6.1.4 Pairs.
+// Unimplemented.
+
+} // namespace tr1
+} // namespace std
+
+#undef GTEST_0_TUPLE_
+#undef GTEST_1_TUPLE_
+#undef GTEST_2_TUPLE_
+#undef GTEST_3_TUPLE_
+#undef GTEST_4_TUPLE_
+#undef GTEST_5_TUPLE_
+#undef GTEST_6_TUPLE_
+#undef GTEST_7_TUPLE_
+#undef GTEST_8_TUPLE_
+#undef GTEST_9_TUPLE_
+#undef GTEST_10_TUPLE_
+
+#undef GTEST_0_TYPENAMES_
+#undef GTEST_1_TYPENAMES_
+#undef GTEST_2_TYPENAMES_
+#undef GTEST_3_TYPENAMES_
+#undef GTEST_4_TYPENAMES_
+#undef GTEST_5_TYPENAMES_
+#undef GTEST_6_TYPENAMES_
+#undef GTEST_7_TYPENAMES_
+#undef GTEST_8_TYPENAMES_
+#undef GTEST_9_TYPENAMES_
+#undef GTEST_10_TYPENAMES_
+
+#undef GTEST_DECLARE_TUPLE_AS_FRIEND_
+#undef GTEST_BY_REF_
+#undef GTEST_ADD_REF_
+#undef GTEST_TUPLE_ELEMENT_
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TUPLE_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-tuple.h.pump b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-tuple.h.pump
new file mode 100644
index 0000000..c7d9e03
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-tuple.h.pump
@@ -0,0 +1,339 @@
+$$ -*- mode: c++; -*-
+$var n = 10 $$ Maximum number of tuple fields we want to support.
+$$ This meta comment fixes auto-indentation in Emacs. }}
+// Copyright 2009 Google Inc.
+// All Rights Reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+
+// Implements a subset of TR1 tuple needed by Google Test and Google Mock.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TUPLE_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TUPLE_H_
+
+#include <utility> // For ::std::pair.
+
+// The compiler used in Symbian has a bug that prevents us from declaring the
+// tuple template as a friend (it complains that tuple is redefined). This
+// hack bypasses the bug by declaring the members that should otherwise be
+// private as public.
+// Sun Studio versions < 12 also have the above bug.
+#if defined(__SYMBIAN32__) || (defined(__SUNPRO_CC) && __SUNPRO_CC < 0x590)
+# define GTEST_DECLARE_TUPLE_AS_FRIEND_ public:
+#else
+# define GTEST_DECLARE_TUPLE_AS_FRIEND_ \
+ template <GTEST_$(n)_TYPENAMES_(U)> friend class tuple; \
+ private:
+#endif
+
+
+$range i 0..n-1
+$range j 0..n
+$range k 1..n
+// GTEST_n_TUPLE_(T) is the type of an n-tuple.
+#define GTEST_0_TUPLE_(T) tuple<>
+
+$for k [[
+$range m 0..k-1
+$range m2 k..n-1
+#define GTEST_$(k)_TUPLE_(T) tuple<$for m, [[T##$m]]$for m2 [[, void]]>
+
+]]
+
+// GTEST_n_TYPENAMES_(T) declares a list of n typenames.
+
+$for j [[
+$range m 0..j-1
+#define GTEST_$(j)_TYPENAMES_(T) $for m, [[typename T##$m]]
+
+
+]]
+
+// In theory, defining stuff in the ::std namespace is undefined
+// behavior. We can do this as we are playing the role of a standard
+// library vendor.
+namespace std {
+namespace tr1 {
+
+template <$for i, [[typename T$i = void]]>
+class tuple;
+
+// Anything in namespace gtest_internal is Google Test's INTERNAL
+// IMPLEMENTATION DETAIL and MUST NOT BE USED DIRECTLY in user code.
+namespace gtest_internal {
+
+// ByRef<T>::type is T if T is a reference; otherwise it's const T&.
+template <typename T>
+struct ByRef { typedef const T& type; }; // NOLINT
+template <typename T>
+struct ByRef<T&> { typedef T& type; }; // NOLINT
+
+// A handy wrapper for ByRef.
+#define GTEST_BY_REF_(T) typename ::std::tr1::gtest_internal::ByRef<T>::type
+
+// AddRef<T>::type is T if T is a reference; otherwise it's T&. This
+// is the same as tr1::add_reference<T>::type.
+template <typename T>
+struct AddRef { typedef T& type; }; // NOLINT
+template <typename T>
+struct AddRef<T&> { typedef T& type; }; // NOLINT
+
+// A handy wrapper for AddRef.
+#define GTEST_ADD_REF_(T) typename ::std::tr1::gtest_internal::AddRef<T>::type
+
+// A helper for implementing get<k>().
+template <int k> class Get;
+
+// A helper for implementing tuple_element<k, T>. kIndexValid is true
+// iff k < the number of fields in tuple type T.
+template <bool kIndexValid, int kIndex, class Tuple>
+struct TupleElement;
+
+
+$for i [[
+template <GTEST_$(n)_TYPENAMES_(T)>
+struct TupleElement<true, $i, GTEST_$(n)_TUPLE_(T) > {
+ typedef T$i type;
+};
+
+
+]]
+} // namespace gtest_internal
+
+template <>
+class tuple<> {
+ public:
+ tuple() {}
+ tuple(const tuple& /* t */) {}
+ tuple& operator=(const tuple& /* t */) { return *this; }
+};
+
+
+$for k [[
+$range m 0..k-1
+template <GTEST_$(k)_TYPENAMES_(T)>
+class $if k < n [[GTEST_$(k)_TUPLE_(T)]] $else [[tuple]] {
+ public:
+ template <int k> friend class gtest_internal::Get;
+
+ tuple() : $for m, [[f$(m)_()]] {}
+
+ explicit tuple($for m, [[GTEST_BY_REF_(T$m) f$m]]) : [[]]
+$for m, [[f$(m)_(f$m)]] {}
+
+ tuple(const tuple& t) : $for m, [[f$(m)_(t.f$(m)_)]] {}
+
+ template <GTEST_$(k)_TYPENAMES_(U)>
+ tuple(const GTEST_$(k)_TUPLE_(U)& t) : $for m, [[f$(m)_(t.f$(m)_)]] {}
+
+$if k == 2 [[
+ template <typename U0, typename U1>
+ tuple(const ::std::pair<U0, U1>& p) : f0_(p.first), f1_(p.second) {}
+
+]]
+
+ tuple& operator=(const tuple& t) { return CopyFrom(t); }
+
+ template <GTEST_$(k)_TYPENAMES_(U)>
+ tuple& operator=(const GTEST_$(k)_TUPLE_(U)& t) {
+ return CopyFrom(t);
+ }
+
+$if k == 2 [[
+ template <typename U0, typename U1>
+ tuple& operator=(const ::std::pair<U0, U1>& p) {
+ f0_ = p.first;
+ f1_ = p.second;
+ return *this;
+ }
+
+]]
+
+ GTEST_DECLARE_TUPLE_AS_FRIEND_
+
+ template <GTEST_$(k)_TYPENAMES_(U)>
+ tuple& CopyFrom(const GTEST_$(k)_TUPLE_(U)& t) {
+
+$for m [[
+ f$(m)_ = t.f$(m)_;
+
+]]
+ return *this;
+ }
+
+
+$for m [[
+ T$m f$(m)_;
+
+]]
+};
+
+
+]]
+// 6.1.3.2 Tuple creation functions.
+
+// Known limitations: we don't support passing an
+// std::tr1::reference_wrapper<T> to make_tuple(). And we don't
+// implement tie().
+
+inline tuple<> make_tuple() { return tuple<>(); }
+
+$for k [[
+$range m 0..k-1
+
+template <GTEST_$(k)_TYPENAMES_(T)>
+inline GTEST_$(k)_TUPLE_(T) make_tuple($for m, [[const T$m& f$m]]) {
+ return GTEST_$(k)_TUPLE_(T)($for m, [[f$m]]);
+}
+
+]]
+
+// 6.1.3.3 Tuple helper classes.
+
+template <typename Tuple> struct tuple_size;
+
+
+$for j [[
+template <GTEST_$(j)_TYPENAMES_(T)>
+struct tuple_size<GTEST_$(j)_TUPLE_(T) > {
+ static const int value = $j;
+};
+
+
+]]
+template <int k, class Tuple>
+struct tuple_element {
+ typedef typename gtest_internal::TupleElement<
+ k < (tuple_size<Tuple>::value), k, Tuple>::type type;
+};
+
+#define GTEST_TUPLE_ELEMENT_(k, Tuple) typename tuple_element<k, Tuple >::type
+
+// 6.1.3.4 Element access.
+
+namespace gtest_internal {
+
+
+$for i [[
+template <>
+class Get<$i> {
+ public:
+ template <class Tuple>
+ static GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_($i, Tuple))
+ Field(Tuple& t) { return t.f$(i)_; } // NOLINT
+
+ template <class Tuple>
+ static GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_($i, Tuple))
+ ConstField(const Tuple& t) { return t.f$(i)_; }
+};
+
+
+]]
+} // namespace gtest_internal
+
+template <int k, GTEST_$(n)_TYPENAMES_(T)>
+GTEST_ADD_REF_(GTEST_TUPLE_ELEMENT_(k, GTEST_$(n)_TUPLE_(T)))
+get(GTEST_$(n)_TUPLE_(T)& t) {
+ return gtest_internal::Get<k>::Field(t);
+}
+
+template <int k, GTEST_$(n)_TYPENAMES_(T)>
+GTEST_BY_REF_(GTEST_TUPLE_ELEMENT_(k, GTEST_$(n)_TUPLE_(T)))
+get(const GTEST_$(n)_TUPLE_(T)& t) {
+ return gtest_internal::Get<k>::ConstField(t);
+}
+
+// 6.1.3.5 Relational operators
+
+// We only implement == and !=, as we don't have a need for the rest yet.
+
+namespace gtest_internal {
+
+// SameSizeTuplePrefixComparator<k, k>::Eq(t1, t2) returns true if the
+// first k fields of t1 equals the first k fields of t2.
+// SameSizeTuplePrefixComparator(k1, k2) would be a compiler error if
+// k1 != k2.
+template <int kSize1, int kSize2>
+struct SameSizeTuplePrefixComparator;
+
+template <>
+struct SameSizeTuplePrefixComparator<0, 0> {
+ template <class Tuple1, class Tuple2>
+ static bool Eq(const Tuple1& /* t1 */, const Tuple2& /* t2 */) {
+ return true;
+ }
+};
+
+template <int k>
+struct SameSizeTuplePrefixComparator<k, k> {
+ template <class Tuple1, class Tuple2>
+ static bool Eq(const Tuple1& t1, const Tuple2& t2) {
+ return SameSizeTuplePrefixComparator<k - 1, k - 1>::Eq(t1, t2) &&
+ ::std::tr1::get<k - 1>(t1) == ::std::tr1::get<k - 1>(t2);
+ }
+};
+
+} // namespace gtest_internal
+
+template <GTEST_$(n)_TYPENAMES_(T), GTEST_$(n)_TYPENAMES_(U)>
+inline bool operator==(const GTEST_$(n)_TUPLE_(T)& t,
+ const GTEST_$(n)_TUPLE_(U)& u) {
+ return gtest_internal::SameSizeTuplePrefixComparator<
+ tuple_size<GTEST_$(n)_TUPLE_(T) >::value,
+ tuple_size<GTEST_$(n)_TUPLE_(U) >::value>::Eq(t, u);
+}
+
+template <GTEST_$(n)_TYPENAMES_(T), GTEST_$(n)_TYPENAMES_(U)>
+inline bool operator!=(const GTEST_$(n)_TUPLE_(T)& t,
+ const GTEST_$(n)_TUPLE_(U)& u) { return !(t == u); }
+
+// 6.1.4 Pairs.
+// Unimplemented.
+
+} // namespace tr1
+} // namespace std
+
+
+$for j [[
+#undef GTEST_$(j)_TUPLE_
+
+]]
+
+
+$for j [[
+#undef GTEST_$(j)_TYPENAMES_
+
+]]
+
+#undef GTEST_DECLARE_TUPLE_AS_FRIEND_
+#undef GTEST_BY_REF_
+#undef GTEST_ADD_REF_
+#undef GTEST_TUPLE_ELEMENT_
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TUPLE_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-type-util.h b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-type-util.h
new file mode 100644
index 0000000..e46f7cf
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-type-util.h
@@ -0,0 +1,3331 @@
+// This file was GENERATED by command:
+// pump.py gtest-type-util.h.pump
+// DO NOT EDIT BY HAND!!!
+
+// Copyright 2008 Google Inc.
+// All Rights Reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+
+// Type utilities needed for implementing typed and type-parameterized
+// tests. This file is generated by a SCRIPT. DO NOT EDIT BY HAND!
+//
+// Currently we support at most 50 types in a list, and at most 50
+// type-parameterized tests in one type-parameterized test case.
+// Please contact googletestframework at googlegroups.com if you need
+// more.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TYPE_UTIL_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TYPE_UTIL_H_
+
+#include "gtest/internal/gtest-port.h"
+
+// #ifdef __GNUC__ is too general here. It is possible to use gcc without using
+// libstdc++ (which is where cxxabi.h comes from).
+# if GTEST_HAS_CXXABI_H_
+# include <cxxabi.h>
+# elif defined(__HP_aCC)
+# include <acxx_demangle.h>
+# endif // GTEST_HASH_CXXABI_H_
+
+namespace testing {
+namespace internal {
+
+// GetTypeName<T>() returns a human-readable name of type T.
+// NB: This function is also used in Google Mock, so don't move it inside of
+// the typed-test-only section below.
+template <typename T>
+std::string GetTypeName() {
+# if GTEST_HAS_RTTI
+
+ const char* const name = typeid(T).name();
+# if GTEST_HAS_CXXABI_H_ || defined(__HP_aCC)
+ int status = 0;
+ // gcc's implementation of typeid(T).name() mangles the type name,
+ // so we have to demangle it.
+# if GTEST_HAS_CXXABI_H_
+ using abi::__cxa_demangle;
+# endif // GTEST_HAS_CXXABI_H_
+ char* const readable_name = __cxa_demangle(name, 0, 0, &status);
+ const std::string name_str(status == 0 ? readable_name : name);
+ free(readable_name);
+ return name_str;
+# else
+ return name;
+# endif // GTEST_HAS_CXXABI_H_ || __HP_aCC
+
+# else
+
+ return "<type>";
+
+# endif // GTEST_HAS_RTTI
+}
+
+#if GTEST_HAS_TYPED_TEST || GTEST_HAS_TYPED_TEST_P
+
+// AssertyTypeEq<T1, T2>::type is defined iff T1 and T2 are the same
+// type. This can be used as a compile-time assertion to ensure that
+// two types are equal.
+
+template <typename T1, typename T2>
+struct AssertTypeEq;
+
+template <typename T>
+struct AssertTypeEq<T, T> {
+ typedef bool type;
+};
+
+// A unique type used as the default value for the arguments of class
+// template Types. This allows us to simulate variadic templates
+// (e.g. Types<int>, Type<int, double>, and etc), which C++ doesn't
+// support directly.
+struct None {};
+
+// The following family of struct and struct templates are used to
+// represent type lists. In particular, TypesN<T1, T2, ..., TN>
+// represents a type list with N types (T1, T2, ..., and TN) in it.
+// Except for Types0, every struct in the family has two member types:
+// Head for the first type in the list, and Tail for the rest of the
+// list.
+
+// The empty type list.
+struct Types0 {};
+
+// Type lists of length 1, 2, 3, and so on.
+
+template <typename T1>
+struct Types1 {
+ typedef T1 Head;
+ typedef Types0 Tail;
+};
+template <typename T1, typename T2>
+struct Types2 {
+ typedef T1 Head;
+ typedef Types1<T2> Tail;
+};
+
+template <typename T1, typename T2, typename T3>
+struct Types3 {
+ typedef T1 Head;
+ typedef Types2<T2, T3> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4>
+struct Types4 {
+ typedef T1 Head;
+ typedef Types3<T2, T3, T4> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5>
+struct Types5 {
+ typedef T1 Head;
+ typedef Types4<T2, T3, T4, T5> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6>
+struct Types6 {
+ typedef T1 Head;
+ typedef Types5<T2, T3, T4, T5, T6> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7>
+struct Types7 {
+ typedef T1 Head;
+ typedef Types6<T2, T3, T4, T5, T6, T7> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8>
+struct Types8 {
+ typedef T1 Head;
+ typedef Types7<T2, T3, T4, T5, T6, T7, T8> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9>
+struct Types9 {
+ typedef T1 Head;
+ typedef Types8<T2, T3, T4, T5, T6, T7, T8, T9> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10>
+struct Types10 {
+ typedef T1 Head;
+ typedef Types9<T2, T3, T4, T5, T6, T7, T8, T9, T10> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11>
+struct Types11 {
+ typedef T1 Head;
+ typedef Types10<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12>
+struct Types12 {
+ typedef T1 Head;
+ typedef Types11<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13>
+struct Types13 {
+ typedef T1 Head;
+ typedef Types12<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14>
+struct Types14 {
+ typedef T1 Head;
+ typedef Types13<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15>
+struct Types15 {
+ typedef T1 Head;
+ typedef Types14<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16>
+struct Types16 {
+ typedef T1 Head;
+ typedef Types15<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17>
+struct Types17 {
+ typedef T1 Head;
+ typedef Types16<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18>
+struct Types18 {
+ typedef T1 Head;
+ typedef Types17<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19>
+struct Types19 {
+ typedef T1 Head;
+ typedef Types18<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20>
+struct Types20 {
+ typedef T1 Head;
+ typedef Types19<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21>
+struct Types21 {
+ typedef T1 Head;
+ typedef Types20<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22>
+struct Types22 {
+ typedef T1 Head;
+ typedef Types21<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23>
+struct Types23 {
+ typedef T1 Head;
+ typedef Types22<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24>
+struct Types24 {
+ typedef T1 Head;
+ typedef Types23<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25>
+struct Types25 {
+ typedef T1 Head;
+ typedef Types24<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26>
+struct Types26 {
+ typedef T1 Head;
+ typedef Types25<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27>
+struct Types27 {
+ typedef T1 Head;
+ typedef Types26<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28>
+struct Types28 {
+ typedef T1 Head;
+ typedef Types27<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29>
+struct Types29 {
+ typedef T1 Head;
+ typedef Types28<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30>
+struct Types30 {
+ typedef T1 Head;
+ typedef Types29<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31>
+struct Types31 {
+ typedef T1 Head;
+ typedef Types30<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32>
+struct Types32 {
+ typedef T1 Head;
+ typedef Types31<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33>
+struct Types33 {
+ typedef T1 Head;
+ typedef Types32<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34>
+struct Types34 {
+ typedef T1 Head;
+ typedef Types33<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35>
+struct Types35 {
+ typedef T1 Head;
+ typedef Types34<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36>
+struct Types36 {
+ typedef T1 Head;
+ typedef Types35<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37>
+struct Types37 {
+ typedef T1 Head;
+ typedef Types36<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38>
+struct Types38 {
+ typedef T1 Head;
+ typedef Types37<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39>
+struct Types39 {
+ typedef T1 Head;
+ typedef Types38<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40>
+struct Types40 {
+ typedef T1 Head;
+ typedef Types39<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41>
+struct Types41 {
+ typedef T1 Head;
+ typedef Types40<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42>
+struct Types42 {
+ typedef T1 Head;
+ typedef Types41<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43>
+struct Types43 {
+ typedef T1 Head;
+ typedef Types42<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42,
+ T43> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44>
+struct Types44 {
+ typedef T1 Head;
+ typedef Types43<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45>
+struct Types45 {
+ typedef T1 Head;
+ typedef Types44<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46>
+struct Types46 {
+ typedef T1 Head;
+ typedef Types45<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45, T46> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47>
+struct Types47 {
+ typedef T1 Head;
+ typedef Types46<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45, T46, T47> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48>
+struct Types48 {
+ typedef T1 Head;
+ typedef Types47<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45, T46, T47, T48> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48, typename T49>
+struct Types49 {
+ typedef T1 Head;
+ typedef Types48<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45, T46, T47, T48, T49> Tail;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48, typename T49, typename T50>
+struct Types50 {
+ typedef T1 Head;
+ typedef Types49<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45, T46, T47, T48, T49, T50> Tail;
+};
+
+
+} // namespace internal
+
+// We don't want to require the users to write TypesN<...> directly,
+// as that would require them to count the length. Types<...> is much
+// easier to write, but generates horrible messages when there is a
+// compiler error, as gcc insists on printing out each template
+// argument, even if it has the default value (this means Types<int>
+// will appear as Types<int, None, None, ..., None> in the compiler
+// errors).
+//
+// Our solution is to combine the best part of the two approaches: a
+// user would write Types<T1, ..., TN>, and Google Test will translate
+// that to TypesN<T1, ..., TN> internally to make error messages
+// readable. The translation is done by the 'type' member of the
+// Types template.
+template <typename T1 = internal::None, typename T2 = internal::None,
+ typename T3 = internal::None, typename T4 = internal::None,
+ typename T5 = internal::None, typename T6 = internal::None,
+ typename T7 = internal::None, typename T8 = internal::None,
+ typename T9 = internal::None, typename T10 = internal::None,
+ typename T11 = internal::None, typename T12 = internal::None,
+ typename T13 = internal::None, typename T14 = internal::None,
+ typename T15 = internal::None, typename T16 = internal::None,
+ typename T17 = internal::None, typename T18 = internal::None,
+ typename T19 = internal::None, typename T20 = internal::None,
+ typename T21 = internal::None, typename T22 = internal::None,
+ typename T23 = internal::None, typename T24 = internal::None,
+ typename T25 = internal::None, typename T26 = internal::None,
+ typename T27 = internal::None, typename T28 = internal::None,
+ typename T29 = internal::None, typename T30 = internal::None,
+ typename T31 = internal::None, typename T32 = internal::None,
+ typename T33 = internal::None, typename T34 = internal::None,
+ typename T35 = internal::None, typename T36 = internal::None,
+ typename T37 = internal::None, typename T38 = internal::None,
+ typename T39 = internal::None, typename T40 = internal::None,
+ typename T41 = internal::None, typename T42 = internal::None,
+ typename T43 = internal::None, typename T44 = internal::None,
+ typename T45 = internal::None, typename T46 = internal::None,
+ typename T47 = internal::None, typename T48 = internal::None,
+ typename T49 = internal::None, typename T50 = internal::None>
+struct Types {
+ typedef internal::Types50<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41, T42, T43, T44, T45, T46, T47, T48, T49, T50> type;
+};
+
+template <>
+struct Types<internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types0 type;
+};
+template <typename T1>
+struct Types<T1, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types1<T1> type;
+};
+template <typename T1, typename T2>
+struct Types<T1, T2, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types2<T1, T2> type;
+};
+template <typename T1, typename T2, typename T3>
+struct Types<T1, T2, T3, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None> {
+ typedef internal::Types3<T1, T2, T3> type;
+};
+template <typename T1, typename T2, typename T3, typename T4>
+struct Types<T1, T2, T3, T4, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None> {
+ typedef internal::Types4<T1, T2, T3, T4> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5>
+struct Types<T1, T2, T3, T4, T5, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None> {
+ typedef internal::Types5<T1, T2, T3, T4, T5> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6>
+struct Types<T1, T2, T3, T4, T5, T6, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types6<T1, T2, T3, T4, T5, T6> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7>
+struct Types<T1, T2, T3, T4, T5, T6, T7, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types7<T1, T2, T3, T4, T5, T6, T7> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None> {
+ typedef internal::Types8<T1, T2, T3, T4, T5, T6, T7, T8> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None> {
+ typedef internal::Types9<T1, T2, T3, T4, T5, T6, T7, T8, T9> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None> {
+ typedef internal::Types10<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types11<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types12<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11,
+ T12> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types13<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None> {
+ typedef internal::Types14<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None> {
+ typedef internal::Types15<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types16<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types17<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types18<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None> {
+ typedef internal::Types19<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None> {
+ typedef internal::Types20<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None> {
+ typedef internal::Types21<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types22<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types23<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types24<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None> {
+ typedef internal::Types25<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None> {
+ typedef internal::Types26<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25,
+ T26> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types27<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types28<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types29<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None> {
+ typedef internal::Types30<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None> {
+ typedef internal::Types31<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types32<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types33<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types34<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None> {
+ typedef internal::Types35<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None> {
+ typedef internal::Types36<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None> {
+ typedef internal::Types37<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types38<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, T39, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types39<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types40<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39,
+ T40> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None, internal::None> {
+ typedef internal::Types41<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, internal::None,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None> {
+ typedef internal::Types42<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41, T42> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None, internal::None> {
+ typedef internal::Types43<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41, T42, T43> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None, internal::None> {
+ typedef internal::Types44<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41, T42, T43, T44> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44, T45,
+ internal::None, internal::None, internal::None, internal::None,
+ internal::None> {
+ typedef internal::Types45<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41, T42, T43, T44, T45> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44, T45,
+ T46, internal::None, internal::None, internal::None, internal::None> {
+ typedef internal::Types46<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41, T42, T43, T44, T45, T46> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44, T45,
+ T46, T47, internal::None, internal::None, internal::None> {
+ typedef internal::Types47<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41, T42, T43, T44, T45, T46, T47> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44, T45,
+ T46, T47, T48, internal::None, internal::None> {
+ typedef internal::Types48<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41, T42, T43, T44, T45, T46, T47, T48> type;
+};
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48, typename T49>
+struct Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15,
+ T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29, T30,
+ T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44, T45,
+ T46, T47, T48, T49, internal::None> {
+ typedef internal::Types49<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41, T42, T43, T44, T45, T46, T47, T48, T49> type;
+};
+
+namespace internal {
+
+# define GTEST_TEMPLATE_ template <typename T> class
+
+// The template "selector" struct TemplateSel<Tmpl> is used to
+// represent Tmpl, which must be a class template with one type
+// parameter, as a type. TemplateSel<Tmpl>::Bind<T>::type is defined
+// as the type Tmpl<T>. This allows us to actually instantiate the
+// template "selected" by TemplateSel<Tmpl>.
+//
+// This trick is necessary for simulating typedef for class templates,
+// which C++ doesn't support directly.
+template <GTEST_TEMPLATE_ Tmpl>
+struct TemplateSel {
+ template <typename T>
+ struct Bind {
+ typedef Tmpl<T> type;
+ };
+};
+
+# define GTEST_BIND_(TmplSel, T) \
+ TmplSel::template Bind<T>::type
+
+// A unique struct template used as the default value for the
+// arguments of class template Templates. This allows us to simulate
+// variadic templates (e.g. Templates<int>, Templates<int, double>,
+// and etc), which C++ doesn't support directly.
+template <typename T>
+struct NoneT {};
+
+// The following family of struct and struct templates are used to
+// represent template lists. In particular, TemplatesN<T1, T2, ...,
+// TN> represents a list of N templates (T1, T2, ..., and TN). Except
+// for Templates0, every struct in the family has two member types:
+// Head for the selector of the first template in the list, and Tail
+// for the rest of the list.
+
+// The empty template list.
+struct Templates0 {};
+
+// Template lists of length 1, 2, 3, and so on.
+
+template <GTEST_TEMPLATE_ T1>
+struct Templates1 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates0 Tail;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2>
+struct Templates2 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates1<T2> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3>
+struct Templates3 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates2<T2, T3> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4>
+struct Templates4 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates3<T2, T3, T4> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5>
+struct Templates5 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates4<T2, T3, T4, T5> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6>
+struct Templates6 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates5<T2, T3, T4, T5, T6> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7>
+struct Templates7 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates6<T2, T3, T4, T5, T6, T7> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8>
+struct Templates8 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates7<T2, T3, T4, T5, T6, T7, T8> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9>
+struct Templates9 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates8<T2, T3, T4, T5, T6, T7, T8, T9> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10>
+struct Templates10 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates9<T2, T3, T4, T5, T6, T7, T8, T9, T10> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11>
+struct Templates11 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates10<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12>
+struct Templates12 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates11<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13>
+struct Templates13 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates12<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14>
+struct Templates14 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates13<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15>
+struct Templates15 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates14<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16>
+struct Templates16 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates15<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17>
+struct Templates17 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates16<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18>
+struct Templates18 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates17<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19>
+struct Templates19 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates18<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20>
+struct Templates20 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates19<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21>
+struct Templates21 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates20<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22>
+struct Templates22 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates21<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23>
+struct Templates23 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates22<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24>
+struct Templates24 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates23<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25>
+struct Templates25 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates24<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26>
+struct Templates26 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates25<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27>
+struct Templates27 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates26<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28>
+struct Templates28 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates27<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29>
+struct Templates29 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates28<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30>
+struct Templates30 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates29<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31>
+struct Templates31 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates30<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32>
+struct Templates32 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates31<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33>
+struct Templates33 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates32<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34>
+struct Templates34 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates33<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35>
+struct Templates35 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates34<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36>
+struct Templates36 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates35<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37>
+struct Templates37 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates36<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38>
+struct Templates38 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates37<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39>
+struct Templates39 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates38<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40>
+struct Templates40 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates39<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41>
+struct Templates41 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates40<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42>
+struct Templates42 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates41<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41,
+ T42> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43>
+struct Templates43 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates42<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42,
+ T43> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44>
+struct Templates44 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates43<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42,
+ T43, T44> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44, GTEST_TEMPLATE_ T45>
+struct Templates45 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates44<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42,
+ T43, T44, T45> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44, GTEST_TEMPLATE_ T45,
+ GTEST_TEMPLATE_ T46>
+struct Templates46 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates45<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42,
+ T43, T44, T45, T46> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44, GTEST_TEMPLATE_ T45,
+ GTEST_TEMPLATE_ T46, GTEST_TEMPLATE_ T47>
+struct Templates47 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates46<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42,
+ T43, T44, T45, T46, T47> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44, GTEST_TEMPLATE_ T45,
+ GTEST_TEMPLATE_ T46, GTEST_TEMPLATE_ T47, GTEST_TEMPLATE_ T48>
+struct Templates48 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates47<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42,
+ T43, T44, T45, T46, T47, T48> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44, GTEST_TEMPLATE_ T45,
+ GTEST_TEMPLATE_ T46, GTEST_TEMPLATE_ T47, GTEST_TEMPLATE_ T48,
+ GTEST_TEMPLATE_ T49>
+struct Templates49 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates48<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42,
+ T43, T44, T45, T46, T47, T48, T49> Tail;
+};
+
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44, GTEST_TEMPLATE_ T45,
+ GTEST_TEMPLATE_ T46, GTEST_TEMPLATE_ T47, GTEST_TEMPLATE_ T48,
+ GTEST_TEMPLATE_ T49, GTEST_TEMPLATE_ T50>
+struct Templates50 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates49<T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42,
+ T43, T44, T45, T46, T47, T48, T49, T50> Tail;
+};
+
+
+// We don't want to require the users to write TemplatesN<...> directly,
+// as that would require them to count the length. Templates<...> is much
+// easier to write, but generates horrible messages when there is a
+// compiler error, as gcc insists on printing out each template
+// argument, even if it has the default value (this means Templates<list>
+// will appear as Templates<list, NoneT, NoneT, ..., NoneT> in the compiler
+// errors).
+//
+// Our solution is to combine the best part of the two approaches: a
+// user would write Templates<T1, ..., TN>, and Google Test will translate
+// that to TemplatesN<T1, ..., TN> internally to make error messages
+// readable. The translation is done by the 'type' member of the
+// Templates template.
+template <GTEST_TEMPLATE_ T1 = NoneT, GTEST_TEMPLATE_ T2 = NoneT,
+ GTEST_TEMPLATE_ T3 = NoneT, GTEST_TEMPLATE_ T4 = NoneT,
+ GTEST_TEMPLATE_ T5 = NoneT, GTEST_TEMPLATE_ T6 = NoneT,
+ GTEST_TEMPLATE_ T7 = NoneT, GTEST_TEMPLATE_ T8 = NoneT,
+ GTEST_TEMPLATE_ T9 = NoneT, GTEST_TEMPLATE_ T10 = NoneT,
+ GTEST_TEMPLATE_ T11 = NoneT, GTEST_TEMPLATE_ T12 = NoneT,
+ GTEST_TEMPLATE_ T13 = NoneT, GTEST_TEMPLATE_ T14 = NoneT,
+ GTEST_TEMPLATE_ T15 = NoneT, GTEST_TEMPLATE_ T16 = NoneT,
+ GTEST_TEMPLATE_ T17 = NoneT, GTEST_TEMPLATE_ T18 = NoneT,
+ GTEST_TEMPLATE_ T19 = NoneT, GTEST_TEMPLATE_ T20 = NoneT,
+ GTEST_TEMPLATE_ T21 = NoneT, GTEST_TEMPLATE_ T22 = NoneT,
+ GTEST_TEMPLATE_ T23 = NoneT, GTEST_TEMPLATE_ T24 = NoneT,
+ GTEST_TEMPLATE_ T25 = NoneT, GTEST_TEMPLATE_ T26 = NoneT,
+ GTEST_TEMPLATE_ T27 = NoneT, GTEST_TEMPLATE_ T28 = NoneT,
+ GTEST_TEMPLATE_ T29 = NoneT, GTEST_TEMPLATE_ T30 = NoneT,
+ GTEST_TEMPLATE_ T31 = NoneT, GTEST_TEMPLATE_ T32 = NoneT,
+ GTEST_TEMPLATE_ T33 = NoneT, GTEST_TEMPLATE_ T34 = NoneT,
+ GTEST_TEMPLATE_ T35 = NoneT, GTEST_TEMPLATE_ T36 = NoneT,
+ GTEST_TEMPLATE_ T37 = NoneT, GTEST_TEMPLATE_ T38 = NoneT,
+ GTEST_TEMPLATE_ T39 = NoneT, GTEST_TEMPLATE_ T40 = NoneT,
+ GTEST_TEMPLATE_ T41 = NoneT, GTEST_TEMPLATE_ T42 = NoneT,
+ GTEST_TEMPLATE_ T43 = NoneT, GTEST_TEMPLATE_ T44 = NoneT,
+ GTEST_TEMPLATE_ T45 = NoneT, GTEST_TEMPLATE_ T46 = NoneT,
+ GTEST_TEMPLATE_ T47 = NoneT, GTEST_TEMPLATE_ T48 = NoneT,
+ GTEST_TEMPLATE_ T49 = NoneT, GTEST_TEMPLATE_ T50 = NoneT>
+struct Templates {
+ typedef Templates50<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41,
+ T42, T43, T44, T45, T46, T47, T48, T49, T50> type;
+};
+
+template <>
+struct Templates<NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT> {
+ typedef Templates0 type;
+};
+template <GTEST_TEMPLATE_ T1>
+struct Templates<T1, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT> {
+ typedef Templates1<T1> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2>
+struct Templates<T1, T2, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT> {
+ typedef Templates2<T1, T2> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3>
+struct Templates<T1, T2, T3, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates3<T1, T2, T3> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4>
+struct Templates<T1, T2, T3, T4, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates4<T1, T2, T3, T4> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5>
+struct Templates<T1, T2, T3, T4, T5, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates5<T1, T2, T3, T4, T5> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6>
+struct Templates<T1, T2, T3, T4, T5, T6, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates6<T1, T2, T3, T4, T5, T6> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates7<T1, T2, T3, T4, T5, T6, T7> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates8<T1, T2, T3, T4, T5, T6, T7, T8> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates9<T1, T2, T3, T4, T5, T6, T7, T8, T9> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates10<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates11<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates12<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates13<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates14<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates15<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates16<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates17<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates18<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates19<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates20<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates21<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT> {
+ typedef Templates22<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT> {
+ typedef Templates23<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT> {
+ typedef Templates24<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT> {
+ typedef Templates25<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT> {
+ typedef Templates26<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT> {
+ typedef Templates27<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT> {
+ typedef Templates28<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT> {
+ typedef Templates29<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates30<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates31<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates32<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates33<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates34<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates35<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates36<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, NoneT, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates37<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, NoneT, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates38<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates39<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, NoneT, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates40<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, NoneT, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates41<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, NoneT,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates42<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41,
+ T42> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates43<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41,
+ T42, T43> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44,
+ NoneT, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates44<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41,
+ T42, T43, T44> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44, GTEST_TEMPLATE_ T45>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44,
+ T45, NoneT, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates45<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41,
+ T42, T43, T44, T45> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44, GTEST_TEMPLATE_ T45,
+ GTEST_TEMPLATE_ T46>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44,
+ T45, T46, NoneT, NoneT, NoneT, NoneT> {
+ typedef Templates46<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41,
+ T42, T43, T44, T45, T46> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44, GTEST_TEMPLATE_ T45,
+ GTEST_TEMPLATE_ T46, GTEST_TEMPLATE_ T47>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44,
+ T45, T46, T47, NoneT, NoneT, NoneT> {
+ typedef Templates47<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41,
+ T42, T43, T44, T45, T46, T47> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44, GTEST_TEMPLATE_ T45,
+ GTEST_TEMPLATE_ T46, GTEST_TEMPLATE_ T47, GTEST_TEMPLATE_ T48>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44,
+ T45, T46, T47, T48, NoneT, NoneT> {
+ typedef Templates48<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41,
+ T42, T43, T44, T45, T46, T47, T48> type;
+};
+template <GTEST_TEMPLATE_ T1, GTEST_TEMPLATE_ T2, GTEST_TEMPLATE_ T3,
+ GTEST_TEMPLATE_ T4, GTEST_TEMPLATE_ T5, GTEST_TEMPLATE_ T6,
+ GTEST_TEMPLATE_ T7, GTEST_TEMPLATE_ T8, GTEST_TEMPLATE_ T9,
+ GTEST_TEMPLATE_ T10, GTEST_TEMPLATE_ T11, GTEST_TEMPLATE_ T12,
+ GTEST_TEMPLATE_ T13, GTEST_TEMPLATE_ T14, GTEST_TEMPLATE_ T15,
+ GTEST_TEMPLATE_ T16, GTEST_TEMPLATE_ T17, GTEST_TEMPLATE_ T18,
+ GTEST_TEMPLATE_ T19, GTEST_TEMPLATE_ T20, GTEST_TEMPLATE_ T21,
+ GTEST_TEMPLATE_ T22, GTEST_TEMPLATE_ T23, GTEST_TEMPLATE_ T24,
+ GTEST_TEMPLATE_ T25, GTEST_TEMPLATE_ T26, GTEST_TEMPLATE_ T27,
+ GTEST_TEMPLATE_ T28, GTEST_TEMPLATE_ T29, GTEST_TEMPLATE_ T30,
+ GTEST_TEMPLATE_ T31, GTEST_TEMPLATE_ T32, GTEST_TEMPLATE_ T33,
+ GTEST_TEMPLATE_ T34, GTEST_TEMPLATE_ T35, GTEST_TEMPLATE_ T36,
+ GTEST_TEMPLATE_ T37, GTEST_TEMPLATE_ T38, GTEST_TEMPLATE_ T39,
+ GTEST_TEMPLATE_ T40, GTEST_TEMPLATE_ T41, GTEST_TEMPLATE_ T42,
+ GTEST_TEMPLATE_ T43, GTEST_TEMPLATE_ T44, GTEST_TEMPLATE_ T45,
+ GTEST_TEMPLATE_ T46, GTEST_TEMPLATE_ T47, GTEST_TEMPLATE_ T48,
+ GTEST_TEMPLATE_ T49>
+struct Templates<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14,
+ T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,
+ T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43, T44,
+ T45, T46, T47, T48, T49, NoneT> {
+ typedef Templates49<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27,
+ T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41,
+ T42, T43, T44, T45, T46, T47, T48, T49> type;
+};
+
+// The TypeList template makes it possible to use either a single type
+// or a Types<...> list in TYPED_TEST_CASE() and
+// INSTANTIATE_TYPED_TEST_CASE_P().
+
+template <typename T>
+struct TypeList {
+ typedef Types1<T> type;
+};
+
+template <typename T1, typename T2, typename T3, typename T4, typename T5,
+ typename T6, typename T7, typename T8, typename T9, typename T10,
+ typename T11, typename T12, typename T13, typename T14, typename T15,
+ typename T16, typename T17, typename T18, typename T19, typename T20,
+ typename T21, typename T22, typename T23, typename T24, typename T25,
+ typename T26, typename T27, typename T28, typename T29, typename T30,
+ typename T31, typename T32, typename T33, typename T34, typename T35,
+ typename T36, typename T37, typename T38, typename T39, typename T40,
+ typename T41, typename T42, typename T43, typename T44, typename T45,
+ typename T46, typename T47, typename T48, typename T49, typename T50>
+struct TypeList<Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13,
+ T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28,
+ T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40, T41, T42, T43,
+ T44, T45, T46, T47, T48, T49, T50> > {
+ typedef typename Types<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12,
+ T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26,
+ T27, T28, T29, T30, T31, T32, T33, T34, T35, T36, T37, T38, T39, T40,
+ T41, T42, T43, T44, T45, T46, T47, T48, T49, T50>::type type;
+};
+
+#endif // GTEST_HAS_TYPED_TEST || GTEST_HAS_TYPED_TEST_P
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TYPE_UTIL_H_
diff --git a/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-type-util.h.pump b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-type-util.h.pump
new file mode 100644
index 0000000..251fdf0
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/include/gtest/internal/gtest-type-util.h.pump
@@ -0,0 +1,297 @@
+$$ -*- mode: c++; -*-
+$var n = 50 $$ Maximum length of type lists we want to support.
+// Copyright 2008 Google Inc.
+// All Rights Reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+
+// Type utilities needed for implementing typed and type-parameterized
+// tests. This file is generated by a SCRIPT. DO NOT EDIT BY HAND!
+//
+// Currently we support at most $n types in a list, and at most $n
+// type-parameterized tests in one type-parameterized test case.
+// Please contact googletestframework at googlegroups.com if you need
+// more.
+
+#ifndef GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TYPE_UTIL_H_
+#define GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TYPE_UTIL_H_
+
+#include "gtest/internal/gtest-port.h"
+
+// #ifdef __GNUC__ is too general here. It is possible to use gcc without using
+// libstdc++ (which is where cxxabi.h comes from).
+# if GTEST_HAS_CXXABI_H_
+# include <cxxabi.h>
+# elif defined(__HP_aCC)
+# include <acxx_demangle.h>
+# endif // GTEST_HASH_CXXABI_H_
+
+namespace testing {
+namespace internal {
+
+// GetTypeName<T>() returns a human-readable name of type T.
+// NB: This function is also used in Google Mock, so don't move it inside of
+// the typed-test-only section below.
+template <typename T>
+std::string GetTypeName() {
+# if GTEST_HAS_RTTI
+
+ const char* const name = typeid(T).name();
+# if GTEST_HAS_CXXABI_H_ || defined(__HP_aCC)
+ int status = 0;
+ // gcc's implementation of typeid(T).name() mangles the type name,
+ // so we have to demangle it.
+# if GTEST_HAS_CXXABI_H_
+ using abi::__cxa_demangle;
+# endif // GTEST_HAS_CXXABI_H_
+ char* const readable_name = __cxa_demangle(name, 0, 0, &status);
+ const std::string name_str(status == 0 ? readable_name : name);
+ free(readable_name);
+ return name_str;
+# else
+ return name;
+# endif // GTEST_HAS_CXXABI_H_ || __HP_aCC
+
+# else
+
+ return "<type>";
+
+# endif // GTEST_HAS_RTTI
+}
+
+#if GTEST_HAS_TYPED_TEST || GTEST_HAS_TYPED_TEST_P
+
+// AssertyTypeEq<T1, T2>::type is defined iff T1 and T2 are the same
+// type. This can be used as a compile-time assertion to ensure that
+// two types are equal.
+
+template <typename T1, typename T2>
+struct AssertTypeEq;
+
+template <typename T>
+struct AssertTypeEq<T, T> {
+ typedef bool type;
+};
+
+// A unique type used as the default value for the arguments of class
+// template Types. This allows us to simulate variadic templates
+// (e.g. Types<int>, Type<int, double>, and etc), which C++ doesn't
+// support directly.
+struct None {};
+
+// The following family of struct and struct templates are used to
+// represent type lists. In particular, TypesN<T1, T2, ..., TN>
+// represents a type list with N types (T1, T2, ..., and TN) in it.
+// Except for Types0, every struct in the family has two member types:
+// Head for the first type in the list, and Tail for the rest of the
+// list.
+
+// The empty type list.
+struct Types0 {};
+
+// Type lists of length 1, 2, 3, and so on.
+
+template <typename T1>
+struct Types1 {
+ typedef T1 Head;
+ typedef Types0 Tail;
+};
+
+$range i 2..n
+
+$for i [[
+$range j 1..i
+$range k 2..i
+template <$for j, [[typename T$j]]>
+struct Types$i {
+ typedef T1 Head;
+ typedef Types$(i-1)<$for k, [[T$k]]> Tail;
+};
+
+
+]]
+
+} // namespace internal
+
+// We don't want to require the users to write TypesN<...> directly,
+// as that would require them to count the length. Types<...> is much
+// easier to write, but generates horrible messages when there is a
+// compiler error, as gcc insists on printing out each template
+// argument, even if it has the default value (this means Types<int>
+// will appear as Types<int, None, None, ..., None> in the compiler
+// errors).
+//
+// Our solution is to combine the best part of the two approaches: a
+// user would write Types<T1, ..., TN>, and Google Test will translate
+// that to TypesN<T1, ..., TN> internally to make error messages
+// readable. The translation is done by the 'type' member of the
+// Types template.
+
+$range i 1..n
+template <$for i, [[typename T$i = internal::None]]>
+struct Types {
+ typedef internal::Types$n<$for i, [[T$i]]> type;
+};
+
+template <>
+struct Types<$for i, [[internal::None]]> {
+ typedef internal::Types0 type;
+};
+
+$range i 1..n-1
+$for i [[
+$range j 1..i
+$range k i+1..n
+template <$for j, [[typename T$j]]>
+struct Types<$for j, [[T$j]]$for k[[, internal::None]]> {
+ typedef internal::Types$i<$for j, [[T$j]]> type;
+};
+
+]]
+
+namespace internal {
+
+# define GTEST_TEMPLATE_ template <typename T> class
+
+// The template "selector" struct TemplateSel<Tmpl> is used to
+// represent Tmpl, which must be a class template with one type
+// parameter, as a type. TemplateSel<Tmpl>::Bind<T>::type is defined
+// as the type Tmpl<T>. This allows us to actually instantiate the
+// template "selected" by TemplateSel<Tmpl>.
+//
+// This trick is necessary for simulating typedef for class templates,
+// which C++ doesn't support directly.
+template <GTEST_TEMPLATE_ Tmpl>
+struct TemplateSel {
+ template <typename T>
+ struct Bind {
+ typedef Tmpl<T> type;
+ };
+};
+
+# define GTEST_BIND_(TmplSel, T) \
+ TmplSel::template Bind<T>::type
+
+// A unique struct template used as the default value for the
+// arguments of class template Templates. This allows us to simulate
+// variadic templates (e.g. Templates<int>, Templates<int, double>,
+// and etc), which C++ doesn't support directly.
+template <typename T>
+struct NoneT {};
+
+// The following family of struct and struct templates are used to
+// represent template lists. In particular, TemplatesN<T1, T2, ...,
+// TN> represents a list of N templates (T1, T2, ..., and TN). Except
+// for Templates0, every struct in the family has two member types:
+// Head for the selector of the first template in the list, and Tail
+// for the rest of the list.
+
+// The empty template list.
+struct Templates0 {};
+
+// Template lists of length 1, 2, 3, and so on.
+
+template <GTEST_TEMPLATE_ T1>
+struct Templates1 {
+ typedef TemplateSel<T1> Head;
+ typedef Templates0 Tail;
+};
+
+$range i 2..n
+
+$for i [[
+$range j 1..i
+$range k 2..i
+template <$for j, [[GTEST_TEMPLATE_ T$j]]>
+struct Templates$i {
+ typedef TemplateSel<T1> Head;
+ typedef Templates$(i-1)<$for k, [[T$k]]> Tail;
+};
+
+
+]]
+
+// We don't want to require the users to write TemplatesN<...> directly,
+// as that would require them to count the length. Templates<...> is much
+// easier to write, but generates horrible messages when there is a
+// compiler error, as gcc insists on printing out each template
+// argument, even if it has the default value (this means Templates<list>
+// will appear as Templates<list, NoneT, NoneT, ..., NoneT> in the compiler
+// errors).
+//
+// Our solution is to combine the best part of the two approaches: a
+// user would write Templates<T1, ..., TN>, and Google Test will translate
+// that to TemplatesN<T1, ..., TN> internally to make error messages
+// readable. The translation is done by the 'type' member of the
+// Templates template.
+
+$range i 1..n
+template <$for i, [[GTEST_TEMPLATE_ T$i = NoneT]]>
+struct Templates {
+ typedef Templates$n<$for i, [[T$i]]> type;
+};
+
+template <>
+struct Templates<$for i, [[NoneT]]> {
+ typedef Templates0 type;
+};
+
+$range i 1..n-1
+$for i [[
+$range j 1..i
+$range k i+1..n
+template <$for j, [[GTEST_TEMPLATE_ T$j]]>
+struct Templates<$for j, [[T$j]]$for k[[, NoneT]]> {
+ typedef Templates$i<$for j, [[T$j]]> type;
+};
+
+]]
+
+// The TypeList template makes it possible to use either a single type
+// or a Types<...> list in TYPED_TEST_CASE() and
+// INSTANTIATE_TYPED_TEST_CASE_P().
+
+template <typename T>
+struct TypeList {
+ typedef Types1<T> type;
+};
+
+
+$range i 1..n
+template <$for i, [[typename T$i]]>
+struct TypeList<Types<$for i, [[T$i]]> > {
+ typedef typename Types<$for i, [[T$i]]>::type type;
+};
+
+#endif // GTEST_HAS_TYPED_TEST || GTEST_HAS_TYPED_TEST_P
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_INCLUDE_GTEST_INTERNAL_GTEST_TYPE_UTIL_H_
diff --git a/examples/stand_alone/test_fixture/gtest/src/gtest-all.cc b/examples/stand_alone/test_fixture/gtest/src/gtest-all.cc
new file mode 100644
index 0000000..0a9cee5
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/src/gtest-all.cc
@@ -0,0 +1,48 @@
+// Copyright 2008, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: mheule at google.com (Markus Heule)
+//
+// Google C++ Testing Framework (Google Test)
+//
+// Sometimes it's desirable to build Google Test by compiling a single file.
+// This file serves this purpose.
+
+// This line ensures that gtest.h can be compiled on its own, even
+// when it's fused.
+#include "gtest/gtest.h"
+
+// The following lines pull in the real gtest *.cc files.
+#include "src/gtest.cc"
+#include "src/gtest-death-test.cc"
+#include "src/gtest-filepath.cc"
+#include "src/gtest-port.cc"
+#include "src/gtest-printers.cc"
+#include "src/gtest-test-part.cc"
+#include "src/gtest-typed-test.cc"
diff --git a/examples/stand_alone/test_fixture/gtest/src/gtest-death-test.cc b/examples/stand_alone/test_fixture/gtest/src/gtest-death-test.cc
new file mode 100644
index 0000000..a6023fc
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/src/gtest-death-test.cc
@@ -0,0 +1,1344 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan), vladl at google.com (Vlad Losev)
+//
+// This file implements death tests.
+
+#include "gtest/gtest-death-test.h"
+#include "gtest/internal/gtest-port.h"
+
+#if GTEST_HAS_DEATH_TEST
+
+# if GTEST_OS_MAC
+# include <crt_externs.h>
+# endif // GTEST_OS_MAC
+
+# include <errno.h>
+# include <fcntl.h>
+# include <limits.h>
+
+# if GTEST_OS_LINUX
+# include <signal.h>
+# endif // GTEST_OS_LINUX
+
+# include <stdarg.h>
+
+# if GTEST_OS_WINDOWS
+# include <windows.h>
+# else
+# include <sys/mman.h>
+# include <sys/wait.h>
+# endif // GTEST_OS_WINDOWS
+
+# if GTEST_OS_QNX
+# include <spawn.h>
+# endif // GTEST_OS_QNX
+
+#endif // GTEST_HAS_DEATH_TEST
+
+#include "gtest/gtest-message.h"
+#include "gtest/internal/gtest-string.h"
+
+// Indicates that this translation unit is part of Google Test's
+// implementation. It must come before gtest-internal-inl.h is
+// included, or there will be a compiler error. This trick is to
+// prevent a user from accidentally including gtest-internal-inl.h in
+// his code.
+#define GTEST_IMPLEMENTATION_ 1
+#include "src/gtest-internal-inl.h"
+#undef GTEST_IMPLEMENTATION_
+
+namespace testing {
+
+// Constants.
+
+// The default death test style.
+static const char kDefaultDeathTestStyle[] = "fast";
+
+GTEST_DEFINE_string_(
+ death_test_style,
+ internal::StringFromGTestEnv("death_test_style", kDefaultDeathTestStyle),
+ "Indicates how to run a death test in a forked child process: "
+ "\"threadsafe\" (child process re-executes the test binary "
+ "from the beginning, running only the specific death test) or "
+ "\"fast\" (child process runs the death test immediately "
+ "after forking).");
+
+GTEST_DEFINE_bool_(
+ death_test_use_fork,
+ internal::BoolFromGTestEnv("death_test_use_fork", false),
+ "Instructs to use fork()/_exit() instead of clone() in death tests. "
+ "Ignored and always uses fork() on POSIX systems where clone() is not "
+ "implemented. Useful when running under valgrind or similar tools if "
+ "those do not support clone(). Valgrind 3.3.1 will just fail if "
+ "it sees an unsupported combination of clone() flags. "
+ "It is not recommended to use this flag w/o valgrind though it will "
+ "work in 99% of the cases. Once valgrind is fixed, this flag will "
+ "most likely be removed.");
+
+namespace internal {
+GTEST_DEFINE_string_(
+ internal_run_death_test, "",
+ "Indicates the file, line number, temporal index of "
+ "the single death test to run, and a file descriptor to "
+ "which a success code may be sent, all separated by "
+ "the '|' characters. This flag is specified if and only if the current "
+ "process is a sub-process launched for running a thread-safe "
+ "death test. FOR INTERNAL USE ONLY.");
+} // namespace internal
+
+#if GTEST_HAS_DEATH_TEST
+
+namespace internal {
+
+// Valid only for fast death tests. Indicates the code is running in the
+// child process of a fast style death test.
+static bool g_in_fast_death_test_child = false;
+
+// Returns a Boolean value indicating whether the caller is currently
+// executing in the context of the death test child process. Tools such as
+// Valgrind heap checkers may need this to modify their behavior in death
+// tests. IMPORTANT: This is an internal utility. Using it may break the
+// implementation of death tests. User code MUST NOT use it.
+bool InDeathTestChild() {
+# if GTEST_OS_WINDOWS
+
+ // On Windows, death tests are thread-safe regardless of the value of the
+ // death_test_style flag.
+ return !GTEST_FLAG(internal_run_death_test).empty();
+
+# else
+
+ if (GTEST_FLAG(death_test_style) == "threadsafe")
+ return !GTEST_FLAG(internal_run_death_test).empty();
+ else
+ return g_in_fast_death_test_child;
+#endif
+}
+
+} // namespace internal
+
+// ExitedWithCode constructor.
+ExitedWithCode::ExitedWithCode(int exit_code) : exit_code_(exit_code) {
+}
+
+// ExitedWithCode function-call operator.
+bool ExitedWithCode::operator()(int exit_status) const {
+# if GTEST_OS_WINDOWS
+
+ return exit_status == exit_code_;
+
+# else
+
+ return WIFEXITED(exit_status) && WEXITSTATUS(exit_status) == exit_code_;
+
+# endif // GTEST_OS_WINDOWS
+}
+
+# if !GTEST_OS_WINDOWS
+// KilledBySignal constructor.
+KilledBySignal::KilledBySignal(int signum) : signum_(signum) {
+}
+
+// KilledBySignal function-call operator.
+bool KilledBySignal::operator()(int exit_status) const {
+ return WIFSIGNALED(exit_status) && WTERMSIG(exit_status) == signum_;
+}
+# endif // !GTEST_OS_WINDOWS
+
+namespace internal {
+
+// Utilities needed for death tests.
+
+// Generates a textual description of a given exit code, in the format
+// specified by wait(2).
+static std::string ExitSummary(int exit_code) {
+ Message m;
+
+# if GTEST_OS_WINDOWS
+
+ m << "Exited with exit status " << exit_code;
+
+# else
+
+ if (WIFEXITED(exit_code)) {
+ m << "Exited with exit status " << WEXITSTATUS(exit_code);
+ } else if (WIFSIGNALED(exit_code)) {
+ m << "Terminated by signal " << WTERMSIG(exit_code);
+ }
+# ifdef WCOREDUMP
+ if (WCOREDUMP(exit_code)) {
+ m << " (core dumped)";
+ }
+# endif
+# endif // GTEST_OS_WINDOWS
+
+ return m.GetString();
+}
+
+// Returns true if exit_status describes a process that was terminated
+// by a signal, or exited normally with a nonzero exit code.
+bool ExitedUnsuccessfully(int exit_status) {
+ return !ExitedWithCode(0)(exit_status);
+}
+
+# if !GTEST_OS_WINDOWS
+// Generates a textual failure message when a death test finds more than
+// one thread running, or cannot determine the number of threads, prior
+// to executing the given statement. It is the responsibility of the
+// caller not to pass a thread_count of 1.
+static std::string DeathTestThreadWarning(size_t thread_count) {
+ Message msg;
+ msg << "Death tests use fork(), which is unsafe particularly"
+ << " in a threaded context. For this test, " << GTEST_NAME_ << " ";
+ if (thread_count == 0)
+ msg << "couldn't detect the number of threads.";
+ else
+ msg << "detected " << thread_count << " threads.";
+ return msg.GetString();
+}
+# endif // !GTEST_OS_WINDOWS
+
+// Flag characters for reporting a death test that did not die.
+static const char kDeathTestLived = 'L';
+static const char kDeathTestReturned = 'R';
+static const char kDeathTestThrew = 'T';
+static const char kDeathTestInternalError = 'I';
+
+// An enumeration describing all of the possible ways that a death test can
+// conclude. DIED means that the process died while executing the test
+// code; LIVED means that process lived beyond the end of the test code;
+// RETURNED means that the test statement attempted to execute a return
+// statement, which is not allowed; THREW means that the test statement
+// returned control by throwing an exception. IN_PROGRESS means the test
+// has not yet concluded.
+// TODO(vladl at google.com): Unify names and possibly values for
+// AbortReason, DeathTestOutcome, and flag characters above.
+enum DeathTestOutcome { IN_PROGRESS, DIED, LIVED, RETURNED, THREW };
+
+// Routine for aborting the program which is safe to call from an
+// exec-style death test child process, in which case the error
+// message is propagated back to the parent process. Otherwise, the
+// message is simply printed to stderr. In either case, the program
+// then exits with status 1.
+void DeathTestAbort(const std::string& message) {
+ // On a POSIX system, this function may be called from a threadsafe-style
+ // death test child process, which operates on a very small stack. Use
+ // the heap for any additional non-minuscule memory requirements.
+ const InternalRunDeathTestFlag* const flag =
+ GetUnitTestImpl()->internal_run_death_test_flag();
+ if (flag != NULL) {
+ FILE* parent = posix::FDOpen(flag->write_fd(), "w");
+ fputc(kDeathTestInternalError, parent);
+ fprintf(parent, "%s", message.c_str());
+ fflush(parent);
+ _exit(1);
+ } else {
+ fprintf(stderr, "%s", message.c_str());
+ fflush(stderr);
+ posix::Abort();
+ }
+}
+
+// A replacement for CHECK that calls DeathTestAbort if the assertion
+// fails.
+# define GTEST_DEATH_TEST_CHECK_(expression) \
+ do { \
+ if (!::testing::internal::IsTrue(expression)) { \
+ DeathTestAbort( \
+ ::std::string("CHECK failed: File ") + __FILE__ + ", line " \
+ + ::testing::internal::StreamableToString(__LINE__) + ": " \
+ + #expression); \
+ } \
+ } while (::testing::internal::AlwaysFalse())
+
+// This macro is similar to GTEST_DEATH_TEST_CHECK_, but it is meant for
+// evaluating any system call that fulfills two conditions: it must return
+// -1 on failure, and set errno to EINTR when it is interrupted and
+// should be tried again. The macro expands to a loop that repeatedly
+// evaluates the expression as long as it evaluates to -1 and sets
+// errno to EINTR. If the expression evaluates to -1 but errno is
+// something other than EINTR, DeathTestAbort is called.
+# define GTEST_DEATH_TEST_CHECK_SYSCALL_(expression) \
+ do { \
+ int gtest_retval; \
+ do { \
+ gtest_retval = (expression); \
+ } while (gtest_retval == -1 && errno == EINTR); \
+ if (gtest_retval == -1) { \
+ DeathTestAbort( \
+ ::std::string("CHECK failed: File ") + __FILE__ + ", line " \
+ + ::testing::internal::StreamableToString(__LINE__) + ": " \
+ + #expression + " != -1"); \
+ } \
+ } while (::testing::internal::AlwaysFalse())
+
+// Returns the message describing the last system error in errno.
+std::string GetLastErrnoDescription() {
+ return errno == 0 ? "" : posix::StrError(errno);
+}
+
+// This is called from a death test parent process to read a failure
+// message from the death test child process and log it with the FATAL
+// severity. On Windows, the message is read from a pipe handle. On other
+// platforms, it is read from a file descriptor.
+static void FailFromInternalError(int fd) {
+ Message error;
+ char buffer[256];
+ int num_read;
+
+ do {
+ while ((num_read = posix::Read(fd, buffer, 255)) > 0) {
+ buffer[num_read] = '\0';
+ error << buffer;
+ }
+ } while (num_read == -1 && errno == EINTR);
+
+ if (num_read == 0) {
+ GTEST_LOG_(FATAL) << error.GetString();
+ } else {
+ const int last_error = errno;
+ GTEST_LOG_(FATAL) << "Error while reading death test internal: "
+ << GetLastErrnoDescription() << " [" << last_error << "]";
+ }
+}
+
+// Death test constructor. Increments the running death test count
+// for the current test.
+DeathTest::DeathTest() {
+ TestInfo* const info = GetUnitTestImpl()->current_test_info();
+ if (info == NULL) {
+ DeathTestAbort("Cannot run a death test outside of a TEST or "
+ "TEST_F construct");
+ }
+}
+
+// Creates and returns a death test by dispatching to the current
+// death test factory.
+bool DeathTest::Create(const char* statement, const RE* regex,
+ const char* file, int line, DeathTest** test) {
+ return GetUnitTestImpl()->death_test_factory()->Create(
+ statement, regex, file, line, test);
+}
+
+const char* DeathTest::LastMessage() {
+ return last_death_test_message_.c_str();
+}
+
+void DeathTest::set_last_death_test_message(const std::string& message) {
+ last_death_test_message_ = message;
+}
+
+std::string DeathTest::last_death_test_message_;
+
+// Provides cross platform implementation for some death functionality.
+class DeathTestImpl : public DeathTest {
+ protected:
+ DeathTestImpl(const char* a_statement, const RE* a_regex)
+ : statement_(a_statement),
+ regex_(a_regex),
+ spawned_(false),
+ status_(-1),
+ outcome_(IN_PROGRESS),
+ read_fd_(-1),
+ write_fd_(-1) {}
+
+ // read_fd_ is expected to be closed and cleared by a derived class.
+ ~DeathTestImpl() { GTEST_DEATH_TEST_CHECK_(read_fd_ == -1); }
+
+ void Abort(AbortReason reason);
+ virtual bool Passed(bool status_ok);
+
+ const char* statement() const { return statement_; }
+ const RE* regex() const { return regex_; }
+ bool spawned() const { return spawned_; }
+ void set_spawned(bool is_spawned) { spawned_ = is_spawned; }
+ int status() const { return status_; }
+ void set_status(int a_status) { status_ = a_status; }
+ DeathTestOutcome outcome() const { return outcome_; }
+ void set_outcome(DeathTestOutcome an_outcome) { outcome_ = an_outcome; }
+ int read_fd() const { return read_fd_; }
+ void set_read_fd(int fd) { read_fd_ = fd; }
+ int write_fd() const { return write_fd_; }
+ void set_write_fd(int fd) { write_fd_ = fd; }
+
+ // Called in the parent process only. Reads the result code of the death
+ // test child process via a pipe, interprets it to set the outcome_
+ // member, and closes read_fd_. Outputs diagnostics and terminates in
+ // case of unexpected codes.
+ void ReadAndInterpretStatusByte();
+
+ private:
+ // The textual content of the code this object is testing. This class
+ // doesn't own this string and should not attempt to delete it.
+ const char* const statement_;
+ // The regular expression which test output must match. DeathTestImpl
+ // doesn't own this object and should not attempt to delete it.
+ const RE* const regex_;
+ // True if the death test child process has been successfully spawned.
+ bool spawned_;
+ // The exit status of the child process.
+ int status_;
+ // How the death test concluded.
+ DeathTestOutcome outcome_;
+ // Descriptor to the read end of the pipe to the child process. It is
+ // always -1 in the child process. The child keeps its write end of the
+ // pipe in write_fd_.
+ int read_fd_;
+ // Descriptor to the child's write end of the pipe to the parent process.
+ // It is always -1 in the parent process. The parent keeps its end of the
+ // pipe in read_fd_.
+ int write_fd_;
+};
+
+// Called in the parent process only. Reads the result code of the death
+// test child process via a pipe, interprets it to set the outcome_
+// member, and closes read_fd_. Outputs diagnostics and terminates in
+// case of unexpected codes.
+void DeathTestImpl::ReadAndInterpretStatusByte() {
+ char flag;
+ int bytes_read;
+
+ // The read() here blocks until data is available (signifying the
+ // failure of the death test) or until the pipe is closed (signifying
+ // its success), so it's okay to call this in the parent before
+ // the child process has exited.
+ do {
+ bytes_read = posix::Read(read_fd(), &flag, 1);
+ } while (bytes_read == -1 && errno == EINTR);
+
+ if (bytes_read == 0) {
+ set_outcome(DIED);
+ } else if (bytes_read == 1) {
+ switch (flag) {
+ case kDeathTestReturned:
+ set_outcome(RETURNED);
+ break;
+ case kDeathTestThrew:
+ set_outcome(THREW);
+ break;
+ case kDeathTestLived:
+ set_outcome(LIVED);
+ break;
+ case kDeathTestInternalError:
+ FailFromInternalError(read_fd()); // Does not return.
+ break;
+ default:
+ GTEST_LOG_(FATAL) << "Death test child process reported "
+ << "unexpected status byte ("
+ << static_cast<unsigned int>(flag) << ")";
+ }
+ } else {
+ GTEST_LOG_(FATAL) << "Read from death test child process failed: "
+ << GetLastErrnoDescription();
+ }
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(posix::Close(read_fd()));
+ set_read_fd(-1);
+}
+
+// Signals that the death test code which should have exited, didn't.
+// Should be called only in a death test child process.
+// Writes a status byte to the child's status file descriptor, then
+// calls _exit(1).
+void DeathTestImpl::Abort(AbortReason reason) {
+ // The parent process considers the death test to be a failure if
+ // it finds any data in our pipe. So, here we write a single flag byte
+ // to the pipe, then exit.
+ const char status_ch =
+ reason == TEST_DID_NOT_DIE ? kDeathTestLived :
+ reason == TEST_THREW_EXCEPTION ? kDeathTestThrew : kDeathTestReturned;
+
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(posix::Write(write_fd(), &status_ch, 1));
+ // We are leaking the descriptor here because on some platforms (i.e.,
+ // when built as Windows DLL), destructors of global objects will still
+ // run after calling _exit(). On such systems, write_fd_ will be
+ // indirectly closed from the destructor of UnitTestImpl, causing double
+ // close if it is also closed here. On debug configurations, double close
+ // may assert. As there are no in-process buffers to flush here, we are
+ // relying on the OS to close the descriptor after the process terminates
+ // when the destructors are not run.
+ _exit(1); // Exits w/o any normal exit hooks (we were supposed to crash)
+}
+
+// Returns an indented copy of stderr output for a death test.
+// This makes distinguishing death test output lines from regular log lines
+// much easier.
+static ::std::string FormatDeathTestOutput(const ::std::string& output) {
+ ::std::string ret;
+ for (size_t at = 0; ; ) {
+ const size_t line_end = output.find('\n', at);
+ ret += "[ DEATH ] ";
+ if (line_end == ::std::string::npos) {
+ ret += output.substr(at);
+ break;
+ }
+ ret += output.substr(at, line_end + 1 - at);
+ at = line_end + 1;
+ }
+ return ret;
+}
+
+// Assesses the success or failure of a death test, using both private
+// members which have previously been set, and one argument:
+//
+// Private data members:
+// outcome: An enumeration describing how the death test
+// concluded: DIED, LIVED, THREW, or RETURNED. The death test
+// fails in the latter three cases.
+// status: The exit status of the child process. On *nix, it is in the
+// in the format specified by wait(2). On Windows, this is the
+// value supplied to the ExitProcess() API or a numeric code
+// of the exception that terminated the program.
+// regex: A regular expression object to be applied to
+// the test's captured standard error output; the death test
+// fails if it does not match.
+//
+// Argument:
+// status_ok: true if exit_status is acceptable in the context of
+// this particular death test, which fails if it is false
+//
+// Returns true iff all of the above conditions are met. Otherwise, the
+// first failing condition, in the order given above, is the one that is
+// reported. Also sets the last death test message string.
+bool DeathTestImpl::Passed(bool status_ok) {
+ if (!spawned())
+ return false;
+
+ const std::string error_message = GetCapturedStderr();
+
+ bool success = false;
+ Message buffer;
+
+ buffer << "Death test: " << statement() << "\n";
+ switch (outcome()) {
+ case LIVED:
+ buffer << " Result: failed to die.\n"
+ << " Error msg:\n" << FormatDeathTestOutput(error_message);
+ break;
+ case THREW:
+ buffer << " Result: threw an exception.\n"
+ << " Error msg:\n" << FormatDeathTestOutput(error_message);
+ break;
+ case RETURNED:
+ buffer << " Result: illegal return in test statement.\n"
+ << " Error msg:\n" << FormatDeathTestOutput(error_message);
+ break;
+ case DIED:
+ if (status_ok) {
+ const bool matched = RE::PartialMatch(error_message.c_str(), *regex());
+ if (matched) {
+ success = true;
+ } else {
+ buffer << " Result: died but not with expected error.\n"
+ << " Expected: " << regex()->pattern() << "\n"
+ << "Actual msg:\n" << FormatDeathTestOutput(error_message);
+ }
+ } else {
+ buffer << " Result: died but not with expected exit code:\n"
+ << " " << ExitSummary(status()) << "\n"
+ << "Actual msg:\n" << FormatDeathTestOutput(error_message);
+ }
+ break;
+ case IN_PROGRESS:
+ default:
+ GTEST_LOG_(FATAL)
+ << "DeathTest::Passed somehow called before conclusion of test";
+ }
+
+ DeathTest::set_last_death_test_message(buffer.GetString());
+ return success;
+}
+
+# if GTEST_OS_WINDOWS
+// WindowsDeathTest implements death tests on Windows. Due to the
+// specifics of starting new processes on Windows, death tests there are
+// always threadsafe, and Google Test considers the
+// --gtest_death_test_style=fast setting to be equivalent to
+// --gtest_death_test_style=threadsafe there.
+//
+// A few implementation notes: Like the Linux version, the Windows
+// implementation uses pipes for child-to-parent communication. But due to
+// the specifics of pipes on Windows, some extra steps are required:
+//
+// 1. The parent creates a communication pipe and stores handles to both
+// ends of it.
+// 2. The parent starts the child and provides it with the information
+// necessary to acquire the handle to the write end of the pipe.
+// 3. The child acquires the write end of the pipe and signals the parent
+// using a Windows event.
+// 4. Now the parent can release the write end of the pipe on its side. If
+// this is done before step 3, the object's reference count goes down to
+// 0 and it is destroyed, preventing the child from acquiring it. The
+// parent now has to release it, or read operations on the read end of
+// the pipe will not return when the child terminates.
+// 5. The parent reads child's output through the pipe (outcome code and
+// any possible error messages) from the pipe, and its stderr and then
+// determines whether to fail the test.
+//
+// Note: to distinguish Win32 API calls from the local method and function
+// calls, the former are explicitly resolved in the global namespace.
+//
+class WindowsDeathTest : public DeathTestImpl {
+ public:
+ WindowsDeathTest(const char* a_statement,
+ const RE* a_regex,
+ const char* file,
+ int line)
+ : DeathTestImpl(a_statement, a_regex), file_(file), line_(line) {}
+
+ // All of these virtual functions are inherited from DeathTest.
+ virtual int Wait();
+ virtual TestRole AssumeRole();
+
+ private:
+ // The name of the file in which the death test is located.
+ const char* const file_;
+ // The line number on which the death test is located.
+ const int line_;
+ // Handle to the write end of the pipe to the child process.
+ AutoHandle write_handle_;
+ // Child process handle.
+ AutoHandle child_handle_;
+ // Event the child process uses to signal the parent that it has
+ // acquired the handle to the write end of the pipe. After seeing this
+ // event the parent can release its own handles to make sure its
+ // ReadFile() calls return when the child terminates.
+ AutoHandle event_handle_;
+};
+
+// Waits for the child in a death test to exit, returning its exit
+// status, or 0 if no child process exists. As a side effect, sets the
+// outcome data member.
+int WindowsDeathTest::Wait() {
+ if (!spawned())
+ return 0;
+
+ // Wait until the child either signals that it has acquired the write end
+ // of the pipe or it dies.
+ const HANDLE wait_handles[2] = { child_handle_.Get(), event_handle_.Get() };
+ switch (::WaitForMultipleObjects(2,
+ wait_handles,
+ FALSE, // Waits for any of the handles.
+ INFINITE)) {
+ case WAIT_OBJECT_0:
+ case WAIT_OBJECT_0 + 1:
+ break;
+ default:
+ GTEST_DEATH_TEST_CHECK_(false); // Should not get here.
+ }
+
+ // The child has acquired the write end of the pipe or exited.
+ // We release the handle on our side and continue.
+ write_handle_.Reset();
+ event_handle_.Reset();
+
+ ReadAndInterpretStatusByte();
+
+ // Waits for the child process to exit if it haven't already. This
+ // returns immediately if the child has already exited, regardless of
+ // whether previous calls to WaitForMultipleObjects synchronized on this
+ // handle or not.
+ GTEST_DEATH_TEST_CHECK_(
+ WAIT_OBJECT_0 == ::WaitForSingleObject(child_handle_.Get(),
+ INFINITE));
+ DWORD status_code;
+ GTEST_DEATH_TEST_CHECK_(
+ ::GetExitCodeProcess(child_handle_.Get(), &status_code) != FALSE);
+ child_handle_.Reset();
+ set_status(static_cast<int>(status_code));
+ return status();
+}
+
+// The AssumeRole process for a Windows death test. It creates a child
+// process with the same executable as the current process to run the
+// death test. The child process is given the --gtest_filter and
+// --gtest_internal_run_death_test flags such that it knows to run the
+// current death test only.
+DeathTest::TestRole WindowsDeathTest::AssumeRole() {
+ const UnitTestImpl* const impl = GetUnitTestImpl();
+ const InternalRunDeathTestFlag* const flag =
+ impl->internal_run_death_test_flag();
+ const TestInfo* const info = impl->current_test_info();
+ const int death_test_index = info->result()->death_test_count();
+
+ if (flag != NULL) {
+ // ParseInternalRunDeathTestFlag() has performed all the necessary
+ // processing.
+ set_write_fd(flag->write_fd());
+ return EXECUTE_TEST;
+ }
+
+ // WindowsDeathTest uses an anonymous pipe to communicate results of
+ // a death test.
+ SECURITY_ATTRIBUTES handles_are_inheritable = {
+ sizeof(SECURITY_ATTRIBUTES), NULL, TRUE };
+ HANDLE read_handle, write_handle;
+ GTEST_DEATH_TEST_CHECK_(
+ ::CreatePipe(&read_handle, &write_handle, &handles_are_inheritable,
+ 0) // Default buffer size.
+ != FALSE);
+ set_read_fd(::_open_osfhandle(reinterpret_cast<intptr_t>(read_handle),
+ O_RDONLY));
+ write_handle_.Reset(write_handle);
+ event_handle_.Reset(::CreateEvent(
+ &handles_are_inheritable,
+ TRUE, // The event will automatically reset to non-signaled state.
+ FALSE, // The initial state is non-signalled.
+ NULL)); // The even is unnamed.
+ GTEST_DEATH_TEST_CHECK_(event_handle_.Get() != NULL);
+ const std::string filter_flag =
+ std::string("--") + GTEST_FLAG_PREFIX_ + kFilterFlag + "=" +
+ info->test_case_name() + "." + info->name();
+ const std::string internal_flag =
+ std::string("--") + GTEST_FLAG_PREFIX_ + kInternalRunDeathTestFlag +
+ "=" + file_ + "|" + StreamableToString(line_) + "|" +
+ StreamableToString(death_test_index) + "|" +
+ StreamableToString(static_cast<unsigned int>(::GetCurrentProcessId())) +
+ // size_t has the same width as pointers on both 32-bit and 64-bit
+ // Windows platforms.
+ // See http://msdn.microsoft.com/en-us/library/tcxf1dw6.aspx.
+ "|" + StreamableToString(reinterpret_cast<size_t>(write_handle)) +
+ "|" + StreamableToString(reinterpret_cast<size_t>(event_handle_.Get()));
+
+ char executable_path[_MAX_PATH + 1]; // NOLINT
+ GTEST_DEATH_TEST_CHECK_(
+ _MAX_PATH + 1 != ::GetModuleFileNameA(NULL,
+ executable_path,
+ _MAX_PATH));
+
+ std::string command_line =
+ std::string(::GetCommandLineA()) + " " + filter_flag + " \"" +
+ internal_flag + "\"";
+
+ DeathTest::set_last_death_test_message("");
+
+ CaptureStderr();
+ // Flush the log buffers since the log streams are shared with the child.
+ FlushInfoLog();
+
+ // The child process will share the standard handles with the parent.
+ STARTUPINFOA startup_info;
+ memset(&startup_info, 0, sizeof(STARTUPINFO));
+ startup_info.dwFlags = STARTF_USESTDHANDLES;
+ startup_info.hStdInput = ::GetStdHandle(STD_INPUT_HANDLE);
+ startup_info.hStdOutput = ::GetStdHandle(STD_OUTPUT_HANDLE);
+ startup_info.hStdError = ::GetStdHandle(STD_ERROR_HANDLE);
+
+ PROCESS_INFORMATION process_info;
+ GTEST_DEATH_TEST_CHECK_(::CreateProcessA(
+ executable_path,
+ const_cast<char*>(command_line.c_str()),
+ NULL, // Retuned process handle is not inheritable.
+ NULL, // Retuned thread handle is not inheritable.
+ TRUE, // Child inherits all inheritable handles (for write_handle_).
+ 0x0, // Default creation flags.
+ NULL, // Inherit the parent's environment.
+ UnitTest::GetInstance()->original_working_dir(),
+ &startup_info,
+ &process_info) != FALSE);
+ child_handle_.Reset(process_info.hProcess);
+ ::CloseHandle(process_info.hThread);
+ set_spawned(true);
+ return OVERSEE_TEST;
+}
+# else // We are not on Windows.
+
+// ForkingDeathTest provides implementations for most of the abstract
+// methods of the DeathTest interface. Only the AssumeRole method is
+// left undefined.
+class ForkingDeathTest : public DeathTestImpl {
+ public:
+ ForkingDeathTest(const char* statement, const RE* regex);
+
+ // All of these virtual functions are inherited from DeathTest.
+ virtual int Wait();
+
+ protected:
+ void set_child_pid(pid_t child_pid) { child_pid_ = child_pid; }
+
+ private:
+ // PID of child process during death test; 0 in the child process itself.
+ pid_t child_pid_;
+};
+
+// Constructs a ForkingDeathTest.
+ForkingDeathTest::ForkingDeathTest(const char* a_statement, const RE* a_regex)
+ : DeathTestImpl(a_statement, a_regex),
+ child_pid_(-1) {}
+
+// Waits for the child in a death test to exit, returning its exit
+// status, or 0 if no child process exists. As a side effect, sets the
+// outcome data member.
+int ForkingDeathTest::Wait() {
+ if (!spawned())
+ return 0;
+
+ ReadAndInterpretStatusByte();
+
+ int status_value;
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(waitpid(child_pid_, &status_value, 0));
+ set_status(status_value);
+ return status_value;
+}
+
+// A concrete death test class that forks, then immediately runs the test
+// in the child process.
+class NoExecDeathTest : public ForkingDeathTest {
+ public:
+ NoExecDeathTest(const char* a_statement, const RE* a_regex) :
+ ForkingDeathTest(a_statement, a_regex) { }
+ virtual TestRole AssumeRole();
+};
+
+// The AssumeRole process for a fork-and-run death test. It implements a
+// straightforward fork, with a simple pipe to transmit the status byte.
+DeathTest::TestRole NoExecDeathTest::AssumeRole() {
+ const size_t thread_count = GetThreadCount();
+ if (thread_count != 1) {
+ GTEST_LOG_(WARNING) << DeathTestThreadWarning(thread_count);
+ }
+
+ int pipe_fd[2];
+ GTEST_DEATH_TEST_CHECK_(pipe(pipe_fd) != -1);
+
+ DeathTest::set_last_death_test_message("");
+ CaptureStderr();
+ // When we fork the process below, the log file buffers are copied, but the
+ // file descriptors are shared. We flush all log files here so that closing
+ // the file descriptors in the child process doesn't throw off the
+ // synchronization between descriptors and buffers in the parent process.
+ // This is as close to the fork as possible to avoid a race condition in case
+ // there are multiple threads running before the death test, and another
+ // thread writes to the log file.
+ FlushInfoLog();
+
+ const pid_t child_pid = fork();
+ GTEST_DEATH_TEST_CHECK_(child_pid != -1);
+ set_child_pid(child_pid);
+ if (child_pid == 0) {
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(close(pipe_fd[0]));
+ set_write_fd(pipe_fd[1]);
+ // Redirects all logging to stderr in the child process to prevent
+ // concurrent writes to the log files. We capture stderr in the parent
+ // process and append the child process' output to a log.
+ LogToStderr();
+ // Event forwarding to the listeners of event listener API mush be shut
+ // down in death test subprocesses.
+ GetUnitTestImpl()->listeners()->SuppressEventForwarding();
+ g_in_fast_death_test_child = true;
+ return EXECUTE_TEST;
+ } else {
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(close(pipe_fd[1]));
+ set_read_fd(pipe_fd[0]);
+ set_spawned(true);
+ return OVERSEE_TEST;
+ }
+}
+
+// A concrete death test class that forks and re-executes the main
+// program from the beginning, with command-line flags set that cause
+// only this specific death test to be run.
+class ExecDeathTest : public ForkingDeathTest {
+ public:
+ ExecDeathTest(const char* a_statement, const RE* a_regex,
+ const char* file, int line) :
+ ForkingDeathTest(a_statement, a_regex), file_(file), line_(line) { }
+ virtual TestRole AssumeRole();
+ private:
+ static ::std::vector<testing::internal::string>
+ GetArgvsForDeathTestChildProcess() {
+ ::std::vector<testing::internal::string> args = GetInjectableArgvs();
+ return args;
+ }
+ // The name of the file in which the death test is located.
+ const char* const file_;
+ // The line number on which the death test is located.
+ const int line_;
+};
+
+// Utility class for accumulating command-line arguments.
+class Arguments {
+ public:
+ Arguments() {
+ args_.push_back(NULL);
+ }
+
+ ~Arguments() {
+ for (std::vector<char*>::iterator i = args_.begin(); i != args_.end();
+ ++i) {
+ free(*i);
+ }
+ }
+ void AddArgument(const char* argument) {
+ args_.insert(args_.end() - 1, posix::StrDup(argument));
+ }
+
+ template <typename Str>
+ void AddArguments(const ::std::vector<Str>& arguments) {
+ for (typename ::std::vector<Str>::const_iterator i = arguments.begin();
+ i != arguments.end();
+ ++i) {
+ args_.insert(args_.end() - 1, posix::StrDup(i->c_str()));
+ }
+ }
+ char* const* Argv() {
+ return &args_[0];
+ }
+
+ private:
+ std::vector<char*> args_;
+};
+
+// A struct that encompasses the arguments to the child process of a
+// threadsafe-style death test process.
+struct ExecDeathTestArgs {
+ char* const* argv; // Command-line arguments for the child's call to exec
+ int close_fd; // File descriptor to close; the read end of a pipe
+};
+
+# if GTEST_OS_MAC
+inline char** GetEnviron() {
+ // When Google Test is built as a framework on MacOS X, the environ variable
+ // is unavailable. Apple's documentation (man environ) recommends using
+ // _NSGetEnviron() instead.
+ return *_NSGetEnviron();
+}
+# else
+// Some POSIX platforms expect you to declare environ. extern "C" makes
+// it reside in the global namespace.
+extern "C" char** environ;
+inline char** GetEnviron() { return environ; }
+# endif // GTEST_OS_MAC
+
+# if !GTEST_OS_QNX
+// The main function for a threadsafe-style death test child process.
+// This function is called in a clone()-ed process and thus must avoid
+// any potentially unsafe operations like malloc or libc functions.
+static int ExecDeathTestChildMain(void* child_arg) {
+ ExecDeathTestArgs* const args = static_cast<ExecDeathTestArgs*>(child_arg);
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(close(args->close_fd));
+
+ // We need to execute the test program in the same environment where
+ // it was originally invoked. Therefore we change to the original
+ // working directory first.
+ const char* const original_dir =
+ UnitTest::GetInstance()->original_working_dir();
+ // We can safely call chdir() as it's a direct system call.
+ if (chdir(original_dir) != 0) {
+ DeathTestAbort(std::string("chdir(\"") + original_dir + "\") failed: " +
+ GetLastErrnoDescription());
+ return EXIT_FAILURE;
+ }
+
+ // We can safely call execve() as it's a direct system call. We
+ // cannot use execvp() as it's a libc function and thus potentially
+ // unsafe. Since execve() doesn't search the PATH, the user must
+ // invoke the test program via a valid path that contains at least
+ // one path separator.
+ execve(args->argv[0], args->argv, GetEnviron());
+ DeathTestAbort(std::string("execve(") + args->argv[0] + ", ...) in " +
+ original_dir + " failed: " +
+ GetLastErrnoDescription());
+ return EXIT_FAILURE;
+}
+# endif // !GTEST_OS_QNX
+
+// Two utility routines that together determine the direction the stack
+// grows.
+// This could be accomplished more elegantly by a single recursive
+// function, but we want to guard against the unlikely possibility of
+// a smart compiler optimizing the recursion away.
+//
+// GTEST_NO_INLINE_ is required to prevent GCC 4.6 from inlining
+// StackLowerThanAddress into StackGrowsDown, which then doesn't give
+// correct answer.
+void StackLowerThanAddress(const void* ptr, bool* result) GTEST_NO_INLINE_;
+void StackLowerThanAddress(const void* ptr, bool* result) {
+ int dummy;
+ *result = (&dummy < ptr);
+}
+
+bool StackGrowsDown() {
+ int dummy;
+ bool result;
+ StackLowerThanAddress(&dummy, &result);
+ return result;
+}
+
+// Spawns a child process with the same executable as the current process in
+// a thread-safe manner and instructs it to run the death test. The
+// implementation uses fork(2) + exec. On systems where clone(2) is
+// available, it is used instead, being slightly more thread-safe. On QNX,
+// fork supports only single-threaded environments, so this function uses
+// spawn(2) there instead. The function dies with an error message if
+// anything goes wrong.
+static pid_t ExecDeathTestSpawnChild(char* const* argv, int close_fd) {
+ ExecDeathTestArgs args = { argv, close_fd };
+ pid_t child_pid = -1;
+
+# if GTEST_OS_QNX
+ // Obtains the current directory and sets it to be closed in the child
+ // process.
+ const int cwd_fd = open(".", O_RDONLY);
+ GTEST_DEATH_TEST_CHECK_(cwd_fd != -1);
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(fcntl(cwd_fd, F_SETFD, FD_CLOEXEC));
+ // We need to execute the test program in the same environment where
+ // it was originally invoked. Therefore we change to the original
+ // working directory first.
+ const char* const original_dir =
+ UnitTest::GetInstance()->original_working_dir();
+ // We can safely call chdir() as it's a direct system call.
+ if (chdir(original_dir) != 0) {
+ DeathTestAbort(std::string("chdir(\"") + original_dir + "\") failed: " +
+ GetLastErrnoDescription());
+ return EXIT_FAILURE;
+ }
+
+ int fd_flags;
+ // Set close_fd to be closed after spawn.
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(fd_flags = fcntl(close_fd, F_GETFD));
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(fcntl(close_fd, F_SETFD,
+ fd_flags | FD_CLOEXEC));
+ struct inheritance inherit = {0};
+ // spawn is a system call.
+ child_pid = spawn(args.argv[0], 0, NULL, &inherit, args.argv, GetEnviron());
+ // Restores the current working directory.
+ GTEST_DEATH_TEST_CHECK_(fchdir(cwd_fd) != -1);
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(close(cwd_fd));
+
+# else // GTEST_OS_QNX
+# if GTEST_OS_LINUX
+ // When a SIGPROF signal is received while fork() or clone() are executing,
+ // the process may hang. To avoid this, we ignore SIGPROF here and re-enable
+ // it after the call to fork()/clone() is complete.
+ struct sigaction saved_sigprof_action;
+ struct sigaction ignore_sigprof_action;
+ memset(&ignore_sigprof_action, 0, sizeof(ignore_sigprof_action));
+ sigemptyset(&ignore_sigprof_action.sa_mask);
+ ignore_sigprof_action.sa_handler = SIG_IGN;
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(sigaction(
+ SIGPROF, &ignore_sigprof_action, &saved_sigprof_action));
+# endif // GTEST_OS_LINUX
+
+# if GTEST_HAS_CLONE
+ const bool use_fork = GTEST_FLAG(death_test_use_fork);
+
+ if (!use_fork) {
+ static const bool stack_grows_down = StackGrowsDown();
+ const size_t stack_size = getpagesize();
+ // MMAP_ANONYMOUS is not defined on Mac, so we use MAP_ANON instead.
+ void* const stack = mmap(NULL, stack_size, PROT_READ | PROT_WRITE,
+ MAP_ANON | MAP_PRIVATE, -1, 0);
+ GTEST_DEATH_TEST_CHECK_(stack != MAP_FAILED);
+
+ // Maximum stack alignment in bytes: For a downward-growing stack, this
+ // amount is subtracted from size of the stack space to get an address
+ // that is within the stack space and is aligned on all systems we care
+ // about. As far as I know there is no ABI with stack alignment greater
+ // than 64. We assume stack and stack_size already have alignment of
+ // kMaxStackAlignment.
+ const size_t kMaxStackAlignment = 64;
+ void* const stack_top =
+ static_cast<char*>(stack) +
+ (stack_grows_down ? stack_size - kMaxStackAlignment : 0);
+ GTEST_DEATH_TEST_CHECK_(stack_size > kMaxStackAlignment &&
+ reinterpret_cast<intptr_t>(stack_top) % kMaxStackAlignment == 0);
+
+ child_pid = clone(&ExecDeathTestChildMain, stack_top, SIGCHLD, &args);
+
+ GTEST_DEATH_TEST_CHECK_(munmap(stack, stack_size) != -1);
+ }
+# else
+ const bool use_fork = true;
+# endif // GTEST_HAS_CLONE
+
+ if (use_fork && (child_pid = fork()) == 0) {
+ ExecDeathTestChildMain(&args);
+ _exit(0);
+ }
+# endif // GTEST_OS_QNX
+# if GTEST_OS_LINUX
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(
+ sigaction(SIGPROF, &saved_sigprof_action, NULL));
+# endif // GTEST_OS_LINUX
+
+ GTEST_DEATH_TEST_CHECK_(child_pid != -1);
+ return child_pid;
+}
+
+// The AssumeRole process for a fork-and-exec death test. It re-executes the
+// main program from the beginning, setting the --gtest_filter
+// and --gtest_internal_run_death_test flags to cause only the current
+// death test to be re-run.
+DeathTest::TestRole ExecDeathTest::AssumeRole() {
+ const UnitTestImpl* const impl = GetUnitTestImpl();
+ const InternalRunDeathTestFlag* const flag =
+ impl->internal_run_death_test_flag();
+ const TestInfo* const info = impl->current_test_info();
+ const int death_test_index = info->result()->death_test_count();
+
+ if (flag != NULL) {
+ set_write_fd(flag->write_fd());
+ return EXECUTE_TEST;
+ }
+
+ int pipe_fd[2];
+ GTEST_DEATH_TEST_CHECK_(pipe(pipe_fd) != -1);
+ // Clear the close-on-exec flag on the write end of the pipe, lest
+ // it be closed when the child process does an exec:
+ GTEST_DEATH_TEST_CHECK_(fcntl(pipe_fd[1], F_SETFD, 0) != -1);
+
+ const std::string filter_flag =
+ std::string("--") + GTEST_FLAG_PREFIX_ + kFilterFlag + "="
+ + info->test_case_name() + "." + info->name();
+ const std::string internal_flag =
+ std::string("--") + GTEST_FLAG_PREFIX_ + kInternalRunDeathTestFlag + "="
+ + file_ + "|" + StreamableToString(line_) + "|"
+ + StreamableToString(death_test_index) + "|"
+ + StreamableToString(pipe_fd[1]);
+ Arguments args;
+ args.AddArguments(GetArgvsForDeathTestChildProcess());
+ args.AddArgument(filter_flag.c_str());
+ args.AddArgument(internal_flag.c_str());
+
+ DeathTest::set_last_death_test_message("");
+
+ CaptureStderr();
+ // See the comment in NoExecDeathTest::AssumeRole for why the next line
+ // is necessary.
+ FlushInfoLog();
+
+ const pid_t child_pid = ExecDeathTestSpawnChild(args.Argv(), pipe_fd[0]);
+ GTEST_DEATH_TEST_CHECK_SYSCALL_(close(pipe_fd[1]));
+ set_child_pid(child_pid);
+ set_read_fd(pipe_fd[0]);
+ set_spawned(true);
+ return OVERSEE_TEST;
+}
+
+# endif // !GTEST_OS_WINDOWS
+
+// Creates a concrete DeathTest-derived class that depends on the
+// --gtest_death_test_style flag, and sets the pointer pointed to
+// by the "test" argument to its address. If the test should be
+// skipped, sets that pointer to NULL. Returns true, unless the
+// flag is set to an invalid value.
+bool DefaultDeathTestFactory::Create(const char* statement, const RE* regex,
+ const char* file, int line,
+ DeathTest** test) {
+ UnitTestImpl* const impl = GetUnitTestImpl();
+ const InternalRunDeathTestFlag* const flag =
+ impl->internal_run_death_test_flag();
+ const int death_test_index = impl->current_test_info()
+ ->increment_death_test_count();
+
+ if (flag != NULL) {
+ if (death_test_index > flag->index()) {
+ DeathTest::set_last_death_test_message(
+ "Death test count (" + StreamableToString(death_test_index)
+ + ") somehow exceeded expected maximum ("
+ + StreamableToString(flag->index()) + ")");
+ return false;
+ }
+
+ if (!(flag->file() == file && flag->line() == line &&
+ flag->index() == death_test_index)) {
+ *test = NULL;
+ return true;
+ }
+ }
+
+# if GTEST_OS_WINDOWS
+
+ if (GTEST_FLAG(death_test_style) == "threadsafe" ||
+ GTEST_FLAG(death_test_style) == "fast") {
+ *test = new WindowsDeathTest(statement, regex, file, line);
+ }
+
+# else
+
+ if (GTEST_FLAG(death_test_style) == "threadsafe") {
+ *test = new ExecDeathTest(statement, regex, file, line);
+ } else if (GTEST_FLAG(death_test_style) == "fast") {
+ *test = new NoExecDeathTest(statement, regex);
+ }
+
+# endif // GTEST_OS_WINDOWS
+
+ else { // NOLINT - this is more readable than unbalanced brackets inside #if.
+ DeathTest::set_last_death_test_message(
+ "Unknown death test style \"" + GTEST_FLAG(death_test_style)
+ + "\" encountered");
+ return false;
+ }
+
+ return true;
+}
+
+// Splits a given string on a given delimiter, populating a given
+// vector with the fields. GTEST_HAS_DEATH_TEST implies that we have
+// ::std::string, so we can use it here.
+static void SplitString(const ::std::string& str, char delimiter,
+ ::std::vector< ::std::string>* dest) {
+ ::std::vector< ::std::string> parsed;
+ ::std::string::size_type pos = 0;
+ while (::testing::internal::AlwaysTrue()) {
+ const ::std::string::size_type colon = str.find(delimiter, pos);
+ if (colon == ::std::string::npos) {
+ parsed.push_back(str.substr(pos));
+ break;
+ } else {
+ parsed.push_back(str.substr(pos, colon - pos));
+ pos = colon + 1;
+ }
+ }
+ dest->swap(parsed);
+}
+
+# if GTEST_OS_WINDOWS
+// Recreates the pipe and event handles from the provided parameters,
+// signals the event, and returns a file descriptor wrapped around the pipe
+// handle. This function is called in the child process only.
+int GetStatusFileDescriptor(unsigned int parent_process_id,
+ size_t write_handle_as_size_t,
+ size_t event_handle_as_size_t) {
+ AutoHandle parent_process_handle(::OpenProcess(PROCESS_DUP_HANDLE,
+ FALSE, // Non-inheritable.
+ parent_process_id));
+ if (parent_process_handle.Get() == INVALID_HANDLE_VALUE) {
+ DeathTestAbort("Unable to open parent process " +
+ StreamableToString(parent_process_id));
+ }
+
+ // TODO(vladl at google.com): Replace the following check with a
+ // compile-time assertion when available.
+ GTEST_CHECK_(sizeof(HANDLE) <= sizeof(size_t));
+
+ const HANDLE write_handle =
+ reinterpret_cast<HANDLE>(write_handle_as_size_t);
+ HANDLE dup_write_handle;
+
+ // The newly initialized handle is accessible only in in the parent
+ // process. To obtain one accessible within the child, we need to use
+ // DuplicateHandle.
+ if (!::DuplicateHandle(parent_process_handle.Get(), write_handle,
+ ::GetCurrentProcess(), &dup_write_handle,
+ 0x0, // Requested privileges ignored since
+ // DUPLICATE_SAME_ACCESS is used.
+ FALSE, // Request non-inheritable handler.
+ DUPLICATE_SAME_ACCESS)) {
+ DeathTestAbort("Unable to duplicate the pipe handle " +
+ StreamableToString(write_handle_as_size_t) +
+ " from the parent process " +
+ StreamableToString(parent_process_id));
+ }
+
+ const HANDLE event_handle = reinterpret_cast<HANDLE>(event_handle_as_size_t);
+ HANDLE dup_event_handle;
+
+ if (!::DuplicateHandle(parent_process_handle.Get(), event_handle,
+ ::GetCurrentProcess(), &dup_event_handle,
+ 0x0,
+ FALSE,
+ DUPLICATE_SAME_ACCESS)) {
+ DeathTestAbort("Unable to duplicate the event handle " +
+ StreamableToString(event_handle_as_size_t) +
+ " from the parent process " +
+ StreamableToString(parent_process_id));
+ }
+
+ const int write_fd =
+ ::_open_osfhandle(reinterpret_cast<intptr_t>(dup_write_handle), O_APPEND);
+ if (write_fd == -1) {
+ DeathTestAbort("Unable to convert pipe handle " +
+ StreamableToString(write_handle_as_size_t) +
+ " to a file descriptor");
+ }
+
+ // Signals the parent that the write end of the pipe has been acquired
+ // so the parent can release its own write end.
+ ::SetEvent(dup_event_handle);
+
+ return write_fd;
+}
+# endif // GTEST_OS_WINDOWS
+
+// Returns a newly created InternalRunDeathTestFlag object with fields
+// initialized from the GTEST_FLAG(internal_run_death_test) flag if
+// the flag is specified; otherwise returns NULL.
+InternalRunDeathTestFlag* ParseInternalRunDeathTestFlag() {
+ if (GTEST_FLAG(internal_run_death_test) == "") return NULL;
+
+ // GTEST_HAS_DEATH_TEST implies that we have ::std::string, so we
+ // can use it here.
+ int line = -1;
+ int index = -1;
+ ::std::vector< ::std::string> fields;
+ SplitString(GTEST_FLAG(internal_run_death_test).c_str(), '|', &fields);
+ int write_fd = -1;
+
+# if GTEST_OS_WINDOWS
+
+ unsigned int parent_process_id = 0;
+ size_t write_handle_as_size_t = 0;
+ size_t event_handle_as_size_t = 0;
+
+ if (fields.size() != 6
+ || !ParseNaturalNumber(fields[1], &line)
+ || !ParseNaturalNumber(fields[2], &index)
+ || !ParseNaturalNumber(fields[3], &parent_process_id)
+ || !ParseNaturalNumber(fields[4], &write_handle_as_size_t)
+ || !ParseNaturalNumber(fields[5], &event_handle_as_size_t)) {
+ DeathTestAbort("Bad --gtest_internal_run_death_test flag: " +
+ GTEST_FLAG(internal_run_death_test));
+ }
+ write_fd = GetStatusFileDescriptor(parent_process_id,
+ write_handle_as_size_t,
+ event_handle_as_size_t);
+# else
+
+ if (fields.size() != 4
+ || !ParseNaturalNumber(fields[1], &line)
+ || !ParseNaturalNumber(fields[2], &index)
+ || !ParseNaturalNumber(fields[3], &write_fd)) {
+ DeathTestAbort("Bad --gtest_internal_run_death_test flag: "
+ + GTEST_FLAG(internal_run_death_test));
+ }
+
+# endif // GTEST_OS_WINDOWS
+
+ return new InternalRunDeathTestFlag(fields[0], line, index, write_fd);
+}
+
+} // namespace internal
+
+#endif // GTEST_HAS_DEATH_TEST
+
+} // namespace testing
diff --git a/examples/stand_alone/test_fixture/gtest/src/gtest-filepath.cc b/examples/stand_alone/test_fixture/gtest/src/gtest-filepath.cc
new file mode 100644
index 0000000..6be58b6
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/src/gtest-filepath.cc
@@ -0,0 +1,382 @@
+// Copyright 2008, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Authors: keith.ray at gmail.com (Keith Ray)
+
+#include "gtest/gtest-message.h"
+#include "gtest/internal/gtest-filepath.h"
+#include "gtest/internal/gtest-port.h"
+
+#include <stdlib.h>
+
+#if GTEST_OS_WINDOWS_MOBILE
+# include <windows.h>
+#elif GTEST_OS_WINDOWS
+# include <direct.h>
+# include <io.h>
+#elif GTEST_OS_SYMBIAN
+// Symbian OpenC has PATH_MAX in sys/syslimits.h
+# include <sys/syslimits.h>
+#else
+# include <limits.h>
+# include <climits> // Some Linux distributions define PATH_MAX here.
+#endif // GTEST_OS_WINDOWS_MOBILE
+
+#if GTEST_OS_WINDOWS
+# define GTEST_PATH_MAX_ _MAX_PATH
+#elif defined(PATH_MAX)
+# define GTEST_PATH_MAX_ PATH_MAX
+#elif defined(_XOPEN_PATH_MAX)
+# define GTEST_PATH_MAX_ _XOPEN_PATH_MAX
+#else
+# define GTEST_PATH_MAX_ _POSIX_PATH_MAX
+#endif // GTEST_OS_WINDOWS
+
+#include "gtest/internal/gtest-string.h"
+
+namespace testing {
+namespace internal {
+
+#if GTEST_OS_WINDOWS
+// On Windows, '\\' is the standard path separator, but many tools and the
+// Windows API also accept '/' as an alternate path separator. Unless otherwise
+// noted, a file path can contain either kind of path separators, or a mixture
+// of them.
+const char kPathSeparator = '\\';
+const char kAlternatePathSeparator = '/';
+const char kPathSeparatorString[] = "\\";
+const char kAlternatePathSeparatorString[] = "/";
+# if GTEST_OS_WINDOWS_MOBILE
+// Windows CE doesn't have a current directory. You should not use
+// the current directory in tests on Windows CE, but this at least
+// provides a reasonable fallback.
+const char kCurrentDirectoryString[] = "\\";
+// Windows CE doesn't define INVALID_FILE_ATTRIBUTES
+const DWORD kInvalidFileAttributes = 0xffffffff;
+# else
+const char kCurrentDirectoryString[] = ".\\";
+# endif // GTEST_OS_WINDOWS_MOBILE
+#else
+const char kPathSeparator = '/';
+const char kPathSeparatorString[] = "/";
+const char kCurrentDirectoryString[] = "./";
+#endif // GTEST_OS_WINDOWS
+
+// Returns whether the given character is a valid path separator.
+static bool IsPathSeparator(char c) {
+#if GTEST_HAS_ALT_PATH_SEP_
+ return (c == kPathSeparator) || (c == kAlternatePathSeparator);
+#else
+ return c == kPathSeparator;
+#endif
+}
+
+// Returns the current working directory, or "" if unsuccessful.
+FilePath FilePath::GetCurrentDir() {
+#if GTEST_OS_WINDOWS_MOBILE
+ // Windows CE doesn't have a current directory, so we just return
+ // something reasonable.
+ return FilePath(kCurrentDirectoryString);
+#elif GTEST_OS_WINDOWS
+ char cwd[GTEST_PATH_MAX_ + 1] = { '\0' };
+ return FilePath(_getcwd(cwd, sizeof(cwd)) == NULL ? "" : cwd);
+#else
+ char cwd[GTEST_PATH_MAX_ + 1] = { '\0' };
+ return FilePath(getcwd(cwd, sizeof(cwd)) == NULL ? "" : cwd);
+#endif // GTEST_OS_WINDOWS_MOBILE
+}
+
+// Returns a copy of the FilePath with the case-insensitive extension removed.
+// Example: FilePath("dir/file.exe").RemoveExtension("EXE") returns
+// FilePath("dir/file"). If a case-insensitive extension is not
+// found, returns a copy of the original FilePath.
+FilePath FilePath::RemoveExtension(const char* extension) const {
+ const std::string dot_extension = std::string(".") + extension;
+ if (String::EndsWithCaseInsensitive(pathname_, dot_extension)) {
+ return FilePath(pathname_.substr(
+ 0, pathname_.length() - dot_extension.length()));
+ }
+ return *this;
+}
+
+// Returns a pointer to the last occurence of a valid path separator in
+// the FilePath. On Windows, for example, both '/' and '\' are valid path
+// separators. Returns NULL if no path separator was found.
+const char* FilePath::FindLastPathSeparator() const {
+ const char* const last_sep = strrchr(c_str(), kPathSeparator);
+#if GTEST_HAS_ALT_PATH_SEP_
+ const char* const last_alt_sep = strrchr(c_str(), kAlternatePathSeparator);
+ // Comparing two pointers of which only one is NULL is undefined.
+ if (last_alt_sep != NULL &&
+ (last_sep == NULL || last_alt_sep > last_sep)) {
+ return last_alt_sep;
+ }
+#endif
+ return last_sep;
+}
+
+// Returns a copy of the FilePath with the directory part removed.
+// Example: FilePath("path/to/file").RemoveDirectoryName() returns
+// FilePath("file"). If there is no directory part ("just_a_file"), it returns
+// the FilePath unmodified. If there is no file part ("just_a_dir/") it
+// returns an empty FilePath ("").
+// On Windows platform, '\' is the path separator, otherwise it is '/'.
+FilePath FilePath::RemoveDirectoryName() const {
+ const char* const last_sep = FindLastPathSeparator();
+ return last_sep ? FilePath(last_sep + 1) : *this;
+}
+
+// RemoveFileName returns the directory path with the filename removed.
+// Example: FilePath("path/to/file").RemoveFileName() returns "path/to/".
+// If the FilePath is "a_file" or "/a_file", RemoveFileName returns
+// FilePath("./") or, on Windows, FilePath(".\\"). If the filepath does
+// not have a file, like "just/a/dir/", it returns the FilePath unmodified.
+// On Windows platform, '\' is the path separator, otherwise it is '/'.
+FilePath FilePath::RemoveFileName() const {
+ const char* const last_sep = FindLastPathSeparator();
+ std::string dir;
+ if (last_sep) {
+ dir = std::string(c_str(), last_sep + 1 - c_str());
+ } else {
+ dir = kCurrentDirectoryString;
+ }
+ return FilePath(dir);
+}
+
+// Helper functions for naming files in a directory for xml output.
+
+// Given directory = "dir", base_name = "test", number = 0,
+// extension = "xml", returns "dir/test.xml". If number is greater
+// than zero (e.g., 12), returns "dir/test_12.xml".
+// On Windows platform, uses \ as the separator rather than /.
+FilePath FilePath::MakeFileName(const FilePath& directory,
+ const FilePath& base_name,
+ int number,
+ const char* extension) {
+ std::string file;
+ if (number == 0) {
+ file = base_name.string() + "." + extension;
+ } else {
+ file = base_name.string() + "_" + StreamableToString(number)
+ + "." + extension;
+ }
+ return ConcatPaths(directory, FilePath(file));
+}
+
+// Given directory = "dir", relative_path = "test.xml", returns "dir/test.xml".
+// On Windows, uses \ as the separator rather than /.
+FilePath FilePath::ConcatPaths(const FilePath& directory,
+ const FilePath& relative_path) {
+ if (directory.IsEmpty())
+ return relative_path;
+ const FilePath dir(directory.RemoveTrailingPathSeparator());
+ return FilePath(dir.string() + kPathSeparator + relative_path.string());
+}
+
+// Returns true if pathname describes something findable in the file-system,
+// either a file, directory, or whatever.
+bool FilePath::FileOrDirectoryExists() const {
+#if GTEST_OS_WINDOWS_MOBILE
+ LPCWSTR unicode = String::AnsiToUtf16(pathname_.c_str());
+ const DWORD attributes = GetFileAttributes(unicode);
+ delete [] unicode;
+ return attributes != kInvalidFileAttributes;
+#else
+ posix::StatStruct file_stat;
+ return posix::Stat(pathname_.c_str(), &file_stat) == 0;
+#endif // GTEST_OS_WINDOWS_MOBILE
+}
+
+// Returns true if pathname describes a directory in the file-system
+// that exists.
+bool FilePath::DirectoryExists() const {
+ bool result = false;
+#if GTEST_OS_WINDOWS
+ // Don't strip off trailing separator if path is a root directory on
+ // Windows (like "C:\\").
+ const FilePath& path(IsRootDirectory() ? *this :
+ RemoveTrailingPathSeparator());
+#else
+ const FilePath& path(*this);
+#endif
+
+#if GTEST_OS_WINDOWS_MOBILE
+ LPCWSTR unicode = String::AnsiToUtf16(path.c_str());
+ const DWORD attributes = GetFileAttributes(unicode);
+ delete [] unicode;
+ if ((attributes != kInvalidFileAttributes) &&
+ (attributes & FILE_ATTRIBUTE_DIRECTORY)) {
+ result = true;
+ }
+#else
+ posix::StatStruct file_stat;
+ result = posix::Stat(path.c_str(), &file_stat) == 0 &&
+ posix::IsDir(file_stat);
+#endif // GTEST_OS_WINDOWS_MOBILE
+
+ return result;
+}
+
+// Returns true if pathname describes a root directory. (Windows has one
+// root directory per disk drive.)
+bool FilePath::IsRootDirectory() const {
+#if GTEST_OS_WINDOWS
+ // TODO(wan at google.com): on Windows a network share like
+ // \\server\share can be a root directory, although it cannot be the
+ // current directory. Handle this properly.
+ return pathname_.length() == 3 && IsAbsolutePath();
+#else
+ return pathname_.length() == 1 && IsPathSeparator(pathname_.c_str()[0]);
+#endif
+}
+
+// Returns true if pathname describes an absolute path.
+bool FilePath::IsAbsolutePath() const {
+ const char* const name = pathname_.c_str();
+#if GTEST_OS_WINDOWS
+ return pathname_.length() >= 3 &&
+ ((name[0] >= 'a' && name[0] <= 'z') ||
+ (name[0] >= 'A' && name[0] <= 'Z')) &&
+ name[1] == ':' &&
+ IsPathSeparator(name[2]);
+#else
+ return IsPathSeparator(name[0]);
+#endif
+}
+
+// Returns a pathname for a file that does not currently exist. The pathname
+// will be directory/base_name.extension or
+// directory/base_name_<number>.extension if directory/base_name.extension
+// already exists. The number will be incremented until a pathname is found
+// that does not already exist.
+// Examples: 'dir/foo_test.xml' or 'dir/foo_test_1.xml'.
+// There could be a race condition if two or more processes are calling this
+// function at the same time -- they could both pick the same filename.
+FilePath FilePath::GenerateUniqueFileName(const FilePath& directory,
+ const FilePath& base_name,
+ const char* extension) {
+ FilePath full_pathname;
+ int number = 0;
+ do {
+ full_pathname.Set(MakeFileName(directory, base_name, number++, extension));
+ } while (full_pathname.FileOrDirectoryExists());
+ return full_pathname;
+}
+
+// Returns true if FilePath ends with a path separator, which indicates that
+// it is intended to represent a directory. Returns false otherwise.
+// This does NOT check that a directory (or file) actually exists.
+bool FilePath::IsDirectory() const {
+ return !pathname_.empty() &&
+ IsPathSeparator(pathname_.c_str()[pathname_.length() - 1]);
+}
+
+// Create directories so that path exists. Returns true if successful or if
+// the directories already exist; returns false if unable to create directories
+// for any reason.
+bool FilePath::CreateDirectoriesRecursively() const {
+ if (!this->IsDirectory()) {
+ return false;
+ }
+
+ if (pathname_.length() == 0 || this->DirectoryExists()) {
+ return true;
+ }
+
+ const FilePath parent(this->RemoveTrailingPathSeparator().RemoveFileName());
+ return parent.CreateDirectoriesRecursively() && this->CreateFolder();
+}
+
+// Create the directory so that path exists. Returns true if successful or
+// if the directory already exists; returns false if unable to create the
+// directory for any reason, including if the parent directory does not
+// exist. Not named "CreateDirectory" because that's a macro on Windows.
+bool FilePath::CreateFolder() const {
+#if GTEST_OS_WINDOWS_MOBILE
+ FilePath removed_sep(this->RemoveTrailingPathSeparator());
+ LPCWSTR unicode = String::AnsiToUtf16(removed_sep.c_str());
+ int result = CreateDirectory(unicode, NULL) ? 0 : -1;
+ delete [] unicode;
+#elif GTEST_OS_WINDOWS
+ int result = _mkdir(pathname_.c_str());
+#else
+ int result = mkdir(pathname_.c_str(), 0777);
+#endif // GTEST_OS_WINDOWS_MOBILE
+
+ if (result == -1) {
+ return this->DirectoryExists(); // An error is OK if the directory exists.
+ }
+ return true; // No error.
+}
+
+// If input name has a trailing separator character, remove it and return the
+// name, otherwise return the name string unmodified.
+// On Windows platform, uses \ as the separator, other platforms use /.
+FilePath FilePath::RemoveTrailingPathSeparator() const {
+ return IsDirectory()
+ ? FilePath(pathname_.substr(0, pathname_.length() - 1))
+ : *this;
+}
+
+// Removes any redundant separators that might be in the pathname.
+// For example, "bar///foo" becomes "bar/foo". Does not eliminate other
+// redundancies that might be in a pathname involving "." or "..".
+// TODO(wan at google.com): handle Windows network shares (e.g. \\server\share).
+void FilePath::Normalize() {
+ if (pathname_.c_str() == NULL) {
+ pathname_ = "";
+ return;
+ }
+ const char* src = pathname_.c_str();
+ char* const dest = new char[pathname_.length() + 1];
+ char* dest_ptr = dest;
+ memset(dest_ptr, 0, pathname_.length() + 1);
+
+ while (*src != '\0') {
+ *dest_ptr = *src;
+ if (!IsPathSeparator(*src)) {
+ src++;
+ } else {
+#if GTEST_HAS_ALT_PATH_SEP_
+ if (*dest_ptr == kAlternatePathSeparator) {
+ *dest_ptr = kPathSeparator;
+ }
+#endif
+ while (IsPathSeparator(*src))
+ src++;
+ }
+ dest_ptr++;
+ }
+ *dest_ptr = '\0';
+ pathname_ = dest;
+ delete[] dest;
+}
+
+} // namespace internal
+} // namespace testing
diff --git a/examples/stand_alone/test_fixture/gtest/src/gtest-internal-inl.h b/examples/stand_alone/test_fixture/gtest/src/gtest-internal-inl.h
new file mode 100644
index 0000000..35df303
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/src/gtest-internal-inl.h
@@ -0,0 +1,1218 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// Utility functions and classes used by the Google C++ testing framework.
+//
+// Author: wan at google.com (Zhanyong Wan)
+//
+// This file contains purely Google Test's internal implementation. Please
+// DO NOT #INCLUDE IT IN A USER PROGRAM.
+
+#ifndef GTEST_SRC_GTEST_INTERNAL_INL_H_
+#define GTEST_SRC_GTEST_INTERNAL_INL_H_
+
+// GTEST_IMPLEMENTATION_ is defined to 1 iff the current translation unit is
+// part of Google Test's implementation; otherwise it's undefined.
+#if !GTEST_IMPLEMENTATION_
+// A user is trying to include this from his code - just say no.
+# error "gtest-internal-inl.h is part of Google Test's internal implementation."
+# error "It must not be included except by Google Test itself."
+#endif // GTEST_IMPLEMENTATION_
+
+#ifndef _WIN32_WCE
+# include <errno.h>
+#endif // !_WIN32_WCE
+#include <stddef.h>
+#include <stdlib.h> // For strtoll/_strtoul64/malloc/free.
+#include <string.h> // For memmove.
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+#include "gtest/internal/gtest-port.h"
+
+#if GTEST_CAN_STREAM_RESULTS_
+# include <arpa/inet.h> // NOLINT
+# include <netdb.h> // NOLINT
+#endif
+
+#if GTEST_OS_WINDOWS
+# include <windows.h> // NOLINT
+#endif // GTEST_OS_WINDOWS
+
+#include "gtest/gtest.h" // NOLINT
+#include "gtest/gtest-spi.h"
+
+namespace testing {
+
+// Declares the flags.
+//
+// We don't want the users to modify this flag in the code, but want
+// Google Test's own unit tests to be able to access it. Therefore we
+// declare it here as opposed to in gtest.h.
+GTEST_DECLARE_bool_(death_test_use_fork);
+
+namespace internal {
+
+// The value of GetTestTypeId() as seen from within the Google Test
+// library. This is solely for testing GetTestTypeId().
+GTEST_API_ extern const TypeId kTestTypeIdInGoogleTest;
+
+// Names of the flags (needed for parsing Google Test flags).
+const char kAlsoRunDisabledTestsFlag[] = "also_run_disabled_tests";
+const char kBreakOnFailureFlag[] = "break_on_failure";
+const char kCatchExceptionsFlag[] = "catch_exceptions";
+const char kColorFlag[] = "color";
+const char kFilterFlag[] = "filter";
+const char kListTestsFlag[] = "list_tests";
+const char kOutputFlag[] = "output";
+const char kPrintTimeFlag[] = "print_time";
+const char kRandomSeedFlag[] = "random_seed";
+const char kRepeatFlag[] = "repeat";
+const char kShuffleFlag[] = "shuffle";
+const char kStackTraceDepthFlag[] = "stack_trace_depth";
+const char kStreamResultToFlag[] = "stream_result_to";
+const char kThrowOnFailureFlag[] = "throw_on_failure";
+
+// A valid random seed must be in [1, kMaxRandomSeed].
+const int kMaxRandomSeed = 99999;
+
+// g_help_flag is true iff the --help flag or an equivalent form is
+// specified on the command line.
+GTEST_API_ extern bool g_help_flag;
+
+// Returns the current time in milliseconds.
+GTEST_API_ TimeInMillis GetTimeInMillis();
+
+// Returns true iff Google Test should use colors in the output.
+GTEST_API_ bool ShouldUseColor(bool stdout_is_tty);
+
+// Formats the given time in milliseconds as seconds.
+GTEST_API_ std::string FormatTimeInMillisAsSeconds(TimeInMillis ms);
+
+// Converts the given time in milliseconds to a date string in the ISO 8601
+// format, without the timezone information. N.B.: due to the use the
+// non-reentrant localtime() function, this function is not thread safe. Do
+// not use it in any code that can be called from multiple threads.
+GTEST_API_ std::string FormatEpochTimeInMillisAsIso8601(TimeInMillis ms);
+
+// Parses a string for an Int32 flag, in the form of "--flag=value".
+//
+// On success, stores the value of the flag in *value, and returns
+// true. On failure, returns false without changing *value.
+GTEST_API_ bool ParseInt32Flag(
+ const char* str, const char* flag, Int32* value);
+
+// Returns a random seed in range [1, kMaxRandomSeed] based on the
+// given --gtest_random_seed flag value.
+inline int GetRandomSeedFromFlag(Int32 random_seed_flag) {
+ const unsigned int raw_seed = (random_seed_flag == 0) ?
+ static_cast<unsigned int>(GetTimeInMillis()) :
+ static_cast<unsigned int>(random_seed_flag);
+
+ // Normalizes the actual seed to range [1, kMaxRandomSeed] such that
+ // it's easy to type.
+ const int normalized_seed =
+ static_cast<int>((raw_seed - 1U) %
+ static_cast<unsigned int>(kMaxRandomSeed)) + 1;
+ return normalized_seed;
+}
+
+// Returns the first valid random seed after 'seed'. The behavior is
+// undefined if 'seed' is invalid. The seed after kMaxRandomSeed is
+// considered to be 1.
+inline int GetNextRandomSeed(int seed) {
+ GTEST_CHECK_(1 <= seed && seed <= kMaxRandomSeed)
+ << "Invalid random seed " << seed << " - must be in [1, "
+ << kMaxRandomSeed << "].";
+ const int next_seed = seed + 1;
+ return (next_seed > kMaxRandomSeed) ? 1 : next_seed;
+}
+
+// This class saves the values of all Google Test flags in its c'tor, and
+// restores them in its d'tor.
+class GTestFlagSaver {
+ public:
+ // The c'tor.
+ GTestFlagSaver() {
+ also_run_disabled_tests_ = GTEST_FLAG(also_run_disabled_tests);
+ break_on_failure_ = GTEST_FLAG(break_on_failure);
+ catch_exceptions_ = GTEST_FLAG(catch_exceptions);
+ color_ = GTEST_FLAG(color);
+ death_test_style_ = GTEST_FLAG(death_test_style);
+ death_test_use_fork_ = GTEST_FLAG(death_test_use_fork);
+ filter_ = GTEST_FLAG(filter);
+ internal_run_death_test_ = GTEST_FLAG(internal_run_death_test);
+ list_tests_ = GTEST_FLAG(list_tests);
+ output_ = GTEST_FLAG(output);
+ print_time_ = GTEST_FLAG(print_time);
+ random_seed_ = GTEST_FLAG(random_seed);
+ repeat_ = GTEST_FLAG(repeat);
+ shuffle_ = GTEST_FLAG(shuffle);
+ stack_trace_depth_ = GTEST_FLAG(stack_trace_depth);
+ stream_result_to_ = GTEST_FLAG(stream_result_to);
+ throw_on_failure_ = GTEST_FLAG(throw_on_failure);
+ }
+
+ // The d'tor is not virtual. DO NOT INHERIT FROM THIS CLASS.
+ ~GTestFlagSaver() {
+ GTEST_FLAG(also_run_disabled_tests) = also_run_disabled_tests_;
+ GTEST_FLAG(break_on_failure) = break_on_failure_;
+ GTEST_FLAG(catch_exceptions) = catch_exceptions_;
+ GTEST_FLAG(color) = color_;
+ GTEST_FLAG(death_test_style) = death_test_style_;
+ GTEST_FLAG(death_test_use_fork) = death_test_use_fork_;
+ GTEST_FLAG(filter) = filter_;
+ GTEST_FLAG(internal_run_death_test) = internal_run_death_test_;
+ GTEST_FLAG(list_tests) = list_tests_;
+ GTEST_FLAG(output) = output_;
+ GTEST_FLAG(print_time) = print_time_;
+ GTEST_FLAG(random_seed) = random_seed_;
+ GTEST_FLAG(repeat) = repeat_;
+ GTEST_FLAG(shuffle) = shuffle_;
+ GTEST_FLAG(stack_trace_depth) = stack_trace_depth_;
+ GTEST_FLAG(stream_result_to) = stream_result_to_;
+ GTEST_FLAG(throw_on_failure) = throw_on_failure_;
+ }
+
+ private:
+ // Fields for saving the original values of flags.
+ bool also_run_disabled_tests_;
+ bool break_on_failure_;
+ bool catch_exceptions_;
+ std::string color_;
+ std::string death_test_style_;
+ bool death_test_use_fork_;
+ std::string filter_;
+ std::string internal_run_death_test_;
+ bool list_tests_;
+ std::string output_;
+ bool print_time_;
+ internal::Int32 random_seed_;
+ internal::Int32 repeat_;
+ bool shuffle_;
+ internal::Int32 stack_trace_depth_;
+ std::string stream_result_to_;
+ bool throw_on_failure_;
+} GTEST_ATTRIBUTE_UNUSED_;
+
+// Converts a Unicode code point to a narrow string in UTF-8 encoding.
+// code_point parameter is of type UInt32 because wchar_t may not be
+// wide enough to contain a code point.
+// If the code_point is not a valid Unicode code point
+// (i.e. outside of Unicode range U+0 to U+10FFFF) it will be converted
+// to "(Invalid Unicode 0xXXXXXXXX)".
+GTEST_API_ std::string CodePointToUtf8(UInt32 code_point);
+
+// Converts a wide string to a narrow string in UTF-8 encoding.
+// The wide string is assumed to have the following encoding:
+// UTF-16 if sizeof(wchar_t) == 2 (on Windows, Cygwin, Symbian OS)
+// UTF-32 if sizeof(wchar_t) == 4 (on Linux)
+// Parameter str points to a null-terminated wide string.
+// Parameter num_chars may additionally limit the number
+// of wchar_t characters processed. -1 is used when the entire string
+// should be processed.
+// If the string contains code points that are not valid Unicode code points
+// (i.e. outside of Unicode range U+0 to U+10FFFF) they will be output
+// as '(Invalid Unicode 0xXXXXXXXX)'. If the string is in UTF16 encoding
+// and contains invalid UTF-16 surrogate pairs, values in those pairs
+// will be encoded as individual Unicode characters from Basic Normal Plane.
+GTEST_API_ std::string WideStringToUtf8(const wchar_t* str, int num_chars);
+
+// Reads the GTEST_SHARD_STATUS_FILE environment variable, and creates the file
+// if the variable is present. If a file already exists at this location, this
+// function will write over it. If the variable is present, but the file cannot
+// be created, prints an error and exits.
+void WriteToShardStatusFileIfNeeded();
+
+// Checks whether sharding is enabled by examining the relevant
+// environment variable values. If the variables are present,
+// but inconsistent (e.g., shard_index >= total_shards), prints
+// an error and exits. If in_subprocess_for_death_test, sharding is
+// disabled because it must only be applied to the original test
+// process. Otherwise, we could filter out death tests we intended to execute.
+GTEST_API_ bool ShouldShard(const char* total_shards_str,
+ const char* shard_index_str,
+ bool in_subprocess_for_death_test);
+
+// Parses the environment variable var as an Int32. If it is unset,
+// returns default_val. If it is not an Int32, prints an error and
+// and aborts.
+GTEST_API_ Int32 Int32FromEnvOrDie(const char* env_var, Int32 default_val);
+
+// Given the total number of shards, the shard index, and the test id,
+// returns true iff the test should be run on this shard. The test id is
+// some arbitrary but unique non-negative integer assigned to each test
+// method. Assumes that 0 <= shard_index < total_shards.
+GTEST_API_ bool ShouldRunTestOnShard(
+ int total_shards, int shard_index, int test_id);
+
+// STL container utilities.
+
+// Returns the number of elements in the given container that satisfy
+// the given predicate.
+template <class Container, typename Predicate>
+inline int CountIf(const Container& c, Predicate predicate) {
+ // Implemented as an explicit loop since std::count_if() in libCstd on
+ // Solaris has a non-standard signature.
+ int count = 0;
+ for (typename Container::const_iterator it = c.begin(); it != c.end(); ++it) {
+ if (predicate(*it))
+ ++count;
+ }
+ return count;
+}
+
+// Applies a function/functor to each element in the container.
+template <class Container, typename Functor>
+void ForEach(const Container& c, Functor functor) {
+ std::for_each(c.begin(), c.end(), functor);
+}
+
+// Returns the i-th element of the vector, or default_value if i is not
+// in range [0, v.size()).
+template <typename E>
+inline E GetElementOr(const std::vector<E>& v, int i, E default_value) {
+ return (i < 0 || i >= static_cast<int>(v.size())) ? default_value : v[i];
+}
+
+// Performs an in-place shuffle of a range of the vector's elements.
+// 'begin' and 'end' are element indices as an STL-style range;
+// i.e. [begin, end) are shuffled, where 'end' == size() means to
+// shuffle to the end of the vector.
+template <typename E>
+void ShuffleRange(internal::Random* random, int begin, int end,
+ std::vector<E>* v) {
+ const int size = static_cast<int>(v->size());
+ GTEST_CHECK_(0 <= begin && begin <= size)
+ << "Invalid shuffle range start " << begin << ": must be in range [0, "
+ << size << "].";
+ GTEST_CHECK_(begin <= end && end <= size)
+ << "Invalid shuffle range finish " << end << ": must be in range ["
+ << begin << ", " << size << "].";
+
+ // Fisher-Yates shuffle, from
+ // http://en.wikipedia.org/wiki/Fisher-Yates_shuffle
+ for (int range_width = end - begin; range_width >= 2; range_width--) {
+ const int last_in_range = begin + range_width - 1;
+ const int selected = begin + random->Generate(range_width);
+ std::swap((*v)[selected], (*v)[last_in_range]);
+ }
+}
+
+// Performs an in-place shuffle of the vector's elements.
+template <typename E>
+inline void Shuffle(internal::Random* random, std::vector<E>* v) {
+ ShuffleRange(random, 0, static_cast<int>(v->size()), v);
+}
+
+// A function for deleting an object. Handy for being used as a
+// functor.
+template <typename T>
+static void Delete(T* x) {
+ delete x;
+}
+
+// A predicate that checks the key of a TestProperty against a known key.
+//
+// TestPropertyKeyIs is copyable.
+class TestPropertyKeyIs {
+ public:
+ // Constructor.
+ //
+ // TestPropertyKeyIs has NO default constructor.
+ explicit TestPropertyKeyIs(const std::string& key) : key_(key) {}
+
+ // Returns true iff the test name of test property matches on key_.
+ bool operator()(const TestProperty& test_property) const {
+ return test_property.key() == key_;
+ }
+
+ private:
+ std::string key_;
+};
+
+// Class UnitTestOptions.
+//
+// This class contains functions for processing options the user
+// specifies when running the tests. It has only static members.
+//
+// In most cases, the user can specify an option using either an
+// environment variable or a command line flag. E.g. you can set the
+// test filter using either GTEST_FILTER or --gtest_filter. If both
+// the variable and the flag are present, the latter overrides the
+// former.
+class GTEST_API_ UnitTestOptions {
+ public:
+ // Functions for processing the gtest_output flag.
+
+ // Returns the output format, or "" for normal printed output.
+ static std::string GetOutputFormat();
+
+ // Returns the absolute path of the requested output file, or the
+ // default (test_detail.xml in the original working directory) if
+ // none was explicitly specified.
+ static std::string GetAbsolutePathToOutputFile();
+
+ // Functions for processing the gtest_filter flag.
+
+ // Returns true iff the wildcard pattern matches the string. The
+ // first ':' or '\0' character in pattern marks the end of it.
+ //
+ // This recursive algorithm isn't very efficient, but is clear and
+ // works well enough for matching test names, which are short.
+ static bool PatternMatchesString(const char *pattern, const char *str);
+
+ // Returns true iff the user-specified filter matches the test case
+ // name and the test name.
+ static bool FilterMatchesTest(const std::string &test_case_name,
+ const std::string &test_name);
+
+#if GTEST_OS_WINDOWS
+ // Function for supporting the gtest_catch_exception flag.
+
+ // Returns EXCEPTION_EXECUTE_HANDLER if Google Test should handle the
+ // given SEH exception, or EXCEPTION_CONTINUE_SEARCH otherwise.
+ // This function is useful as an __except condition.
+ static int GTestShouldProcessSEH(DWORD exception_code);
+#endif // GTEST_OS_WINDOWS
+
+ // Returns true if "name" matches the ':' separated list of glob-style
+ // filters in "filter".
+ static bool MatchesFilter(const std::string& name, const char* filter);
+};
+
+// Returns the current application's name, removing directory path if that
+// is present. Used by UnitTestOptions::GetOutputFile.
+GTEST_API_ FilePath GetCurrentExecutableName();
+
+// The role interface for getting the OS stack trace as a string.
+class OsStackTraceGetterInterface {
+ public:
+ OsStackTraceGetterInterface() {}
+ virtual ~OsStackTraceGetterInterface() {}
+
+ // Returns the current OS stack trace as an std::string. Parameters:
+ //
+ // max_depth - the maximum number of stack frames to be included
+ // in the trace.
+ // skip_count - the number of top frames to be skipped; doesn't count
+ // against max_depth.
+ virtual string CurrentStackTrace(int max_depth, int skip_count) = 0;
+
+ // UponLeavingGTest() should be called immediately before Google Test calls
+ // user code. It saves some information about the current stack that
+ // CurrentStackTrace() will use to find and hide Google Test stack frames.
+ virtual void UponLeavingGTest() = 0;
+
+ private:
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(OsStackTraceGetterInterface);
+};
+
+// A working implementation of the OsStackTraceGetterInterface interface.
+class OsStackTraceGetter : public OsStackTraceGetterInterface {
+ public:
+ OsStackTraceGetter() : caller_frame_(NULL) {}
+
+ virtual string CurrentStackTrace(int max_depth, int skip_count)
+ GTEST_LOCK_EXCLUDED_(mutex_);
+
+ virtual void UponLeavingGTest() GTEST_LOCK_EXCLUDED_(mutex_);
+
+ // This string is inserted in place of stack frames that are part of
+ // Google Test's implementation.
+ static const char* const kElidedFramesMarker;
+
+ private:
+ Mutex mutex_; // protects all internal state
+
+ // We save the stack frame below the frame that calls user code.
+ // We do this because the address of the frame immediately below
+ // the user code changes between the call to UponLeavingGTest()
+ // and any calls to CurrentStackTrace() from within the user code.
+ void* caller_frame_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(OsStackTraceGetter);
+};
+
+// Information about a Google Test trace point.
+struct TraceInfo {
+ const char* file;
+ int line;
+ std::string message;
+};
+
+// This is the default global test part result reporter used in UnitTestImpl.
+// This class should only be used by UnitTestImpl.
+class DefaultGlobalTestPartResultReporter
+ : public TestPartResultReporterInterface {
+ public:
+ explicit DefaultGlobalTestPartResultReporter(UnitTestImpl* unit_test);
+ // Implements the TestPartResultReporterInterface. Reports the test part
+ // result in the current test.
+ virtual void ReportTestPartResult(const TestPartResult& result);
+
+ private:
+ UnitTestImpl* const unit_test_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(DefaultGlobalTestPartResultReporter);
+};
+
+// This is the default per thread test part result reporter used in
+// UnitTestImpl. This class should only be used by UnitTestImpl.
+class DefaultPerThreadTestPartResultReporter
+ : public TestPartResultReporterInterface {
+ public:
+ explicit DefaultPerThreadTestPartResultReporter(UnitTestImpl* unit_test);
+ // Implements the TestPartResultReporterInterface. The implementation just
+ // delegates to the current global test part result reporter of *unit_test_.
+ virtual void ReportTestPartResult(const TestPartResult& result);
+
+ private:
+ UnitTestImpl* const unit_test_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(DefaultPerThreadTestPartResultReporter);
+};
+
+// The private implementation of the UnitTest class. We don't protect
+// the methods under a mutex, as this class is not accessible by a
+// user and the UnitTest class that delegates work to this class does
+// proper locking.
+class GTEST_API_ UnitTestImpl {
+ public:
+ explicit UnitTestImpl(UnitTest* parent);
+ virtual ~UnitTestImpl();
+
+ // There are two different ways to register your own TestPartResultReporter.
+ // You can register your own repoter to listen either only for test results
+ // from the current thread or for results from all threads.
+ // By default, each per-thread test result repoter just passes a new
+ // TestPartResult to the global test result reporter, which registers the
+ // test part result for the currently running test.
+
+ // Returns the global test part result reporter.
+ TestPartResultReporterInterface* GetGlobalTestPartResultReporter();
+
+ // Sets the global test part result reporter.
+ void SetGlobalTestPartResultReporter(
+ TestPartResultReporterInterface* reporter);
+
+ // Returns the test part result reporter for the current thread.
+ TestPartResultReporterInterface* GetTestPartResultReporterForCurrentThread();
+
+ // Sets the test part result reporter for the current thread.
+ void SetTestPartResultReporterForCurrentThread(
+ TestPartResultReporterInterface* reporter);
+
+ // Gets the number of successful test cases.
+ int successful_test_case_count() const;
+
+ // Gets the number of failed test cases.
+ int failed_test_case_count() const;
+
+ // Gets the number of all test cases.
+ int total_test_case_count() const;
+
+ // Gets the number of all test cases that contain at least one test
+ // that should run.
+ int test_case_to_run_count() const;
+
+ // Gets the number of successful tests.
+ int successful_test_count() const;
+
+ // Gets the number of failed tests.
+ int failed_test_count() const;
+
+ // Gets the number of disabled tests that will be reported in the XML report.
+ int reportable_disabled_test_count() const;
+
+ // Gets the number of disabled tests.
+ int disabled_test_count() const;
+
+ // Gets the number of tests to be printed in the XML report.
+ int reportable_test_count() const;
+
+ // Gets the number of all tests.
+ int total_test_count() const;
+
+ // Gets the number of tests that should run.
+ int test_to_run_count() const;
+
+ // Gets the time of the test program start, in ms from the start of the
+ // UNIX epoch.
+ TimeInMillis start_timestamp() const { return start_timestamp_; }
+
+ // Gets the elapsed time, in milliseconds.
+ TimeInMillis elapsed_time() const { return elapsed_time_; }
+
+ // Returns true iff the unit test passed (i.e. all test cases passed).
+ bool Passed() const { return !Failed(); }
+
+ // Returns true iff the unit test failed (i.e. some test case failed
+ // or something outside of all tests failed).
+ bool Failed() const {
+ return failed_test_case_count() > 0 || ad_hoc_test_result()->Failed();
+ }
+
+ // Gets the i-th test case among all the test cases. i can range from 0 to
+ // total_test_case_count() - 1. If i is not in that range, returns NULL.
+ const TestCase* GetTestCase(int i) const {
+ const int index = GetElementOr(test_case_indices_, i, -1);
+ return index < 0 ? NULL : test_cases_[i];
+ }
+
+ // Gets the i-th test case among all the test cases. i can range from 0 to
+ // total_test_case_count() - 1. If i is not in that range, returns NULL.
+ TestCase* GetMutableTestCase(int i) {
+ const int index = GetElementOr(test_case_indices_, i, -1);
+ return index < 0 ? NULL : test_cases_[index];
+ }
+
+ // Provides access to the event listener list.
+ TestEventListeners* listeners() { return &listeners_; }
+
+ // Returns the TestResult for the test that's currently running, or
+ // the TestResult for the ad hoc test if no test is running.
+ TestResult* current_test_result();
+
+ // Returns the TestResult for the ad hoc test.
+ const TestResult* ad_hoc_test_result() const { return &ad_hoc_test_result_; }
+
+ // Sets the OS stack trace getter.
+ //
+ // Does nothing if the input and the current OS stack trace getter
+ // are the same; otherwise, deletes the old getter and makes the
+ // input the current getter.
+ void set_os_stack_trace_getter(OsStackTraceGetterInterface* getter);
+
+ // Returns the current OS stack trace getter if it is not NULL;
+ // otherwise, creates an OsStackTraceGetter, makes it the current
+ // getter, and returns it.
+ OsStackTraceGetterInterface* os_stack_trace_getter();
+
+ // Returns the current OS stack trace as an std::string.
+ //
+ // The maximum number of stack frames to be included is specified by
+ // the gtest_stack_trace_depth flag. The skip_count parameter
+ // specifies the number of top frames to be skipped, which doesn't
+ // count against the number of frames to be included.
+ //
+ // For example, if Foo() calls Bar(), which in turn calls
+ // CurrentOsStackTraceExceptTop(1), Foo() will be included in the
+ // trace but Bar() and CurrentOsStackTraceExceptTop() won't.
+ std::string CurrentOsStackTraceExceptTop(int skip_count) GTEST_NO_INLINE_;
+
+ // Finds and returns a TestCase with the given name. If one doesn't
+ // exist, creates one and returns it.
+ //
+ // Arguments:
+ //
+ // test_case_name: name of the test case
+ // type_param: the name of the test's type parameter, or NULL if
+ // this is not a typed or a type-parameterized test.
+ // set_up_tc: pointer to the function that sets up the test case
+ // tear_down_tc: pointer to the function that tears down the test case
+ TestCase* GetTestCase(const char* test_case_name,
+ const char* type_param,
+ Test::SetUpTestCaseFunc set_up_tc,
+ Test::TearDownTestCaseFunc tear_down_tc);
+
+ // Adds a TestInfo to the unit test.
+ //
+ // Arguments:
+ //
+ // set_up_tc: pointer to the function that sets up the test case
+ // tear_down_tc: pointer to the function that tears down the test case
+ // test_info: the TestInfo object
+ void AddTestInfo(Test::SetUpTestCaseFunc set_up_tc,
+ Test::TearDownTestCaseFunc tear_down_tc,
+ TestInfo* test_info) {
+ // In order to support thread-safe death tests, we need to
+ // remember the original working directory when the test program
+ // was first invoked. We cannot do this in RUN_ALL_TESTS(), as
+ // the user may have changed the current directory before calling
+ // RUN_ALL_TESTS(). Therefore we capture the current directory in
+ // AddTestInfo(), which is called to register a TEST or TEST_F
+ // before main() is reached.
+ if (original_working_dir_.IsEmpty()) {
+ original_working_dir_.Set(FilePath::GetCurrentDir());
+ GTEST_CHECK_(!original_working_dir_.IsEmpty())
+ << "Failed to get the current working directory.";
+ }
+
+ GetTestCase(test_info->test_case_name(),
+ test_info->type_param(),
+ set_up_tc,
+ tear_down_tc)->AddTestInfo(test_info);
+ }
+
+#if GTEST_HAS_PARAM_TEST
+ // Returns ParameterizedTestCaseRegistry object used to keep track of
+ // value-parameterized tests and instantiate and register them.
+ internal::ParameterizedTestCaseRegistry& parameterized_test_registry() {
+ return parameterized_test_registry_;
+ }
+#endif // GTEST_HAS_PARAM_TEST
+
+ // Sets the TestCase object for the test that's currently running.
+ void set_current_test_case(TestCase* a_current_test_case) {
+ current_test_case_ = a_current_test_case;
+ }
+
+ // Sets the TestInfo object for the test that's currently running. If
+ // current_test_info is NULL, the assertion results will be stored in
+ // ad_hoc_test_result_.
+ void set_current_test_info(TestInfo* a_current_test_info) {
+ current_test_info_ = a_current_test_info;
+ }
+
+ // Registers all parameterized tests defined using TEST_P and
+ // INSTANTIATE_TEST_CASE_P, creating regular tests for each test/parameter
+ // combination. This method can be called more then once; it has guards
+ // protecting from registering the tests more then once. If
+ // value-parameterized tests are disabled, RegisterParameterizedTests is
+ // present but does nothing.
+ void RegisterParameterizedTests();
+
+ // Runs all tests in this UnitTest object, prints the result, and
+ // returns true if all tests are successful. If any exception is
+ // thrown during a test, this test is considered to be failed, but
+ // the rest of the tests will still be run.
+ bool RunAllTests();
+
+ // Clears the results of all tests, except the ad hoc tests.
+ void ClearNonAdHocTestResult() {
+ ForEach(test_cases_, TestCase::ClearTestCaseResult);
+ }
+
+ // Clears the results of ad-hoc test assertions.
+ void ClearAdHocTestResult() {
+ ad_hoc_test_result_.Clear();
+ }
+
+ // Adds a TestProperty to the current TestResult object when invoked in a
+ // context of a test or a test case, or to the global property set. If the
+ // result already contains a property with the same key, the value will be
+ // updated.
+ void RecordProperty(const TestProperty& test_property);
+
+ enum ReactionToSharding {
+ HONOR_SHARDING_PROTOCOL,
+ IGNORE_SHARDING_PROTOCOL
+ };
+
+ // Matches the full name of each test against the user-specified
+ // filter to decide whether the test should run, then records the
+ // result in each TestCase and TestInfo object.
+ // If shard_tests == HONOR_SHARDING_PROTOCOL, further filters tests
+ // based on sharding variables in the environment.
+ // Returns the number of tests that should run.
+ int FilterTests(ReactionToSharding shard_tests);
+
+ // Prints the names of the tests matching the user-specified filter flag.
+ void ListTestsMatchingFilter();
+
+ const TestCase* current_test_case() const { return current_test_case_; }
+ TestInfo* current_test_info() { return current_test_info_; }
+ const TestInfo* current_test_info() const { return current_test_info_; }
+
+ // Returns the vector of environments that need to be set-up/torn-down
+ // before/after the tests are run.
+ std::vector<Environment*>& environments() { return environments_; }
+
+ // Getters for the per-thread Google Test trace stack.
+ std::vector<TraceInfo>& gtest_trace_stack() {
+ return *(gtest_trace_stack_.pointer());
+ }
+ const std::vector<TraceInfo>& gtest_trace_stack() const {
+ return gtest_trace_stack_.get();
+ }
+
+#if GTEST_HAS_DEATH_TEST
+ void InitDeathTestSubprocessControlInfo() {
+ internal_run_death_test_flag_.reset(ParseInternalRunDeathTestFlag());
+ }
+ // Returns a pointer to the parsed --gtest_internal_run_death_test
+ // flag, or NULL if that flag was not specified.
+ // This information is useful only in a death test child process.
+ // Must not be called before a call to InitGoogleTest.
+ const InternalRunDeathTestFlag* internal_run_death_test_flag() const {
+ return internal_run_death_test_flag_.get();
+ }
+
+ // Returns a pointer to the current death test factory.
+ internal::DeathTestFactory* death_test_factory() {
+ return death_test_factory_.get();
+ }
+
+ void SuppressTestEventsIfInSubprocess();
+
+ friend class ReplaceDeathTestFactory;
+#endif // GTEST_HAS_DEATH_TEST
+
+ // Initializes the event listener performing XML output as specified by
+ // UnitTestOptions. Must not be called before InitGoogleTest.
+ void ConfigureXmlOutput();
+
+#if GTEST_CAN_STREAM_RESULTS_
+ // Initializes the event listener for streaming test results to a socket.
+ // Must not be called before InitGoogleTest.
+ void ConfigureStreamingOutput();
+#endif
+
+ // Performs initialization dependent upon flag values obtained in
+ // ParseGoogleTestFlagsOnly. Is called from InitGoogleTest after the call to
+ // ParseGoogleTestFlagsOnly. In case a user neglects to call InitGoogleTest
+ // this function is also called from RunAllTests. Since this function can be
+ // called more than once, it has to be idempotent.
+ void PostFlagParsingInit();
+
+ // Gets the random seed used at the start of the current test iteration.
+ int random_seed() const { return random_seed_; }
+
+ // Gets the random number generator.
+ internal::Random* random() { return &random_; }
+
+ // Shuffles all test cases, and the tests within each test case,
+ // making sure that death tests are still run first.
+ void ShuffleTests();
+
+ // Restores the test cases and tests to their order before the first shuffle.
+ void UnshuffleTests();
+
+ // Returns the value of GTEST_FLAG(catch_exceptions) at the moment
+ // UnitTest::Run() starts.
+ bool catch_exceptions() const { return catch_exceptions_; }
+
+ private:
+ friend class ::testing::UnitTest;
+
+ // Used by UnitTest::Run() to capture the state of
+ // GTEST_FLAG(catch_exceptions) at the moment it starts.
+ void set_catch_exceptions(bool value) { catch_exceptions_ = value; }
+
+ // The UnitTest object that owns this implementation object.
+ UnitTest* const parent_;
+
+ // The working directory when the first TEST() or TEST_F() was
+ // executed.
+ internal::FilePath original_working_dir_;
+
+ // The default test part result reporters.
+ DefaultGlobalTestPartResultReporter default_global_test_part_result_reporter_;
+ DefaultPerThreadTestPartResultReporter
+ default_per_thread_test_part_result_reporter_;
+
+ // Points to (but doesn't own) the global test part result reporter.
+ TestPartResultReporterInterface* global_test_part_result_repoter_;
+
+ // Protects read and write access to global_test_part_result_reporter_.
+ internal::Mutex global_test_part_result_reporter_mutex_;
+
+ // Points to (but doesn't own) the per-thread test part result reporter.
+ internal::ThreadLocal<TestPartResultReporterInterface*>
+ per_thread_test_part_result_reporter_;
+
+ // The vector of environments that need to be set-up/torn-down
+ // before/after the tests are run.
+ std::vector<Environment*> environments_;
+
+ // The vector of TestCases in their original order. It owns the
+ // elements in the vector.
+ std::vector<TestCase*> test_cases_;
+
+ // Provides a level of indirection for the test case list to allow
+ // easy shuffling and restoring the test case order. The i-th
+ // element of this vector is the index of the i-th test case in the
+ // shuffled order.
+ std::vector<int> test_case_indices_;
+
+#if GTEST_HAS_PARAM_TEST
+ // ParameterizedTestRegistry object used to register value-parameterized
+ // tests.
+ internal::ParameterizedTestCaseRegistry parameterized_test_registry_;
+
+ // Indicates whether RegisterParameterizedTests() has been called already.
+ bool parameterized_tests_registered_;
+#endif // GTEST_HAS_PARAM_TEST
+
+ // Index of the last death test case registered. Initially -1.
+ int last_death_test_case_;
+
+ // This points to the TestCase for the currently running test. It
+ // changes as Google Test goes through one test case after another.
+ // When no test is running, this is set to NULL and Google Test
+ // stores assertion results in ad_hoc_test_result_. Initially NULL.
+ TestCase* current_test_case_;
+
+ // This points to the TestInfo for the currently running test. It
+ // changes as Google Test goes through one test after another. When
+ // no test is running, this is set to NULL and Google Test stores
+ // assertion results in ad_hoc_test_result_. Initially NULL.
+ TestInfo* current_test_info_;
+
+ // Normally, a user only writes assertions inside a TEST or TEST_F,
+ // or inside a function called by a TEST or TEST_F. Since Google
+ // Test keeps track of which test is current running, it can
+ // associate such an assertion with the test it belongs to.
+ //
+ // If an assertion is encountered when no TEST or TEST_F is running,
+ // Google Test attributes the assertion result to an imaginary "ad hoc"
+ // test, and records the result in ad_hoc_test_result_.
+ TestResult ad_hoc_test_result_;
+
+ // The list of event listeners that can be used to track events inside
+ // Google Test.
+ TestEventListeners listeners_;
+
+ // The OS stack trace getter. Will be deleted when the UnitTest
+ // object is destructed. By default, an OsStackTraceGetter is used,
+ // but the user can set this field to use a custom getter if that is
+ // desired.
+ OsStackTraceGetterInterface* os_stack_trace_getter_;
+
+ // True iff PostFlagParsingInit() has been called.
+ bool post_flag_parse_init_performed_;
+
+ // The random number seed used at the beginning of the test run.
+ int random_seed_;
+
+ // Our random number generator.
+ internal::Random random_;
+
+ // The time of the test program start, in ms from the start of the
+ // UNIX epoch.
+ TimeInMillis start_timestamp_;
+
+ // How long the test took to run, in milliseconds.
+ TimeInMillis elapsed_time_;
+
+#if GTEST_HAS_DEATH_TEST
+ // The decomposed components of the gtest_internal_run_death_test flag,
+ // parsed when RUN_ALL_TESTS is called.
+ internal::scoped_ptr<InternalRunDeathTestFlag> internal_run_death_test_flag_;
+ internal::scoped_ptr<internal::DeathTestFactory> death_test_factory_;
+#endif // GTEST_HAS_DEATH_TEST
+
+ // A per-thread stack of traces created by the SCOPED_TRACE() macro.
+ internal::ThreadLocal<std::vector<TraceInfo> > gtest_trace_stack_;
+
+ // The value of GTEST_FLAG(catch_exceptions) at the moment RunAllTests()
+ // starts.
+ bool catch_exceptions_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(UnitTestImpl);
+}; // class UnitTestImpl
+
+// Convenience function for accessing the global UnitTest
+// implementation object.
+inline UnitTestImpl* GetUnitTestImpl() {
+ return UnitTest::GetInstance()->impl();
+}
+
+#if GTEST_USES_SIMPLE_RE
+
+// Internal helper functions for implementing the simple regular
+// expression matcher.
+GTEST_API_ bool IsInSet(char ch, const char* str);
+GTEST_API_ bool IsAsciiDigit(char ch);
+GTEST_API_ bool IsAsciiPunct(char ch);
+GTEST_API_ bool IsRepeat(char ch);
+GTEST_API_ bool IsAsciiWhiteSpace(char ch);
+GTEST_API_ bool IsAsciiWordChar(char ch);
+GTEST_API_ bool IsValidEscape(char ch);
+GTEST_API_ bool AtomMatchesChar(bool escaped, char pattern, char ch);
+GTEST_API_ bool ValidateRegex(const char* regex);
+GTEST_API_ bool MatchRegexAtHead(const char* regex, const char* str);
+GTEST_API_ bool MatchRepetitionAndRegexAtHead(
+ bool escaped, char ch, char repeat, const char* regex, const char* str);
+GTEST_API_ bool MatchRegexAnywhere(const char* regex, const char* str);
+
+#endif // GTEST_USES_SIMPLE_RE
+
+// Parses the command line for Google Test flags, without initializing
+// other parts of Google Test.
+GTEST_API_ void ParseGoogleTestFlagsOnly(int* argc, char** argv);
+GTEST_API_ void ParseGoogleTestFlagsOnly(int* argc, wchar_t** argv);
+
+#if GTEST_HAS_DEATH_TEST
+
+// Returns the message describing the last system error, regardless of the
+// platform.
+GTEST_API_ std::string GetLastErrnoDescription();
+
+# if GTEST_OS_WINDOWS
+// Provides leak-safe Windows kernel handle ownership.
+class AutoHandle {
+ public:
+ AutoHandle() : handle_(INVALID_HANDLE_VALUE) {}
+ explicit AutoHandle(HANDLE handle) : handle_(handle) {}
+
+ ~AutoHandle() { Reset(); }
+
+ HANDLE Get() const { return handle_; }
+ void Reset() { Reset(INVALID_HANDLE_VALUE); }
+ void Reset(HANDLE handle) {
+ if (handle != handle_) {
+ if (handle_ != INVALID_HANDLE_VALUE)
+ ::CloseHandle(handle_);
+ handle_ = handle;
+ }
+ }
+
+ private:
+ HANDLE handle_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(AutoHandle);
+};
+# endif // GTEST_OS_WINDOWS
+
+// Attempts to parse a string into a positive integer pointed to by the
+// number parameter. Returns true if that is possible.
+// GTEST_HAS_DEATH_TEST implies that we have ::std::string, so we can use
+// it here.
+template <typename Integer>
+bool ParseNaturalNumber(const ::std::string& str, Integer* number) {
+ // Fail fast if the given string does not begin with a digit;
+ // this bypasses strtoXXX's "optional leading whitespace and plus
+ // or minus sign" semantics, which are undesirable here.
+ if (str.empty() || !IsDigit(str[0])) {
+ return false;
+ }
+ errno = 0;
+
+ char* end;
+ // BiggestConvertible is the largest integer type that system-provided
+ // string-to-number conversion routines can return.
+
+# if GTEST_OS_WINDOWS && !defined(__GNUC__)
+
+ // MSVC and C++ Builder define __int64 instead of the standard long long.
+ typedef unsigned __int64 BiggestConvertible;
+ const BiggestConvertible parsed = _strtoui64(str.c_str(), &end, 10);
+
+# else
+
+ typedef unsigned long long BiggestConvertible; // NOLINT
+ const BiggestConvertible parsed = strtoull(str.c_str(), &end, 10);
+
+# endif // GTEST_OS_WINDOWS && !defined(__GNUC__)
+
+ const bool parse_success = *end == '\0' && errno == 0;
+
+ // TODO(vladl at google.com): Convert this to compile time assertion when it is
+ // available.
+ GTEST_CHECK_(sizeof(Integer) <= sizeof(parsed));
+
+ const Integer result = static_cast<Integer>(parsed);
+ if (parse_success && static_cast<BiggestConvertible>(result) == parsed) {
+ *number = result;
+ return true;
+ }
+ return false;
+}
+#endif // GTEST_HAS_DEATH_TEST
+
+// TestResult contains some private methods that should be hidden from
+// Google Test user but are required for testing. This class allow our tests
+// to access them.
+//
+// This class is supplied only for the purpose of testing Google Test's own
+// constructs. Do not use it in user tests, either directly or indirectly.
+class TestResultAccessor {
+ public:
+ static void RecordProperty(TestResult* test_result,
+ const std::string& xml_element,
+ const TestProperty& property) {
+ test_result->RecordProperty(xml_element, property);
+ }
+
+ static void ClearTestPartResults(TestResult* test_result) {
+ test_result->ClearTestPartResults();
+ }
+
+ static const std::vector<testing::TestPartResult>& test_part_results(
+ const TestResult& test_result) {
+ return test_result.test_part_results();
+ }
+};
+
+#if GTEST_CAN_STREAM_RESULTS_
+
+// Streams test results to the given port on the given host machine.
+class StreamingListener : public EmptyTestEventListener {
+ public:
+ // Abstract base class for writing strings to a socket.
+ class AbstractSocketWriter {
+ public:
+ virtual ~AbstractSocketWriter() {}
+
+ // Sends a string to the socket.
+ virtual void Send(const string& message) = 0;
+
+ // Closes the socket.
+ virtual void CloseConnection() {}
+
+ // Sends a string and a newline to the socket.
+ void SendLn(const string& message) {
+ Send(message + "\n");
+ }
+ };
+
+ // Concrete class for actually writing strings to a socket.
+ class SocketWriter : public AbstractSocketWriter {
+ public:
+ SocketWriter(const string& host, const string& port)
+ : sockfd_(-1), host_name_(host), port_num_(port) {
+ MakeConnection();
+ }
+
+ virtual ~SocketWriter() {
+ if (sockfd_ != -1)
+ CloseConnection();
+ }
+
+ // Sends a string to the socket.
+ virtual void Send(const string& message) {
+ GTEST_CHECK_(sockfd_ != -1)
+ << "Send() can be called only when there is a connection.";
+
+ const int len = static_cast<int>(message.length());
+ if (write(sockfd_, message.c_str(), len) != len) {
+ GTEST_LOG_(WARNING)
+ << "stream_result_to: failed to stream to "
+ << host_name_ << ":" << port_num_;
+ }
+ }
+
+ private:
+ // Creates a client socket and connects to the server.
+ void MakeConnection();
+
+ // Closes the socket.
+ void CloseConnection() {
+ GTEST_CHECK_(sockfd_ != -1)
+ << "CloseConnection() can be called only when there is a connection.";
+
+ close(sockfd_);
+ sockfd_ = -1;
+ }
+
+ int sockfd_; // socket file descriptor
+ const string host_name_;
+ const string port_num_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(SocketWriter);
+ }; // class SocketWriter
+
+ // Escapes '=', '&', '%', and '\n' characters in str as "%xx".
+ static string UrlEncode(const char* str);
+
+ StreamingListener(const string& host, const string& port)
+ : socket_writer_(new SocketWriter(host, port)) { Start(); }
+
+ explicit StreamingListener(AbstractSocketWriter* socket_writer)
+ : socket_writer_(socket_writer) { Start(); }
+
+ void OnTestProgramStart(const UnitTest& /* unit_test */) {
+ SendLn("event=TestProgramStart");
+ }
+
+ void OnTestProgramEnd(const UnitTest& unit_test) {
+ // Note that Google Test current only report elapsed time for each
+ // test iteration, not for the entire test program.
+ SendLn("event=TestProgramEnd&passed=" + FormatBool(unit_test.Passed()));
+
+ // Notify the streaming server to stop.
+ socket_writer_->CloseConnection();
+ }
+
+ void OnTestIterationStart(const UnitTest& /* unit_test */, int iteration) {
+ SendLn("event=TestIterationStart&iteration=" +
+ StreamableToString(iteration));
+ }
+
+ void OnTestIterationEnd(const UnitTest& unit_test, int /* iteration */) {
+ SendLn("event=TestIterationEnd&passed=" +
+ FormatBool(unit_test.Passed()) + "&elapsed_time=" +
+ StreamableToString(unit_test.elapsed_time()) + "ms");
+ }
+
+ void OnTestCaseStart(const TestCase& test_case) {
+ SendLn(std::string("event=TestCaseStart&name=") + test_case.name());
+ }
+
+ void OnTestCaseEnd(const TestCase& test_case) {
+ SendLn("event=TestCaseEnd&passed=" + FormatBool(test_case.Passed())
+ + "&elapsed_time=" + StreamableToString(test_case.elapsed_time())
+ + "ms");
+ }
+
+ void OnTestStart(const TestInfo& test_info) {
+ SendLn(std::string("event=TestStart&name=") + test_info.name());
+ }
+
+ void OnTestEnd(const TestInfo& test_info) {
+ SendLn("event=TestEnd&passed=" +
+ FormatBool((test_info.result())->Passed()) +
+ "&elapsed_time=" +
+ StreamableToString((test_info.result())->elapsed_time()) + "ms");
+ }
+
+ void OnTestPartResult(const TestPartResult& test_part_result) {
+ const char* file_name = test_part_result.file_name();
+ if (file_name == NULL)
+ file_name = "";
+ SendLn("event=TestPartResult&file=" + UrlEncode(file_name) +
+ "&line=" + StreamableToString(test_part_result.line_number()) +
+ "&message=" + UrlEncode(test_part_result.message()));
+ }
+
+ private:
+ // Sends the given message and a newline to the socket.
+ void SendLn(const string& message) { socket_writer_->SendLn(message); }
+
+ // Called at the start of streaming to notify the receiver what
+ // protocol we are using.
+ void Start() { SendLn("gtest_streaming_protocol_version=1.0"); }
+
+ string FormatBool(bool value) { return value ? "1" : "0"; }
+
+ const scoped_ptr<AbstractSocketWriter> socket_writer_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(StreamingListener);
+}; // class StreamingListener
+
+#endif // GTEST_CAN_STREAM_RESULTS_
+
+} // namespace internal
+} // namespace testing
+
+#endif // GTEST_SRC_GTEST_INTERNAL_INL_H_
diff --git a/examples/stand_alone/test_fixture/gtest/src/gtest-port.cc b/examples/stand_alone/test_fixture/gtest/src/gtest-port.cc
new file mode 100644
index 0000000..0c4df5f
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/src/gtest-port.cc
@@ -0,0 +1,805 @@
+// Copyright 2008, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+
+#include "gtest/internal/gtest-port.h"
+
+#include <limits.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#if GTEST_OS_WINDOWS_MOBILE
+# include <windows.h> // For TerminateProcess()
+#elif GTEST_OS_WINDOWS
+# include <io.h>
+# include <sys/stat.h>
+#else
+# include <unistd.h>
+#endif // GTEST_OS_WINDOWS_MOBILE
+
+#if GTEST_OS_MAC
+# include <mach/mach_init.h>
+# include <mach/task.h>
+# include <mach/vm_map.h>
+#endif // GTEST_OS_MAC
+
+#if GTEST_OS_QNX
+# include <devctl.h>
+# include <sys/procfs.h>
+#endif // GTEST_OS_QNX
+
+#include "gtest/gtest-spi.h"
+#include "gtest/gtest-message.h"
+#include "gtest/internal/gtest-internal.h"
+#include "gtest/internal/gtest-string.h"
+
+// Indicates that this translation unit is part of Google Test's
+// implementation. It must come before gtest-internal-inl.h is
+// included, or there will be a compiler error. This trick is to
+// prevent a user from accidentally including gtest-internal-inl.h in
+// his code.
+#define GTEST_IMPLEMENTATION_ 1
+#include "src/gtest-internal-inl.h"
+#undef GTEST_IMPLEMENTATION_
+
+namespace testing {
+namespace internal {
+
+#if defined(_MSC_VER) || defined(__BORLANDC__)
+// MSVC and C++Builder do not provide a definition of STDERR_FILENO.
+const int kStdOutFileno = 1;
+const int kStdErrFileno = 2;
+#else
+const int kStdOutFileno = STDOUT_FILENO;
+const int kStdErrFileno = STDERR_FILENO;
+#endif // _MSC_VER
+
+#if GTEST_OS_MAC
+
+// Returns the number of threads running in the process, or 0 to indicate that
+// we cannot detect it.
+size_t GetThreadCount() {
+ const task_t task = mach_task_self();
+ mach_msg_type_number_t thread_count;
+ thread_act_array_t thread_list;
+ const kern_return_t status = task_threads(task, &thread_list, &thread_count);
+ if (status == KERN_SUCCESS) {
+ // task_threads allocates resources in thread_list and we need to free them
+ // to avoid leaks.
+ vm_deallocate(task,
+ reinterpret_cast<vm_address_t>(thread_list),
+ sizeof(thread_t) * thread_count);
+ return static_cast<size_t>(thread_count);
+ } else {
+ return 0;
+ }
+}
+
+#elif GTEST_OS_QNX
+
+// Returns the number of threads running in the process, or 0 to indicate that
+// we cannot detect it.
+size_t GetThreadCount() {
+ const int fd = open("/proc/self/as", O_RDONLY);
+ if (fd < 0) {
+ return 0;
+ }
+ procfs_info process_info;
+ const int status =
+ devctl(fd, DCMD_PROC_INFO, &process_info, sizeof(process_info), NULL);
+ close(fd);
+ if (status == EOK) {
+ return static_cast<size_t>(process_info.num_threads);
+ } else {
+ return 0;
+ }
+}
+
+#else
+
+size_t GetThreadCount() {
+ // There's no portable way to detect the number of threads, so we just
+ // return 0 to indicate that we cannot detect it.
+ return 0;
+}
+
+#endif // GTEST_OS_MAC
+
+#if GTEST_USES_POSIX_RE
+
+// Implements RE. Currently only needed for death tests.
+
+RE::~RE() {
+ if (is_valid_) {
+ // regfree'ing an invalid regex might crash because the content
+ // of the regex is undefined. Since the regex's are essentially
+ // the same, one cannot be valid (or invalid) without the other
+ // being so too.
+ regfree(&partial_regex_);
+ regfree(&full_regex_);
+ }
+ free(const_cast<char*>(pattern_));
+}
+
+// Returns true iff regular expression re matches the entire str.
+bool RE::FullMatch(const char* str, const RE& re) {
+ if (!re.is_valid_) return false;
+
+ regmatch_t match;
+ return regexec(&re.full_regex_, str, 1, &match, 0) == 0;
+}
+
+// Returns true iff regular expression re matches a substring of str
+// (including str itself).
+bool RE::PartialMatch(const char* str, const RE& re) {
+ if (!re.is_valid_) return false;
+
+ regmatch_t match;
+ return regexec(&re.partial_regex_, str, 1, &match, 0) == 0;
+}
+
+// Initializes an RE from its string representation.
+void RE::Init(const char* regex) {
+ pattern_ = posix::StrDup(regex);
+
+ // Reserves enough bytes to hold the regular expression used for a
+ // full match.
+ const size_t full_regex_len = strlen(regex) + 10;
+ char* const full_pattern = new char[full_regex_len];
+
+ snprintf(full_pattern, full_regex_len, "^(%s)$", regex);
+ is_valid_ = regcomp(&full_regex_, full_pattern, REG_EXTENDED) == 0;
+ // We want to call regcomp(&partial_regex_, ...) even if the
+ // previous expression returns false. Otherwise partial_regex_ may
+ // not be properly initialized can may cause trouble when it's
+ // freed.
+ //
+ // Some implementation of POSIX regex (e.g. on at least some
+ // versions of Cygwin) doesn't accept the empty string as a valid
+ // regex. We change it to an equivalent form "()" to be safe.
+ if (is_valid_) {
+ const char* const partial_regex = (*regex == '\0') ? "()" : regex;
+ is_valid_ = regcomp(&partial_regex_, partial_regex, REG_EXTENDED) == 0;
+ }
+ EXPECT_TRUE(is_valid_)
+ << "Regular expression \"" << regex
+ << "\" is not a valid POSIX Extended regular expression.";
+
+ delete[] full_pattern;
+}
+
+#elif GTEST_USES_SIMPLE_RE
+
+// Returns true iff ch appears anywhere in str (excluding the
+// terminating '\0' character).
+bool IsInSet(char ch, const char* str) {
+ return ch != '\0' && strchr(str, ch) != NULL;
+}
+
+// Returns true iff ch belongs to the given classification. Unlike
+// similar functions in <ctype.h>, these aren't affected by the
+// current locale.
+bool IsAsciiDigit(char ch) { return '0' <= ch && ch <= '9'; }
+bool IsAsciiPunct(char ch) {
+ return IsInSet(ch, "^-!\"#$%&'()*+,./:;<=>?@[\\]_`{|}~");
+}
+bool IsRepeat(char ch) { return IsInSet(ch, "?*+"); }
+bool IsAsciiWhiteSpace(char ch) { return IsInSet(ch, " \f\n\r\t\v"); }
+bool IsAsciiWordChar(char ch) {
+ return ('a' <= ch && ch <= 'z') || ('A' <= ch && ch <= 'Z') ||
+ ('0' <= ch && ch <= '9') || ch == '_';
+}
+
+// Returns true iff "\\c" is a supported escape sequence.
+bool IsValidEscape(char c) {
+ return (IsAsciiPunct(c) || IsInSet(c, "dDfnrsStvwW"));
+}
+
+// Returns true iff the given atom (specified by escaped and pattern)
+// matches ch. The result is undefined if the atom is invalid.
+bool AtomMatchesChar(bool escaped, char pattern_char, char ch) {
+ if (escaped) { // "\\p" where p is pattern_char.
+ switch (pattern_char) {
+ case 'd': return IsAsciiDigit(ch);
+ case 'D': return !IsAsciiDigit(ch);
+ case 'f': return ch == '\f';
+ case 'n': return ch == '\n';
+ case 'r': return ch == '\r';
+ case 's': return IsAsciiWhiteSpace(ch);
+ case 'S': return !IsAsciiWhiteSpace(ch);
+ case 't': return ch == '\t';
+ case 'v': return ch == '\v';
+ case 'w': return IsAsciiWordChar(ch);
+ case 'W': return !IsAsciiWordChar(ch);
+ }
+ return IsAsciiPunct(pattern_char) && pattern_char == ch;
+ }
+
+ return (pattern_char == '.' && ch != '\n') || pattern_char == ch;
+}
+
+// Helper function used by ValidateRegex() to format error messages.
+std::string FormatRegexSyntaxError(const char* regex, int index) {
+ return (Message() << "Syntax error at index " << index
+ << " in simple regular expression \"" << regex << "\": ").GetString();
+}
+
+// Generates non-fatal failures and returns false if regex is invalid;
+// otherwise returns true.
+bool ValidateRegex(const char* regex) {
+ if (regex == NULL) {
+ // TODO(wan at google.com): fix the source file location in the
+ // assertion failures to match where the regex is used in user
+ // code.
+ ADD_FAILURE() << "NULL is not a valid simple regular expression.";
+ return false;
+ }
+
+ bool is_valid = true;
+
+ // True iff ?, *, or + can follow the previous atom.
+ bool prev_repeatable = false;
+ for (int i = 0; regex[i]; i++) {
+ if (regex[i] == '\\') { // An escape sequence
+ i++;
+ if (regex[i] == '\0') {
+ ADD_FAILURE() << FormatRegexSyntaxError(regex, i - 1)
+ << "'\\' cannot appear at the end.";
+ return false;
+ }
+
+ if (!IsValidEscape(regex[i])) {
+ ADD_FAILURE() << FormatRegexSyntaxError(regex, i - 1)
+ << "invalid escape sequence \"\\" << regex[i] << "\".";
+ is_valid = false;
+ }
+ prev_repeatable = true;
+ } else { // Not an escape sequence.
+ const char ch = regex[i];
+
+ if (ch == '^' && i > 0) {
+ ADD_FAILURE() << FormatRegexSyntaxError(regex, i)
+ << "'^' can only appear at the beginning.";
+ is_valid = false;
+ } else if (ch == '$' && regex[i + 1] != '\0') {
+ ADD_FAILURE() << FormatRegexSyntaxError(regex, i)
+ << "'$' can only appear at the end.";
+ is_valid = false;
+ } else if (IsInSet(ch, "()[]{}|")) {
+ ADD_FAILURE() << FormatRegexSyntaxError(regex, i)
+ << "'" << ch << "' is unsupported.";
+ is_valid = false;
+ } else if (IsRepeat(ch) && !prev_repeatable) {
+ ADD_FAILURE() << FormatRegexSyntaxError(regex, i)
+ << "'" << ch << "' can only follow a repeatable token.";
+ is_valid = false;
+ }
+
+ prev_repeatable = !IsInSet(ch, "^$?*+");
+ }
+ }
+
+ return is_valid;
+}
+
+// Matches a repeated regex atom followed by a valid simple regular
+// expression. The regex atom is defined as c if escaped is false,
+// or \c otherwise. repeat is the repetition meta character (?, *,
+// or +). The behavior is undefined if str contains too many
+// characters to be indexable by size_t, in which case the test will
+// probably time out anyway. We are fine with this limitation as
+// std::string has it too.
+bool MatchRepetitionAndRegexAtHead(
+ bool escaped, char c, char repeat, const char* regex,
+ const char* str) {
+ const size_t min_count = (repeat == '+') ? 1 : 0;
+ const size_t max_count = (repeat == '?') ? 1 :
+ static_cast<size_t>(-1) - 1;
+ // We cannot call numeric_limits::max() as it conflicts with the
+ // max() macro on Windows.
+
+ for (size_t i = 0; i <= max_count; ++i) {
+ // We know that the atom matches each of the first i characters in str.
+ if (i >= min_count && MatchRegexAtHead(regex, str + i)) {
+ // We have enough matches at the head, and the tail matches too.
+ // Since we only care about *whether* the pattern matches str
+ // (as opposed to *how* it matches), there is no need to find a
+ // greedy match.
+ return true;
+ }
+ if (str[i] == '\0' || !AtomMatchesChar(escaped, c, str[i]))
+ return false;
+ }
+ return false;
+}
+
+// Returns true iff regex matches a prefix of str. regex must be a
+// valid simple regular expression and not start with "^", or the
+// result is undefined.
+bool MatchRegexAtHead(const char* regex, const char* str) {
+ if (*regex == '\0') // An empty regex matches a prefix of anything.
+ return true;
+
+ // "$" only matches the end of a string. Note that regex being
+ // valid guarantees that there's nothing after "$" in it.
+ if (*regex == '$')
+ return *str == '\0';
+
+ // Is the first thing in regex an escape sequence?
+ const bool escaped = *regex == '\\';
+ if (escaped)
+ ++regex;
+ if (IsRepeat(regex[1])) {
+ // MatchRepetitionAndRegexAtHead() calls MatchRegexAtHead(), so
+ // here's an indirect recursion. It terminates as the regex gets
+ // shorter in each recursion.
+ return MatchRepetitionAndRegexAtHead(
+ escaped, regex[0], regex[1], regex + 2, str);
+ } else {
+ // regex isn't empty, isn't "$", and doesn't start with a
+ // repetition. We match the first atom of regex with the first
+ // character of str and recurse.
+ return (*str != '\0') && AtomMatchesChar(escaped, *regex, *str) &&
+ MatchRegexAtHead(regex + 1, str + 1);
+ }
+}
+
+// Returns true iff regex matches any substring of str. regex must be
+// a valid simple regular expression, or the result is undefined.
+//
+// The algorithm is recursive, but the recursion depth doesn't exceed
+// the regex length, so we won't need to worry about running out of
+// stack space normally. In rare cases the time complexity can be
+// exponential with respect to the regex length + the string length,
+// but usually it's must faster (often close to linear).
+bool MatchRegexAnywhere(const char* regex, const char* str) {
+ if (regex == NULL || str == NULL)
+ return false;
+
+ if (*regex == '^')
+ return MatchRegexAtHead(regex + 1, str);
+
+ // A successful match can be anywhere in str.
+ do {
+ if (MatchRegexAtHead(regex, str))
+ return true;
+ } while (*str++ != '\0');
+ return false;
+}
+
+// Implements the RE class.
+
+RE::~RE() {
+ free(const_cast<char*>(pattern_));
+ free(const_cast<char*>(full_pattern_));
+}
+
+// Returns true iff regular expression re matches the entire str.
+bool RE::FullMatch(const char* str, const RE& re) {
+ return re.is_valid_ && MatchRegexAnywhere(re.full_pattern_, str);
+}
+
+// Returns true iff regular expression re matches a substring of str
+// (including str itself).
+bool RE::PartialMatch(const char* str, const RE& re) {
+ return re.is_valid_ && MatchRegexAnywhere(re.pattern_, str);
+}
+
+// Initializes an RE from its string representation.
+void RE::Init(const char* regex) {
+ pattern_ = full_pattern_ = NULL;
+ if (regex != NULL) {
+ pattern_ = posix::StrDup(regex);
+ }
+
+ is_valid_ = ValidateRegex(regex);
+ if (!is_valid_) {
+ // No need to calculate the full pattern when the regex is invalid.
+ return;
+ }
+
+ const size_t len = strlen(regex);
+ // Reserves enough bytes to hold the regular expression used for a
+ // full match: we need space to prepend a '^', append a '$', and
+ // terminate the string with '\0'.
+ char* buffer = static_cast<char*>(malloc(len + 3));
+ full_pattern_ = buffer;
+
+ if (*regex != '^')
+ *buffer++ = '^'; // Makes sure full_pattern_ starts with '^'.
+
+ // We don't use snprintf or strncpy, as they trigger a warning when
+ // compiled with VC++ 8.0.
+ memcpy(buffer, regex, len);
+ buffer += len;
+
+ if (len == 0 || regex[len - 1] != '$')
+ *buffer++ = '$'; // Makes sure full_pattern_ ends with '$'.
+
+ *buffer = '\0';
+}
+
+#endif // GTEST_USES_POSIX_RE
+
+const char kUnknownFile[] = "unknown file";
+
+// Formats a source file path and a line number as they would appear
+// in an error message from the compiler used to compile this code.
+GTEST_API_ ::std::string FormatFileLocation(const char* file, int line) {
+ const std::string file_name(file == NULL ? kUnknownFile : file);
+
+ if (line < 0) {
+ return file_name + ":";
+ }
+#ifdef _MSC_VER
+ return file_name + "(" + StreamableToString(line) + "):";
+#else
+ return file_name + ":" + StreamableToString(line) + ":";
+#endif // _MSC_VER
+}
+
+// Formats a file location for compiler-independent XML output.
+// Although this function is not platform dependent, we put it next to
+// FormatFileLocation in order to contrast the two functions.
+// Note that FormatCompilerIndependentFileLocation() does NOT append colon
+// to the file location it produces, unlike FormatFileLocation().
+GTEST_API_ ::std::string FormatCompilerIndependentFileLocation(
+ const char* file, int line) {
+ const std::string file_name(file == NULL ? kUnknownFile : file);
+
+ if (line < 0)
+ return file_name;
+ else
+ return file_name + ":" + StreamableToString(line);
+}
+
+
+GTestLog::GTestLog(GTestLogSeverity severity, const char* file, int line)
+ : severity_(severity) {
+ const char* const marker =
+ severity == GTEST_INFO ? "[ INFO ]" :
+ severity == GTEST_WARNING ? "[WARNING]" :
+ severity == GTEST_ERROR ? "[ ERROR ]" : "[ FATAL ]";
+ GetStream() << ::std::endl << marker << " "
+ << FormatFileLocation(file, line).c_str() << ": ";
+}
+
+// Flushes the buffers and, if severity is GTEST_FATAL, aborts the program.
+GTestLog::~GTestLog() {
+ GetStream() << ::std::endl;
+ if (severity_ == GTEST_FATAL) {
+ fflush(stderr);
+ posix::Abort();
+ }
+}
+// Disable Microsoft deprecation warnings for POSIX functions called from
+// this class (creat, dup, dup2, and close)
+#ifdef _MSC_VER
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif // _MSC_VER
+
+#if GTEST_HAS_STREAM_REDIRECTION
+
+// Object that captures an output stream (stdout/stderr).
+class CapturedStream {
+ public:
+ // The ctor redirects the stream to a temporary file.
+ explicit CapturedStream(int fd) : fd_(fd), uncaptured_fd_(dup(fd)) {
+# if GTEST_OS_WINDOWS
+ char temp_dir_path[MAX_PATH + 1] = { '\0' }; // NOLINT
+ char temp_file_path[MAX_PATH + 1] = { '\0' }; // NOLINT
+
+ ::GetTempPathA(sizeof(temp_dir_path), temp_dir_path);
+ const UINT success = ::GetTempFileNameA(temp_dir_path,
+ "gtest_redir",
+ 0, // Generate unique file name.
+ temp_file_path);
+ GTEST_CHECK_(success != 0)
+ << "Unable to create a temporary file in " << temp_dir_path;
+ const int captured_fd = creat(temp_file_path, _S_IREAD | _S_IWRITE);
+ GTEST_CHECK_(captured_fd != -1) << "Unable to open temporary file "
+ << temp_file_path;
+ filename_ = temp_file_path;
+# else
+ // There's no guarantee that a test has write access to the current
+ // directory, so we create the temporary file in the /tmp directory
+ // instead. We use /tmp on most systems, and /sdcard on Android.
+ // That's because Android doesn't have /tmp.
+# if GTEST_OS_LINUX_ANDROID
+ // Note: Android applications are expected to call the framework's
+ // Context.getExternalStorageDirectory() method through JNI to get
+ // the location of the world-writable SD Card directory. However,
+ // this requires a Context handle, which cannot be retrieved
+ // globally from native code. Doing so also precludes running the
+ // code as part of a regular standalone executable, which doesn't
+ // run in a Dalvik process (e.g. when running it through 'adb shell').
+ //
+ // The location /sdcard is directly accessible from native code
+ // and is the only location (unofficially) supported by the Android
+ // team. It's generally a symlink to the real SD Card mount point
+ // which can be /mnt/sdcard, /mnt/sdcard0, /system/media/sdcard, or
+ // other OEM-customized locations. Never rely on these, and always
+ // use /sdcard.
+ char name_template[] = "/sdcard/gtest_captured_stream.XXXXXX";
+# else
+ char name_template[] = "/tmp/captured_stream.XXXXXX";
+# endif // GTEST_OS_LINUX_ANDROID
+ const int captured_fd = mkstemp(name_template);
+ filename_ = name_template;
+# endif // GTEST_OS_WINDOWS
+ fflush(NULL);
+ dup2(captured_fd, fd_);
+ close(captured_fd);
+ }
+
+ ~CapturedStream() {
+ remove(filename_.c_str());
+ }
+
+ std::string GetCapturedString() {
+ if (uncaptured_fd_ != -1) {
+ // Restores the original stream.
+ fflush(NULL);
+ dup2(uncaptured_fd_, fd_);
+ close(uncaptured_fd_);
+ uncaptured_fd_ = -1;
+ }
+
+ FILE* const file = posix::FOpen(filename_.c_str(), "r");
+ const std::string content = ReadEntireFile(file);
+ posix::FClose(file);
+ return content;
+ }
+
+ private:
+ // Reads the entire content of a file as an std::string.
+ static std::string ReadEntireFile(FILE* file);
+
+ // Returns the size (in bytes) of a file.
+ static size_t GetFileSize(FILE* file);
+
+ const int fd_; // A stream to capture.
+ int uncaptured_fd_;
+ // Name of the temporary file holding the stderr output.
+ ::std::string filename_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(CapturedStream);
+};
+
+// Returns the size (in bytes) of a file.
+size_t CapturedStream::GetFileSize(FILE* file) {
+ fseek(file, 0, SEEK_END);
+ return static_cast<size_t>(ftell(file));
+}
+
+// Reads the entire content of a file as a string.
+std::string CapturedStream::ReadEntireFile(FILE* file) {
+ const size_t file_size = GetFileSize(file);
+ char* const buffer = new char[file_size];
+
+ size_t bytes_last_read = 0; // # of bytes read in the last fread()
+ size_t bytes_read = 0; // # of bytes read so far
+
+ fseek(file, 0, SEEK_SET);
+
+ // Keeps reading the file until we cannot read further or the
+ // pre-determined file size is reached.
+ do {
+ bytes_last_read = fread(buffer+bytes_read, 1, file_size-bytes_read, file);
+ bytes_read += bytes_last_read;
+ } while (bytes_last_read > 0 && bytes_read < file_size);
+
+ const std::string content(buffer, bytes_read);
+ delete[] buffer;
+
+ return content;
+}
+
+# ifdef _MSC_VER
+# pragma warning(pop)
+# endif // _MSC_VER
+
+static CapturedStream* g_captured_stderr = NULL;
+static CapturedStream* g_captured_stdout = NULL;
+
+// Starts capturing an output stream (stdout/stderr).
+void CaptureStream(int fd, const char* stream_name, CapturedStream** stream) {
+ if (*stream != NULL) {
+ GTEST_LOG_(FATAL) << "Only one " << stream_name
+ << " capturer can exist at a time.";
+ }
+ *stream = new CapturedStream(fd);
+}
+
+// Stops capturing the output stream and returns the captured string.
+std::string GetCapturedStream(CapturedStream** captured_stream) {
+ const std::string content = (*captured_stream)->GetCapturedString();
+
+ delete *captured_stream;
+ *captured_stream = NULL;
+
+ return content;
+}
+
+// Starts capturing stdout.
+void CaptureStdout() {
+ CaptureStream(kStdOutFileno, "stdout", &g_captured_stdout);
+}
+
+// Starts capturing stderr.
+void CaptureStderr() {
+ CaptureStream(kStdErrFileno, "stderr", &g_captured_stderr);
+}
+
+// Stops capturing stdout and returns the captured string.
+std::string GetCapturedStdout() {
+ return GetCapturedStream(&g_captured_stdout);
+}
+
+// Stops capturing stderr and returns the captured string.
+std::string GetCapturedStderr() {
+ return GetCapturedStream(&g_captured_stderr);
+}
+
+#endif // GTEST_HAS_STREAM_REDIRECTION
+
+#if GTEST_HAS_DEATH_TEST
+
+// A copy of all command line arguments. Set by InitGoogleTest().
+::std::vector<testing::internal::string> g_argvs;
+
+static const ::std::vector<testing::internal::string>* g_injected_test_argvs =
+ NULL; // Owned.
+
+void SetInjectableArgvs(const ::std::vector<testing::internal::string>* argvs) {
+ if (g_injected_test_argvs != argvs)
+ delete g_injected_test_argvs;
+ g_injected_test_argvs = argvs;
+}
+
+const ::std::vector<testing::internal::string>& GetInjectableArgvs() {
+ if (g_injected_test_argvs != NULL) {
+ return *g_injected_test_argvs;
+ }
+ return g_argvs;
+}
+#endif // GTEST_HAS_DEATH_TEST
+
+#if GTEST_OS_WINDOWS_MOBILE
+namespace posix {
+void Abort() {
+ DebugBreak();
+ TerminateProcess(GetCurrentProcess(), 1);
+}
+} // namespace posix
+#endif // GTEST_OS_WINDOWS_MOBILE
+
+// Returns the name of the environment variable corresponding to the
+// given flag. For example, FlagToEnvVar("foo") will return
+// "GTEST_FOO" in the open-source version.
+static std::string FlagToEnvVar(const char* flag) {
+ const std::string full_flag =
+ (Message() << GTEST_FLAG_PREFIX_ << flag).GetString();
+
+ Message env_var;
+ for (size_t i = 0; i != full_flag.length(); i++) {
+ env_var << ToUpper(full_flag.c_str()[i]);
+ }
+
+ return env_var.GetString();
+}
+
+// Parses 'str' for a 32-bit signed integer. If successful, writes
+// the result to *value and returns true; otherwise leaves *value
+// unchanged and returns false.
+bool ParseInt32(const Message& src_text, const char* str, Int32* value) {
+ // Parses the environment variable as a decimal integer.
+ char* end = NULL;
+ const long long_value = strtol(str, &end, 10); // NOLINT
+
+ // Has strtol() consumed all characters in the string?
+ if (*end != '\0') {
+ // No - an invalid character was encountered.
+ Message msg;
+ msg << "WARNING: " << src_text
+ << " is expected to be a 32-bit integer, but actually"
+ << " has value \"" << str << "\".\n";
+ printf("%s", msg.GetString().c_str());
+ fflush(stdout);
+ return false;
+ }
+
+ // Is the parsed value in the range of an Int32?
+ const Int32 result = static_cast<Int32>(long_value);
+ if (long_value == LONG_MAX || long_value == LONG_MIN ||
+ // The parsed value overflows as a long. (strtol() returns
+ // LONG_MAX or LONG_MIN when the input overflows.)
+ result != long_value
+ // The parsed value overflows as an Int32.
+ ) {
+ Message msg;
+ msg << "WARNING: " << src_text
+ << " is expected to be a 32-bit integer, but actually"
+ << " has value " << str << ", which overflows.\n";
+ printf("%s", msg.GetString().c_str());
+ fflush(stdout);
+ return false;
+ }
+
+ *value = result;
+ return true;
+}
+
+// Reads and returns the Boolean environment variable corresponding to
+// the given flag; if it's not set, returns default_value.
+//
+// The value is considered true iff it's not "0".
+bool BoolFromGTestEnv(const char* flag, bool default_value) {
+ const std::string env_var = FlagToEnvVar(flag);
+ const char* const string_value = posix::GetEnv(env_var.c_str());
+ return string_value == NULL ?
+ default_value : strcmp(string_value, "0") != 0;
+}
+
+// Reads and returns a 32-bit integer stored in the environment
+// variable corresponding to the given flag; if it isn't set or
+// doesn't represent a valid 32-bit integer, returns default_value.
+Int32 Int32FromGTestEnv(const char* flag, Int32 default_value) {
+ const std::string env_var = FlagToEnvVar(flag);
+ const char* const string_value = posix::GetEnv(env_var.c_str());
+ if (string_value == NULL) {
+ // The environment variable is not set.
+ return default_value;
+ }
+
+ Int32 result = default_value;
+ if (!ParseInt32(Message() << "Environment variable " << env_var,
+ string_value, &result)) {
+ printf("The default value %s is used.\n",
+ (Message() << default_value).GetString().c_str());
+ fflush(stdout);
+ return default_value;
+ }
+
+ return result;
+}
+
+// Reads and returns the string environment variable corresponding to
+// the given flag; if it's not set, returns default_value.
+const char* StringFromGTestEnv(const char* flag, const char* default_value) {
+ const std::string env_var = FlagToEnvVar(flag);
+ const char* const value = posix::GetEnv(env_var.c_str());
+ return value == NULL ? default_value : value;
+}
+
+} // namespace internal
+} // namespace testing
diff --git a/examples/stand_alone/test_fixture/gtest/src/gtest-printers.cc b/examples/stand_alone/test_fixture/gtest/src/gtest-printers.cc
new file mode 100644
index 0000000..75fa408
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/src/gtest-printers.cc
@@ -0,0 +1,363 @@
+// Copyright 2007, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+
+// Google Test - The Google C++ Testing Framework
+//
+// This file implements a universal value printer that can print a
+// value of any type T:
+//
+// void ::testing::internal::UniversalPrinter<T>::Print(value, ostream_ptr);
+//
+// It uses the << operator when possible, and prints the bytes in the
+// object otherwise. A user can override its behavior for a class
+// type Foo by defining either operator<<(::std::ostream&, const Foo&)
+// or void PrintTo(const Foo&, ::std::ostream*) in the namespace that
+// defines Foo.
+
+#include "gtest/gtest-printers.h"
+#include <ctype.h>
+#include <stdio.h>
+#include <ostream> // NOLINT
+#include <string>
+#include "gtest/internal/gtest-port.h"
+
+namespace testing {
+
+namespace {
+
+using ::std::ostream;
+
+// Prints a segment of bytes in the given object.
+void PrintByteSegmentInObjectTo(const unsigned char* obj_bytes, size_t start,
+ size_t count, ostream* os) {
+ char text[5] = "";
+ for (size_t i = 0; i != count; i++) {
+ const size_t j = start + i;
+ if (i != 0) {
+ // Organizes the bytes into groups of 2 for easy parsing by
+ // human.
+ if ((j % 2) == 0)
+ *os << ' ';
+ else
+ *os << '-';
+ }
+ GTEST_SNPRINTF_(text, sizeof(text), "%02X", obj_bytes[j]);
+ *os << text;
+ }
+}
+
+// Prints the bytes in the given value to the given ostream.
+void PrintBytesInObjectToImpl(const unsigned char* obj_bytes, size_t count,
+ ostream* os) {
+ // Tells the user how big the object is.
+ *os << count << "-byte object <";
+
+ const size_t kThreshold = 132;
+ const size_t kChunkSize = 64;
+ // If the object size is bigger than kThreshold, we'll have to omit
+ // some details by printing only the first and the last kChunkSize
+ // bytes.
+ // TODO(wan): let the user control the threshold using a flag.
+ if (count < kThreshold) {
+ PrintByteSegmentInObjectTo(obj_bytes, 0, count, os);
+ } else {
+ PrintByteSegmentInObjectTo(obj_bytes, 0, kChunkSize, os);
+ *os << " ... ";
+ // Rounds up to 2-byte boundary.
+ const size_t resume_pos = (count - kChunkSize + 1)/2*2;
+ PrintByteSegmentInObjectTo(obj_bytes, resume_pos, count - resume_pos, os);
+ }
+ *os << ">";
+}
+
+} // namespace
+
+namespace internal2 {
+
+// Delegates to PrintBytesInObjectToImpl() to print the bytes in the
+// given object. The delegation simplifies the implementation, which
+// uses the << operator and thus is easier done outside of the
+// ::testing::internal namespace, which contains a << operator that
+// sometimes conflicts with the one in STL.
+void PrintBytesInObjectTo(const unsigned char* obj_bytes, size_t count,
+ ostream* os) {
+ PrintBytesInObjectToImpl(obj_bytes, count, os);
+}
+
+} // namespace internal2
+
+namespace internal {
+
+// Depending on the value of a char (or wchar_t), we print it in one
+// of three formats:
+// - as is if it's a printable ASCII (e.g. 'a', '2', ' '),
+// - as a hexidecimal escape sequence (e.g. '\x7F'), or
+// - as a special escape sequence (e.g. '\r', '\n').
+enum CharFormat {
+ kAsIs,
+ kHexEscape,
+ kSpecialEscape
+};
+
+// Returns true if c is a printable ASCII character. We test the
+// value of c directly instead of calling isprint(), which is buggy on
+// Windows Mobile.
+inline bool IsPrintableAscii(wchar_t c) {
+ return 0x20 <= c && c <= 0x7E;
+}
+
+// Prints a wide or narrow char c as a character literal without the
+// quotes, escaping it when necessary; returns how c was formatted.
+// The template argument UnsignedChar is the unsigned version of Char,
+// which is the type of c.
+template <typename UnsignedChar, typename Char>
+static CharFormat PrintAsCharLiteralTo(Char c, ostream* os) {
+ switch (static_cast<wchar_t>(c)) {
+ case L'\0':
+ *os << "\\0";
+ break;
+ case L'\'':
+ *os << "\\'";
+ break;
+ case L'\\':
+ *os << "\\\\";
+ break;
+ case L'\a':
+ *os << "\\a";
+ break;
+ case L'\b':
+ *os << "\\b";
+ break;
+ case L'\f':
+ *os << "\\f";
+ break;
+ case L'\n':
+ *os << "\\n";
+ break;
+ case L'\r':
+ *os << "\\r";
+ break;
+ case L'\t':
+ *os << "\\t";
+ break;
+ case L'\v':
+ *os << "\\v";
+ break;
+ default:
+ if (IsPrintableAscii(c)) {
+ *os << static_cast<char>(c);
+ return kAsIs;
+ } else {
+ *os << "\\x" + String::FormatHexInt(static_cast<UnsignedChar>(c));
+ return kHexEscape;
+ }
+ }
+ return kSpecialEscape;
+}
+
+// Prints a wchar_t c as if it's part of a string literal, escaping it when
+// necessary; returns how c was formatted.
+static CharFormat PrintAsStringLiteralTo(wchar_t c, ostream* os) {
+ switch (c) {
+ case L'\'':
+ *os << "'";
+ return kAsIs;
+ case L'"':
+ *os << "\\\"";
+ return kSpecialEscape;
+ default:
+ return PrintAsCharLiteralTo<wchar_t>(c, os);
+ }
+}
+
+// Prints a char c as if it's part of a string literal, escaping it when
+// necessary; returns how c was formatted.
+static CharFormat PrintAsStringLiteralTo(char c, ostream* os) {
+ return PrintAsStringLiteralTo(
+ static_cast<wchar_t>(static_cast<unsigned char>(c)), os);
+}
+
+// Prints a wide or narrow character c and its code. '\0' is printed
+// as "'\\0'", other unprintable characters are also properly escaped
+// using the standard C++ escape sequence. The template argument
+// UnsignedChar is the unsigned version of Char, which is the type of c.
+template <typename UnsignedChar, typename Char>
+void PrintCharAndCodeTo(Char c, ostream* os) {
+ // First, print c as a literal in the most readable form we can find.
+ *os << ((sizeof(c) > 1) ? "L'" : "'");
+ const CharFormat format = PrintAsCharLiteralTo<UnsignedChar>(c, os);
+ *os << "'";
+
+ // To aid user debugging, we also print c's code in decimal, unless
+ // it's 0 (in which case c was printed as '\\0', making the code
+ // obvious).
+ if (c == 0)
+ return;
+ *os << " (" << static_cast<int>(c);
+
+ // For more convenience, we print c's code again in hexidecimal,
+ // unless c was already printed in the form '\x##' or the code is in
+ // [1, 9].
+ if (format == kHexEscape || (1 <= c && c <= 9)) {
+ // Do nothing.
+ } else {
+ *os << ", 0x" << String::FormatHexInt(static_cast<UnsignedChar>(c));
+ }
+ *os << ")";
+}
+
+void PrintTo(unsigned char c, ::std::ostream* os) {
+ PrintCharAndCodeTo<unsigned char>(c, os);
+}
+void PrintTo(signed char c, ::std::ostream* os) {
+ PrintCharAndCodeTo<unsigned char>(c, os);
+}
+
+// Prints a wchar_t as a symbol if it is printable or as its internal
+// code otherwise and also as its code. L'\0' is printed as "L'\\0'".
+void PrintTo(wchar_t wc, ostream* os) {
+ PrintCharAndCodeTo<wchar_t>(wc, os);
+}
+
+// Prints the given array of characters to the ostream. CharType must be either
+// char or wchar_t.
+// The array starts at begin, the length is len, it may include '\0' characters
+// and may not be NUL-terminated.
+template <typename CharType>
+static void PrintCharsAsStringTo(
+ const CharType* begin, size_t len, ostream* os) {
+ const char* const kQuoteBegin = sizeof(CharType) == 1 ? "\"" : "L\"";
+ *os << kQuoteBegin;
+ bool is_previous_hex = false;
+ for (size_t index = 0; index < len; ++index) {
+ const CharType cur = begin[index];
+ if (is_previous_hex && IsXDigit(cur)) {
+ // Previous character is of '\x..' form and this character can be
+ // interpreted as another hexadecimal digit in its number. Break string to
+ // disambiguate.
+ *os << "\" " << kQuoteBegin;
+ }
+ is_previous_hex = PrintAsStringLiteralTo(cur, os) == kHexEscape;
+ }
+ *os << "\"";
+}
+
+// Prints a (const) char/wchar_t array of 'len' elements, starting at address
+// 'begin'. CharType must be either char or wchar_t.
+template <typename CharType>
+static void UniversalPrintCharArray(
+ const CharType* begin, size_t len, ostream* os) {
+ // The code
+ // const char kFoo[] = "foo";
+ // generates an array of 4, not 3, elements, with the last one being '\0'.
+ //
+ // Therefore when printing a char array, we don't print the last element if
+ // it's '\0', such that the output matches the string literal as it's
+ // written in the source code.
+ if (len > 0 && begin[len - 1] == '\0') {
+ PrintCharsAsStringTo(begin, len - 1, os);
+ return;
+ }
+
+ // If, however, the last element in the array is not '\0', e.g.
+ // const char kFoo[] = { 'f', 'o', 'o' };
+ // we must print the entire array. We also print a message to indicate
+ // that the array is not NUL-terminated.
+ PrintCharsAsStringTo(begin, len, os);
+ *os << " (no terminating NUL)";
+}
+
+// Prints a (const) char array of 'len' elements, starting at address 'begin'.
+void UniversalPrintArray(const char* begin, size_t len, ostream* os) {
+ UniversalPrintCharArray(begin, len, os);
+}
+
+// Prints a (const) wchar_t array of 'len' elements, starting at address
+// 'begin'.
+void UniversalPrintArray(const wchar_t* begin, size_t len, ostream* os) {
+ UniversalPrintCharArray(begin, len, os);
+}
+
+// Prints the given C string to the ostream.
+void PrintTo(const char* s, ostream* os) {
+ if (s == NULL) {
+ *os << "NULL";
+ } else {
+ *os << ImplicitCast_<const void*>(s) << " pointing to ";
+ PrintCharsAsStringTo(s, strlen(s), os);
+ }
+}
+
+// MSVC compiler can be configured to define whar_t as a typedef
+// of unsigned short. Defining an overload for const wchar_t* in that case
+// would cause pointers to unsigned shorts be printed as wide strings,
+// possibly accessing more memory than intended and causing invalid
+// memory accesses. MSVC defines _NATIVE_WCHAR_T_DEFINED symbol when
+// wchar_t is implemented as a native type.
+#if !defined(_MSC_VER) || defined(_NATIVE_WCHAR_T_DEFINED)
+// Prints the given wide C string to the ostream.
+void PrintTo(const wchar_t* s, ostream* os) {
+ if (s == NULL) {
+ *os << "NULL";
+ } else {
+ *os << ImplicitCast_<const void*>(s) << " pointing to ";
+ PrintCharsAsStringTo(s, wcslen(s), os);
+ }
+}
+#endif // wchar_t is native
+
+// Prints a ::string object.
+#if GTEST_HAS_GLOBAL_STRING
+void PrintStringTo(const ::string& s, ostream* os) {
+ PrintCharsAsStringTo(s.data(), s.size(), os);
+}
+#endif // GTEST_HAS_GLOBAL_STRING
+
+void PrintStringTo(const ::std::string& s, ostream* os) {
+ PrintCharsAsStringTo(s.data(), s.size(), os);
+}
+
+// Prints a ::wstring object.
+#if GTEST_HAS_GLOBAL_WSTRING
+void PrintWideStringTo(const ::wstring& s, ostream* os) {
+ PrintCharsAsStringTo(s.data(), s.size(), os);
+}
+#endif // GTEST_HAS_GLOBAL_WSTRING
+
+#if GTEST_HAS_STD_WSTRING
+void PrintWideStringTo(const ::std::wstring& s, ostream* os) {
+ PrintCharsAsStringTo(s.data(), s.size(), os);
+}
+#endif // GTEST_HAS_STD_WSTRING
+
+} // namespace internal
+
+} // namespace testing
diff --git a/examples/stand_alone/test_fixture/gtest/src/gtest-test-part.cc b/examples/stand_alone/test_fixture/gtest/src/gtest-test-part.cc
new file mode 100644
index 0000000..c60eef3
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/src/gtest-test-part.cc
@@ -0,0 +1,110 @@
+// Copyright 2008, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: mheule at google.com (Markus Heule)
+//
+// The Google C++ Testing Framework (Google Test)
+
+#include "gtest/gtest-test-part.h"
+
+// Indicates that this translation unit is part of Google Test's
+// implementation. It must come before gtest-internal-inl.h is
+// included, or there will be a compiler error. This trick is to
+// prevent a user from accidentally including gtest-internal-inl.h in
+// his code.
+#define GTEST_IMPLEMENTATION_ 1
+#include "src/gtest-internal-inl.h"
+#undef GTEST_IMPLEMENTATION_
+
+namespace testing {
+
+using internal::GetUnitTestImpl;
+
+// Gets the summary of the failure message by omitting the stack trace
+// in it.
+std::string TestPartResult::ExtractSummary(const char* message) {
+ const char* const stack_trace = strstr(message, internal::kStackTraceMarker);
+ return stack_trace == NULL ? message :
+ std::string(message, stack_trace);
+}
+
+// Prints a TestPartResult object.
+std::ostream& operator<<(std::ostream& os, const TestPartResult& result) {
+ return os
+ << result.file_name() << ":" << result.line_number() << ": "
+ << (result.type() == TestPartResult::kSuccess ? "Success" :
+ result.type() == TestPartResult::kFatalFailure ? "Fatal failure" :
+ "Non-fatal failure") << ":\n"
+ << result.message() << std::endl;
+}
+
+// Appends a TestPartResult to the array.
+void TestPartResultArray::Append(const TestPartResult& result) {
+ array_.push_back(result);
+}
+
+// Returns the TestPartResult at the given index (0-based).
+const TestPartResult& TestPartResultArray::GetTestPartResult(int index) const {
+ if (index < 0 || index >= size()) {
+ printf("\nInvalid index (%d) into TestPartResultArray.\n", index);
+ internal::posix::Abort();
+ }
+
+ return array_[index];
+}
+
+// Returns the number of TestPartResult objects in the array.
+int TestPartResultArray::size() const {
+ return static_cast<int>(array_.size());
+}
+
+namespace internal {
+
+HasNewFatalFailureHelper::HasNewFatalFailureHelper()
+ : has_new_fatal_failure_(false),
+ original_reporter_(GetUnitTestImpl()->
+ GetTestPartResultReporterForCurrentThread()) {
+ GetUnitTestImpl()->SetTestPartResultReporterForCurrentThread(this);
+}
+
+HasNewFatalFailureHelper::~HasNewFatalFailureHelper() {
+ GetUnitTestImpl()->SetTestPartResultReporterForCurrentThread(
+ original_reporter_);
+}
+
+void HasNewFatalFailureHelper::ReportTestPartResult(
+ const TestPartResult& result) {
+ if (result.fatally_failed())
+ has_new_fatal_failure_ = true;
+ original_reporter_->ReportTestPartResult(result);
+}
+
+} // namespace internal
+
+} // namespace testing
diff --git a/examples/stand_alone/test_fixture/gtest/src/gtest-typed-test.cc b/examples/stand_alone/test_fixture/gtest/src/gtest-typed-test.cc
new file mode 100644
index 0000000..f0079f4
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/src/gtest-typed-test.cc
@@ -0,0 +1,110 @@
+// Copyright 2008 Google Inc.
+// All Rights Reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+
+#include "gtest/gtest-typed-test.h"
+#include "gtest/gtest.h"
+
+namespace testing {
+namespace internal {
+
+#if GTEST_HAS_TYPED_TEST_P
+
+// Skips to the first non-space char in str. Returns an empty string if str
+// contains only whitespace characters.
+static const char* SkipSpaces(const char* str) {
+ while (IsSpace(*str))
+ str++;
+ return str;
+}
+
+// Verifies that registered_tests match the test names in
+// defined_test_names_; returns registered_tests if successful, or
+// aborts the program otherwise.
+const char* TypedTestCasePState::VerifyRegisteredTestNames(
+ const char* file, int line, const char* registered_tests) {
+ typedef ::std::set<const char*>::const_iterator DefinedTestIter;
+ registered_ = true;
+
+ // Skip initial whitespace in registered_tests since some
+ // preprocessors prefix stringizied literals with whitespace.
+ registered_tests = SkipSpaces(registered_tests);
+
+ Message errors;
+ ::std::set<std::string> tests;
+ for (const char* names = registered_tests; names != NULL;
+ names = SkipComma(names)) {
+ const std::string name = GetPrefixUntilComma(names);
+ if (tests.count(name) != 0) {
+ errors << "Test " << name << " is listed more than once.\n";
+ continue;
+ }
+
+ bool found = false;
+ for (DefinedTestIter it = defined_test_names_.begin();
+ it != defined_test_names_.end();
+ ++it) {
+ if (name == *it) {
+ found = true;
+ break;
+ }
+ }
+
+ if (found) {
+ tests.insert(name);
+ } else {
+ errors << "No test named " << name
+ << " can be found in this test case.\n";
+ }
+ }
+
+ for (DefinedTestIter it = defined_test_names_.begin();
+ it != defined_test_names_.end();
+ ++it) {
+ if (tests.count(*it) == 0) {
+ errors << "You forgot to list test " << *it << ".\n";
+ }
+ }
+
+ const std::string& errors_str = errors.GetString();
+ if (errors_str != "") {
+ fprintf(stderr, "%s %s", FormatFileLocation(file, line).c_str(),
+ errors_str.c_str());
+ fflush(stderr);
+ posix::Abort();
+ }
+
+ return registered_tests;
+}
+
+#endif // GTEST_HAS_TYPED_TEST_P
+
+} // namespace internal
+} // namespace testing
diff --git a/examples/stand_alone/test_fixture/gtest/src/gtest.cc b/examples/stand_alone/test_fixture/gtest/src/gtest.cc
new file mode 100644
index 0000000..7ee9368
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/src/gtest.cc
@@ -0,0 +1,5016 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: wan at google.com (Zhanyong Wan)
+//
+// The Google C++ Testing Framework (Google Test)
+
+#include "gtest/gtest.h"
+#include "gtest/gtest-spi.h"
+#pragma GCC system_header
+
+#include <ctype.h>
+#include <math.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
+#include <wchar.h>
+#include <wctype.h>
+
+#include <algorithm>
+#include <iomanip>
+#include <limits>
+#include <ostream> // NOLINT
+#include <sstream>
+#include <vector>
+
+#if GTEST_OS_LINUX
+
+// TODO(kenton at google.com): Use autoconf to detect availability of
+// gettimeofday().
+# define GTEST_HAS_GETTIMEOFDAY_ 1
+
+# include <fcntl.h> // NOLINT
+# include <limits.h> // NOLINT
+# include <sched.h> // NOLINT
+// Declares vsnprintf(). This header is not available on Windows.
+# include <strings.h> // NOLINT
+# include <sys/mman.h> // NOLINT
+# include <sys/time.h> // NOLINT
+# include <unistd.h> // NOLINT
+# include <string>
+
+#elif GTEST_OS_SYMBIAN
+# define GTEST_HAS_GETTIMEOFDAY_ 1
+# include <sys/time.h> // NOLINT
+
+#elif GTEST_OS_ZOS
+# define GTEST_HAS_GETTIMEOFDAY_ 1
+# include <sys/time.h> // NOLINT
+
+// On z/OS we additionally need strings.h for strcasecmp.
+# include <strings.h> // NOLINT
+
+#elif GTEST_OS_WINDOWS_MOBILE // We are on Windows CE.
+
+# include <windows.h> // NOLINT
+
+#elif GTEST_OS_WINDOWS // We are on Windows proper.
+
+# include <io.h> // NOLINT
+# include <sys/timeb.h> // NOLINT
+# include <sys/types.h> // NOLINT
+# include <sys/stat.h> // NOLINT
+
+# if GTEST_OS_WINDOWS_MINGW
+// MinGW has gettimeofday() but not _ftime64().
+// TODO(kenton at google.com): Use autoconf to detect availability of
+// gettimeofday().
+// TODO(kenton at google.com): There are other ways to get the time on
+// Windows, like GetTickCount() or GetSystemTimeAsFileTime(). MinGW
+// supports these. consider using them instead.
+# define GTEST_HAS_GETTIMEOFDAY_ 1
+# include <sys/time.h> // NOLINT
+# endif // GTEST_OS_WINDOWS_MINGW
+
+// cpplint thinks that the header is already included, so we want to
+// silence it.
+# include <windows.h> // NOLINT
+
+#else
+
+// Assume other platforms have gettimeofday().
+// TODO(kenton at google.com): Use autoconf to detect availability of
+// gettimeofday().
+# define GTEST_HAS_GETTIMEOFDAY_ 1
+
+// cpplint thinks that the header is already included, so we want to
+// silence it.
+# include <sys/time.h> // NOLINT
+# include <unistd.h> // NOLINT
+
+#endif // GTEST_OS_LINUX
+
+#if GTEST_HAS_EXCEPTIONS
+# include <stdexcept>
+#endif
+
+#if GTEST_CAN_STREAM_RESULTS_
+# include <arpa/inet.h> // NOLINT
+# include <netdb.h> // NOLINT
+#endif
+
+// Indicates that this translation unit is part of Google Test's
+// implementation. It must come before gtest-internal-inl.h is
+// included, or there will be a compiler error. This trick is to
+// prevent a user from accidentally including gtest-internal-inl.h in
+// his code.
+#define GTEST_IMPLEMENTATION_ 1
+#include "src/gtest-internal-inl.h"
+#undef GTEST_IMPLEMENTATION_
+
+#if GTEST_OS_WINDOWS
+# define vsnprintf _vsnprintf
+#endif // GTEST_OS_WINDOWS
+
+namespace testing {
+
+using internal::CountIf;
+using internal::ForEach;
+using internal::GetElementOr;
+using internal::Shuffle;
+
+// Constants.
+
+// A test whose test case name or test name matches this filter is
+// disabled and not run.
+static const char kDisableTestFilter[] = "DISABLED_*:*/DISABLED_*";
+
+// A test case whose name matches this filter is considered a death
+// test case and will be run before test cases whose name doesn't
+// match this filter.
+static const char kDeathTestCaseFilter[] = "*DeathTest:*DeathTest/*";
+
+// A test filter that matches everything.
+static const char kUniversalFilter[] = "*";
+
+// The default output file for XML output.
+static const char kDefaultOutputFile[] = "test_detail.xml";
+
+// The environment variable name for the test shard index.
+static const char kTestShardIndex[] = "GTEST_SHARD_INDEX";
+// The environment variable name for the total number of test shards.
+static const char kTestTotalShards[] = "GTEST_TOTAL_SHARDS";
+// The environment variable name for the test shard status file.
+static const char kTestShardStatusFile[] = "GTEST_SHARD_STATUS_FILE";
+
+namespace internal {
+
+// The text used in failure messages to indicate the start of the
+// stack trace.
+const char kStackTraceMarker[] = "\nStack trace:\n";
+
+// g_help_flag is true iff the --help flag or an equivalent form is
+// specified on the command line.
+bool g_help_flag = false;
+
+} // namespace internal
+
+static const char* GetDefaultFilter() {
+ return kUniversalFilter;
+}
+
+GTEST_DEFINE_bool_(
+ also_run_disabled_tests,
+ internal::BoolFromGTestEnv("also_run_disabled_tests", false),
+ "Run disabled tests too, in addition to the tests normally being run.");
+
+GTEST_DEFINE_bool_(
+ break_on_failure,
+ internal::BoolFromGTestEnv("break_on_failure", false),
+ "True iff a failed assertion should be a debugger break-point.");
+
+GTEST_DEFINE_bool_(
+ catch_exceptions,
+ internal::BoolFromGTestEnv("catch_exceptions", true),
+ "True iff " GTEST_NAME_
+ " should catch exceptions and treat them as test failures.");
+
+GTEST_DEFINE_string_(
+ color,
+ internal::StringFromGTestEnv("color", "auto"),
+ "Whether to use colors in the output. Valid values: yes, no, "
+ "and auto. 'auto' means to use colors if the output is "
+ "being sent to a terminal and the TERM environment variable "
+ "is set to a terminal type that supports colors.");
+
+GTEST_DEFINE_string_(
+ filter,
+ internal::StringFromGTestEnv("filter", GetDefaultFilter()),
+ "A colon-separated list of glob (not regex) patterns "
+ "for filtering the tests to run, optionally followed by a "
+ "'-' and a : separated list of negative patterns (tests to "
+ "exclude). A test is run if it matches one of the positive "
+ "patterns and does not match any of the negative patterns.");
+
+GTEST_DEFINE_bool_(list_tests, false,
+ "List all tests without running them.");
+
+GTEST_DEFINE_string_(
+ output,
+ internal::StringFromGTestEnv("output", ""),
+ "A format (currently must be \"xml\"), optionally followed "
+ "by a colon and an output file name or directory. A directory "
+ "is indicated by a trailing pathname separator. "
+ "Examples: \"xml:filename.xml\", \"xml::directoryname/\". "
+ "If a directory is specified, output files will be created "
+ "within that directory, with file-names based on the test "
+ "executable's name and, if necessary, made unique by adding "
+ "digits.");
+
+GTEST_DEFINE_bool_(
+ print_time,
+ internal::BoolFromGTestEnv("print_time", true),
+ "True iff " GTEST_NAME_
+ " should display elapsed time in text output.");
+
+GTEST_DEFINE_int32_(
+ random_seed,
+ internal::Int32FromGTestEnv("random_seed", 0),
+ "Random number seed to use when shuffling test orders. Must be in range "
+ "[1, 99999], or 0 to use a seed based on the current time.");
+
+GTEST_DEFINE_int32_(
+ repeat,
+ internal::Int32FromGTestEnv("repeat", 1),
+ "How many times to repeat each test. Specify a negative number "
+ "for repeating forever. Useful for shaking out flaky tests.");
+
+GTEST_DEFINE_bool_(
+ show_internal_stack_frames, false,
+ "True iff " GTEST_NAME_ " should include internal stack frames when "
+ "printing test failure stack traces.");
+
+GTEST_DEFINE_bool_(
+ shuffle,
+ internal::BoolFromGTestEnv("shuffle", false),
+ "True iff " GTEST_NAME_
+ " should randomize tests' order on every run.");
+
+GTEST_DEFINE_int32_(
+ stack_trace_depth,
+ internal::Int32FromGTestEnv("stack_trace_depth", kMaxStackTraceDepth),
+ "The maximum number of stack frames to print when an "
+ "assertion fails. The valid range is 0 through 100, inclusive.");
+
+GTEST_DEFINE_string_(
+ stream_result_to,
+ internal::StringFromGTestEnv("stream_result_to", ""),
+ "This flag specifies the host name and the port number on which to stream "
+ "test results. Example: \"localhost:555\". The flag is effective only on "
+ "Linux.");
+
+GTEST_DEFINE_bool_(
+ throw_on_failure,
+ internal::BoolFromGTestEnv("throw_on_failure", false),
+ "When this flag is specified, a failed assertion will throw an exception "
+ "if exceptions are enabled or exit the program with a non-zero code "
+ "otherwise.");
+
+namespace internal {
+
+// Generates a random number from [0, range), using a Linear
+// Congruential Generator (LCG). Crashes if 'range' is 0 or greater
+// than kMaxRange.
+UInt32 Random::Generate(UInt32 range) {
+ // These constants are the same as are used in glibc's rand(3).
+ state_ = (1103515245U*state_ + 12345U) % kMaxRange;
+
+ GTEST_CHECK_(range > 0)
+ << "Cannot generate a number in the range [0, 0).";
+ GTEST_CHECK_(range <= kMaxRange)
+ << "Generation of a number in [0, " << range << ") was requested, "
+ << "but this can only generate numbers in [0, " << kMaxRange << ").";
+
+ // Converting via modulus introduces a bit of downward bias, but
+ // it's simple, and a linear congruential generator isn't too good
+ // to begin with.
+ return state_ % range;
+}
+
+// GTestIsInitialized() returns true iff the user has initialized
+// Google Test. Useful for catching the user mistake of not initializing
+// Google Test before calling RUN_ALL_TESTS().
+//
+// A user must call testing::InitGoogleTest() to initialize Google
+// Test. g_init_gtest_count is set to the number of times
+// InitGoogleTest() has been called. We don't protect this variable
+// under a mutex as it is only accessed in the main thread.
+GTEST_API_ int g_init_gtest_count = 0;
+static bool GTestIsInitialized() { return g_init_gtest_count != 0; }
+
+// Iterates over a vector of TestCases, keeping a running sum of the
+// results of calling a given int-returning method on each.
+// Returns the sum.
+static int SumOverTestCaseList(const std::vector<TestCase*>& case_list,
+ int (TestCase::*method)() const) {
+ int sum = 0;
+ for (size_t i = 0; i < case_list.size(); i++) {
+ sum += (case_list[i]->*method)();
+ }
+ return sum;
+}
+
+// Returns true iff the test case passed.
+static bool TestCasePassed(const TestCase* test_case) {
+ return test_case->should_run() && test_case->Passed();
+}
+
+// Returns true iff the test case failed.
+static bool TestCaseFailed(const TestCase* test_case) {
+ return test_case->should_run() && test_case->Failed();
+}
+
+// Returns true iff test_case contains at least one test that should
+// run.
+static bool ShouldRunTestCase(const TestCase* test_case) {
+ return test_case->should_run();
+}
+
+// AssertHelper constructor.
+AssertHelper::AssertHelper(TestPartResult::Type type,
+ const char* file,
+ int line,
+ const char* message)
+ : data_(new AssertHelperData(type, file, line, message)) {
+}
+
+AssertHelper::~AssertHelper() {
+ delete data_;
+}
+
+// Message assignment, for assertion streaming support.
+void AssertHelper::operator=(const Message& message) const {
+ UnitTest::GetInstance()->
+ AddTestPartResult(data_->type, data_->file, data_->line,
+ AppendUserMessage(data_->message, message),
+ UnitTest::GetInstance()->impl()
+ ->CurrentOsStackTraceExceptTop(1)
+ // Skips the stack frame for this function itself.
+ ); // NOLINT
+}
+
+// Mutex for linked pointers.
+GTEST_API_ GTEST_DEFINE_STATIC_MUTEX_(g_linked_ptr_mutex);
+
+// Application pathname gotten in InitGoogleTest.
+std::string g_executable_path;
+
+// Returns the current application's name, removing directory path if that
+// is present.
+FilePath GetCurrentExecutableName() {
+ FilePath result;
+
+#if GTEST_OS_WINDOWS
+ result.Set(FilePath(g_executable_path).RemoveExtension("exe"));
+#else
+ result.Set(FilePath(g_executable_path));
+#endif // GTEST_OS_WINDOWS
+
+ return result.RemoveDirectoryName();
+}
+
+// Functions for processing the gtest_output flag.
+
+// Returns the output format, or "" for normal printed output.
+std::string UnitTestOptions::GetOutputFormat() {
+ const char* const gtest_output_flag = GTEST_FLAG(output).c_str();
+ if (gtest_output_flag == NULL) return std::string("");
+
+ const char* const colon = strchr(gtest_output_flag, ':');
+ return (colon == NULL) ?
+ std::string(gtest_output_flag) :
+ std::string(gtest_output_flag, colon - gtest_output_flag);
+}
+
+// Returns the name of the requested output file, or the default if none
+// was explicitly specified.
+std::string UnitTestOptions::GetAbsolutePathToOutputFile() {
+ const char* const gtest_output_flag = GTEST_FLAG(output).c_str();
+ if (gtest_output_flag == NULL)
+ return "";
+
+ const char* const colon = strchr(gtest_output_flag, ':');
+ if (colon == NULL)
+ return internal::FilePath::ConcatPaths(
+ internal::FilePath(
+ UnitTest::GetInstance()->original_working_dir()),
+ internal::FilePath(kDefaultOutputFile)).string();
+
+ internal::FilePath output_name(colon + 1);
+ if (!output_name.IsAbsolutePath())
+ // TODO(wan at google.com): on Windows \some\path is not an absolute
+ // path (as its meaning depends on the current drive), yet the
+ // following logic for turning it into an absolute path is wrong.
+ // Fix it.
+ output_name = internal::FilePath::ConcatPaths(
+ internal::FilePath(UnitTest::GetInstance()->original_working_dir()),
+ internal::FilePath(colon + 1));
+
+ if (!output_name.IsDirectory())
+ return output_name.string();
+
+ internal::FilePath result(internal::FilePath::GenerateUniqueFileName(
+ output_name, internal::GetCurrentExecutableName(),
+ GetOutputFormat().c_str()));
+ return result.string();
+}
+
+// Returns true iff the wildcard pattern matches the string. The
+// first ':' or '\0' character in pattern marks the end of it.
+//
+// This recursive algorithm isn't very efficient, but is clear and
+// works well enough for matching test names, which are short.
+bool UnitTestOptions::PatternMatchesString(const char *pattern,
+ const char *str) {
+ switch (*pattern) {
+ case '\0':
+ case ':': // Either ':' or '\0' marks the end of the pattern.
+ return *str == '\0';
+ case '?': // Matches any single character.
+ return *str != '\0' && PatternMatchesString(pattern + 1, str + 1);
+ case '*': // Matches any string (possibly empty) of characters.
+ return (*str != '\0' && PatternMatchesString(pattern, str + 1)) ||
+ PatternMatchesString(pattern + 1, str);
+ default: // Non-special character. Matches itself.
+ return *pattern == *str &&
+ PatternMatchesString(pattern + 1, str + 1);
+ }
+}
+
+bool UnitTestOptions::MatchesFilter(
+ const std::string& name, const char* filter) {
+ const char *cur_pattern = filter;
+ for (;;) {
+ if (PatternMatchesString(cur_pattern, name.c_str())) {
+ return true;
+ }
+
+ // Finds the next pattern in the filter.
+ cur_pattern = strchr(cur_pattern, ':');
+
+ // Returns if no more pattern can be found.
+ if (cur_pattern == NULL) {
+ return false;
+ }
+
+ // Skips the pattern separater (the ':' character).
+ cur_pattern++;
+ }
+}
+
+// Returns true iff the user-specified filter matches the test case
+// name and the test name.
+bool UnitTestOptions::FilterMatchesTest(const std::string &test_case_name,
+ const std::string &test_name) {
+ const std::string& full_name = test_case_name + "." + test_name.c_str();
+
+ // Split --gtest_filter at '-', if there is one, to separate into
+ // positive filter and negative filter portions
+ const char* const p = GTEST_FLAG(filter).c_str();
+ const char* const dash = strchr(p, '-');
+ std::string positive;
+ std::string negative;
+ if (dash == NULL) {
+ positive = GTEST_FLAG(filter).c_str(); // Whole string is a positive filter
+ negative = "";
+ } else {
+ positive = std::string(p, dash); // Everything up to the dash
+ negative = std::string(dash + 1); // Everything after the dash
+ if (positive.empty()) {
+ // Treat '-test1' as the same as '*-test1'
+ positive = kUniversalFilter;
+ }
+ }
+
+ // A filter is a colon-separated list of patterns. It matches a
+ // test if any pattern in it matches the test.
+ return (MatchesFilter(full_name, positive.c_str()) &&
+ !MatchesFilter(full_name, negative.c_str()));
+}
+
+#if GTEST_HAS_SEH
+// Returns EXCEPTION_EXECUTE_HANDLER if Google Test should handle the
+// given SEH exception, or EXCEPTION_CONTINUE_SEARCH otherwise.
+// This function is useful as an __except condition.
+int UnitTestOptions::GTestShouldProcessSEH(DWORD exception_code) {
+ // Google Test should handle a SEH exception if:
+ // 1. the user wants it to, AND
+ // 2. this is not a breakpoint exception, AND
+ // 3. this is not a C++ exception (VC++ implements them via SEH,
+ // apparently).
+ //
+ // SEH exception code for C++ exceptions.
+ // (see http://support.microsoft.com/kb/185294 for more information).
+ const DWORD kCxxExceptionCode = 0xe06d7363;
+
+ bool should_handle = true;
+
+ if (!GTEST_FLAG(catch_exceptions))
+ should_handle = false;
+ else if (exception_code == EXCEPTION_BREAKPOINT)
+ should_handle = false;
+ else if (exception_code == kCxxExceptionCode)
+ should_handle = false;
+
+ return should_handle ? EXCEPTION_EXECUTE_HANDLER : EXCEPTION_CONTINUE_SEARCH;
+}
+#endif // GTEST_HAS_SEH
+
+} // namespace internal
+
+// The c'tor sets this object as the test part result reporter used by
+// Google Test. The 'result' parameter specifies where to report the
+// results. Intercepts only failures from the current thread.
+ScopedFakeTestPartResultReporter::ScopedFakeTestPartResultReporter(
+ TestPartResultArray* result)
+ : intercept_mode_(INTERCEPT_ONLY_CURRENT_THREAD),
+ result_(result) {
+ Init();
+}
+
+// The c'tor sets this object as the test part result reporter used by
+// Google Test. The 'result' parameter specifies where to report the
+// results.
+ScopedFakeTestPartResultReporter::ScopedFakeTestPartResultReporter(
+ InterceptMode intercept_mode, TestPartResultArray* result)
+ : intercept_mode_(intercept_mode),
+ result_(result) {
+ Init();
+}
+
+void ScopedFakeTestPartResultReporter::Init() {
+ internal::UnitTestImpl* const impl = internal::GetUnitTestImpl();
+ if (intercept_mode_ == INTERCEPT_ALL_THREADS) {
+ old_reporter_ = impl->GetGlobalTestPartResultReporter();
+ impl->SetGlobalTestPartResultReporter(this);
+ } else {
+ old_reporter_ = impl->GetTestPartResultReporterForCurrentThread();
+ impl->SetTestPartResultReporterForCurrentThread(this);
+ }
+}
+
+// The d'tor restores the test part result reporter used by Google Test
+// before.
+ScopedFakeTestPartResultReporter::~ScopedFakeTestPartResultReporter() {
+ internal::UnitTestImpl* const impl = internal::GetUnitTestImpl();
+ if (intercept_mode_ == INTERCEPT_ALL_THREADS) {
+ impl->SetGlobalTestPartResultReporter(old_reporter_);
+ } else {
+ impl->SetTestPartResultReporterForCurrentThread(old_reporter_);
+ }
+}
+
+// Increments the test part result count and remembers the result.
+// This method is from the TestPartResultReporterInterface interface.
+void ScopedFakeTestPartResultReporter::ReportTestPartResult(
+ const TestPartResult& result) {
+ result_->Append(result);
+}
+
+namespace internal {
+
+// Returns the type ID of ::testing::Test. We should always call this
+// instead of GetTypeId< ::testing::Test>() to get the type ID of
+// testing::Test. This is to work around a suspected linker bug when
+// using Google Test as a framework on Mac OS X. The bug causes
+// GetTypeId< ::testing::Test>() to return different values depending
+// on whether the call is from the Google Test framework itself or
+// from user test code. GetTestTypeId() is guaranteed to always
+// return the same value, as it always calls GetTypeId<>() from the
+// gtest.cc, which is within the Google Test framework.
+TypeId GetTestTypeId() {
+ return GetTypeId<Test>();
+}
+
+// The value of GetTestTypeId() as seen from within the Google Test
+// library. This is solely for testing GetTestTypeId().
+extern const TypeId kTestTypeIdInGoogleTest = GetTestTypeId();
+
+// This predicate-formatter checks that 'results' contains a test part
+// failure of the given type and that the failure message contains the
+// given substring.
+AssertionResult HasOneFailure(const char* /* results_expr */,
+ const char* /* type_expr */,
+ const char* /* substr_expr */,
+ const TestPartResultArray& results,
+ TestPartResult::Type type,
+ const string& substr) {
+ const std::string expected(type == TestPartResult::kFatalFailure ?
+ "1 fatal failure" :
+ "1 non-fatal failure");
+ Message msg;
+ if (results.size() != 1) {
+ msg << "Expected: " << expected << "\n"
+ << " Actual: " << results.size() << " failures";
+ for (int i = 0; i < results.size(); i++) {
+ msg << "\n" << results.GetTestPartResult(i);
+ }
+ return AssertionFailure() << msg;
+ }
+
+ const TestPartResult& r = results.GetTestPartResult(0);
+ if (r.type() != type) {
+ return AssertionFailure() << "Expected: " << expected << "\n"
+ << " Actual:\n"
+ << r;
+ }
+
+ if (strstr(r.message(), substr.c_str()) == NULL) {
+ return AssertionFailure() << "Expected: " << expected << " containing \""
+ << substr << "\"\n"
+ << " Actual:\n"
+ << r;
+ }
+
+ return AssertionSuccess();
+}
+
+// The constructor of SingleFailureChecker remembers where to look up
+// test part results, what type of failure we expect, and what
+// substring the failure message should contain.
+SingleFailureChecker:: SingleFailureChecker(
+ const TestPartResultArray* results,
+ TestPartResult::Type type,
+ const string& substr)
+ : results_(results),
+ type_(type),
+ substr_(substr) {}
+
+// The destructor of SingleFailureChecker verifies that the given
+// TestPartResultArray contains exactly one failure that has the given
+// type and contains the given substring. If that's not the case, a
+// non-fatal failure will be generated.
+SingleFailureChecker::~SingleFailureChecker() {
+ EXPECT_PRED_FORMAT3(HasOneFailure, *results_, type_, substr_);
+}
+
+DefaultGlobalTestPartResultReporter::DefaultGlobalTestPartResultReporter(
+ UnitTestImpl* unit_test) : unit_test_(unit_test) {}
+
+void DefaultGlobalTestPartResultReporter::ReportTestPartResult(
+ const TestPartResult& result) {
+ unit_test_->current_test_result()->AddTestPartResult(result);
+ unit_test_->listeners()->repeater()->OnTestPartResult(result);
+}
+
+DefaultPerThreadTestPartResultReporter::DefaultPerThreadTestPartResultReporter(
+ UnitTestImpl* unit_test) : unit_test_(unit_test) {}
+
+void DefaultPerThreadTestPartResultReporter::ReportTestPartResult(
+ const TestPartResult& result) {
+ unit_test_->GetGlobalTestPartResultReporter()->ReportTestPartResult(result);
+}
+
+// Returns the global test part result reporter.
+TestPartResultReporterInterface*
+UnitTestImpl::GetGlobalTestPartResultReporter() {
+ internal::MutexLock lock(&global_test_part_result_reporter_mutex_);
+ return global_test_part_result_repoter_;
+}
+
+// Sets the global test part result reporter.
+void UnitTestImpl::SetGlobalTestPartResultReporter(
+ TestPartResultReporterInterface* reporter) {
+ internal::MutexLock lock(&global_test_part_result_reporter_mutex_);
+ global_test_part_result_repoter_ = reporter;
+}
+
+// Returns the test part result reporter for the current thread.
+TestPartResultReporterInterface*
+UnitTestImpl::GetTestPartResultReporterForCurrentThread() {
+ return per_thread_test_part_result_reporter_.get();
+}
+
+// Sets the test part result reporter for the current thread.
+void UnitTestImpl::SetTestPartResultReporterForCurrentThread(
+ TestPartResultReporterInterface* reporter) {
+ per_thread_test_part_result_reporter_.set(reporter);
+}
+
+// Gets the number of successful test cases.
+int UnitTestImpl::successful_test_case_count() const {
+ return CountIf(test_cases_, TestCasePassed);
+}
+
+// Gets the number of failed test cases.
+int UnitTestImpl::failed_test_case_count() const {
+ return CountIf(test_cases_, TestCaseFailed);
+}
+
+// Gets the number of all test cases.
+int UnitTestImpl::total_test_case_count() const {
+ return static_cast<int>(test_cases_.size());
+}
+
+// Gets the number of all test cases that contain at least one test
+// that should run.
+int UnitTestImpl::test_case_to_run_count() const {
+ return CountIf(test_cases_, ShouldRunTestCase);
+}
+
+// Gets the number of successful tests.
+int UnitTestImpl::successful_test_count() const {
+ return SumOverTestCaseList(test_cases_, &TestCase::successful_test_count);
+}
+
+// Gets the number of failed tests.
+int UnitTestImpl::failed_test_count() const {
+ return SumOverTestCaseList(test_cases_, &TestCase::failed_test_count);
+}
+
+// Gets the number of disabled tests that will be reported in the XML report.
+int UnitTestImpl::reportable_disabled_test_count() const {
+ return SumOverTestCaseList(test_cases_,
+ &TestCase::reportable_disabled_test_count);
+}
+
+// Gets the number of disabled tests.
+int UnitTestImpl::disabled_test_count() const {
+ return SumOverTestCaseList(test_cases_, &TestCase::disabled_test_count);
+}
+
+// Gets the number of tests to be printed in the XML report.
+int UnitTestImpl::reportable_test_count() const {
+ return SumOverTestCaseList(test_cases_, &TestCase::reportable_test_count);
+}
+
+// Gets the number of all tests.
+int UnitTestImpl::total_test_count() const {
+ return SumOverTestCaseList(test_cases_, &TestCase::total_test_count);
+}
+
+// Gets the number of tests that should run.
+int UnitTestImpl::test_to_run_count() const {
+ return SumOverTestCaseList(test_cases_, &TestCase::test_to_run_count);
+}
+
+// Returns the current OS stack trace as an std::string.
+//
+// The maximum number of stack frames to be included is specified by
+// the gtest_stack_trace_depth flag. The skip_count parameter
+// specifies the number of top frames to be skipped, which doesn't
+// count against the number of frames to be included.
+//
+// For example, if Foo() calls Bar(), which in turn calls
+// CurrentOsStackTraceExceptTop(1), Foo() will be included in the
+// trace but Bar() and CurrentOsStackTraceExceptTop() won't.
+std::string UnitTestImpl::CurrentOsStackTraceExceptTop(int skip_count) {
+ (void)skip_count;
+ return "";
+}
+
+// Returns the current time in milliseconds.
+TimeInMillis GetTimeInMillis() {
+#if GTEST_OS_WINDOWS_MOBILE || defined(__BORLANDC__)
+ // Difference between 1970-01-01 and 1601-01-01 in milliseconds.
+ // http://analogous.blogspot.com/2005/04/epoch.html
+ const TimeInMillis kJavaEpochToWinFileTimeDelta =
+ static_cast<TimeInMillis>(116444736UL) * 100000UL;
+ const DWORD kTenthMicrosInMilliSecond = 10000;
+
+ SYSTEMTIME now_systime;
+ FILETIME now_filetime;
+ ULARGE_INTEGER now_int64;
+ // TODO(kenton at google.com): Shouldn't this just use
+ // GetSystemTimeAsFileTime()?
+ GetSystemTime(&now_systime);
+ if (SystemTimeToFileTime(&now_systime, &now_filetime)) {
+ now_int64.LowPart = now_filetime.dwLowDateTime;
+ now_int64.HighPart = now_filetime.dwHighDateTime;
+ now_int64.QuadPart = (now_int64.QuadPart / kTenthMicrosInMilliSecond) -
+ kJavaEpochToWinFileTimeDelta;
+ return now_int64.QuadPart;
+ }
+ return 0;
+#elif GTEST_OS_WINDOWS && !GTEST_HAS_GETTIMEOFDAY_
+ __timeb64 now;
+
+# ifdef _MSC_VER
+
+ // MSVC 8 deprecates _ftime64(), so we want to suppress warning 4996
+ // (deprecated function) there.
+ // TODO(kenton at google.com): Use GetTickCount()? Or use
+ // SystemTimeToFileTime()
+# pragma warning(push) // Saves the current warning state.
+# pragma warning(disable:4996) // Temporarily disables warning 4996.
+ _ftime64(&now);
+# pragma warning(pop) // Restores the warning state.
+# else
+
+ _ftime64(&now);
+
+# endif // _MSC_VER
+
+ return static_cast<TimeInMillis>(now.time) * 1000 + now.millitm;
+#elif GTEST_HAS_GETTIMEOFDAY_
+ struct timeval now;
+ gettimeofday(&now, NULL);
+ return static_cast<TimeInMillis>(now.tv_sec) * 1000 + now.tv_usec / 1000;
+#else
+# error "Don't know how to get the current time on your system."
+#endif
+}
+
+// Utilities
+
+// class String.
+
+#if GTEST_OS_WINDOWS_MOBILE
+// Creates a UTF-16 wide string from the given ANSI string, allocating
+// memory using new. The caller is responsible for deleting the return
+// value using delete[]. Returns the wide string, or NULL if the
+// input is NULL.
+LPCWSTR String::AnsiToUtf16(const char* ansi) {
+ if (!ansi) return NULL;
+ const int length = strlen(ansi);
+ const int unicode_length =
+ MultiByteToWideChar(CP_ACP, 0, ansi, length,
+ NULL, 0);
+ WCHAR* unicode = new WCHAR[unicode_length + 1];
+ MultiByteToWideChar(CP_ACP, 0, ansi, length,
+ unicode, unicode_length);
+ unicode[unicode_length] = 0;
+ return unicode;
+}
+
+// Creates an ANSI string from the given wide string, allocating
+// memory using new. The caller is responsible for deleting the return
+// value using delete[]. Returns the ANSI string, or NULL if the
+// input is NULL.
+const char* String::Utf16ToAnsi(LPCWSTR utf16_str) {
+ if (!utf16_str) return NULL;
+ const int ansi_length =
+ WideCharToMultiByte(CP_ACP, 0, utf16_str, -1,
+ NULL, 0, NULL, NULL);
+ char* ansi = new char[ansi_length + 1];
+ WideCharToMultiByte(CP_ACP, 0, utf16_str, -1,
+ ansi, ansi_length, NULL, NULL);
+ ansi[ansi_length] = 0;
+ return ansi;
+}
+
+#endif // GTEST_OS_WINDOWS_MOBILE
+
+// Compares two C strings. Returns true iff they have the same content.
+//
+// Unlike strcmp(), this function can handle NULL argument(s). A NULL
+// C string is considered different to any non-NULL C string,
+// including the empty string.
+bool String::CStringEquals(const char * lhs, const char * rhs) {
+ if ( lhs == NULL ) return rhs == NULL;
+
+ if ( rhs == NULL ) return false;
+
+ return strcmp(lhs, rhs) == 0;
+}
+
+#if GTEST_HAS_STD_WSTRING || GTEST_HAS_GLOBAL_WSTRING
+
+// Converts an array of wide chars to a narrow string using the UTF-8
+// encoding, and streams the result to the given Message object.
+static void StreamWideCharsToMessage(const wchar_t* wstr, size_t length,
+ Message* msg) {
+ for (size_t i = 0; i != length; ) { // NOLINT
+ if (wstr[i] != L'\0') {
+ *msg << WideStringToUtf8(wstr + i, static_cast<int>(length - i));
+ while (i != length && wstr[i] != L'\0')
+ i++;
+ } else {
+ *msg << '\0';
+ i++;
+ }
+ }
+}
+
+#endif // GTEST_HAS_STD_WSTRING || GTEST_HAS_GLOBAL_WSTRING
+
+} // namespace internal
+
+// Constructs an empty Message.
+// We allocate the stringstream separately because otherwise each use of
+// ASSERT/EXPECT in a procedure adds over 200 bytes to the procedure's
+// stack frame leading to huge stack frames in some cases; gcc does not reuse
+// the stack space.
+Message::Message() : ss_(new ::std::stringstream) {
+ // By default, we want there to be enough precision when printing
+ // a double to a Message.
+ *ss_ << std::setprecision(std::numeric_limits<double>::digits10 + 2);
+}
+
+// These two overloads allow streaming a wide C string to a Message
+// using the UTF-8 encoding.
+Message& Message::operator <<(const wchar_t* wide_c_str) {
+ return *this << internal::String::ShowWideCString(wide_c_str);
+}
+Message& Message::operator <<(wchar_t* wide_c_str) {
+ return *this << internal::String::ShowWideCString(wide_c_str);
+}
+
+#if GTEST_HAS_STD_WSTRING
+// Converts the given wide string to a narrow string using the UTF-8
+// encoding, and streams the result to this Message object.
+Message& Message::operator <<(const ::std::wstring& wstr) {
+ internal::StreamWideCharsToMessage(wstr.c_str(), wstr.length(), this);
+ return *this;
+}
+#endif // GTEST_HAS_STD_WSTRING
+
+#if GTEST_HAS_GLOBAL_WSTRING
+// Converts the given wide string to a narrow string using the UTF-8
+// encoding, and streams the result to this Message object.
+Message& Message::operator <<(const ::wstring& wstr) {
+ internal::StreamWideCharsToMessage(wstr.c_str(), wstr.length(), this);
+ return *this;
+}
+#endif // GTEST_HAS_GLOBAL_WSTRING
+
+// Gets the text streamed to this object so far as an std::string.
+// Each '\0' character in the buffer is replaced with "\\0".
+std::string Message::GetString() const {
+ return internal::StringStreamToString(ss_.get());
+}
+
+// AssertionResult constructors.
+// Used in EXPECT_TRUE/FALSE(assertion_result).
+AssertionResult::AssertionResult(const AssertionResult& other)
+ : success_(other.success_),
+ message_(other.message_.get() != NULL ?
+ new ::std::string(*other.message_) :
+ static_cast< ::std::string*>(NULL)) {
+}
+
+// Returns the assertion's negation. Used with EXPECT/ASSERT_FALSE.
+AssertionResult AssertionResult::operator!() const {
+ AssertionResult negation(!success_);
+ if (message_.get() != NULL)
+ negation << *message_;
+ return negation;
+}
+
+// Makes a successful assertion result.
+AssertionResult AssertionSuccess() {
+ return AssertionResult(true);
+}
+
+// Makes a failed assertion result.
+AssertionResult AssertionFailure() {
+ return AssertionResult(false);
+}
+
+// Makes a failed assertion result with the given failure message.
+// Deprecated; use AssertionFailure() << message.
+AssertionResult AssertionFailure(const Message& message) {
+ return AssertionFailure() << message;
+}
+
+namespace internal {
+
+// Constructs and returns the message for an equality assertion
+// (e.g. ASSERT_EQ, EXPECT_STREQ, etc) failure.
+//
+// The first four parameters are the expressions used in the assertion
+// and their values, as strings. For example, for ASSERT_EQ(foo, bar)
+// where foo is 5 and bar is 6, we have:
+//
+// expected_expression: "foo"
+// actual_expression: "bar"
+// expected_value: "5"
+// actual_value: "6"
+//
+// The ignoring_case parameter is true iff the assertion is a
+// *_STRCASEEQ*. When it's true, the string " (ignoring case)" will
+// be inserted into the message.
+AssertionResult EqFailure(const char* expected_expression,
+ const char* actual_expression,
+ const std::string& expected_value,
+ const std::string& actual_value,
+ bool ignoring_case) {
+ Message msg;
+ msg << "Value of: " << actual_expression;
+ if (actual_value != actual_expression) {
+ msg << "\n Actual: " << actual_value;
+ }
+
+ msg << "\nExpected: " << expected_expression;
+ if (ignoring_case) {
+ msg << " (ignoring case)";
+ }
+ if (expected_value != expected_expression) {
+ msg << "\nWhich is: " << expected_value;
+ }
+
+ return AssertionFailure() << msg;
+}
+
+// Constructs a failure message for Boolean assertions such as EXPECT_TRUE.
+std::string GetBoolAssertionFailureMessage(
+ const AssertionResult& assertion_result,
+ const char* expression_text,
+ const char* actual_predicate_value,
+ const char* expected_predicate_value) {
+ const char* actual_message = assertion_result.message();
+ Message msg;
+ msg << "Value of: " << expression_text
+ << "\n Actual: " << actual_predicate_value;
+ if (actual_message[0] != '\0')
+ msg << " (" << actual_message << ")";
+ msg << "\nExpected: " << expected_predicate_value;
+ return msg.GetString();
+}
+
+// Helper function for implementing ASSERT_NEAR.
+AssertionResult DoubleNearPredFormat(const char* expr1,
+ const char* expr2,
+ const char* abs_error_expr,
+ double val1,
+ double val2,
+ double abs_error) {
+ const double diff = fabs(val1 - val2);
+ if (diff <= abs_error) return AssertionSuccess();
+
+ // TODO(wan): do not print the value of an expression if it's
+ // already a literal.
+ return AssertionFailure()
+ << "The difference between " << expr1 << " and " << expr2
+ << " is " << diff << ", which exceeds " << abs_error_expr << ", where\n"
+ << expr1 << " evaluates to " << val1 << ",\n"
+ << expr2 << " evaluates to " << val2 << ", and\n"
+ << abs_error_expr << " evaluates to " << abs_error << ".";
+}
+
+
+// Helper template for implementing FloatLE() and DoubleLE().
+template <typename RawType>
+AssertionResult FloatingPointLE(const char* expr1,
+ const char* expr2,
+ RawType val1,
+ RawType val2) {
+ // Returns success if val1 is less than val2,
+ if (val1 < val2) {
+ return AssertionSuccess();
+ }
+
+ // or if val1 is almost equal to val2.
+ const FloatingPoint<RawType> lhs(val1), rhs(val2);
+ if (lhs.AlmostEquals(rhs)) {
+ return AssertionSuccess();
+ }
+
+ // Note that the above two checks will both fail if either val1 or
+ // val2 is NaN, as the IEEE floating-point standard requires that
+ // any predicate involving a NaN must return false.
+
+ ::std::stringstream val1_ss;
+ val1_ss << std::setprecision(std::numeric_limits<RawType>::digits10 + 2)
+ << val1;
+
+ ::std::stringstream val2_ss;
+ val2_ss << std::setprecision(std::numeric_limits<RawType>::digits10 + 2)
+ << val2;
+
+ return AssertionFailure()
+ << "Expected: (" << expr1 << ") <= (" << expr2 << ")\n"
+ << " Actual: " << StringStreamToString(&val1_ss) << " vs "
+ << StringStreamToString(&val2_ss);
+}
+
+} // namespace internal
+
+// Asserts that val1 is less than, or almost equal to, val2. Fails
+// otherwise. In particular, it fails if either val1 or val2 is NaN.
+AssertionResult FloatLE(const char* expr1, const char* expr2,
+ float val1, float val2) {
+ return internal::FloatingPointLE<float>(expr1, expr2, val1, val2);
+}
+
+// Asserts that val1 is less than, or almost equal to, val2. Fails
+// otherwise. In particular, it fails if either val1 or val2 is NaN.
+AssertionResult DoubleLE(const char* expr1, const char* expr2,
+ double val1, double val2) {
+ return internal::FloatingPointLE<double>(expr1, expr2, val1, val2);
+}
+
+namespace internal {
+
+// The helper function for {ASSERT|EXPECT}_EQ with int or enum
+// arguments.
+AssertionResult CmpHelperEQ(const char* expected_expression,
+ const char* actual_expression,
+ BiggestInt expected,
+ BiggestInt actual) {
+ if (expected == actual) {
+ return AssertionSuccess();
+ }
+
+ return EqFailure(expected_expression,
+ actual_expression,
+ FormatForComparisonFailureMessage(expected, actual),
+ FormatForComparisonFailureMessage(actual, expected),
+ false);
+}
+
+// A macro for implementing the helper functions needed to implement
+// ASSERT_?? and EXPECT_?? with integer or enum arguments. It is here
+// just to avoid copy-and-paste of similar code.
+#define GTEST_IMPL_CMP_HELPER_(op_name, op)\
+AssertionResult CmpHelper##op_name(const char* expr1, const char* expr2, \
+ BiggestInt val1, BiggestInt val2) {\
+ if (val1 op val2) {\
+ return AssertionSuccess();\
+ } else {\
+ return AssertionFailure() \
+ << "Expected: (" << expr1 << ") " #op " (" << expr2\
+ << "), actual: " << FormatForComparisonFailureMessage(val1, val2)\
+ << " vs " << FormatForComparisonFailureMessage(val2, val1);\
+ }\
+}
+
+// Implements the helper function for {ASSERT|EXPECT}_NE with int or
+// enum arguments.
+GTEST_IMPL_CMP_HELPER_(NE, !=)
+// Implements the helper function for {ASSERT|EXPECT}_LE with int or
+// enum arguments.
+GTEST_IMPL_CMP_HELPER_(LE, <=)
+// Implements the helper function for {ASSERT|EXPECT}_LT with int or
+// enum arguments.
+GTEST_IMPL_CMP_HELPER_(LT, < )
+// Implements the helper function for {ASSERT|EXPECT}_GE with int or
+// enum arguments.
+GTEST_IMPL_CMP_HELPER_(GE, >=)
+// Implements the helper function for {ASSERT|EXPECT}_GT with int or
+// enum arguments.
+GTEST_IMPL_CMP_HELPER_(GT, > )
+
+#undef GTEST_IMPL_CMP_HELPER_
+
+// The helper function for {ASSERT|EXPECT}_STREQ.
+AssertionResult CmpHelperSTREQ(const char* expected_expression,
+ const char* actual_expression,
+ const char* expected,
+ const char* actual) {
+ if (String::CStringEquals(expected, actual)) {
+ return AssertionSuccess();
+ }
+
+ return EqFailure(expected_expression,
+ actual_expression,
+ PrintToString(expected),
+ PrintToString(actual),
+ false);
+}
+
+// The helper function for {ASSERT|EXPECT}_STRCASEEQ.
+AssertionResult CmpHelperSTRCASEEQ(const char* expected_expression,
+ const char* actual_expression,
+ const char* expected,
+ const char* actual) {
+ if (String::CaseInsensitiveCStringEquals(expected, actual)) {
+ return AssertionSuccess();
+ }
+
+ return EqFailure(expected_expression,
+ actual_expression,
+ PrintToString(expected),
+ PrintToString(actual),
+ true);
+}
+
+// The helper function for {ASSERT|EXPECT}_STRNE.
+AssertionResult CmpHelperSTRNE(const char* s1_expression,
+ const char* s2_expression,
+ const char* s1,
+ const char* s2) {
+ if (!String::CStringEquals(s1, s2)) {
+ return AssertionSuccess();
+ } else {
+ return AssertionFailure() << "Expected: (" << s1_expression << ") != ("
+ << s2_expression << "), actual: \""
+ << s1 << "\" vs \"" << s2 << "\"";
+ }
+}
+
+// The helper function for {ASSERT|EXPECT}_STRCASENE.
+AssertionResult CmpHelperSTRCASENE(const char* s1_expression,
+ const char* s2_expression,
+ const char* s1,
+ const char* s2) {
+ if (!String::CaseInsensitiveCStringEquals(s1, s2)) {
+ return AssertionSuccess();
+ } else {
+ return AssertionFailure()
+ << "Expected: (" << s1_expression << ") != ("
+ << s2_expression << ") (ignoring case), actual: \""
+ << s1 << "\" vs \"" << s2 << "\"";
+ }
+}
+
+} // namespace internal
+
+namespace {
+
+// Helper functions for implementing IsSubString() and IsNotSubstring().
+
+// This group of overloaded functions return true iff needle is a
+// substring of haystack. NULL is considered a substring of itself
+// only.
+
+bool IsSubstringPred(const char* needle, const char* haystack) {
+ if (needle == NULL || haystack == NULL)
+ return needle == haystack;
+
+ return strstr(haystack, needle) != NULL;
+}
+
+bool IsSubstringPred(const wchar_t* needle, const wchar_t* haystack) {
+ if (needle == NULL || haystack == NULL)
+ return needle == haystack;
+
+ return wcsstr(haystack, needle) != NULL;
+}
+
+// StringType here can be either ::std::string or ::std::wstring.
+template <typename StringType>
+bool IsSubstringPred(const StringType& needle,
+ const StringType& haystack) {
+ return haystack.find(needle) != StringType::npos;
+}
+
+// This function implements either IsSubstring() or IsNotSubstring(),
+// depending on the value of the expected_to_be_substring parameter.
+// StringType here can be const char*, const wchar_t*, ::std::string,
+// or ::std::wstring.
+template <typename StringType>
+AssertionResult IsSubstringImpl(
+ bool expected_to_be_substring,
+ const char* needle_expr, const char* haystack_expr,
+ const StringType& needle, const StringType& haystack) {
+ if (IsSubstringPred(needle, haystack) == expected_to_be_substring)
+ return AssertionSuccess();
+
+ const bool is_wide_string = sizeof(needle[0]) > 1;
+ const char* const begin_string_quote = is_wide_string ? "L\"" : "\"";
+ return AssertionFailure()
+ << "Value of: " << needle_expr << "\n"
+ << " Actual: " << begin_string_quote << needle << "\"\n"
+ << "Expected: " << (expected_to_be_substring ? "" : "not ")
+ << "a substring of " << haystack_expr << "\n"
+ << "Which is: " << begin_string_quote << haystack << "\"";
+}
+
+} // namespace
+
+// IsSubstring() and IsNotSubstring() check whether needle is a
+// substring of haystack (NULL is considered a substring of itself
+// only), and return an appropriate error message when they fail.
+
+AssertionResult IsSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const char* needle, const char* haystack) {
+ return IsSubstringImpl(true, needle_expr, haystack_expr, needle, haystack);
+}
+
+AssertionResult IsSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const wchar_t* needle, const wchar_t* haystack) {
+ return IsSubstringImpl(true, needle_expr, haystack_expr, needle, haystack);
+}
+
+AssertionResult IsNotSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const char* needle, const char* haystack) {
+ return IsSubstringImpl(false, needle_expr, haystack_expr, needle, haystack);
+}
+
+AssertionResult IsNotSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const wchar_t* needle, const wchar_t* haystack) {
+ return IsSubstringImpl(false, needle_expr, haystack_expr, needle, haystack);
+}
+
+AssertionResult IsSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const ::std::string& needle, const ::std::string& haystack) {
+ return IsSubstringImpl(true, needle_expr, haystack_expr, needle, haystack);
+}
+
+AssertionResult IsNotSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const ::std::string& needle, const ::std::string& haystack) {
+ return IsSubstringImpl(false, needle_expr, haystack_expr, needle, haystack);
+}
+
+#if GTEST_HAS_STD_WSTRING
+AssertionResult IsSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const ::std::wstring& needle, const ::std::wstring& haystack) {
+ return IsSubstringImpl(true, needle_expr, haystack_expr, needle, haystack);
+}
+
+AssertionResult IsNotSubstring(
+ const char* needle_expr, const char* haystack_expr,
+ const ::std::wstring& needle, const ::std::wstring& haystack) {
+ return IsSubstringImpl(false, needle_expr, haystack_expr, needle, haystack);
+}
+#endif // GTEST_HAS_STD_WSTRING
+
+namespace internal {
+
+#if GTEST_OS_WINDOWS
+
+namespace {
+
+// Helper function for IsHRESULT{SuccessFailure} predicates
+AssertionResult HRESULTFailureHelper(const char* expr,
+ const char* expected,
+ long hr) { // NOLINT
+# if GTEST_OS_WINDOWS_MOBILE
+
+ // Windows CE doesn't support FormatMessage.
+ const char error_text[] = "";
+
+# else
+
+ // Looks up the human-readable system message for the HRESULT code
+ // and since we're not passing any params to FormatMessage, we don't
+ // want inserts expanded.
+ const DWORD kFlags = FORMAT_MESSAGE_FROM_SYSTEM |
+ FORMAT_MESSAGE_IGNORE_INSERTS;
+ const DWORD kBufSize = 4096;
+ // Gets the system's human readable message string for this HRESULT.
+ char error_text[kBufSize] = { '\0' };
+ DWORD message_length = ::FormatMessageA(kFlags,
+ 0, // no source, we're asking system
+ hr, // the error
+ 0, // no line width restrictions
+ error_text, // output buffer
+ kBufSize, // buf size
+ NULL); // no arguments for inserts
+ // Trims tailing white space (FormatMessage leaves a trailing CR-LF)
+ for (; message_length && IsSpace(error_text[message_length - 1]);
+ --message_length) {
+ error_text[message_length - 1] = '\0';
+ }
+
+# endif // GTEST_OS_WINDOWS_MOBILE
+
+ const std::string error_hex("0x" + String::FormatHexInt(hr));
+ return ::testing::AssertionFailure()
+ << "Expected: " << expr << " " << expected << ".\n"
+ << " Actual: " << error_hex << " " << error_text << "\n";
+}
+
+} // namespace
+
+AssertionResult IsHRESULTSuccess(const char* expr, long hr) { // NOLINT
+ if (SUCCEEDED(hr)) {
+ return AssertionSuccess();
+ }
+ return HRESULTFailureHelper(expr, "succeeds", hr);
+}
+
+AssertionResult IsHRESULTFailure(const char* expr, long hr) { // NOLINT
+ if (FAILED(hr)) {
+ return AssertionSuccess();
+ }
+ return HRESULTFailureHelper(expr, "fails", hr);
+}
+
+#endif // GTEST_OS_WINDOWS
+
+// Utility functions for encoding Unicode text (wide strings) in
+// UTF-8.
+
+// A Unicode code-point can have upto 21 bits, and is encoded in UTF-8
+// like this:
+//
+// Code-point length Encoding
+// 0 - 7 bits 0xxxxxxx
+// 8 - 11 bits 110xxxxx 10xxxxxx
+// 12 - 16 bits 1110xxxx 10xxxxxx 10xxxxxx
+// 17 - 21 bits 11110xxx 10xxxxxx 10xxxxxx 10xxxxxx
+
+// The maximum code-point a one-byte UTF-8 sequence can represent.
+const UInt32 kMaxCodePoint1 = (static_cast<UInt32>(1) << 7) - 1;
+
+// The maximum code-point a two-byte UTF-8 sequence can represent.
+const UInt32 kMaxCodePoint2 = (static_cast<UInt32>(1) << (5 + 6)) - 1;
+
+// The maximum code-point a three-byte UTF-8 sequence can represent.
+const UInt32 kMaxCodePoint3 = (static_cast<UInt32>(1) << (4 + 2*6)) - 1;
+
+// The maximum code-point a four-byte UTF-8 sequence can represent.
+const UInt32 kMaxCodePoint4 = (static_cast<UInt32>(1) << (3 + 3*6)) - 1;
+
+// Chops off the n lowest bits from a bit pattern. Returns the n
+// lowest bits. As a side effect, the original bit pattern will be
+// shifted to the right by n bits.
+inline UInt32 ChopLowBits(UInt32* bits, int n) {
+ const UInt32 low_bits = *bits & ((static_cast<UInt32>(1) << n) - 1);
+ *bits >>= n;
+ return low_bits;
+}
+
+// Converts a Unicode code point to a narrow string in UTF-8 encoding.
+// code_point parameter is of type UInt32 because wchar_t may not be
+// wide enough to contain a code point.
+// If the code_point is not a valid Unicode code point
+// (i.e. outside of Unicode range U+0 to U+10FFFF) it will be converted
+// to "(Invalid Unicode 0xXXXXXXXX)".
+std::string CodePointToUtf8(UInt32 code_point) {
+ if (code_point > kMaxCodePoint4) {
+ return "(Invalid Unicode 0x" + String::FormatHexInt(code_point) + ")";
+ }
+
+ char str[5]; // Big enough for the largest valid code point.
+ if (code_point <= kMaxCodePoint1) {
+ str[1] = '\0';
+ str[0] = static_cast<char>(code_point); // 0xxxxxxx
+ } else if (code_point <= kMaxCodePoint2) {
+ str[2] = '\0';
+ str[1] = static_cast<char>(0x80 | ChopLowBits(&code_point, 6)); // 10xxxxxx
+ str[0] = static_cast<char>(0xC0 | code_point); // 110xxxxx
+ } else if (code_point <= kMaxCodePoint3) {
+ str[3] = '\0';
+ str[2] = static_cast<char>(0x80 | ChopLowBits(&code_point, 6)); // 10xxxxxx
+ str[1] = static_cast<char>(0x80 | ChopLowBits(&code_point, 6)); // 10xxxxxx
+ str[0] = static_cast<char>(0xE0 | code_point); // 1110xxxx
+ } else { // code_point <= kMaxCodePoint4
+ str[4] = '\0';
+ str[3] = static_cast<char>(0x80 | ChopLowBits(&code_point, 6)); // 10xxxxxx
+ str[2] = static_cast<char>(0x80 | ChopLowBits(&code_point, 6)); // 10xxxxxx
+ str[1] = static_cast<char>(0x80 | ChopLowBits(&code_point, 6)); // 10xxxxxx
+ str[0] = static_cast<char>(0xF0 | code_point); // 11110xxx
+ }
+ return str;
+}
+
+// The following two functions only make sense if the the system
+// uses UTF-16 for wide string encoding. All supported systems
+// with 16 bit wchar_t (Windows, Cygwin, Symbian OS) do use UTF-16.
+
+// Determines if the arguments constitute UTF-16 surrogate pair
+// and thus should be combined into a single Unicode code point
+// using CreateCodePointFromUtf16SurrogatePair.
+inline bool IsUtf16SurrogatePair(wchar_t first, wchar_t second) {
+ return sizeof(wchar_t) == 2 &&
+ (first & 0xFC00) == 0xD800 && (second & 0xFC00) == 0xDC00;
+}
+
+// Creates a Unicode code point from UTF16 surrogate pair.
+inline UInt32 CreateCodePointFromUtf16SurrogatePair(wchar_t first,
+ wchar_t second) {
+ const UInt32 mask = (1 << 10) - 1;
+ return (sizeof(wchar_t) == 2) ?
+ (((first & mask) << 10) | (second & mask)) + 0x10000 :
+ // This function should not be called when the condition is
+ // false, but we provide a sensible default in case it is.
+ static_cast<UInt32>(first);
+}
+
+// Converts a wide string to a narrow string in UTF-8 encoding.
+// The wide string is assumed to have the following encoding:
+// UTF-16 if sizeof(wchar_t) == 2 (on Windows, Cygwin, Symbian OS)
+// UTF-32 if sizeof(wchar_t) == 4 (on Linux)
+// Parameter str points to a null-terminated wide string.
+// Parameter num_chars may additionally limit the number
+// of wchar_t characters processed. -1 is used when the entire string
+// should be processed.
+// If the string contains code points that are not valid Unicode code points
+// (i.e. outside of Unicode range U+0 to U+10FFFF) they will be output
+// as '(Invalid Unicode 0xXXXXXXXX)'. If the string is in UTF16 encoding
+// and contains invalid UTF-16 surrogate pairs, values in those pairs
+// will be encoded as individual Unicode characters from Basic Normal Plane.
+std::string WideStringToUtf8(const wchar_t* str, int num_chars) {
+ if (num_chars == -1)
+ num_chars = static_cast<int>(wcslen(str));
+
+ ::std::stringstream stream;
+ for (int i = 0; i < num_chars; ++i) {
+ UInt32 unicode_code_point;
+
+ if (str[i] == L'\0') {
+ break;
+ } else if (i + 1 < num_chars && IsUtf16SurrogatePair(str[i], str[i + 1])) {
+ unicode_code_point = CreateCodePointFromUtf16SurrogatePair(str[i],
+ str[i + 1]);
+ i++;
+ } else {
+ unicode_code_point = static_cast<UInt32>(str[i]);
+ }
+
+ stream << CodePointToUtf8(unicode_code_point);
+ }
+ return StringStreamToString(&stream);
+}
+
+// Converts a wide C string to an std::string using the UTF-8 encoding.
+// NULL will be converted to "(null)".
+std::string String::ShowWideCString(const wchar_t * wide_c_str) {
+ if (wide_c_str == NULL) return "(null)";
+
+ return internal::WideStringToUtf8(wide_c_str, -1);
+}
+
+// Compares two wide C strings. Returns true iff they have the same
+// content.
+//
+// Unlike wcscmp(), this function can handle NULL argument(s). A NULL
+// C string is considered different to any non-NULL C string,
+// including the empty string.
+bool String::WideCStringEquals(const wchar_t * lhs, const wchar_t * rhs) {
+ if (lhs == NULL) return rhs == NULL;
+
+ if (rhs == NULL) return false;
+
+ return wcscmp(lhs, rhs) == 0;
+}
+
+// Helper function for *_STREQ on wide strings.
+AssertionResult CmpHelperSTREQ(const char* expected_expression,
+ const char* actual_expression,
+ const wchar_t* expected,
+ const wchar_t* actual) {
+ if (String::WideCStringEquals(expected, actual)) {
+ return AssertionSuccess();
+ }
+
+ return EqFailure(expected_expression,
+ actual_expression,
+ PrintToString(expected),
+ PrintToString(actual),
+ false);
+}
+
+// Helper function for *_STRNE on wide strings.
+AssertionResult CmpHelperSTRNE(const char* s1_expression,
+ const char* s2_expression,
+ const wchar_t* s1,
+ const wchar_t* s2) {
+ if (!String::WideCStringEquals(s1, s2)) {
+ return AssertionSuccess();
+ }
+
+ return AssertionFailure() << "Expected: (" << s1_expression << ") != ("
+ << s2_expression << "), actual: "
+ << PrintToString(s1)
+ << " vs " << PrintToString(s2);
+}
+
+// Compares two C strings, ignoring case. Returns true iff they have
+// the same content.
+//
+// Unlike strcasecmp(), this function can handle NULL argument(s). A
+// NULL C string is considered different to any non-NULL C string,
+// including the empty string.
+bool String::CaseInsensitiveCStringEquals(const char * lhs, const char * rhs) {
+ if (lhs == NULL)
+ return rhs == NULL;
+ if (rhs == NULL)
+ return false;
+ return posix::StrCaseCmp(lhs, rhs) == 0;
+}
+
+ // Compares two wide C strings, ignoring case. Returns true iff they
+ // have the same content.
+ //
+ // Unlike wcscasecmp(), this function can handle NULL argument(s).
+ // A NULL C string is considered different to any non-NULL wide C string,
+ // including the empty string.
+ // NB: The implementations on different platforms slightly differ.
+ // On windows, this method uses _wcsicmp which compares according to LC_CTYPE
+ // environment variable. On GNU platform this method uses wcscasecmp
+ // which compares according to LC_CTYPE category of the current locale.
+ // On MacOS X, it uses towlower, which also uses LC_CTYPE category of the
+ // current locale.
+bool String::CaseInsensitiveWideCStringEquals(const wchar_t* lhs,
+ const wchar_t* rhs) {
+ if (lhs == NULL) return rhs == NULL;
+
+ if (rhs == NULL) return false;
+
+#if GTEST_OS_WINDOWS
+ return _wcsicmp(lhs, rhs) == 0;
+#elif GTEST_OS_LINUX && !GTEST_OS_LINUX_ANDROID
+ return wcscasecmp(lhs, rhs) == 0;
+#else
+ // Android, Mac OS X and Cygwin don't define wcscasecmp.
+ // Other unknown OSes may not define it either.
+ wint_t left, right;
+ do {
+ left = towlower(*lhs++);
+ right = towlower(*rhs++);
+ } while (left && left == right);
+ return left == right;
+#endif // OS selector
+}
+
+// Returns true iff str ends with the given suffix, ignoring case.
+// Any string is considered to end with an empty suffix.
+bool String::EndsWithCaseInsensitive(
+ const std::string& str, const std::string& suffix) {
+ const size_t str_len = str.length();
+ const size_t suffix_len = suffix.length();
+ return (str_len >= suffix_len) &&
+ CaseInsensitiveCStringEquals(str.c_str() + str_len - suffix_len,
+ suffix.c_str());
+}
+
+// Formats an int value as "%02d".
+std::string String::FormatIntWidth2(int value) {
+ std::stringstream ss;
+ ss << std::setfill('0') << std::setw(2) << value;
+ return ss.str();
+}
+
+// Formats an int value as "%X".
+std::string String::FormatHexInt(int value) {
+ std::stringstream ss;
+ ss << std::hex << std::uppercase << value;
+ return ss.str();
+}
+
+// Formats a byte as "%02X".
+std::string String::FormatByte(unsigned char value) {
+ std::stringstream ss;
+ ss << std::setfill('0') << std::setw(2) << std::hex << std::uppercase
+ << static_cast<unsigned int>(value);
+ return ss.str();
+}
+
+// Converts the buffer in a stringstream to an std::string, converting NUL
+// bytes to "\\0" along the way.
+std::string StringStreamToString(::std::stringstream* ss) {
+ const ::std::string& str = ss->str();
+ const char* const start = str.c_str();
+ const char* const end = start + str.length();
+
+ std::string result;
+ result.reserve(2 * (end - start));
+ for (const char* ch = start; ch != end; ++ch) {
+ if (*ch == '\0') {
+ result += "\\0"; // Replaces NUL with "\\0";
+ } else {
+ result += *ch;
+ }
+ }
+
+ return result;
+}
+
+// Appends the user-supplied message to the Google-Test-generated message.
+std::string AppendUserMessage(const std::string& gtest_msg,
+ const Message& user_msg) {
+ // Appends the user message if it's non-empty.
+ const std::string user_msg_string = user_msg.GetString();
+ if (user_msg_string.empty()) {
+ return gtest_msg;
+ }
+
+ return gtest_msg + "\n" + user_msg_string;
+}
+
+} // namespace internal
+
+// class TestResult
+
+// Creates an empty TestResult.
+TestResult::TestResult()
+ : death_test_count_(0),
+ elapsed_time_(0) {
+}
+
+// D'tor.
+TestResult::~TestResult() {
+}
+
+// Returns the i-th test part result among all the results. i can
+// range from 0 to total_part_count() - 1. If i is not in that range,
+// aborts the program.
+const TestPartResult& TestResult::GetTestPartResult(int i) const {
+ if (i < 0 || i >= total_part_count())
+ internal::posix::Abort();
+ return test_part_results_.at(i);
+}
+
+// Returns the i-th test property. i can range from 0 to
+// test_property_count() - 1. If i is not in that range, aborts the
+// program.
+const TestProperty& TestResult::GetTestProperty(int i) const {
+ if (i < 0 || i >= test_property_count())
+ internal::posix::Abort();
+ return test_properties_.at(i);
+}
+
+// Clears the test part results.
+void TestResult::ClearTestPartResults() {
+ test_part_results_.clear();
+}
+
+// Adds a test part result to the list.
+void TestResult::AddTestPartResult(const TestPartResult& test_part_result) {
+ test_part_results_.push_back(test_part_result);
+}
+
+// Adds a test property to the list. If a property with the same key as the
+// supplied property is already represented, the value of this test_property
+// replaces the old value for that key.
+void TestResult::RecordProperty(const std::string& xml_element,
+ const TestProperty& test_property) {
+ if (!ValidateTestProperty(xml_element, test_property)) {
+ return;
+ }
+ internal::MutexLock lock(&test_properites_mutex_);
+ const std::vector<TestProperty>::iterator property_with_matching_key =
+ std::find_if(test_properties_.begin(), test_properties_.end(),
+ internal::TestPropertyKeyIs(test_property.key()));
+ if (property_with_matching_key == test_properties_.end()) {
+ test_properties_.push_back(test_property);
+ return;
+ }
+ property_with_matching_key->SetValue(test_property.value());
+}
+
+// The list of reserved attributes used in the <testsuites> element of XML
+// output.
+static const char* const kReservedTestSuitesAttributes[] = {
+ "disabled",
+ "errors",
+ "failures",
+ "name",
+ "random_seed",
+ "tests",
+ "time",
+ "timestamp"
+};
+
+// The list of reserved attributes used in the <testsuite> element of XML
+// output.
+static const char* const kReservedTestSuiteAttributes[] = {
+ "disabled",
+ "errors",
+ "failures",
+ "name",
+ "tests",
+ "time"
+};
+
+// The list of reserved attributes used in the <testcase> element of XML output.
+static const char* const kReservedTestCaseAttributes[] = {
+ "classname",
+ "name",
+ "status",
+ "time",
+ "type_param",
+ "value_param"
+};
+
+template <int kSize>
+std::vector<std::string> ArrayAsVector(const char* const (&array)[kSize]) {
+ return std::vector<std::string>(array, array + kSize);
+}
+
+static std::vector<std::string> GetReservedAttributesForElement(
+ const std::string& xml_element) {
+ if (xml_element == "testsuites") {
+ return ArrayAsVector(kReservedTestSuitesAttributes);
+ } else if (xml_element == "testsuite") {
+ return ArrayAsVector(kReservedTestSuiteAttributes);
+ } else if (xml_element == "testcase") {
+ return ArrayAsVector(kReservedTestCaseAttributes);
+ } else {
+ GTEST_CHECK_(false) << "Unrecognized xml_element provided: " << xml_element;
+ }
+ // This code is unreachable but some compilers may not realizes that.
+ return std::vector<std::string>();
+}
+
+static std::string FormatWordList(const std::vector<std::string>& words) {
+ Message word_list;
+ for (size_t i = 0; i < words.size(); ++i) {
+ if (i > 0 && words.size() > 2) {
+ word_list << ", ";
+ }
+ if (i == words.size() - 1) {
+ word_list << "and ";
+ }
+ word_list << "'" << words[i] << "'";
+ }
+ return word_list.GetString();
+}
+
+bool ValidateTestPropertyName(const std::string& property_name,
+ const std::vector<std::string>& reserved_names) {
+ if (std::find(reserved_names.begin(), reserved_names.end(), property_name) !=
+ reserved_names.end()) {
+ ADD_FAILURE() << "Reserved key used in RecordProperty(): " << property_name
+ << " (" << FormatWordList(reserved_names)
+ << " are reserved by " << GTEST_NAME_ << ")";
+ return false;
+ }
+ return true;
+}
+
+// Adds a failure if the key is a reserved attribute of the element named
+// xml_element. Returns true if the property is valid.
+bool TestResult::ValidateTestProperty(const std::string& xml_element,
+ const TestProperty& test_property) {
+ return ValidateTestPropertyName(test_property.key(),
+ GetReservedAttributesForElement(xml_element));
+}
+
+// Clears the object.
+void TestResult::Clear() {
+ test_part_results_.clear();
+ test_properties_.clear();
+ death_test_count_ = 0;
+ elapsed_time_ = 0;
+}
+
+// Returns true iff the test failed.
+bool TestResult::Failed() const {
+ for (int i = 0; i < total_part_count(); ++i) {
+ if (GetTestPartResult(i).failed())
+ return true;
+ }
+ return false;
+}
+
+// Returns true iff the test part fatally failed.
+static bool TestPartFatallyFailed(const TestPartResult& result) {
+ return result.fatally_failed();
+}
+
+// Returns true iff the test fatally failed.
+bool TestResult::HasFatalFailure() const {
+ return CountIf(test_part_results_, TestPartFatallyFailed) > 0;
+}
+
+// Returns true iff the test part non-fatally failed.
+static bool TestPartNonfatallyFailed(const TestPartResult& result) {
+ return result.nonfatally_failed();
+}
+
+// Returns true iff the test has a non-fatal failure.
+bool TestResult::HasNonfatalFailure() const {
+ return CountIf(test_part_results_, TestPartNonfatallyFailed) > 0;
+}
+
+// Gets the number of all test parts. This is the sum of the number
+// of successful test parts and the number of failed test parts.
+int TestResult::total_part_count() const {
+ return static_cast<int>(test_part_results_.size());
+}
+
+// Returns the number of the test properties.
+int TestResult::test_property_count() const {
+ return static_cast<int>(test_properties_.size());
+}
+
+// class Test
+
+// Creates a Test object.
+
+// The c'tor saves the values of all Google Test flags.
+Test::Test()
+ : gtest_flag_saver_(new internal::GTestFlagSaver) {
+}
+
+// The d'tor restores the values of all Google Test flags.
+Test::~Test() {
+ delete gtest_flag_saver_;
+}
+
+// Sets up the test fixture.
+//
+// A sub-class may override this.
+void Test::SetUp() {
+}
+
+// Tears down the test fixture.
+//
+// A sub-class may override this.
+void Test::TearDown() {
+}
+
+// Allows user supplied key value pairs to be recorded for later output.
+void Test::RecordProperty(const std::string& key, const std::string& value) {
+ UnitTest::GetInstance()->RecordProperty(key, value);
+}
+
+// Allows user supplied key value pairs to be recorded for later output.
+void Test::RecordProperty(const std::string& key, int value) {
+ Message value_message;
+ value_message << value;
+ RecordProperty(key, value_message.GetString().c_str());
+}
+
+namespace internal {
+
+void ReportFailureInUnknownLocation(TestPartResult::Type result_type,
+ const std::string& message) {
+ // This function is a friend of UnitTest and as such has access to
+ // AddTestPartResult.
+ UnitTest::GetInstance()->AddTestPartResult(
+ result_type,
+ NULL, // No info about the source file where the exception occurred.
+ -1, // We have no info on which line caused the exception.
+ message,
+ ""); // No stack trace, either.
+}
+
+} // namespace internal
+
+// Google Test requires all tests in the same test case to use the same test
+// fixture class. This function checks if the current test has the
+// same fixture class as the first test in the current test case. If
+// yes, it returns true; otherwise it generates a Google Test failure and
+// returns false.
+bool Test::HasSameFixtureClass() {
+ internal::UnitTestImpl* const impl = internal::GetUnitTestImpl();
+ const TestCase* const test_case = impl->current_test_case();
+
+ // Info about the first test in the current test case.
+ const TestInfo* const first_test_info = test_case->test_info_list()[0];
+ const internal::TypeId first_fixture_id = first_test_info->fixture_class_id_;
+ const char* const first_test_name = first_test_info->name();
+
+ // Info about the current test.
+ const TestInfo* const this_test_info = impl->current_test_info();
+ const internal::TypeId this_fixture_id = this_test_info->fixture_class_id_;
+ const char* const this_test_name = this_test_info->name();
+
+ if (this_fixture_id != first_fixture_id) {
+ // Is the first test defined using TEST?
+ const bool first_is_TEST = first_fixture_id == internal::GetTestTypeId();
+ // Is this test defined using TEST?
+ const bool this_is_TEST = this_fixture_id == internal::GetTestTypeId();
+
+ if (first_is_TEST || this_is_TEST) {
+ // The user mixed TEST and TEST_F in this test case - we'll tell
+ // him/her how to fix it.
+
+ // Gets the name of the TEST and the name of the TEST_F. Note
+ // that first_is_TEST and this_is_TEST cannot both be true, as
+ // the fixture IDs are different for the two tests.
+ const char* const TEST_name =
+ first_is_TEST ? first_test_name : this_test_name;
+ const char* const TEST_F_name =
+ first_is_TEST ? this_test_name : first_test_name;
+
+ ADD_FAILURE()
+ << "All tests in the same test case must use the same test fixture\n"
+ << "class, so mixing TEST_F and TEST in the same test case is\n"
+ << "illegal. In test case " << this_test_info->test_case_name()
+ << ",\n"
+ << "test " << TEST_F_name << " is defined using TEST_F but\n"
+ << "test " << TEST_name << " is defined using TEST. You probably\n"
+ << "want to change the TEST to TEST_F or move it to another test\n"
+ << "case.";
+ } else {
+ // The user defined two fixture classes with the same name in
+ // two namespaces - we'll tell him/her how to fix it.
+ ADD_FAILURE()
+ << "All tests in the same test case must use the same test fixture\n"
+ << "class. However, in test case "
+ << this_test_info->test_case_name() << ",\n"
+ << "you defined test " << first_test_name
+ << " and test " << this_test_name << "\n"
+ << "using two different test fixture classes. This can happen if\n"
+ << "the two classes are from different namespaces or translation\n"
+ << "units and have the same name. You should probably rename one\n"
+ << "of the classes to put the tests into different test cases.";
+ }
+ return false;
+ }
+
+ return true;
+}
+
+#if GTEST_HAS_SEH
+
+// Adds an "exception thrown" fatal failure to the current test. This
+// function returns its result via an output parameter pointer because VC++
+// prohibits creation of objects with destructors on stack in functions
+// using __try (see error C2712).
+static std::string* FormatSehExceptionMessage(DWORD exception_code,
+ const char* location) {
+ Message message;
+ message << "SEH exception with code 0x" << std::setbase(16) <<
+ exception_code << std::setbase(10) << " thrown in " << location << ".";
+
+ return new std::string(message.GetString());
+}
+
+#endif // GTEST_HAS_SEH
+
+namespace internal {
+
+#if GTEST_HAS_EXCEPTIONS
+
+// Adds an "exception thrown" fatal failure to the current test.
+static std::string FormatCxxExceptionMessage(const char* description,
+ const char* location) {
+ Message message;
+ if (description != NULL) {
+ message << "C++ exception with description \"" << description << "\"";
+ } else {
+ message << "Unknown C++ exception";
+ }
+ message << " thrown in " << location << ".";
+
+ return message.GetString();
+}
+
+static std::string PrintTestPartResultToString(
+ const TestPartResult& test_part_result);
+
+GoogleTestFailureException::GoogleTestFailureException(
+ const TestPartResult& failure)
+ : ::std::runtime_error(PrintTestPartResultToString(failure).c_str()) {}
+
+#endif // GTEST_HAS_EXCEPTIONS
+
+// We put these helper functions in the internal namespace as IBM's xlC
+// compiler rejects the code if they were declared static.
+
+// Runs the given method and handles SEH exceptions it throws, when
+// SEH is supported; returns the 0-value for type Result in case of an
+// SEH exception. (Microsoft compilers cannot handle SEH and C++
+// exceptions in the same function. Therefore, we provide a separate
+// wrapper function for handling SEH exceptions.)
+template <class T, typename Result>
+Result HandleSehExceptionsInMethodIfSupported(
+ T* object, Result (T::*method)(), const char* location) {
+#if GTEST_HAS_SEH
+ __try {
+ return (object->*method)();
+ } __except (internal::UnitTestOptions::GTestShouldProcessSEH( // NOLINT
+ GetExceptionCode())) {
+ // We create the exception message on the heap because VC++ prohibits
+ // creation of objects with destructors on stack in functions using __try
+ // (see error C2712).
+ std::string* exception_message = FormatSehExceptionMessage(
+ GetExceptionCode(), location);
+ internal::ReportFailureInUnknownLocation(TestPartResult::kFatalFailure,
+ *exception_message);
+ delete exception_message;
+ return static_cast<Result>(0);
+ }
+#else
+ (void)location;
+ return (object->*method)();
+#endif // GTEST_HAS_SEH
+}
+
+// Runs the given method and catches and reports C++ and/or SEH-style
+// exceptions, if they are supported; returns the 0-value for type
+// Result in case of an SEH exception.
+template <class T, typename Result>
+Result HandleExceptionsInMethodIfSupported(
+ T* object, Result (T::*method)(), const char* location) {
+ // NOTE: The user code can affect the way in which Google Test handles
+ // exceptions by setting GTEST_FLAG(catch_exceptions), but only before
+ // RUN_ALL_TESTS() starts. It is technically possible to check the flag
+ // after the exception is caught and either report or re-throw the
+ // exception based on the flag's value:
+ //
+ // try {
+ // // Perform the test method.
+ // } catch (...) {
+ // if (GTEST_FLAG(catch_exceptions))
+ // // Report the exception as failure.
+ // else
+ // throw; // Re-throws the original exception.
+ // }
+ //
+ // However, the purpose of this flag is to allow the program to drop into
+ // the debugger when the exception is thrown. On most platforms, once the
+ // control enters the catch block, the exception origin information is
+ // lost and the debugger will stop the program at the point of the
+ // re-throw in this function -- instead of at the point of the original
+ // throw statement in the code under test. For this reason, we perform
+ // the check early, sacrificing the ability to affect Google Test's
+ // exception handling in the method where the exception is thrown.
+ if (internal::GetUnitTestImpl()->catch_exceptions()) {
+#if GTEST_HAS_EXCEPTIONS
+ try {
+ return HandleSehExceptionsInMethodIfSupported(object, method, location);
+ } catch (const internal::GoogleTestFailureException&) { // NOLINT
+ // This exception type can only be thrown by a failed Google
+ // Test assertion with the intention of letting another testing
+ // framework catch it. Therefore we just re-throw it.
+ throw;
+ } catch (const std::exception& e) { // NOLINT
+ internal::ReportFailureInUnknownLocation(
+ TestPartResult::kFatalFailure,
+ FormatCxxExceptionMessage(e.what(), location));
+ } catch (...) { // NOLINT
+ internal::ReportFailureInUnknownLocation(
+ TestPartResult::kFatalFailure,
+ FormatCxxExceptionMessage(NULL, location));
+ }
+ return static_cast<Result>(0);
+#else
+ return HandleSehExceptionsInMethodIfSupported(object, method, location);
+#endif // GTEST_HAS_EXCEPTIONS
+ } else {
+ return (object->*method)();
+ }
+}
+
+} // namespace internal
+
+// Runs the test and updates the test result.
+void Test::Run() {
+ if (!HasSameFixtureClass()) return;
+
+ internal::UnitTestImpl* const impl = internal::GetUnitTestImpl();
+ impl->os_stack_trace_getter()->UponLeavingGTest();
+ internal::HandleExceptionsInMethodIfSupported(this, &Test::SetUp, "SetUp()");
+ // We will run the test only if SetUp() was successful.
+ if (!HasFatalFailure()) {
+ impl->os_stack_trace_getter()->UponLeavingGTest();
+ internal::HandleExceptionsInMethodIfSupported(
+ this, &Test::TestBody, "the test body");
+ }
+
+ // However, we want to clean up as much as possible. Hence we will
+ // always call TearDown(), even if SetUp() or the test body has
+ // failed.
+ impl->os_stack_trace_getter()->UponLeavingGTest();
+ internal::HandleExceptionsInMethodIfSupported(
+ this, &Test::TearDown, "TearDown()");
+}
+
+// Returns true iff the current test has a fatal failure.
+bool Test::HasFatalFailure() {
+ return internal::GetUnitTestImpl()->current_test_result()->HasFatalFailure();
+}
+
+// Returns true iff the current test has a non-fatal failure.
+bool Test::HasNonfatalFailure() {
+ return internal::GetUnitTestImpl()->current_test_result()->
+ HasNonfatalFailure();
+}
+
+// class TestInfo
+
+// Constructs a TestInfo object. It assumes ownership of the test factory
+// object.
+TestInfo::TestInfo(const std::string& a_test_case_name,
+ const std::string& a_name,
+ const char* a_type_param,
+ const char* a_value_param,
+ internal::TypeId fixture_class_id,
+ internal::TestFactoryBase* factory)
+ : test_case_name_(a_test_case_name),
+ name_(a_name),
+ type_param_(a_type_param ? new std::string(a_type_param) : NULL),
+ value_param_(a_value_param ? new std::string(a_value_param) : NULL),
+ fixture_class_id_(fixture_class_id),
+ should_run_(false),
+ is_disabled_(false),
+ matches_filter_(false),
+ factory_(factory),
+ result_() {}
+
+// Destructs a TestInfo object.
+TestInfo::~TestInfo() { delete factory_; }
+
+namespace internal {
+
+// Creates a new TestInfo object and registers it with Google Test;
+// returns the created object.
+//
+// Arguments:
+//
+// test_case_name: name of the test case
+// name: name of the test
+// type_param: the name of the test's type parameter, or NULL if
+// this is not a typed or a type-parameterized test.
+// value_param: text representation of the test's value parameter,
+// or NULL if this is not a value-parameterized test.
+// fixture_class_id: ID of the test fixture class
+// set_up_tc: pointer to the function that sets up the test case
+// tear_down_tc: pointer to the function that tears down the test case
+// factory: pointer to the factory that creates a test object.
+// The newly created TestInfo instance will assume
+// ownership of the factory object.
+TestInfo* MakeAndRegisterTestInfo(
+ const char* test_case_name,
+ const char* name,
+ const char* type_param,
+ const char* value_param,
+ TypeId fixture_class_id,
+ SetUpTestCaseFunc set_up_tc,
+ TearDownTestCaseFunc tear_down_tc,
+ TestFactoryBase* factory) {
+ TestInfo* const test_info =
+ new TestInfo(test_case_name, name, type_param, value_param,
+ fixture_class_id, factory);
+ GetUnitTestImpl()->AddTestInfo(set_up_tc, tear_down_tc, test_info);
+ return test_info;
+}
+
+#if GTEST_HAS_PARAM_TEST
+void ReportInvalidTestCaseType(const char* test_case_name,
+ const char* file, int line) {
+ Message errors;
+ errors
+ << "Attempted redefinition of test case " << test_case_name << ".\n"
+ << "All tests in the same test case must use the same test fixture\n"
+ << "class. However, in test case " << test_case_name << ", you tried\n"
+ << "to define a test using a fixture class different from the one\n"
+ << "used earlier. This can happen if the two fixture classes are\n"
+ << "from different namespaces and have the same name. You should\n"
+ << "probably rename one of the classes to put the tests into different\n"
+ << "test cases.";
+
+ fprintf(stderr, "%s %s", FormatFileLocation(file, line).c_str(),
+ errors.GetString().c_str());
+}
+#endif // GTEST_HAS_PARAM_TEST
+
+} // namespace internal
+
+namespace {
+
+// A predicate that checks the test name of a TestInfo against a known
+// value.
+//
+// This is used for implementation of the TestCase class only. We put
+// it in the anonymous namespace to prevent polluting the outer
+// namespace.
+//
+// TestNameIs is copyable.
+class TestNameIs {
+ public:
+ // Constructor.
+ //
+ // TestNameIs has NO default constructor.
+ explicit TestNameIs(const char* name)
+ : name_(name) {}
+
+ // Returns true iff the test name of test_info matches name_.
+ bool operator()(const TestInfo * test_info) const {
+ return test_info && test_info->name() == name_;
+ }
+
+ private:
+ std::string name_;
+};
+
+} // namespace
+
+namespace internal {
+
+// This method expands all parameterized tests registered with macros TEST_P
+// and INSTANTIATE_TEST_CASE_P into regular tests and registers those.
+// This will be done just once during the program runtime.
+void UnitTestImpl::RegisterParameterizedTests() {
+#if GTEST_HAS_PARAM_TEST
+ if (!parameterized_tests_registered_) {
+ parameterized_test_registry_.RegisterTests();
+ parameterized_tests_registered_ = true;
+ }
+#endif
+}
+
+} // namespace internal
+
+// Creates the test object, runs it, records its result, and then
+// deletes it.
+void TestInfo::Run() {
+ if (!should_run_) return;
+
+ // Tells UnitTest where to store test result.
+ internal::UnitTestImpl* const impl = internal::GetUnitTestImpl();
+ impl->set_current_test_info(this);
+
+ TestEventListener* repeater = UnitTest::GetInstance()->listeners().repeater();
+
+ // Notifies the unit test event listeners that a test is about to start.
+ repeater->OnTestStart(*this);
+
+ const TimeInMillis start = internal::GetTimeInMillis();
+
+ impl->os_stack_trace_getter()->UponLeavingGTest();
+
+ // Creates the test object.
+ Test* const test = internal::HandleExceptionsInMethodIfSupported(
+ factory_, &internal::TestFactoryBase::CreateTest,
+ "the test fixture's constructor");
+
+ // Runs the test only if the test object was created and its
+ // constructor didn't generate a fatal failure.
+ if ((test != NULL) && !Test::HasFatalFailure()) {
+ // This doesn't throw as all user code that can throw are wrapped into
+ // exception handling code.
+ test->Run();
+ }
+
+ // Deletes the test object.
+ impl->os_stack_trace_getter()->UponLeavingGTest();
+ internal::HandleExceptionsInMethodIfSupported(
+ test, &Test::DeleteSelf_, "the test fixture's destructor");
+
+ result_.set_elapsed_time(internal::GetTimeInMillis() - start);
+
+ // Notifies the unit test event listener that a test has just finished.
+ repeater->OnTestEnd(*this);
+
+ // Tells UnitTest to stop associating assertion results to this
+ // test.
+ impl->set_current_test_info(NULL);
+}
+
+// class TestCase
+
+// Gets the number of successful tests in this test case.
+int TestCase::successful_test_count() const {
+ return CountIf(test_info_list_, TestPassed);
+}
+
+// Gets the number of failed tests in this test case.
+int TestCase::failed_test_count() const {
+ return CountIf(test_info_list_, TestFailed);
+}
+
+// Gets the number of disabled tests that will be reported in the XML report.
+int TestCase::reportable_disabled_test_count() const {
+ return CountIf(test_info_list_, TestReportableDisabled);
+}
+
+// Gets the number of disabled tests in this test case.
+int TestCase::disabled_test_count() const {
+ return CountIf(test_info_list_, TestDisabled);
+}
+
+// Gets the number of tests to be printed in the XML report.
+int TestCase::reportable_test_count() const {
+ return CountIf(test_info_list_, TestReportable);
+}
+
+// Get the number of tests in this test case that should run.
+int TestCase::test_to_run_count() const {
+ return CountIf(test_info_list_, ShouldRunTest);
+}
+
+// Gets the number of all tests.
+int TestCase::total_test_count() const {
+ return static_cast<int>(test_info_list_.size());
+}
+
+// Creates a TestCase with the given name.
+//
+// Arguments:
+//
+// name: name of the test case
+// a_type_param: the name of the test case's type parameter, or NULL if
+// this is not a typed or a type-parameterized test case.
+// set_up_tc: pointer to the function that sets up the test case
+// tear_down_tc: pointer to the function that tears down the test case
+TestCase::TestCase(const char* a_name, const char* a_type_param,
+ Test::SetUpTestCaseFunc set_up_tc,
+ Test::TearDownTestCaseFunc tear_down_tc)
+ : name_(a_name),
+ type_param_(a_type_param ? new std::string(a_type_param) : NULL),
+ set_up_tc_(set_up_tc),
+ tear_down_tc_(tear_down_tc),
+ should_run_(false),
+ elapsed_time_(0) {
+}
+
+// Destructor of TestCase.
+TestCase::~TestCase() {
+ // Deletes every Test in the collection.
+ ForEach(test_info_list_, internal::Delete<TestInfo>);
+}
+
+// Returns the i-th test among all the tests. i can range from 0 to
+// total_test_count() - 1. If i is not in that range, returns NULL.
+const TestInfo* TestCase::GetTestInfo(int i) const {
+ const int index = GetElementOr(test_indices_, i, -1);
+ return index < 0 ? NULL : test_info_list_[index];
+}
+
+// Returns the i-th test among all the tests. i can range from 0 to
+// total_test_count() - 1. If i is not in that range, returns NULL.
+TestInfo* TestCase::GetMutableTestInfo(int i) {
+ const int index = GetElementOr(test_indices_, i, -1);
+ return index < 0 ? NULL : test_info_list_[index];
+}
+
+// Adds a test to this test case. Will delete the test upon
+// destruction of the TestCase object.
+void TestCase::AddTestInfo(TestInfo * test_info) {
+ test_info_list_.push_back(test_info);
+ test_indices_.push_back(static_cast<int>(test_indices_.size()));
+}
+
+// Runs every test in this TestCase.
+void TestCase::Run() {
+ if (!should_run_) return;
+
+ internal::UnitTestImpl* const impl = internal::GetUnitTestImpl();
+ impl->set_current_test_case(this);
+
+ TestEventListener* repeater = UnitTest::GetInstance()->listeners().repeater();
+
+ repeater->OnTestCaseStart(*this);
+ impl->os_stack_trace_getter()->UponLeavingGTest();
+ internal::HandleExceptionsInMethodIfSupported(
+ this, &TestCase::RunSetUpTestCase, "SetUpTestCase()");
+
+ const internal::TimeInMillis start = internal::GetTimeInMillis();
+ for (int i = 0; i < total_test_count(); i++) {
+ GetMutableTestInfo(i)->Run();
+ }
+ elapsed_time_ = internal::GetTimeInMillis() - start;
+
+ impl->os_stack_trace_getter()->UponLeavingGTest();
+ internal::HandleExceptionsInMethodIfSupported(
+ this, &TestCase::RunTearDownTestCase, "TearDownTestCase()");
+
+ repeater->OnTestCaseEnd(*this);
+ impl->set_current_test_case(NULL);
+}
+
+// Clears the results of all tests in this test case.
+void TestCase::ClearResult() {
+ ad_hoc_test_result_.Clear();
+ ForEach(test_info_list_, TestInfo::ClearTestResult);
+}
+
+// Shuffles the tests in this test case.
+void TestCase::ShuffleTests(internal::Random* random) {
+ Shuffle(random, &test_indices_);
+}
+
+// Restores the test order to before the first shuffle.
+void TestCase::UnshuffleTests() {
+ for (size_t i = 0; i < test_indices_.size(); i++) {
+ test_indices_[i] = static_cast<int>(i);
+ }
+}
+
+// Formats a countable noun. Depending on its quantity, either the
+// singular form or the plural form is used. e.g.
+//
+// FormatCountableNoun(1, "formula", "formuli") returns "1 formula".
+// FormatCountableNoun(5, "book", "books") returns "5 books".
+static std::string FormatCountableNoun(int count,
+ const char * singular_form,
+ const char * plural_form) {
+ return internal::StreamableToString(count) + " " +
+ (count == 1 ? singular_form : plural_form);
+}
+
+// Formats the count of tests.
+static std::string FormatTestCount(int test_count) {
+ return FormatCountableNoun(test_count, "test", "tests");
+}
+
+// Formats the count of test cases.
+static std::string FormatTestCaseCount(int test_case_count) {
+ return FormatCountableNoun(test_case_count, "test case", "test cases");
+}
+
+// Converts a TestPartResult::Type enum to human-friendly string
+// representation. Both kNonFatalFailure and kFatalFailure are translated
+// to "Failure", as the user usually doesn't care about the difference
+// between the two when viewing the test result.
+static const char * TestPartResultTypeToString(TestPartResult::Type type) {
+ switch (type) {
+ case TestPartResult::kSuccess:
+ return "Success";
+
+ case TestPartResult::kNonFatalFailure:
+ case TestPartResult::kFatalFailure:
+#ifdef _MSC_VER
+ return "error: ";
+#else
+ return "Failure\n";
+#endif
+ default:
+ return "Unknown result type";
+ }
+}
+
+namespace internal {
+
+// Prints a TestPartResult to an std::string.
+static std::string PrintTestPartResultToString(
+ const TestPartResult& test_part_result) {
+ return (Message()
+ << internal::FormatFileLocation(test_part_result.file_name(),
+ test_part_result.line_number())
+ << " " << TestPartResultTypeToString(test_part_result.type())
+ << test_part_result.message()).GetString();
+}
+
+// Prints a TestPartResult.
+static void PrintTestPartResult(const TestPartResult& test_part_result) {
+ const std::string& result =
+ PrintTestPartResultToString(test_part_result);
+ printf("%s\n", result.c_str());
+ fflush(stdout);
+ // If the test program runs in Visual Studio or a debugger, the
+ // following statements add the test part result message to the Output
+ // window such that the user can double-click on it to jump to the
+ // corresponding source code location; otherwise they do nothing.
+#if GTEST_OS_WINDOWS && !GTEST_OS_WINDOWS_MOBILE
+ // We don't call OutputDebugString*() on Windows Mobile, as printing
+ // to stdout is done by OutputDebugString() there already - we don't
+ // want the same message printed twice.
+ ::OutputDebugStringA(result.c_str());
+ ::OutputDebugStringA("\n");
+#endif
+}
+
+// class PrettyUnitTestResultPrinter
+
+enum GTestColor {
+ COLOR_DEFAULT,
+ COLOR_RED,
+ COLOR_GREEN,
+ COLOR_YELLOW
+};
+
+#if GTEST_OS_WINDOWS && !GTEST_OS_WINDOWS_MOBILE
+
+// Returns the character attribute for the given color.
+WORD GetColorAttribute(GTestColor color) {
+ switch (color) {
+ case COLOR_RED: return FOREGROUND_RED;
+ case COLOR_GREEN: return FOREGROUND_GREEN;
+ case COLOR_YELLOW: return FOREGROUND_RED | FOREGROUND_GREEN;
+ default: return 0;
+ }
+}
+
+#else
+
+// Returns the ANSI color code for the given color. COLOR_DEFAULT is
+// an invalid input.
+const char* GetAnsiColorCode(GTestColor color) {
+ switch (color) {
+ case COLOR_RED: return "1";
+ case COLOR_GREEN: return "2";
+ case COLOR_YELLOW: return "3";
+ default: return NULL;
+ };
+}
+
+#endif // GTEST_OS_WINDOWS && !GTEST_OS_WINDOWS_MOBILE
+
+// Returns true iff Google Test should use colors in the output.
+bool ShouldUseColor(bool stdout_is_tty) {
+ const char* const gtest_color = GTEST_FLAG(color).c_str();
+
+ if (String::CaseInsensitiveCStringEquals(gtest_color, "auto")) {
+#if GTEST_OS_WINDOWS
+ // On Windows the TERM variable is usually not set, but the
+ // console there does support colors.
+ return stdout_is_tty;
+#else
+ // On non-Windows platforms, we rely on the TERM variable.
+ const char* const term = posix::GetEnv("TERM");
+ const bool term_supports_color =
+ String::CStringEquals(term, "xterm") ||
+ String::CStringEquals(term, "xterm-color") ||
+ String::CStringEquals(term, "xterm-256color") ||
+ String::CStringEquals(term, "screen") ||
+ String::CStringEquals(term, "screen-256color") ||
+ String::CStringEquals(term, "linux") ||
+ String::CStringEquals(term, "cygwin");
+ return stdout_is_tty && term_supports_color;
+#endif // GTEST_OS_WINDOWS
+ }
+
+ return String::CaseInsensitiveCStringEquals(gtest_color, "yes") ||
+ String::CaseInsensitiveCStringEquals(gtest_color, "true") ||
+ String::CaseInsensitiveCStringEquals(gtest_color, "t") ||
+ String::CStringEquals(gtest_color, "1");
+ // We take "yes", "true", "t", and "1" as meaning "yes". If the
+ // value is neither one of these nor "auto", we treat it as "no" to
+ // be conservative.
+}
+
+// Helpers for printing colored strings to stdout. Note that on Windows, we
+// cannot simply emit special characters and have the terminal change colors.
+// This routine must actually emit the characters rather than return a string
+// that would be colored when printed, as can be done on Linux.
+void ColoredPrintf(GTestColor color, const char* fmt, ...) {
+ va_list args;
+ va_start(args, fmt);
+
+#if GTEST_OS_WINDOWS_MOBILE || GTEST_OS_SYMBIAN || GTEST_OS_ZOS || GTEST_OS_IOS
+ const bool use_color = false;
+#else
+ static const bool in_color_mode =
+ ShouldUseColor(posix::IsATTY(posix::FileNo(stdout)) != 0);
+ const bool use_color = in_color_mode && (color != COLOR_DEFAULT);
+#endif // GTEST_OS_WINDOWS_MOBILE || GTEST_OS_SYMBIAN || GTEST_OS_ZOS
+ // The '!= 0' comparison is necessary to satisfy MSVC 7.1.
+
+ if (!use_color) {
+ vprintf(fmt, args);
+ va_end(args);
+ return;
+ }
+
+#if GTEST_OS_WINDOWS && !GTEST_OS_WINDOWS_MOBILE
+ const HANDLE stdout_handle = GetStdHandle(STD_OUTPUT_HANDLE);
+
+ // Gets the current text color.
+ CONSOLE_SCREEN_BUFFER_INFO buffer_info;
+ GetConsoleScreenBufferInfo(stdout_handle, &buffer_info);
+ const WORD old_color_attrs = buffer_info.wAttributes;
+
+ // We need to flush the stream buffers into the console before each
+ // SetConsoleTextAttribute call lest it affect the text that is already
+ // printed but has not yet reached the console.
+ fflush(stdout);
+ SetConsoleTextAttribute(stdout_handle,
+ GetColorAttribute(color) | FOREGROUND_INTENSITY);
+ vprintf(fmt, args);
+
+ fflush(stdout);
+ // Restores the text color.
+ SetConsoleTextAttribute(stdout_handle, old_color_attrs);
+#else
+ printf("\033[0;3%sm", GetAnsiColorCode(color));
+ vprintf(fmt, args);
+ printf("\033[m"); // Resets the terminal to default.
+#endif // GTEST_OS_WINDOWS && !GTEST_OS_WINDOWS_MOBILE
+ va_end(args);
+}
+
+// Text printed in Google Test's text output and --gunit_list_tests
+// output to label the type parameter and value parameter for a test.
+static const char kTypeParamLabel[] = "TypeParam";
+static const char kValueParamLabel[] = "GetParam()";
+
+void PrintFullTestCommentIfPresent(const TestInfo& test_info) {
+ const char* const type_param = test_info.type_param();
+ const char* const value_param = test_info.value_param();
+
+ if (type_param != NULL || value_param != NULL) {
+ printf(", where ");
+ if (type_param != NULL) {
+ printf("%s = %s", kTypeParamLabel, type_param);
+ if (value_param != NULL)
+ printf(" and ");
+ }
+ if (value_param != NULL) {
+ printf("%s = %s", kValueParamLabel, value_param);
+ }
+ }
+}
+
+// This class implements the TestEventListener interface.
+//
+// Class PrettyUnitTestResultPrinter is copyable.
+class PrettyUnitTestResultPrinter : public TestEventListener {
+ public:
+ PrettyUnitTestResultPrinter() {}
+ static void PrintTestName(const char * test_case, const char * test) {
+ printf("%s.%s", test_case, test);
+ }
+
+ // The following methods override what's in the TestEventListener class.
+ virtual void OnTestProgramStart(const UnitTest& /*unit_test*/) {}
+ virtual void OnTestIterationStart(const UnitTest& unit_test, int iteration);
+ virtual void OnEnvironmentsSetUpStart(const UnitTest& unit_test);
+ virtual void OnEnvironmentsSetUpEnd(const UnitTest& /*unit_test*/) {}
+ virtual void OnTestCaseStart(const TestCase& test_case);
+ virtual void OnTestStart(const TestInfo& test_info);
+ virtual void OnTestPartResult(const TestPartResult& result);
+ virtual void OnTestEnd(const TestInfo& test_info);
+ virtual void OnTestCaseEnd(const TestCase& test_case);
+ virtual void OnEnvironmentsTearDownStart(const UnitTest& unit_test);
+ virtual void OnEnvironmentsTearDownEnd(const UnitTest& /*unit_test*/) {}
+ virtual void OnTestIterationEnd(const UnitTest& unit_test, int iteration);
+ virtual void OnTestProgramEnd(const UnitTest& /*unit_test*/) {}
+
+ private:
+ static void PrintFailedTests(const UnitTest& unit_test);
+};
+
+ // Fired before each iteration of tests starts.
+void PrettyUnitTestResultPrinter::OnTestIterationStart(
+ const UnitTest& unit_test, int iteration) {
+ if (GTEST_FLAG(repeat) != 1)
+ printf("\nRepeating all tests (iteration %d) . . .\n\n", iteration + 1);
+
+ const char* const filter = GTEST_FLAG(filter).c_str();
+
+ // Prints the filter if it's not *. This reminds the user that some
+ // tests may be skipped.
+ if (!String::CStringEquals(filter, kUniversalFilter)) {
+ ColoredPrintf(COLOR_YELLOW,
+ "Note: %s filter = %s\n", GTEST_NAME_, filter);
+ }
+
+ if (internal::ShouldShard(kTestTotalShards, kTestShardIndex, false)) {
+ const Int32 shard_index = Int32FromEnvOrDie(kTestShardIndex, -1);
+ ColoredPrintf(COLOR_YELLOW,
+ "Note: This is test shard %d of %s.\n",
+ static_cast<int>(shard_index) + 1,
+ internal::posix::GetEnv(kTestTotalShards));
+ }
+
+ if (GTEST_FLAG(shuffle)) {
+ ColoredPrintf(COLOR_YELLOW,
+ "Note: Randomizing tests' orders with a seed of %d .\n",
+ unit_test.random_seed());
+ }
+
+ ColoredPrintf(COLOR_GREEN, "[==========] ");
+ printf("Running %s from %s.\n",
+ FormatTestCount(unit_test.test_to_run_count()).c_str(),
+ FormatTestCaseCount(unit_test.test_case_to_run_count()).c_str());
+ fflush(stdout);
+}
+
+void PrettyUnitTestResultPrinter::OnEnvironmentsSetUpStart(
+ const UnitTest& /*unit_test*/) {
+ ColoredPrintf(COLOR_GREEN, "[----------] ");
+ printf("Global test environment set-up.\n");
+ fflush(stdout);
+}
+
+void PrettyUnitTestResultPrinter::OnTestCaseStart(const TestCase& test_case) {
+ const std::string counts =
+ FormatCountableNoun(test_case.test_to_run_count(), "test", "tests");
+ ColoredPrintf(COLOR_GREEN, "[----------] ");
+ printf("%s from %s", counts.c_str(), test_case.name());
+ if (test_case.type_param() == NULL) {
+ printf("\n");
+ } else {
+ printf(", where %s = %s\n", kTypeParamLabel, test_case.type_param());
+ }
+ fflush(stdout);
+}
+
+void PrettyUnitTestResultPrinter::OnTestStart(const TestInfo& test_info) {
+ ColoredPrintf(COLOR_GREEN, "[ RUN ] ");
+ PrintTestName(test_info.test_case_name(), test_info.name());
+ printf("\n");
+ fflush(stdout);
+}
+
+// Called after an assertion failure.
+void PrettyUnitTestResultPrinter::OnTestPartResult(
+ const TestPartResult& result) {
+ // If the test part succeeded, we don't need to do anything.
+ if (result.type() == TestPartResult::kSuccess)
+ return;
+
+ // Print failure message from the assertion (e.g. expected this and got that).
+ PrintTestPartResult(result);
+ fflush(stdout);
+}
+
+void PrettyUnitTestResultPrinter::OnTestEnd(const TestInfo& test_info) {
+ if (test_info.result()->Passed()) {
+ ColoredPrintf(COLOR_GREEN, "[ OK ] ");
+ } else {
+ ColoredPrintf(COLOR_RED, "[ FAILED ] ");
+ }
+ PrintTestName(test_info.test_case_name(), test_info.name());
+ if (test_info.result()->Failed())
+ PrintFullTestCommentIfPresent(test_info);
+
+ if (GTEST_FLAG(print_time)) {
+ printf(" (%s ms)\n", internal::StreamableToString(
+ test_info.result()->elapsed_time()).c_str());
+ } else {
+ printf("\n");
+ }
+ fflush(stdout);
+}
+
+void PrettyUnitTestResultPrinter::OnTestCaseEnd(const TestCase& test_case) {
+ if (!GTEST_FLAG(print_time)) return;
+
+ const std::string counts =
+ FormatCountableNoun(test_case.test_to_run_count(), "test", "tests");
+ ColoredPrintf(COLOR_GREEN, "[----------] ");
+ printf("%s from %s (%s ms total)\n\n",
+ counts.c_str(), test_case.name(),
+ internal::StreamableToString(test_case.elapsed_time()).c_str());
+ fflush(stdout);
+}
+
+void PrettyUnitTestResultPrinter::OnEnvironmentsTearDownStart(
+ const UnitTest& /*unit_test*/) {
+ ColoredPrintf(COLOR_GREEN, "[----------] ");
+ printf("Global test environment tear-down\n");
+ fflush(stdout);
+}
+
+// Internal helper for printing the list of failed tests.
+void PrettyUnitTestResultPrinter::PrintFailedTests(const UnitTest& unit_test) {
+ const int failed_test_count = unit_test.failed_test_count();
+ if (failed_test_count == 0) {
+ return;
+ }
+
+ for (int i = 0; i < unit_test.total_test_case_count(); ++i) {
+ const TestCase& test_case = *unit_test.GetTestCase(i);
+ if (!test_case.should_run() || (test_case.failed_test_count() == 0)) {
+ continue;
+ }
+ for (int j = 0; j < test_case.total_test_count(); ++j) {
+ const TestInfo& test_info = *test_case.GetTestInfo(j);
+ if (!test_info.should_run() || test_info.result()->Passed()) {
+ continue;
+ }
+ ColoredPrintf(COLOR_RED, "[ FAILED ] ");
+ printf("%s.%s", test_case.name(), test_info.name());
+ PrintFullTestCommentIfPresent(test_info);
+ printf("\n");
+ }
+ }
+}
+
+void PrettyUnitTestResultPrinter::OnTestIterationEnd(const UnitTest& unit_test,
+ int /*iteration*/) {
+ ColoredPrintf(COLOR_GREEN, "[==========] ");
+ printf("%s from %s ran.",
+ FormatTestCount(unit_test.test_to_run_count()).c_str(),
+ FormatTestCaseCount(unit_test.test_case_to_run_count()).c_str());
+ if (GTEST_FLAG(print_time)) {
+ printf(" (%s ms total)",
+ internal::StreamableToString(unit_test.elapsed_time()).c_str());
+ }
+ printf("\n");
+ ColoredPrintf(COLOR_GREEN, "[ PASSED ] ");
+ printf("%s.\n", FormatTestCount(unit_test.successful_test_count()).c_str());
+
+ int num_failures = unit_test.failed_test_count();
+ if (!unit_test.Passed()) {
+ const int failed_test_count = unit_test.failed_test_count();
+ ColoredPrintf(COLOR_RED, "[ FAILED ] ");
+ printf("%s, listed below:\n", FormatTestCount(failed_test_count).c_str());
+ PrintFailedTests(unit_test);
+ printf("\n%2d FAILED %s\n", num_failures,
+ num_failures == 1 ? "TEST" : "TESTS");
+ }
+
+ int num_disabled = unit_test.reportable_disabled_test_count();
+ if (num_disabled && !GTEST_FLAG(also_run_disabled_tests)) {
+ if (!num_failures) {
+ printf("\n"); // Add a spacer if no FAILURE banner is displayed.
+ }
+ ColoredPrintf(COLOR_YELLOW,
+ " YOU HAVE %d DISABLED %s\n\n",
+ num_disabled,
+ num_disabled == 1 ? "TEST" : "TESTS");
+ }
+ // Ensure that Google Test output is printed before, e.g., heapchecker output.
+ fflush(stdout);
+}
+
+// End PrettyUnitTestResultPrinter
+
+// class TestEventRepeater
+//
+// This class forwards events to other event listeners.
+class TestEventRepeater : public TestEventListener {
+ public:
+ TestEventRepeater() : forwarding_enabled_(true) {}
+ virtual ~TestEventRepeater();
+ void Append(TestEventListener *listener);
+ TestEventListener* Release(TestEventListener* listener);
+
+ // Controls whether events will be forwarded to listeners_. Set to false
+ // in death test child processes.
+ bool forwarding_enabled() const { return forwarding_enabled_; }
+ void set_forwarding_enabled(bool enable) { forwarding_enabled_ = enable; }
+
+ virtual void OnTestProgramStart(const UnitTest& unit_test);
+ virtual void OnTestIterationStart(const UnitTest& unit_test, int iteration);
+ virtual void OnEnvironmentsSetUpStart(const UnitTest& unit_test);
+ virtual void OnEnvironmentsSetUpEnd(const UnitTest& unit_test);
+ virtual void OnTestCaseStart(const TestCase& test_case);
+ virtual void OnTestStart(const TestInfo& test_info);
+ virtual void OnTestPartResult(const TestPartResult& result);
+ virtual void OnTestEnd(const TestInfo& test_info);
+ virtual void OnTestCaseEnd(const TestCase& test_case);
+ virtual void OnEnvironmentsTearDownStart(const UnitTest& unit_test);
+ virtual void OnEnvironmentsTearDownEnd(const UnitTest& unit_test);
+ virtual void OnTestIterationEnd(const UnitTest& unit_test, int iteration);
+ virtual void OnTestProgramEnd(const UnitTest& unit_test);
+
+ private:
+ // Controls whether events will be forwarded to listeners_. Set to false
+ // in death test child processes.
+ bool forwarding_enabled_;
+ // The list of listeners that receive events.
+ std::vector<TestEventListener*> listeners_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(TestEventRepeater);
+};
+
+TestEventRepeater::~TestEventRepeater() {
+ ForEach(listeners_, Delete<TestEventListener>);
+}
+
+void TestEventRepeater::Append(TestEventListener *listener) {
+ listeners_.push_back(listener);
+}
+
+// TODO(vladl at google.com): Factor the search functionality into Vector::Find.
+TestEventListener* TestEventRepeater::Release(TestEventListener *listener) {
+ for (size_t i = 0; i < listeners_.size(); ++i) {
+ if (listeners_[i] == listener) {
+ listeners_.erase(listeners_.begin() + i);
+ return listener;
+ }
+ }
+
+ return NULL;
+}
+
+// Since most methods are very similar, use macros to reduce boilerplate.
+// This defines a member that forwards the call to all listeners.
+#define GTEST_REPEATER_METHOD_(Name, Type) \
+void TestEventRepeater::Name(const Type& parameter) { \
+ if (forwarding_enabled_) { \
+ for (size_t i = 0; i < listeners_.size(); i++) { \
+ listeners_[i]->Name(parameter); \
+ } \
+ } \
+}
+// This defines a member that forwards the call to all listeners in reverse
+// order.
+#define GTEST_REVERSE_REPEATER_METHOD_(Name, Type) \
+void TestEventRepeater::Name(const Type& parameter) { \
+ if (forwarding_enabled_) { \
+ for (int i = static_cast<int>(listeners_.size()) - 1; i >= 0; i--) { \
+ listeners_[i]->Name(parameter); \
+ } \
+ } \
+}
+
+GTEST_REPEATER_METHOD_(OnTestProgramStart, UnitTest)
+GTEST_REPEATER_METHOD_(OnEnvironmentsSetUpStart, UnitTest)
+GTEST_REPEATER_METHOD_(OnTestCaseStart, TestCase)
+GTEST_REPEATER_METHOD_(OnTestStart, TestInfo)
+GTEST_REPEATER_METHOD_(OnTestPartResult, TestPartResult)
+GTEST_REPEATER_METHOD_(OnEnvironmentsTearDownStart, UnitTest)
+GTEST_REVERSE_REPEATER_METHOD_(OnEnvironmentsSetUpEnd, UnitTest)
+GTEST_REVERSE_REPEATER_METHOD_(OnEnvironmentsTearDownEnd, UnitTest)
+GTEST_REVERSE_REPEATER_METHOD_(OnTestEnd, TestInfo)
+GTEST_REVERSE_REPEATER_METHOD_(OnTestCaseEnd, TestCase)
+GTEST_REVERSE_REPEATER_METHOD_(OnTestProgramEnd, UnitTest)
+
+#undef GTEST_REPEATER_METHOD_
+#undef GTEST_REVERSE_REPEATER_METHOD_
+
+void TestEventRepeater::OnTestIterationStart(const UnitTest& unit_test,
+ int iteration) {
+ if (forwarding_enabled_) {
+ for (size_t i = 0; i < listeners_.size(); i++) {
+ listeners_[i]->OnTestIterationStart(unit_test, iteration);
+ }
+ }
+}
+
+void TestEventRepeater::OnTestIterationEnd(const UnitTest& unit_test,
+ int iteration) {
+ if (forwarding_enabled_) {
+ for (int i = static_cast<int>(listeners_.size()) - 1; i >= 0; i--) {
+ listeners_[i]->OnTestIterationEnd(unit_test, iteration);
+ }
+ }
+}
+
+// End TestEventRepeater
+
+// This class generates an XML output file.
+class XmlUnitTestResultPrinter : public EmptyTestEventListener {
+ public:
+ explicit XmlUnitTestResultPrinter(const char* output_file);
+
+ virtual void OnTestIterationEnd(const UnitTest& unit_test, int iteration);
+
+ private:
+ // Is c a whitespace character that is normalized to a space character
+ // when it appears in an XML attribute value?
+ static bool IsNormalizableWhitespace(char c) {
+ return c == 0x9 || c == 0xA || c == 0xD;
+ }
+
+ // May c appear in a well-formed XML document?
+ static bool IsValidXmlCharacter(char c) {
+ return IsNormalizableWhitespace(c) || c >= 0x20;
+ }
+
+ // Returns an XML-escaped copy of the input string str. If
+ // is_attribute is true, the text is meant to appear as an attribute
+ // value, and normalizable whitespace is preserved by replacing it
+ // with character references.
+ static std::string EscapeXml(const std::string& str, bool is_attribute);
+
+ // Returns the given string with all characters invalid in XML removed.
+ static std::string RemoveInvalidXmlCharacters(const std::string& str);
+
+ // Convenience wrapper around EscapeXml when str is an attribute value.
+ static std::string EscapeXmlAttribute(const std::string& str) {
+ return EscapeXml(str, true);
+ }
+
+ // Convenience wrapper around EscapeXml when str is not an attribute value.
+ static std::string EscapeXmlText(const char* str) {
+ return EscapeXml(str, false);
+ }
+
+ // Verifies that the given attribute belongs to the given element and
+ // streams the attribute as XML.
+ static void OutputXmlAttribute(std::ostream* stream,
+ const std::string& element_name,
+ const std::string& name,
+ const std::string& value);
+
+ // Streams an XML CDATA section, escaping invalid CDATA sequences as needed.
+ static void OutputXmlCDataSection(::std::ostream* stream, const char* data);
+
+ // Streams an XML representation of a TestInfo object.
+ static void OutputXmlTestInfo(::std::ostream* stream,
+ const char* test_case_name,
+ const TestInfo& test_info);
+
+ // Prints an XML representation of a TestCase object
+ static void PrintXmlTestCase(::std::ostream* stream,
+ const TestCase& test_case);
+
+ // Prints an XML summary of unit_test to output stream out.
+ static void PrintXmlUnitTest(::std::ostream* stream,
+ const UnitTest& unit_test);
+
+ // Produces a string representing the test properties in a result as space
+ // delimited XML attributes based on the property key="value" pairs.
+ // When the std::string is not empty, it includes a space at the beginning,
+ // to delimit this attribute from prior attributes.
+ static std::string TestPropertiesAsXmlAttributes(const TestResult& result);
+
+ // The output file.
+ const std::string output_file_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(XmlUnitTestResultPrinter);
+};
+
+// Creates a new XmlUnitTestResultPrinter.
+XmlUnitTestResultPrinter::XmlUnitTestResultPrinter(const char* output_file)
+ : output_file_(output_file) {
+ if (output_file_.c_str() == NULL || output_file_.empty()) {
+ fprintf(stderr, "XML output file may not be null\n");
+ fflush(stderr);
+ exit(EXIT_FAILURE);
+ }
+}
+
+// Called after the unit test ends.
+void XmlUnitTestResultPrinter::OnTestIterationEnd(const UnitTest& unit_test,
+ int /*iteration*/) {
+ FILE* xmlout = NULL;
+ FilePath output_file(output_file_);
+ FilePath output_dir(output_file.RemoveFileName());
+
+ if (output_dir.CreateDirectoriesRecursively()) {
+ xmlout = posix::FOpen(output_file_.c_str(), "w");
+ }
+ if (xmlout == NULL) {
+ // TODO(wan): report the reason of the failure.
+ //
+ // We don't do it for now as:
+ //
+ // 1. There is no urgent need for it.
+ // 2. It's a bit involved to make the errno variable thread-safe on
+ // all three operating systems (Linux, Windows, and Mac OS).
+ // 3. To interpret the meaning of errno in a thread-safe way,
+ // we need the strerror_r() function, which is not available on
+ // Windows.
+ fprintf(stderr,
+ "Unable to open file \"%s\"\n",
+ output_file_.c_str());
+ fflush(stderr);
+ exit(EXIT_FAILURE);
+ }
+ std::stringstream stream;
+ PrintXmlUnitTest(&stream, unit_test);
+ fprintf(xmlout, "%s", StringStreamToString(&stream).c_str());
+ fclose(xmlout);
+}
+
+// Returns an XML-escaped copy of the input string str. If is_attribute
+// is true, the text is meant to appear as an attribute value, and
+// normalizable whitespace is preserved by replacing it with character
+// references.
+//
+// Invalid XML characters in str, if any, are stripped from the output.
+// It is expected that most, if not all, of the text processed by this
+// module will consist of ordinary English text.
+// If this module is ever modified to produce version 1.1 XML output,
+// most invalid characters can be retained using character references.
+// TODO(wan): It might be nice to have a minimally invasive, human-readable
+// escaping scheme for invalid characters, rather than dropping them.
+std::string XmlUnitTestResultPrinter::EscapeXml(
+ const std::string& str, bool is_attribute) {
+ Message m;
+
+ for (size_t i = 0; i < str.size(); ++i) {
+ const char ch = str[i];
+ switch (ch) {
+ case '<':
+ m << "<";
+ break;
+ case '>':
+ m << ">";
+ break;
+ case '&':
+ m << "&";
+ break;
+ case '\'':
+ if (is_attribute)
+ m << "'";
+ else
+ m << '\'';
+ break;
+ case '"':
+ if (is_attribute)
+ m << """;
+ else
+ m << '"';
+ break;
+ default:
+ if (IsValidXmlCharacter(ch)) {
+ if (is_attribute && IsNormalizableWhitespace(ch))
+ m << "&#x" << String::FormatByte(static_cast<unsigned char>(ch))
+ << ";";
+ else
+ m << ch;
+ }
+ break;
+ }
+ }
+
+ return m.GetString();
+}
+
+// Returns the given string with all characters invalid in XML removed.
+// Currently invalid characters are dropped from the string. An
+// alternative is to replace them with certain characters such as . or ?.
+std::string XmlUnitTestResultPrinter::RemoveInvalidXmlCharacters(
+ const std::string& str) {
+ std::string output;
+ output.reserve(str.size());
+ for (std::string::const_iterator it = str.begin(); it != str.end(); ++it)
+ if (IsValidXmlCharacter(*it))
+ output.push_back(*it);
+
+ return output;
+}
+
+// The following routines generate an XML representation of a UnitTest
+// object.
+//
+// This is how Google Test concepts map to the DTD:
+//
+// <testsuites name="AllTests"> <-- corresponds to a UnitTest object
+// <testsuite name="testcase-name"> <-- corresponds to a TestCase object
+// <testcase name="test-name"> <-- corresponds to a TestInfo object
+// <failure message="...">...</failure>
+// <failure message="...">...</failure>
+// <failure message="...">...</failure>
+// <-- individual assertion failures
+// </testcase>
+// </testsuite>
+// </testsuites>
+
+// Formats the given time in milliseconds as seconds.
+std::string FormatTimeInMillisAsSeconds(TimeInMillis ms) {
+ ::std::stringstream ss;
+ ss << ms/1000.0;
+ return ss.str();
+}
+
+// Converts the given epoch time in milliseconds to a date string in the ISO
+// 8601 format, without the timezone information.
+std::string FormatEpochTimeInMillisAsIso8601(TimeInMillis ms) {
+ // Using non-reentrant version as localtime_r is not portable.
+ time_t seconds = static_cast<time_t>(ms / 1000);
+#ifdef _MSC_VER
+# pragma warning(push) // Saves the current warning state.
+# pragma warning(disable:4996) // Temporarily disables warning 4996
+ // (function or variable may be unsafe).
+ const struct tm* const time_struct = localtime(&seconds); // NOLINT
+# pragma warning(pop) // Restores the warning state again.
+#else
+ const struct tm* const time_struct = localtime(&seconds); // NOLINT
+#endif
+ if (time_struct == NULL)
+ return ""; // Invalid ms value
+
+ // YYYY-MM-DDThh:mm:ss
+ return StreamableToString(time_struct->tm_year + 1900) + "-" +
+ String::FormatIntWidth2(time_struct->tm_mon + 1) + "-" +
+ String::FormatIntWidth2(time_struct->tm_mday) + "T" +
+ String::FormatIntWidth2(time_struct->tm_hour) + ":" +
+ String::FormatIntWidth2(time_struct->tm_min) + ":" +
+ String::FormatIntWidth2(time_struct->tm_sec);
+}
+
+// Streams an XML CDATA section, escaping invalid CDATA sequences as needed.
+void XmlUnitTestResultPrinter::OutputXmlCDataSection(::std::ostream* stream,
+ const char* data) {
+ const char* segment = data;
+ *stream << "<![CDATA[";
+ for (;;) {
+ const char* const next_segment = strstr(segment, "]]>");
+ if (next_segment != NULL) {
+ stream->write(
+ segment, static_cast<std::streamsize>(next_segment - segment));
+ *stream << "]]>]]><![CDATA[";
+ segment = next_segment + strlen("]]>");
+ } else {
+ *stream << segment;
+ break;
+ }
+ }
+ *stream << "]]>";
+}
+
+void XmlUnitTestResultPrinter::OutputXmlAttribute(
+ std::ostream* stream,
+ const std::string& element_name,
+ const std::string& name,
+ const std::string& value) {
+ const std::vector<std::string>& allowed_names =
+ GetReservedAttributesForElement(element_name);
+
+ GTEST_CHECK_(std::find(allowed_names.begin(), allowed_names.end(), name) !=
+ allowed_names.end())
+ << "Attribute " << name << " is not allowed for element <" << element_name
+ << ">.";
+
+ *stream << " " << name << "=\"" << EscapeXmlAttribute(value) << "\"";
+}
+
+// Prints an XML representation of a TestInfo object.
+// TODO(wan): There is also value in printing properties with the plain printer.
+void XmlUnitTestResultPrinter::OutputXmlTestInfo(::std::ostream* stream,
+ const char* test_case_name,
+ const TestInfo& test_info) {
+ const TestResult& result = *test_info.result();
+ const std::string kTestcase = "testcase";
+
+ *stream << " <testcase";
+ OutputXmlAttribute(stream, kTestcase, "name", test_info.name());
+
+ if (test_info.value_param() != NULL) {
+ OutputXmlAttribute(stream, kTestcase, "value_param",
+ test_info.value_param());
+ }
+ if (test_info.type_param() != NULL) {
+ OutputXmlAttribute(stream, kTestcase, "type_param", test_info.type_param());
+ }
+
+ OutputXmlAttribute(stream, kTestcase, "status",
+ test_info.should_run() ? "run" : "notrun");
+ OutputXmlAttribute(stream, kTestcase, "time",
+ FormatTimeInMillisAsSeconds(result.elapsed_time()));
+ OutputXmlAttribute(stream, kTestcase, "classname", test_case_name);
+ *stream << TestPropertiesAsXmlAttributes(result);
+
+ int failures = 0;
+ for (int i = 0; i < result.total_part_count(); ++i) {
+ const TestPartResult& part = result.GetTestPartResult(i);
+ if (part.failed()) {
+ if (++failures == 1) {
+ *stream << ">\n";
+ }
+ const string location = internal::FormatCompilerIndependentFileLocation(
+ part.file_name(), part.line_number());
+ const string summary = location + "\n" + part.summary();
+ *stream << " <failure message=\""
+ << EscapeXmlAttribute(summary.c_str())
+ << "\" type=\"\">";
+ const string detail = location + "\n" + part.message();
+ OutputXmlCDataSection(stream, RemoveInvalidXmlCharacters(detail).c_str());
+ *stream << "</failure>\n";
+ }
+ }
+
+ if (failures == 0)
+ *stream << " />\n";
+ else
+ *stream << " </testcase>\n";
+}
+
+// Prints an XML representation of a TestCase object
+void XmlUnitTestResultPrinter::PrintXmlTestCase(std::ostream* stream,
+ const TestCase& test_case) {
+ const std::string kTestsuite = "testsuite";
+ *stream << " <" << kTestsuite;
+ OutputXmlAttribute(stream, kTestsuite, "name", test_case.name());
+ OutputXmlAttribute(stream, kTestsuite, "tests",
+ StreamableToString(test_case.reportable_test_count()));
+ OutputXmlAttribute(stream, kTestsuite, "failures",
+ StreamableToString(test_case.failed_test_count()));
+ OutputXmlAttribute(
+ stream, kTestsuite, "disabled",
+ StreamableToString(test_case.reportable_disabled_test_count()));
+ OutputXmlAttribute(stream, kTestsuite, "errors", "0");
+ OutputXmlAttribute(stream, kTestsuite, "time",
+ FormatTimeInMillisAsSeconds(test_case.elapsed_time()));
+ *stream << TestPropertiesAsXmlAttributes(test_case.ad_hoc_test_result())
+ << ">\n";
+
+ for (int i = 0; i < test_case.total_test_count(); ++i) {
+ if (test_case.GetTestInfo(i)->is_reportable())
+ OutputXmlTestInfo(stream, test_case.name(), *test_case.GetTestInfo(i));
+ }
+ *stream << " </" << kTestsuite << ">\n";
+}
+
+// Prints an XML summary of unit_test to output stream out.
+void XmlUnitTestResultPrinter::PrintXmlUnitTest(std::ostream* stream,
+ const UnitTest& unit_test) {
+ const std::string kTestsuites = "testsuites";
+
+ *stream << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n";
+ *stream << "<" << kTestsuites;
+
+ OutputXmlAttribute(stream, kTestsuites, "tests",
+ StreamableToString(unit_test.reportable_test_count()));
+ OutputXmlAttribute(stream, kTestsuites, "failures",
+ StreamableToString(unit_test.failed_test_count()));
+ OutputXmlAttribute(
+ stream, kTestsuites, "disabled",
+ StreamableToString(unit_test.reportable_disabled_test_count()));
+ OutputXmlAttribute(stream, kTestsuites, "errors", "0");
+ OutputXmlAttribute(
+ stream, kTestsuites, "timestamp",
+ FormatEpochTimeInMillisAsIso8601(unit_test.start_timestamp()));
+ OutputXmlAttribute(stream, kTestsuites, "time",
+ FormatTimeInMillisAsSeconds(unit_test.elapsed_time()));
+
+ if (GTEST_FLAG(shuffle)) {
+ OutputXmlAttribute(stream, kTestsuites, "random_seed",
+ StreamableToString(unit_test.random_seed()));
+ }
+
+ *stream << TestPropertiesAsXmlAttributes(unit_test.ad_hoc_test_result());
+
+ OutputXmlAttribute(stream, kTestsuites, "name", "AllTests");
+ *stream << ">\n";
+
+ for (int i = 0; i < unit_test.total_test_case_count(); ++i) {
+ if (unit_test.GetTestCase(i)->reportable_test_count() > 0)
+ PrintXmlTestCase(stream, *unit_test.GetTestCase(i));
+ }
+ *stream << "</" << kTestsuites << ">\n";
+}
+
+// Produces a string representing the test properties in a result as space
+// delimited XML attributes based on the property key="value" pairs.
+std::string XmlUnitTestResultPrinter::TestPropertiesAsXmlAttributes(
+ const TestResult& result) {
+ Message attributes;
+ for (int i = 0; i < result.test_property_count(); ++i) {
+ const TestProperty& property = result.GetTestProperty(i);
+ attributes << " " << property.key() << "="
+ << "\"" << EscapeXmlAttribute(property.value()) << "\"";
+ }
+ return attributes.GetString();
+}
+
+// End XmlUnitTestResultPrinter
+
+#if GTEST_CAN_STREAM_RESULTS_
+
+// Checks if str contains '=', '&', '%' or '\n' characters. If yes,
+// replaces them by "%xx" where xx is their hexadecimal value. For
+// example, replaces "=" with "%3D". This algorithm is O(strlen(str))
+// in both time and space -- important as the input str may contain an
+// arbitrarily long test failure message and stack trace.
+string StreamingListener::UrlEncode(const char* str) {
+ string result;
+ result.reserve(strlen(str) + 1);
+ for (char ch = *str; ch != '\0'; ch = *++str) {
+ switch (ch) {
+ case '%':
+ case '=':
+ case '&':
+ case '\n':
+ result.append("%" + String::FormatByte(static_cast<unsigned char>(ch)));
+ break;
+ default:
+ result.push_back(ch);
+ break;
+ }
+ }
+ return result;
+}
+
+void StreamingListener::SocketWriter::MakeConnection() {
+ GTEST_CHECK_(sockfd_ == -1)
+ << "MakeConnection() can't be called when there is already a connection.";
+
+ addrinfo hints;
+ memset(&hints, 0, sizeof(hints));
+ hints.ai_family = AF_UNSPEC; // To allow both IPv4 and IPv6 addresses.
+ hints.ai_socktype = SOCK_STREAM;
+ addrinfo* servinfo = NULL;
+
+ // Use the getaddrinfo() to get a linked list of IP addresses for
+ // the given host name.
+ const int error_num = getaddrinfo(
+ host_name_.c_str(), port_num_.c_str(), &hints, &servinfo);
+ if (error_num != 0) {
+ GTEST_LOG_(WARNING) << "stream_result_to: getaddrinfo() failed: "
+ << gai_strerror(error_num);
+ }
+
+ // Loop through all the results and connect to the first we can.
+ for (addrinfo* cur_addr = servinfo; sockfd_ == -1 && cur_addr != NULL;
+ cur_addr = cur_addr->ai_next) {
+ sockfd_ = socket(
+ cur_addr->ai_family, cur_addr->ai_socktype, cur_addr->ai_protocol);
+ if (sockfd_ != -1) {
+ // Connect the client socket to the server socket.
+ if (connect(sockfd_, cur_addr->ai_addr, cur_addr->ai_addrlen) == -1) {
+ close(sockfd_);
+ sockfd_ = -1;
+ }
+ }
+ }
+
+ freeaddrinfo(servinfo); // all done with this structure
+
+ if (sockfd_ == -1) {
+ GTEST_LOG_(WARNING) << "stream_result_to: failed to connect to "
+ << host_name_ << ":" << port_num_;
+ }
+}
+
+// End of class Streaming Listener
+#endif // GTEST_CAN_STREAM_RESULTS__
+
+// Class ScopedTrace
+
+// Pushes the given source file location and message onto a per-thread
+// trace stack maintained by Google Test.
+ScopedTrace::ScopedTrace(const char* file, int line, const Message& message)
+ GTEST_LOCK_EXCLUDED_(&UnitTest::mutex_) {
+ TraceInfo trace;
+ trace.file = file;
+ trace.line = line;
+ trace.message = message.GetString();
+
+ UnitTest::GetInstance()->PushGTestTrace(trace);
+}
+
+// Pops the info pushed by the c'tor.
+ScopedTrace::~ScopedTrace()
+ GTEST_LOCK_EXCLUDED_(&UnitTest::mutex_) {
+ UnitTest::GetInstance()->PopGTestTrace();
+}
+
+
+// class OsStackTraceGetter
+
+// Returns the current OS stack trace as an std::string. Parameters:
+//
+// max_depth - the maximum number of stack frames to be included
+// in the trace.
+// skip_count - the number of top frames to be skipped; doesn't count
+// against max_depth.
+//
+string OsStackTraceGetter::CurrentStackTrace(int /* max_depth */,
+ int /* skip_count */)
+ GTEST_LOCK_EXCLUDED_(mutex_) {
+ return "";
+}
+
+void OsStackTraceGetter::UponLeavingGTest()
+ GTEST_LOCK_EXCLUDED_(mutex_) {
+}
+
+const char* const
+OsStackTraceGetter::kElidedFramesMarker =
+ "... " GTEST_NAME_ " internal frames ...";
+
+// A helper class that creates the premature-exit file in its
+// constructor and deletes the file in its destructor.
+class ScopedPrematureExitFile {
+ public:
+ explicit ScopedPrematureExitFile(const char* premature_exit_filepath)
+ : premature_exit_filepath_(premature_exit_filepath) {
+ // If a path to the premature-exit file is specified...
+ if (premature_exit_filepath != NULL && *premature_exit_filepath != '\0') {
+ // create the file with a single "0" character in it. I/O
+ // errors are ignored as there's nothing better we can do and we
+ // don't want to fail the test because of this.
+ FILE* pfile = posix::FOpen(premature_exit_filepath, "w");
+ fwrite("0", 1, 1, pfile);
+ fclose(pfile);
+ }
+ }
+
+ ~ScopedPrematureExitFile() {
+ if (premature_exit_filepath_ != NULL && *premature_exit_filepath_ != '\0') {
+ remove(premature_exit_filepath_);
+ }
+ }
+
+ private:
+ const char* const premature_exit_filepath_;
+
+ GTEST_DISALLOW_COPY_AND_ASSIGN_(ScopedPrematureExitFile);
+};
+
+} // namespace internal
+
+// class TestEventListeners
+
+TestEventListeners::TestEventListeners()
+ : repeater_(new internal::TestEventRepeater()),
+ default_result_printer_(NULL),
+ default_xml_generator_(NULL) {
+}
+
+TestEventListeners::~TestEventListeners() { delete repeater_; }
+
+// Returns the standard listener responsible for the default console
+// output. Can be removed from the listeners list to shut down default
+// console output. Note that removing this object from the listener list
+// with Release transfers its ownership to the user.
+void TestEventListeners::Append(TestEventListener* listener) {
+ repeater_->Append(listener);
+}
+
+// Removes the given event listener from the list and returns it. It then
+// becomes the caller's responsibility to delete the listener. Returns
+// NULL if the listener is not found in the list.
+TestEventListener* TestEventListeners::Release(TestEventListener* listener) {
+ if (listener == default_result_printer_)
+ default_result_printer_ = NULL;
+ else if (listener == default_xml_generator_)
+ default_xml_generator_ = NULL;
+ return repeater_->Release(listener);
+}
+
+// Returns repeater that broadcasts the TestEventListener events to all
+// subscribers.
+TestEventListener* TestEventListeners::repeater() { return repeater_; }
+
+// Sets the default_result_printer attribute to the provided listener.
+// The listener is also added to the listener list and previous
+// default_result_printer is removed from it and deleted. The listener can
+// also be NULL in which case it will not be added to the list. Does
+// nothing if the previous and the current listener objects are the same.
+void TestEventListeners::SetDefaultResultPrinter(TestEventListener* listener) {
+ if (default_result_printer_ != listener) {
+ // It is an error to pass this method a listener that is already in the
+ // list.
+ delete Release(default_result_printer_);
+ default_result_printer_ = listener;
+ if (listener != NULL)
+ Append(listener);
+ }
+}
+
+// Sets the default_xml_generator attribute to the provided listener. The
+// listener is also added to the listener list and previous
+// default_xml_generator is removed from it and deleted. The listener can
+// also be NULL in which case it will not be added to the list. Does
+// nothing if the previous and the current listener objects are the same.
+void TestEventListeners::SetDefaultXmlGenerator(TestEventListener* listener) {
+ if (default_xml_generator_ != listener) {
+ // It is an error to pass this method a listener that is already in the
+ // list.
+ delete Release(default_xml_generator_);
+ default_xml_generator_ = listener;
+ if (listener != NULL)
+ Append(listener);
+ }
+}
+
+// Controls whether events will be forwarded by the repeater to the
+// listeners in the list.
+bool TestEventListeners::EventForwardingEnabled() const {
+ return repeater_->forwarding_enabled();
+}
+
+void TestEventListeners::SuppressEventForwarding() {
+ repeater_->set_forwarding_enabled(false);
+}
+
+// class UnitTest
+
+// Gets the singleton UnitTest object. The first time this method is
+// called, a UnitTest object is constructed and returned. Consecutive
+// calls will return the same object.
+//
+// We don't protect this under mutex_ as a user is not supposed to
+// call this before main() starts, from which point on the return
+// value will never change.
+UnitTest* UnitTest::GetInstance() {
+ // When compiled with MSVC 7.1 in optimized mode, destroying the
+ // UnitTest object upon exiting the program messes up the exit code,
+ // causing successful tests to appear failed. We have to use a
+ // different implementation in this case to bypass the compiler bug.
+ // This implementation makes the compiler happy, at the cost of
+ // leaking the UnitTest object.
+
+ // CodeGear C++Builder insists on a public destructor for the
+ // default implementation. Use this implementation to keep good OO
+ // design with private destructor.
+
+#if (_MSC_VER == 1310 && !defined(_DEBUG)) || defined(__BORLANDC__)
+ static UnitTest* const instance = new UnitTest;
+ return instance;
+#else
+ static UnitTest instance;
+ return &instance;
+#endif // (_MSC_VER == 1310 && !defined(_DEBUG)) || defined(__BORLANDC__)
+}
+
+// Gets the number of successful test cases.
+int UnitTest::successful_test_case_count() const {
+ return impl()->successful_test_case_count();
+}
+
+// Gets the number of failed test cases.
+int UnitTest::failed_test_case_count() const {
+ return impl()->failed_test_case_count();
+}
+
+// Gets the number of all test cases.
+int UnitTest::total_test_case_count() const {
+ return impl()->total_test_case_count();
+}
+
+// Gets the number of all test cases that contain at least one test
+// that should run.
+int UnitTest::test_case_to_run_count() const {
+ return impl()->test_case_to_run_count();
+}
+
+// Gets the number of successful tests.
+int UnitTest::successful_test_count() const {
+ return impl()->successful_test_count();
+}
+
+// Gets the number of failed tests.
+int UnitTest::failed_test_count() const { return impl()->failed_test_count(); }
+
+// Gets the number of disabled tests that will be reported in the XML report.
+int UnitTest::reportable_disabled_test_count() const {
+ return impl()->reportable_disabled_test_count();
+}
+
+// Gets the number of disabled tests.
+int UnitTest::disabled_test_count() const {
+ return impl()->disabled_test_count();
+}
+
+// Gets the number of tests to be printed in the XML report.
+int UnitTest::reportable_test_count() const {
+ return impl()->reportable_test_count();
+}
+
+// Gets the number of all tests.
+int UnitTest::total_test_count() const { return impl()->total_test_count(); }
+
+// Gets the number of tests that should run.
+int UnitTest::test_to_run_count() const { return impl()->test_to_run_count(); }
+
+// Gets the time of the test program start, in ms from the start of the
+// UNIX epoch.
+internal::TimeInMillis UnitTest::start_timestamp() const {
+ return impl()->start_timestamp();
+}
+
+// Gets the elapsed time, in milliseconds.
+internal::TimeInMillis UnitTest::elapsed_time() const {
+ return impl()->elapsed_time();
+}
+
+// Returns true iff the unit test passed (i.e. all test cases passed).
+bool UnitTest::Passed() const { return impl()->Passed(); }
+
+// Returns true iff the unit test failed (i.e. some test case failed
+// or something outside of all tests failed).
+bool UnitTest::Failed() const { return impl()->Failed(); }
+
+// Gets the i-th test case among all the test cases. i can range from 0 to
+// total_test_case_count() - 1. If i is not in that range, returns NULL.
+const TestCase* UnitTest::GetTestCase(int i) const {
+ return impl()->GetTestCase(i);
+}
+
+// Returns the TestResult containing information on test failures and
+// properties logged outside of individual test cases.
+const TestResult& UnitTest::ad_hoc_test_result() const {
+ return *impl()->ad_hoc_test_result();
+}
+
+// Gets the i-th test case among all the test cases. i can range from 0 to
+// total_test_case_count() - 1. If i is not in that range, returns NULL.
+TestCase* UnitTest::GetMutableTestCase(int i) {
+ return impl()->GetMutableTestCase(i);
+}
+
+// Returns the list of event listeners that can be used to track events
+// inside Google Test.
+TestEventListeners& UnitTest::listeners() {
+ return *impl()->listeners();
+}
+
+// Registers and returns a global test environment. When a test
+// program is run, all global test environments will be set-up in the
+// order they were registered. After all tests in the program have
+// finished, all global test environments will be torn-down in the
+// *reverse* order they were registered.
+//
+// The UnitTest object takes ownership of the given environment.
+//
+// We don't protect this under mutex_, as we only support calling it
+// from the main thread.
+Environment* UnitTest::AddEnvironment(Environment* env) {
+ if (env == NULL) {
+ return NULL;
+ }
+
+ impl_->environments().push_back(env);
+ return env;
+}
+
+// Adds a TestPartResult to the current TestResult object. All Google Test
+// assertion macros (e.g. ASSERT_TRUE, EXPECT_EQ, etc) eventually call
+// this to report their results. The user code should use the
+// assertion macros instead of calling this directly.
+void UnitTest::AddTestPartResult(
+ TestPartResult::Type result_type,
+ const char* file_name,
+ int line_number,
+ const std::string& message,
+ const std::string& os_stack_trace) GTEST_LOCK_EXCLUDED_(mutex_) {
+ Message msg;
+ msg << message;
+
+ internal::MutexLock lock(&mutex_);
+ if (impl_->gtest_trace_stack().size() > 0) {
+ msg << "\n" << GTEST_NAME_ << " trace:";
+
+ for (int i = static_cast<int>(impl_->gtest_trace_stack().size());
+ i > 0; --i) {
+ const internal::TraceInfo& trace = impl_->gtest_trace_stack()[i - 1];
+ msg << "\n" << internal::FormatFileLocation(trace.file, trace.line)
+ << " " << trace.message;
+ }
+ }
+
+ if (os_stack_trace.c_str() != NULL && !os_stack_trace.empty()) {
+ msg << internal::kStackTraceMarker << os_stack_trace;
+ }
+
+ const TestPartResult result =
+ TestPartResult(result_type, file_name, line_number,
+ msg.GetString().c_str());
+ impl_->GetTestPartResultReporterForCurrentThread()->
+ ReportTestPartResult(result);
+
+ if (result_type != TestPartResult::kSuccess) {
+ // gtest_break_on_failure takes precedence over
+ // gtest_throw_on_failure. This allows a user to set the latter
+ // in the code (perhaps in order to use Google Test assertions
+ // with another testing framework) and specify the former on the
+ // command line for debugging.
+ if (GTEST_FLAG(break_on_failure)) {
+#if GTEST_OS_WINDOWS
+ // Using DebugBreak on Windows allows gtest to still break into a debugger
+ // when a failure happens and both the --gtest_break_on_failure and
+ // the --gtest_catch_exceptions flags are specified.
+ DebugBreak();
+#else
+ // Dereference NULL through a volatile pointer to prevent the compiler
+ // from removing. We use this rather than abort() or __builtin_trap() for
+ // portability: Symbian doesn't implement abort() well, and some debuggers
+ // don't correctly trap abort().
+ *static_cast<volatile int*>(NULL) = 1;
+#endif // GTEST_OS_WINDOWS
+ } else if (GTEST_FLAG(throw_on_failure)) {
+#if GTEST_HAS_EXCEPTIONS
+ throw internal::GoogleTestFailureException(result);
+#else
+ // We cannot call abort() as it generates a pop-up in debug mode
+ // that cannot be suppressed in VC 7.1 or below.
+ exit(1);
+#endif
+ }
+ }
+}
+
+// Adds a TestProperty to the current TestResult object when invoked from
+// inside a test, to current TestCase's ad_hoc_test_result_ when invoked
+// from SetUpTestCase or TearDownTestCase, or to the global property set
+// when invoked elsewhere. If the result already contains a property with
+// the same key, the value will be updated.
+void UnitTest::RecordProperty(const std::string& key,
+ const std::string& value) {
+ impl_->RecordProperty(TestProperty(key, value));
+}
+
+// Runs all tests in this UnitTest object and prints the result.
+// Returns 0 if successful, or 1 otherwise.
+//
+// We don't protect this under mutex_, as we only support calling it
+// from the main thread.
+int UnitTest::Run() {
+ const bool in_death_test_child_process =
+ internal::GTEST_FLAG(internal_run_death_test).length() > 0;
+
+ // Google Test implements this protocol for catching that a test
+ // program exits before returning control to Google Test:
+ //
+ // 1. Upon start, Google Test creates a file whose absolute path
+ // is specified by the environment variable
+ // TEST_PREMATURE_EXIT_FILE.
+ // 2. When Google Test has finished its work, it deletes the file.
+ //
+ // This allows a test runner to set TEST_PREMATURE_EXIT_FILE before
+ // running a Google-Test-based test program and check the existence
+ // of the file at the end of the test execution to see if it has
+ // exited prematurely.
+
+ // If we are in the child process of a death test, don't
+ // create/delete the premature exit file, as doing so is unnecessary
+ // and will confuse the parent process. Otherwise, create/delete
+ // the file upon entering/leaving this function. If the program
+ // somehow exits before this function has a chance to return, the
+ // premature-exit file will be left undeleted, causing a test runner
+ // that understands the premature-exit-file protocol to report the
+ // test as having failed.
+ const internal::ScopedPrematureExitFile premature_exit_file(
+ in_death_test_child_process ?
+ NULL : internal::posix::GetEnv("TEST_PREMATURE_EXIT_FILE"));
+
+ // Captures the value of GTEST_FLAG(catch_exceptions). This value will be
+ // used for the duration of the program.
+ impl()->set_catch_exceptions(GTEST_FLAG(catch_exceptions));
+
+#if GTEST_HAS_SEH
+ // Either the user wants Google Test to catch exceptions thrown by the
+ // tests or this is executing in the context of death test child
+ // process. In either case the user does not want to see pop-up dialogs
+ // about crashes - they are expected.
+ if (impl()->catch_exceptions() || in_death_test_child_process) {
+# if !GTEST_OS_WINDOWS_MOBILE
+ // SetErrorMode doesn't exist on CE.
+ SetErrorMode(SEM_FAILCRITICALERRORS | SEM_NOALIGNMENTFAULTEXCEPT |
+ SEM_NOGPFAULTERRORBOX | SEM_NOOPENFILEERRORBOX);
+# endif // !GTEST_OS_WINDOWS_MOBILE
+
+# if (defined(_MSC_VER) || GTEST_OS_WINDOWS_MINGW) && !GTEST_OS_WINDOWS_MOBILE
+ // Death test children can be terminated with _abort(). On Windows,
+ // _abort() can show a dialog with a warning message. This forces the
+ // abort message to go to stderr instead.
+ _set_error_mode(_OUT_TO_STDERR);
+# endif
+
+# if _MSC_VER >= 1400 && !GTEST_OS_WINDOWS_MOBILE
+ // In the debug version, Visual Studio pops up a separate dialog
+ // offering a choice to debug the aborted program. We need to suppress
+ // this dialog or it will pop up for every EXPECT/ASSERT_DEATH statement
+ // executed. Google Test will notify the user of any unexpected
+ // failure via stderr.
+ //
+ // VC++ doesn't define _set_abort_behavior() prior to the version 8.0.
+ // Users of prior VC versions shall suffer the agony and pain of
+ // clicking through the countless debug dialogs.
+ // TODO(vladl at google.com): find a way to suppress the abort dialog() in the
+ // debug mode when compiled with VC 7.1 or lower.
+ if (!GTEST_FLAG(break_on_failure))
+ _set_abort_behavior(
+ 0x0, // Clear the following flags:
+ _WRITE_ABORT_MSG | _CALL_REPORTFAULT); // pop-up window, core dump.
+# endif
+ }
+#endif // GTEST_HAS_SEH
+
+ return internal::HandleExceptionsInMethodIfSupported(
+ impl(),
+ &internal::UnitTestImpl::RunAllTests,
+ "auxiliary test code (environments or event listeners)") ? 0 : 1;
+}
+
+// Returns the working directory when the first TEST() or TEST_F() was
+// executed.
+const char* UnitTest::original_working_dir() const {
+ return impl_->original_working_dir_.c_str();
+}
+
+// Returns the TestCase object for the test that's currently running,
+// or NULL if no test is running.
+const TestCase* UnitTest::current_test_case() const
+ GTEST_LOCK_EXCLUDED_(mutex_) {
+ internal::MutexLock lock(&mutex_);
+ return impl_->current_test_case();
+}
+
+// Returns the TestInfo object for the test that's currently running,
+// or NULL if no test is running.
+const TestInfo* UnitTest::current_test_info() const
+ GTEST_LOCK_EXCLUDED_(mutex_) {
+ internal::MutexLock lock(&mutex_);
+ return impl_->current_test_info();
+}
+
+// Returns the random seed used at the start of the current test run.
+int UnitTest::random_seed() const { return impl_->random_seed(); }
+
+#if GTEST_HAS_PARAM_TEST
+// Returns ParameterizedTestCaseRegistry object used to keep track of
+// value-parameterized tests and instantiate and register them.
+internal::ParameterizedTestCaseRegistry&
+ UnitTest::parameterized_test_registry()
+ GTEST_LOCK_EXCLUDED_(mutex_) {
+ return impl_->parameterized_test_registry();
+}
+#endif // GTEST_HAS_PARAM_TEST
+
+// Creates an empty UnitTest.
+UnitTest::UnitTest() {
+ impl_ = new internal::UnitTestImpl(this);
+}
+
+// Destructor of UnitTest.
+UnitTest::~UnitTest() {
+ delete impl_;
+}
+
+// Pushes a trace defined by SCOPED_TRACE() on to the per-thread
+// Google Test trace stack.
+void UnitTest::PushGTestTrace(const internal::TraceInfo& trace)
+ GTEST_LOCK_EXCLUDED_(mutex_) {
+ internal::MutexLock lock(&mutex_);
+ impl_->gtest_trace_stack().push_back(trace);
+}
+
+// Pops a trace from the per-thread Google Test trace stack.
+void UnitTest::PopGTestTrace()
+ GTEST_LOCK_EXCLUDED_(mutex_) {
+ internal::MutexLock lock(&mutex_);
+ impl_->gtest_trace_stack().pop_back();
+}
+
+namespace internal {
+
+UnitTestImpl::UnitTestImpl(UnitTest* parent)
+ : parent_(parent),
+#ifdef _MSC_VER
+# pragma warning(push) // Saves the current warning state.
+# pragma warning(disable:4355) // Temporarily disables warning 4355
+ // (using this in initializer).
+ default_global_test_part_result_reporter_(this),
+ default_per_thread_test_part_result_reporter_(this),
+# pragma warning(pop) // Restores the warning state again.
+#else
+ default_global_test_part_result_reporter_(this),
+ default_per_thread_test_part_result_reporter_(this),
+#endif // _MSC_VER
+ global_test_part_result_repoter_(
+ &default_global_test_part_result_reporter_),
+ per_thread_test_part_result_reporter_(
+ &default_per_thread_test_part_result_reporter_),
+#if GTEST_HAS_PARAM_TEST
+ parameterized_test_registry_(),
+ parameterized_tests_registered_(false),
+#endif // GTEST_HAS_PARAM_TEST
+ last_death_test_case_(-1),
+ current_test_case_(NULL),
+ current_test_info_(NULL),
+ ad_hoc_test_result_(),
+ os_stack_trace_getter_(NULL),
+ post_flag_parse_init_performed_(false),
+ random_seed_(0), // Will be overridden by the flag before first use.
+ random_(0), // Will be reseeded before first use.
+ start_timestamp_(0),
+ elapsed_time_(0),
+#if GTEST_HAS_DEATH_TEST
+ death_test_factory_(new DefaultDeathTestFactory),
+#endif
+ // Will be overridden by the flag before first use.
+ catch_exceptions_(false) {
+ listeners()->SetDefaultResultPrinter(new PrettyUnitTestResultPrinter);
+}
+
+UnitTestImpl::~UnitTestImpl() {
+ // Deletes every TestCase.
+ ForEach(test_cases_, internal::Delete<TestCase>);
+
+ // Deletes every Environment.
+ ForEach(environments_, internal::Delete<Environment>);
+
+ delete os_stack_trace_getter_;
+}
+
+// Adds a TestProperty to the current TestResult object when invoked in a
+// context of a test, to current test case's ad_hoc_test_result when invoke
+// from SetUpTestCase/TearDownTestCase, or to the global property set
+// otherwise. If the result already contains a property with the same key,
+// the value will be updated.
+void UnitTestImpl::RecordProperty(const TestProperty& test_property) {
+ std::string xml_element;
+ TestResult* test_result; // TestResult appropriate for property recording.
+
+ if (current_test_info_ != NULL) {
+ xml_element = "testcase";
+ test_result = &(current_test_info_->result_);
+ } else if (current_test_case_ != NULL) {
+ xml_element = "testsuite";
+ test_result = &(current_test_case_->ad_hoc_test_result_);
+ } else {
+ xml_element = "testsuites";
+ test_result = &ad_hoc_test_result_;
+ }
+ test_result->RecordProperty(xml_element, test_property);
+}
+
+#if GTEST_HAS_DEATH_TEST
+// Disables event forwarding if the control is currently in a death test
+// subprocess. Must not be called before InitGoogleTest.
+void UnitTestImpl::SuppressTestEventsIfInSubprocess() {
+ if (internal_run_death_test_flag_.get() != NULL)
+ listeners()->SuppressEventForwarding();
+}
+#endif // GTEST_HAS_DEATH_TEST
+
+// Initializes event listeners performing XML output as specified by
+// UnitTestOptions. Must not be called before InitGoogleTest.
+void UnitTestImpl::ConfigureXmlOutput() {
+ const std::string& output_format = UnitTestOptions::GetOutputFormat();
+ if (output_format == "xml") {
+ listeners()->SetDefaultXmlGenerator(new XmlUnitTestResultPrinter(
+ UnitTestOptions::GetAbsolutePathToOutputFile().c_str()));
+ } else if (output_format != "") {
+ printf("WARNING: unrecognized output format \"%s\" ignored.\n",
+ output_format.c_str());
+ fflush(stdout);
+ }
+}
+
+#if GTEST_CAN_STREAM_RESULTS_
+// Initializes event listeners for streaming test results in string form.
+// Must not be called before InitGoogleTest.
+void UnitTestImpl::ConfigureStreamingOutput() {
+ const std::string& target = GTEST_FLAG(stream_result_to);
+ if (!target.empty()) {
+ const size_t pos = target.find(':');
+ if (pos != std::string::npos) {
+ listeners()->Append(new StreamingListener(target.substr(0, pos),
+ target.substr(pos+1)));
+ } else {
+ printf("WARNING: unrecognized streaming target \"%s\" ignored.\n",
+ target.c_str());
+ fflush(stdout);
+ }
+ }
+}
+#endif // GTEST_CAN_STREAM_RESULTS_
+
+// Performs initialization dependent upon flag values obtained in
+// ParseGoogleTestFlagsOnly. Is called from InitGoogleTest after the call to
+// ParseGoogleTestFlagsOnly. In case a user neglects to call InitGoogleTest
+// this function is also called from RunAllTests. Since this function can be
+// called more than once, it has to be idempotent.
+void UnitTestImpl::PostFlagParsingInit() {
+ // Ensures that this function does not execute more than once.
+ if (!post_flag_parse_init_performed_) {
+ post_flag_parse_init_performed_ = true;
+
+#if GTEST_HAS_DEATH_TEST
+ InitDeathTestSubprocessControlInfo();
+ SuppressTestEventsIfInSubprocess();
+#endif // GTEST_HAS_DEATH_TEST
+
+ // Registers parameterized tests. This makes parameterized tests
+ // available to the UnitTest reflection API without running
+ // RUN_ALL_TESTS.
+ RegisterParameterizedTests();
+
+ // Configures listeners for XML output. This makes it possible for users
+ // to shut down the default XML output before invoking RUN_ALL_TESTS.
+ ConfigureXmlOutput();
+
+#if GTEST_CAN_STREAM_RESULTS_
+ // Configures listeners for streaming test results to the specified server.
+ ConfigureStreamingOutput();
+#endif // GTEST_CAN_STREAM_RESULTS_
+ }
+}
+
+// A predicate that checks the name of a TestCase against a known
+// value.
+//
+// This is used for implementation of the UnitTest class only. We put
+// it in the anonymous namespace to prevent polluting the outer
+// namespace.
+//
+// TestCaseNameIs is copyable.
+class TestCaseNameIs {
+ public:
+ // Constructor.
+ explicit TestCaseNameIs(const std::string& name)
+ : name_(name) {}
+
+ // Returns true iff the name of test_case matches name_.
+ bool operator()(const TestCase* test_case) const {
+ return test_case != NULL && strcmp(test_case->name(), name_.c_str()) == 0;
+ }
+
+ private:
+ std::string name_;
+};
+
+// Finds and returns a TestCase with the given name. If one doesn't
+// exist, creates one and returns it. It's the CALLER'S
+// RESPONSIBILITY to ensure that this function is only called WHEN THE
+// TESTS ARE NOT SHUFFLED.
+//
+// Arguments:
+//
+// test_case_name: name of the test case
+// type_param: the name of the test case's type parameter, or NULL if
+// this is not a typed or a type-parameterized test case.
+// set_up_tc: pointer to the function that sets up the test case
+// tear_down_tc: pointer to the function that tears down the test case
+TestCase* UnitTestImpl::GetTestCase(const char* test_case_name,
+ const char* type_param,
+ Test::SetUpTestCaseFunc set_up_tc,
+ Test::TearDownTestCaseFunc tear_down_tc) {
+ // Can we find a TestCase with the given name?
+ const std::vector<TestCase*>::const_iterator test_case =
+ std::find_if(test_cases_.begin(), test_cases_.end(),
+ TestCaseNameIs(test_case_name));
+
+ if (test_case != test_cases_.end())
+ return *test_case;
+
+ // No. Let's create one.
+ TestCase* const new_test_case =
+ new TestCase(test_case_name, type_param, set_up_tc, tear_down_tc);
+
+ // Is this a death test case?
+ if (internal::UnitTestOptions::MatchesFilter(test_case_name,
+ kDeathTestCaseFilter)) {
+ // Yes. Inserts the test case after the last death test case
+ // defined so far. This only works when the test cases haven't
+ // been shuffled. Otherwise we may end up running a death test
+ // after a non-death test.
+ ++last_death_test_case_;
+ test_cases_.insert(test_cases_.begin() + last_death_test_case_,
+ new_test_case);
+ } else {
+ // No. Appends to the end of the list.
+ test_cases_.push_back(new_test_case);
+ }
+
+ test_case_indices_.push_back(static_cast<int>(test_case_indices_.size()));
+ return new_test_case;
+}
+
+// Helpers for setting up / tearing down the given environment. They
+// are for use in the ForEach() function.
+static void SetUpEnvironment(Environment* env) { env->SetUp(); }
+static void TearDownEnvironment(Environment* env) { env->TearDown(); }
+
+// Runs all tests in this UnitTest object, prints the result, and
+// returns true if all tests are successful. If any exception is
+// thrown during a test, the test is considered to be failed, but the
+// rest of the tests will still be run.
+//
+// When parameterized tests are enabled, it expands and registers
+// parameterized tests first in RegisterParameterizedTests().
+// All other functions called from RunAllTests() may safely assume that
+// parameterized tests are ready to be counted and run.
+bool UnitTestImpl::RunAllTests() {
+ // Makes sure InitGoogleTest() was called.
+ if (!GTestIsInitialized()) {
+ printf("%s",
+ "\nThis test program did NOT call ::testing::InitGoogleTest "
+ "before calling RUN_ALL_TESTS(). Please fix it.\n");
+ return false;
+ }
+
+ // Do not run any test if the --help flag was specified.
+ if (g_help_flag)
+ return true;
+
+ // Repeats the call to the post-flag parsing initialization in case the
+ // user didn't call InitGoogleTest.
+ PostFlagParsingInit();
+
+ // Even if sharding is not on, test runners may want to use the
+ // GTEST_SHARD_STATUS_FILE to query whether the test supports the sharding
+ // protocol.
+ internal::WriteToShardStatusFileIfNeeded();
+
+ // True iff we are in a subprocess for running a thread-safe-style
+ // death test.
+ bool in_subprocess_for_death_test = false;
+
+#if GTEST_HAS_DEATH_TEST
+ in_subprocess_for_death_test = (internal_run_death_test_flag_.get() != NULL);
+#endif // GTEST_HAS_DEATH_TEST
+
+ const bool should_shard = ShouldShard(kTestTotalShards, kTestShardIndex,
+ in_subprocess_for_death_test);
+
+ // Compares the full test names with the filter to decide which
+ // tests to run.
+ const bool has_tests_to_run = FilterTests(should_shard
+ ? HONOR_SHARDING_PROTOCOL
+ : IGNORE_SHARDING_PROTOCOL) > 0;
+
+ // Lists the tests and exits if the --gtest_list_tests flag was specified.
+ if (GTEST_FLAG(list_tests)) {
+ // This must be called *after* FilterTests() has been called.
+ ListTestsMatchingFilter();
+ return true;
+ }
+
+ random_seed_ = GTEST_FLAG(shuffle) ?
+ GetRandomSeedFromFlag(GTEST_FLAG(random_seed)) : 0;
+
+ // True iff at least one test has failed.
+ bool failed = false;
+
+ TestEventListener* repeater = listeners()->repeater();
+
+ start_timestamp_ = GetTimeInMillis();
+ repeater->OnTestProgramStart(*parent_);
+
+ // How many times to repeat the tests? We don't want to repeat them
+ // when we are inside the subprocess of a death test.
+ const int repeat = in_subprocess_for_death_test ? 1 : GTEST_FLAG(repeat);
+ // Repeats forever if the repeat count is negative.
+ const bool forever = repeat < 0;
+ for (int i = 0; forever || i != repeat; i++) {
+ // We want to preserve failures generated by ad-hoc test
+ // assertions executed before RUN_ALL_TESTS().
+ ClearNonAdHocTestResult();
+
+ const TimeInMillis start = GetTimeInMillis();
+
+ // Shuffles test cases and tests if requested.
+ if (has_tests_to_run && GTEST_FLAG(shuffle)) {
+ random()->Reseed(random_seed_);
+ // This should be done before calling OnTestIterationStart(),
+ // such that a test event listener can see the actual test order
+ // in the event.
+ ShuffleTests();
+ }
+
+ // Tells the unit test event listeners that the tests are about to start.
+ repeater->OnTestIterationStart(*parent_, i);
+
+ // Runs each test case if there is at least one test to run.
+ if (has_tests_to_run) {
+ // Sets up all environments beforehand.
+ repeater->OnEnvironmentsSetUpStart(*parent_);
+ ForEach(environments_, SetUpEnvironment);
+ repeater->OnEnvironmentsSetUpEnd(*parent_);
+
+ // Runs the tests only if there was no fatal failure during global
+ // set-up.
+ if (!Test::HasFatalFailure()) {
+ for (int test_index = 0; test_index < total_test_case_count();
+ test_index++) {
+ GetMutableTestCase(test_index)->Run();
+ }
+ }
+
+ // Tears down all environments in reverse order afterwards.
+ repeater->OnEnvironmentsTearDownStart(*parent_);
+ std::for_each(environments_.rbegin(), environments_.rend(),
+ TearDownEnvironment);
+ repeater->OnEnvironmentsTearDownEnd(*parent_);
+ }
+
+ elapsed_time_ = GetTimeInMillis() - start;
+
+ // Tells the unit test event listener that the tests have just finished.
+ repeater->OnTestIterationEnd(*parent_, i);
+
+ // Gets the result and clears it.
+ if (!Passed()) {
+ failed = true;
+ }
+
+ // Restores the original test order after the iteration. This
+ // allows the user to quickly repro a failure that happens in the
+ // N-th iteration without repeating the first (N - 1) iterations.
+ // This is not enclosed in "if (GTEST_FLAG(shuffle)) { ... }", in
+ // case the user somehow changes the value of the flag somewhere
+ // (it's always safe to unshuffle the tests).
+ UnshuffleTests();
+
+ if (GTEST_FLAG(shuffle)) {
+ // Picks a new random seed for each iteration.
+ random_seed_ = GetNextRandomSeed(random_seed_);
+ }
+ }
+
+ repeater->OnTestProgramEnd(*parent_);
+
+ return !failed;
+}
+
+// Reads the GTEST_SHARD_STATUS_FILE environment variable, and creates the file
+// if the variable is present. If a file already exists at this location, this
+// function will write over it. If the variable is present, but the file cannot
+// be created, prints an error and exits.
+void WriteToShardStatusFileIfNeeded() {
+ const char* const test_shard_file = posix::GetEnv(kTestShardStatusFile);
+ if (test_shard_file != NULL) {
+ FILE* const file = posix::FOpen(test_shard_file, "w");
+ if (file == NULL) {
+ ColoredPrintf(COLOR_RED,
+ "Could not write to the test shard status file \"%s\" "
+ "specified by the %s environment variable.\n",
+ test_shard_file, kTestShardStatusFile);
+ fflush(stdout);
+ exit(EXIT_FAILURE);
+ }
+ fclose(file);
+ }
+}
+
+// Checks whether sharding is enabled by examining the relevant
+// environment variable values. If the variables are present,
+// but inconsistent (i.e., shard_index >= total_shards), prints
+// an error and exits. If in_subprocess_for_death_test, sharding is
+// disabled because it must only be applied to the original test
+// process. Otherwise, we could filter out death tests we intended to execute.
+bool ShouldShard(const char* total_shards_env,
+ const char* shard_index_env,
+ bool in_subprocess_for_death_test) {
+ if (in_subprocess_for_death_test) {
+ return false;
+ }
+
+ const Int32 total_shards = Int32FromEnvOrDie(total_shards_env, -1);
+ const Int32 shard_index = Int32FromEnvOrDie(shard_index_env, -1);
+
+ if (total_shards == -1 && shard_index == -1) {
+ return false;
+ } else if (total_shards == -1 && shard_index != -1) {
+ const Message msg = Message()
+ << "Invalid environment variables: you have "
+ << kTestShardIndex << " = " << shard_index
+ << ", but have left " << kTestTotalShards << " unset.\n";
+ ColoredPrintf(COLOR_RED, msg.GetString().c_str());
+ fflush(stdout);
+ exit(EXIT_FAILURE);
+ } else if (total_shards != -1 && shard_index == -1) {
+ const Message msg = Message()
+ << "Invalid environment variables: you have "
+ << kTestTotalShards << " = " << total_shards
+ << ", but have left " << kTestShardIndex << " unset.\n";
+ ColoredPrintf(COLOR_RED, msg.GetString().c_str());
+ fflush(stdout);
+ exit(EXIT_FAILURE);
+ } else if (shard_index < 0 || shard_index >= total_shards) {
+ const Message msg = Message()
+ << "Invalid environment variables: we require 0 <= "
+ << kTestShardIndex << " < " << kTestTotalShards
+ << ", but you have " << kTestShardIndex << "=" << shard_index
+ << ", " << kTestTotalShards << "=" << total_shards << ".\n";
+ ColoredPrintf(COLOR_RED, msg.GetString().c_str());
+ fflush(stdout);
+ exit(EXIT_FAILURE);
+ }
+
+ return total_shards > 1;
+}
+
+// Parses the environment variable var as an Int32. If it is unset,
+// returns default_val. If it is not an Int32, prints an error
+// and aborts.
+Int32 Int32FromEnvOrDie(const char* var, Int32 default_val) {
+ const char* str_val = posix::GetEnv(var);
+ if (str_val == NULL) {
+ return default_val;
+ }
+
+ Int32 result;
+ if (!ParseInt32(Message() << "The value of environment variable " << var,
+ str_val, &result)) {
+ exit(EXIT_FAILURE);
+ }
+ return result;
+}
+
+// Given the total number of shards, the shard index, and the test id,
+// returns true iff the test should be run on this shard. The test id is
+// some arbitrary but unique non-negative integer assigned to each test
+// method. Assumes that 0 <= shard_index < total_shards.
+bool ShouldRunTestOnShard(int total_shards, int shard_index, int test_id) {
+ return (test_id % total_shards) == shard_index;
+}
+
+// Compares the name of each test with the user-specified filter to
+// decide whether the test should be run, then records the result in
+// each TestCase and TestInfo object.
+// If shard_tests == true, further filters tests based on sharding
+// variables in the environment - see
+// http://code.google.com/p/googletest/wiki/GoogleTestAdvancedGuide.
+// Returns the number of tests that should run.
+int UnitTestImpl::FilterTests(ReactionToSharding shard_tests) {
+ const Int32 total_shards = shard_tests == HONOR_SHARDING_PROTOCOL ?
+ Int32FromEnvOrDie(kTestTotalShards, -1) : -1;
+ const Int32 shard_index = shard_tests == HONOR_SHARDING_PROTOCOL ?
+ Int32FromEnvOrDie(kTestShardIndex, -1) : -1;
+
+ // num_runnable_tests are the number of tests that will
+ // run across all shards (i.e., match filter and are not disabled).
+ // num_selected_tests are the number of tests to be run on
+ // this shard.
+ int num_runnable_tests = 0;
+ int num_selected_tests = 0;
+ for (size_t i = 0; i < test_cases_.size(); i++) {
+ TestCase* const test_case = test_cases_[i];
+ const std::string &test_case_name = test_case->name();
+ test_case->set_should_run(false);
+
+ for (size_t j = 0; j < test_case->test_info_list().size(); j++) {
+ TestInfo* const test_info = test_case->test_info_list()[j];
+ const std::string test_name(test_info->name());
+ // A test is disabled if test case name or test name matches
+ // kDisableTestFilter.
+ const bool is_disabled =
+ internal::UnitTestOptions::MatchesFilter(test_case_name,
+ kDisableTestFilter) ||
+ internal::UnitTestOptions::MatchesFilter(test_name,
+ kDisableTestFilter);
+ test_info->is_disabled_ = is_disabled;
+
+ const bool matches_filter =
+ internal::UnitTestOptions::FilterMatchesTest(test_case_name,
+ test_name);
+ test_info->matches_filter_ = matches_filter;
+
+ const bool is_runnable =
+ (GTEST_FLAG(also_run_disabled_tests) || !is_disabled) &&
+ matches_filter;
+
+ const bool is_selected = is_runnable &&
+ (shard_tests == IGNORE_SHARDING_PROTOCOL ||
+ ShouldRunTestOnShard(total_shards, shard_index,
+ num_runnable_tests));
+
+ num_runnable_tests += is_runnable;
+ num_selected_tests += is_selected;
+
+ test_info->should_run_ = is_selected;
+ test_case->set_should_run(test_case->should_run() || is_selected);
+ }
+ }
+ return num_selected_tests;
+}
+
+// Prints the given C-string on a single line by replacing all '\n'
+// characters with string "\\n". If the output takes more than
+// max_length characters, only prints the first max_length characters
+// and "...".
+static void PrintOnOneLine(const char* str, int max_length) {
+ if (str != NULL) {
+ for (int i = 0; *str != '\0'; ++str) {
+ if (i >= max_length) {
+ printf("...");
+ break;
+ }
+ if (*str == '\n') {
+ printf("\\n");
+ i += 2;
+ } else {
+ printf("%c", *str);
+ ++i;
+ }
+ }
+ }
+}
+
+// Prints the names of the tests matching the user-specified filter flag.
+void UnitTestImpl::ListTestsMatchingFilter() {
+ // Print at most this many characters for each type/value parameter.
+ const int kMaxParamLength = 250;
+
+ for (size_t i = 0; i < test_cases_.size(); i++) {
+ const TestCase* const test_case = test_cases_[i];
+ bool printed_test_case_name = false;
+
+ for (size_t j = 0; j < test_case->test_info_list().size(); j++) {
+ const TestInfo* const test_info =
+ test_case->test_info_list()[j];
+ if (test_info->matches_filter_) {
+ if (!printed_test_case_name) {
+ printed_test_case_name = true;
+ printf("%s.", test_case->name());
+ if (test_case->type_param() != NULL) {
+ printf(" # %s = ", kTypeParamLabel);
+ // We print the type parameter on a single line to make
+ // the output easy to parse by a program.
+ PrintOnOneLine(test_case->type_param(), kMaxParamLength);
+ }
+ printf("\n");
+ }
+ printf(" %s", test_info->name());
+ if (test_info->value_param() != NULL) {
+ printf(" # %s = ", kValueParamLabel);
+ // We print the value parameter on a single line to make the
+ // output easy to parse by a program.
+ PrintOnOneLine(test_info->value_param(), kMaxParamLength);
+ }
+ printf("\n");
+ }
+ }
+ }
+ fflush(stdout);
+}
+
+// Sets the OS stack trace getter.
+//
+// Does nothing if the input and the current OS stack trace getter are
+// the same; otherwise, deletes the old getter and makes the input the
+// current getter.
+void UnitTestImpl::set_os_stack_trace_getter(
+ OsStackTraceGetterInterface* getter) {
+ if (os_stack_trace_getter_ != getter) {
+ delete os_stack_trace_getter_;
+ os_stack_trace_getter_ = getter;
+ }
+}
+
+// Returns the current OS stack trace getter if it is not NULL;
+// otherwise, creates an OsStackTraceGetter, makes it the current
+// getter, and returns it.
+OsStackTraceGetterInterface* UnitTestImpl::os_stack_trace_getter() {
+ if (os_stack_trace_getter_ == NULL) {
+ os_stack_trace_getter_ = new OsStackTraceGetter;
+ }
+
+ return os_stack_trace_getter_;
+}
+
+// Returns the TestResult for the test that's currently running, or
+// the TestResult for the ad hoc test if no test is running.
+TestResult* UnitTestImpl::current_test_result() {
+ return current_test_info_ ?
+ &(current_test_info_->result_) : &ad_hoc_test_result_;
+}
+
+// Shuffles all test cases, and the tests within each test case,
+// making sure that death tests are still run first.
+void UnitTestImpl::ShuffleTests() {
+ // Shuffles the death test cases.
+ ShuffleRange(random(), 0, last_death_test_case_ + 1, &test_case_indices_);
+
+ // Shuffles the non-death test cases.
+ ShuffleRange(random(), last_death_test_case_ + 1,
+ static_cast<int>(test_cases_.size()), &test_case_indices_);
+
+ // Shuffles the tests inside each test case.
+ for (size_t i = 0; i < test_cases_.size(); i++) {
+ test_cases_[i]->ShuffleTests(random());
+ }
+}
+
+// Restores the test cases and tests to their order before the first shuffle.
+void UnitTestImpl::UnshuffleTests() {
+ for (size_t i = 0; i < test_cases_.size(); i++) {
+ // Unshuffles the tests in each test case.
+ test_cases_[i]->UnshuffleTests();
+ // Resets the index of each test case.
+ test_case_indices_[i] = static_cast<int>(i);
+ }
+}
+
+// Returns the current OS stack trace as an std::string.
+//
+// The maximum number of stack frames to be included is specified by
+// the gtest_stack_trace_depth flag. The skip_count parameter
+// specifies the number of top frames to be skipped, which doesn't
+// count against the number of frames to be included.
+//
+// For example, if Foo() calls Bar(), which in turn calls
+// GetCurrentOsStackTraceExceptTop(..., 1), Foo() will be included in
+// the trace but Bar() and GetCurrentOsStackTraceExceptTop() won't.
+std::string GetCurrentOsStackTraceExceptTop(UnitTest* /*unit_test*/,
+ int skip_count) {
+ // We pass skip_count + 1 to skip this wrapper function in addition
+ // to what the user really wants to skip.
+ return GetUnitTestImpl()->CurrentOsStackTraceExceptTop(skip_count + 1);
+}
+
+// Used by the GTEST_SUPPRESS_UNREACHABLE_CODE_WARNING_BELOW_ macro to
+// suppress unreachable code warnings.
+namespace {
+class ClassUniqueToAlwaysTrue {};
+}
+
+bool IsTrue(bool condition) { return condition; }
+
+bool AlwaysTrue() {
+#if GTEST_HAS_EXCEPTIONS
+ // This condition is always false so AlwaysTrue() never actually throws,
+ // but it makes the compiler think that it may throw.
+ if (IsTrue(false))
+ throw ClassUniqueToAlwaysTrue();
+#endif // GTEST_HAS_EXCEPTIONS
+ return true;
+}
+
+// If *pstr starts with the given prefix, modifies *pstr to be right
+// past the prefix and returns true; otherwise leaves *pstr unchanged
+// and returns false. None of pstr, *pstr, and prefix can be NULL.
+bool SkipPrefix(const char* prefix, const char** pstr) {
+ const size_t prefix_len = strlen(prefix);
+ if (strncmp(*pstr, prefix, prefix_len) == 0) {
+ *pstr += prefix_len;
+ return true;
+ }
+ return false;
+}
+
+// Parses a string as a command line flag. The string should have
+// the format "--flag=value". When def_optional is true, the "=value"
+// part can be omitted.
+//
+// Returns the value of the flag, or NULL if the parsing failed.
+const char* ParseFlagValue(const char* str,
+ const char* flag,
+ bool def_optional) {
+ // str and flag must not be NULL.
+ if (str == NULL || flag == NULL) return NULL;
+
+ // The flag must start with "--" followed by GTEST_FLAG_PREFIX_.
+ const std::string flag_str = std::string("--") + GTEST_FLAG_PREFIX_ + flag;
+ const size_t flag_len = flag_str.length();
+ if (strncmp(str, flag_str.c_str(), flag_len) != 0) return NULL;
+
+ // Skips the flag name.
+ const char* flag_end = str + flag_len;
+
+ // When def_optional is true, it's OK to not have a "=value" part.
+ if (def_optional && (flag_end[0] == '\0')) {
+ return flag_end;
+ }
+
+ // If def_optional is true and there are more characters after the
+ // flag name, or if def_optional is false, there must be a '=' after
+ // the flag name.
+ if (flag_end[0] != '=') return NULL;
+
+ // Returns the string after "=".
+ return flag_end + 1;
+}
+
+// Parses a string for a bool flag, in the form of either
+// "--flag=value" or "--flag".
+//
+// In the former case, the value is taken as true as long as it does
+// not start with '0', 'f', or 'F'.
+//
+// In the latter case, the value is taken as true.
+//
+// On success, stores the value of the flag in *value, and returns
+// true. On failure, returns false without changing *value.
+bool ParseBoolFlag(const char* str, const char* flag, bool* value) {
+ // Gets the value of the flag as a string.
+ const char* const value_str = ParseFlagValue(str, flag, true);
+
+ // Aborts if the parsing failed.
+ if (value_str == NULL) return false;
+
+ // Converts the string value to a bool.
+ *value = !(*value_str == '0' || *value_str == 'f' || *value_str == 'F');
+ return true;
+}
+
+// Parses a string for an Int32 flag, in the form of
+// "--flag=value".
+//
+// On success, stores the value of the flag in *value, and returns
+// true. On failure, returns false without changing *value.
+bool ParseInt32Flag(const char* str, const char* flag, Int32* value) {
+ // Gets the value of the flag as a string.
+ const char* const value_str = ParseFlagValue(str, flag, false);
+
+ // Aborts if the parsing failed.
+ if (value_str == NULL) return false;
+
+ // Sets *value to the value of the flag.
+ return ParseInt32(Message() << "The value of flag --" << flag,
+ value_str, value);
+}
+
+// Parses a string for a string flag, in the form of
+// "--flag=value".
+//
+// On success, stores the value of the flag in *value, and returns
+// true. On failure, returns false without changing *value.
+bool ParseStringFlag(const char* str, const char* flag, std::string* value) {
+ // Gets the value of the flag as a string.
+ const char* const value_str = ParseFlagValue(str, flag, false);
+
+ // Aborts if the parsing failed.
+ if (value_str == NULL) return false;
+
+ // Sets *value to the value of the flag.
+ *value = value_str;
+ return true;
+}
+
+// Determines whether a string has a prefix that Google Test uses for its
+// flags, i.e., starts with GTEST_FLAG_PREFIX_ or GTEST_FLAG_PREFIX_DASH_.
+// If Google Test detects that a command line flag has its prefix but is not
+// recognized, it will print its help message. Flags starting with
+// GTEST_INTERNAL_PREFIX_ followed by "internal_" are considered Google Test
+// internal flags and do not trigger the help message.
+static bool HasGoogleTestFlagPrefix(const char* str) {
+ return (SkipPrefix("--", &str) ||
+ SkipPrefix("-", &str) ||
+ SkipPrefix("/", &str)) &&
+ !SkipPrefix(GTEST_FLAG_PREFIX_ "internal_", &str) &&
+ (SkipPrefix(GTEST_FLAG_PREFIX_, &str) ||
+ SkipPrefix(GTEST_FLAG_PREFIX_DASH_, &str));
+}
+
+// Prints a string containing code-encoded text. The following escape
+// sequences can be used in the string to control the text color:
+//
+// @@ prints a single '@' character.
+// @R changes the color to red.
+// @G changes the color to green.
+// @Y changes the color to yellow.
+// @D changes to the default terminal text color.
+//
+// TODO(wan at google.com): Write tests for this once we add stdout
+// capturing to Google Test.
+static void PrintColorEncoded(const char* str) {
+ GTestColor color = COLOR_DEFAULT; // The current color.
+
+ // Conceptually, we split the string into segments divided by escape
+ // sequences. Then we print one segment at a time. At the end of
+ // each iteration, the str pointer advances to the beginning of the
+ // next segment.
+ for (;;) {
+ const char* p = strchr(str, '@');
+ if (p == NULL) {
+ ColoredPrintf(color, "%s", str);
+ return;
+ }
+
+ ColoredPrintf(color, "%s", std::string(str, p).c_str());
+
+ const char ch = p[1];
+ str = p + 2;
+ if (ch == '@') {
+ ColoredPrintf(color, "@");
+ } else if (ch == 'D') {
+ color = COLOR_DEFAULT;
+ } else if (ch == 'R') {
+ color = COLOR_RED;
+ } else if (ch == 'G') {
+ color = COLOR_GREEN;
+ } else if (ch == 'Y') {
+ color = COLOR_YELLOW;
+ } else {
+ --str;
+ }
+ }
+}
+
+static const char kColorEncodedHelpMessage[] =
+"This program contains tests written using " GTEST_NAME_ ". You can use the\n"
+"following command line flags to control its behavior:\n"
+"\n"
+"Test Selection:\n"
+" @G--" GTEST_FLAG_PREFIX_ "list_tests at D\n"
+" List the names of all tests instead of running them. The name of\n"
+" TEST(Foo, Bar) is \"Foo.Bar\".\n"
+" @G--" GTEST_FLAG_PREFIX_ "filter=@YPOSTIVE_PATTERNS"
+ "[@G- at YNEGATIVE_PATTERNS]@D\n"
+" Run only the tests whose name matches one of the positive patterns but\n"
+" none of the negative patterns. '?' matches any single character; '*'\n"
+" matches any substring; ':' separates two patterns.\n"
+" @G--" GTEST_FLAG_PREFIX_ "also_run_disabled_tests at D\n"
+" Run all disabled tests too.\n"
+"\n"
+"Test Execution:\n"
+" @G--" GTEST_FLAG_PREFIX_ "repeat=@Y[COUNT]@D\n"
+" Run the tests repeatedly; use a negative count to repeat forever.\n"
+" @G--" GTEST_FLAG_PREFIX_ "shuffle at D\n"
+" Randomize tests' orders on every iteration.\n"
+" @G--" GTEST_FLAG_PREFIX_ "random_seed=@Y[NUMBER]@D\n"
+" Random number seed to use for shuffling test orders (between 1 and\n"
+" 99999, or 0 to use a seed based on the current time).\n"
+"\n"
+"Test Output:\n"
+" @G--" GTEST_FLAG_PREFIX_ "color=@Y(@Gyes at Y|@Gno at Y|@Gauto at Y)@D\n"
+" Enable/disable colored output. The default is @Gauto at D.\n"
+" - at G-" GTEST_FLAG_PREFIX_ "print_time=0 at D\n"
+" Don't print the elapsed time of each test.\n"
+" @G--" GTEST_FLAG_PREFIX_ "output=xml at Y[@G:@YDIRECTORY_PATH at G"
+ GTEST_PATH_SEP_ "@Y|@G:@YFILE_PATH]@D\n"
+" Generate an XML report in the given directory or with the given file\n"
+" name. @YFILE_PATH at D defaults to @Gtest_details.xml at D.\n"
+#if GTEST_CAN_STREAM_RESULTS_
+" @G--" GTEST_FLAG_PREFIX_ "stream_result_to=@YHOST at G:@YPORT at D\n"
+" Stream test results to the given server.\n"
+#endif // GTEST_CAN_STREAM_RESULTS_
+"\n"
+"Assertion Behavior:\n"
+#if GTEST_HAS_DEATH_TEST && !GTEST_OS_WINDOWS
+" @G--" GTEST_FLAG_PREFIX_ "death_test_style=@Y(@Gfast at Y|@Gthreadsafe at Y)@D\n"
+" Set the default death test style.\n"
+#endif // GTEST_HAS_DEATH_TEST && !GTEST_OS_WINDOWS
+" @G--" GTEST_FLAG_PREFIX_ "break_on_failure at D\n"
+" Turn assertion failures into debugger break-points.\n"
+" @G--" GTEST_FLAG_PREFIX_ "throw_on_failure at D\n"
+" Turn assertion failures into C++ exceptions.\n"
+" @G--" GTEST_FLAG_PREFIX_ "catch_exceptions=0 at D\n"
+" Do not report exceptions as test failures. Instead, allow them\n"
+" to crash the program or throw a pop-up (on Windows).\n"
+"\n"
+"Except for @G--" GTEST_FLAG_PREFIX_ "list_tests at D, you can alternatively set "
+ "the corresponding\n"
+"environment variable of a flag (all letters in upper-case). For example, to\n"
+"disable colored text output, you can either specify @G--" GTEST_FLAG_PREFIX_
+ "color=no at D or set\n"
+"the @G" GTEST_FLAG_PREFIX_UPPER_ "COLOR at D environment variable to @Gno at D.\n"
+"\n"
+"For more information, please read the " GTEST_NAME_ " documentation at\n"
+"@G" GTEST_PROJECT_URL_ "@D. If you find a bug in " GTEST_NAME_ "\n"
+"(not one in your own code or tests), please report it to\n"
+"@G<" GTEST_DEV_EMAIL_ ">@D.\n";
+
+// Parses the command line for Google Test flags, without initializing
+// other parts of Google Test. The type parameter CharType can be
+// instantiated to either char or wchar_t.
+template <typename CharType>
+void ParseGoogleTestFlagsOnlyImpl(int* argc, CharType** argv) {
+ for (int i = 1; i < *argc; i++) {
+ const std::string arg_string = StreamableToString(argv[i]);
+ const char* const arg = arg_string.c_str();
+
+ using internal::ParseBoolFlag;
+ using internal::ParseInt32Flag;
+ using internal::ParseStringFlag;
+
+ // Do we see a Google Test flag?
+ if (ParseBoolFlag(arg, kAlsoRunDisabledTestsFlag,
+ >EST_FLAG(also_run_disabled_tests)) ||
+ ParseBoolFlag(arg, kBreakOnFailureFlag,
+ >EST_FLAG(break_on_failure)) ||
+ ParseBoolFlag(arg, kCatchExceptionsFlag,
+ >EST_FLAG(catch_exceptions)) ||
+ ParseStringFlag(arg, kColorFlag, >EST_FLAG(color)) ||
+ ParseStringFlag(arg, kDeathTestStyleFlag,
+ >EST_FLAG(death_test_style)) ||
+ ParseBoolFlag(arg, kDeathTestUseFork,
+ >EST_FLAG(death_test_use_fork)) ||
+ ParseStringFlag(arg, kFilterFlag, >EST_FLAG(filter)) ||
+ ParseStringFlag(arg, kInternalRunDeathTestFlag,
+ >EST_FLAG(internal_run_death_test)) ||
+ ParseBoolFlag(arg, kListTestsFlag, >EST_FLAG(list_tests)) ||
+ ParseStringFlag(arg, kOutputFlag, >EST_FLAG(output)) ||
+ ParseBoolFlag(arg, kPrintTimeFlag, >EST_FLAG(print_time)) ||
+ ParseInt32Flag(arg, kRandomSeedFlag, >EST_FLAG(random_seed)) ||
+ ParseInt32Flag(arg, kRepeatFlag, >EST_FLAG(repeat)) ||
+ ParseBoolFlag(arg, kShuffleFlag, >EST_FLAG(shuffle)) ||
+ ParseInt32Flag(arg, kStackTraceDepthFlag,
+ >EST_FLAG(stack_trace_depth)) ||
+ ParseStringFlag(arg, kStreamResultToFlag,
+ >EST_FLAG(stream_result_to)) ||
+ ParseBoolFlag(arg, kThrowOnFailureFlag,
+ >EST_FLAG(throw_on_failure))
+ ) {
+ // Yes. Shift the remainder of the argv list left by one. Note
+ // that argv has (*argc + 1) elements, the last one always being
+ // NULL. The following loop moves the trailing NULL element as
+ // well.
+ for (int j = i; j != *argc; j++) {
+ argv[j] = argv[j + 1];
+ }
+
+ // Decrements the argument count.
+ (*argc)--;
+
+ // We also need to decrement the iterator as we just removed
+ // an element.
+ i--;
+ } else if (arg_string == "--help" || arg_string == "-h" ||
+ arg_string == "-?" || arg_string == "/?" ||
+ HasGoogleTestFlagPrefix(arg)) {
+ // Both help flag and unrecognized Google Test flags (excluding
+ // internal ones) trigger help display.
+ g_help_flag = true;
+ }
+ }
+
+ if (g_help_flag) {
+ // We print the help here instead of in RUN_ALL_TESTS(), as the
+ // latter may not be called at all if the user is using Google
+ // Test with another testing framework.
+ PrintColorEncoded(kColorEncodedHelpMessage);
+ }
+}
+
+// Parses the command line for Google Test flags, without initializing
+// other parts of Google Test.
+void ParseGoogleTestFlagsOnly(int* argc, char** argv) {
+ ParseGoogleTestFlagsOnlyImpl(argc, argv);
+}
+void ParseGoogleTestFlagsOnly(int* argc, wchar_t** argv) {
+ ParseGoogleTestFlagsOnlyImpl(argc, argv);
+}
+
+// The internal implementation of InitGoogleTest().
+//
+// The type parameter CharType can be instantiated to either char or
+// wchar_t.
+template <typename CharType>
+void InitGoogleTestImpl(int* argc, CharType** argv) {
+ g_init_gtest_count++;
+
+ // We don't want to run the initialization code twice.
+ if (g_init_gtest_count != 1) return;
+
+ if (*argc <= 0) return;
+
+ internal::g_executable_path = internal::StreamableToString(argv[0]);
+
+#if GTEST_HAS_DEATH_TEST
+
+ g_argvs.clear();
+ for (int i = 0; i != *argc; i++) {
+ g_argvs.push_back(StreamableToString(argv[i]));
+ }
+
+#endif // GTEST_HAS_DEATH_TEST
+
+ ParseGoogleTestFlagsOnly(argc, argv);
+ GetUnitTestImpl()->PostFlagParsingInit();
+}
+
+} // namespace internal
+
+// Initializes Google Test. This must be called before calling
+// RUN_ALL_TESTS(). In particular, it parses a command line for the
+// flags that Google Test recognizes. Whenever a Google Test flag is
+// seen, it is removed from argv, and *argc is decremented.
+//
+// No value is returned. Instead, the Google Test flag variables are
+// updated.
+//
+// Calling the function for the second time has no user-visible effect.
+void InitGoogleTest(int* argc, char** argv) {
+ internal::InitGoogleTestImpl(argc, argv);
+}
+
+// This overloaded version can be used in Windows programs compiled in
+// UNICODE mode.
+void InitGoogleTest(int* argc, wchar_t** argv) {
+ internal::InitGoogleTestImpl(argc, argv);
+}
+
+} // namespace testing
diff --git a/examples/stand_alone/test_fixture/gtest/src/gtest_main.cc b/examples/stand_alone/test_fixture/gtest/src/gtest_main.cc
new file mode 100644
index 0000000..f302822
--- /dev/null
+++ b/examples/stand_alone/test_fixture/gtest/src/gtest_main.cc
@@ -0,0 +1,38 @@
+// Copyright 2006, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <stdio.h>
+
+#include "gtest/gtest.h"
+
+GTEST_API_ int main(int argc, char **argv) {
+ printf("Running main() from gtest_main.cc\n");
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/examples/stand_alone/test_fixture/test.cc b/examples/stand_alone/test_fixture/test.cc
new file mode 100644
index 0000000..24f50d8
--- /dev/null
+++ b/examples/stand_alone/test_fixture/test.cc
@@ -0,0 +1,38 @@
+/*
+ * 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/test/ServerFixture.hh>
+#include <gazebo/physics/physics.hh>
+
+// Example usage of Gazebo's ServerFixture
+class Test : public gazebo::ServerFixture
+{
+};
+
+// A simple test
+TEST_F(Test, MyTest)
+{
+ Load("worlds/empty.world");
+ gazebo::physics::WorldPtr world = gazebo::physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+}
+
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/examples/stand_alone/transporter/CMakeLists.txt b/examples/stand_alone/transporter/CMakeLists.txt
new file mode 100644
index 0000000..0366942
--- /dev/null
+++ b/examples/stand_alone/transporter/CMakeLists.txt
@@ -0,0 +1,17 @@
+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}")
+
+add_executable(transporter transporter.cc)
+target_link_libraries(transporter ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} )
+
+if (UNIX)
+ target_link_libraries(transporter pthread)
+endif()
diff --git a/examples/stand_alone/transporter/README.md b/examples/stand_alone/transporter/README.md
new file mode 100644
index 0000000..2028d9b
--- /dev/null
+++ b/examples/stand_alone/transporter/README.md
@@ -0,0 +1,19 @@
+This example program will trigger a manual transporter.
+
+# Build
+
+~~~
+mkdir build
+cd build
+cmake ../
+make
+~~~
+
+# Run
+
+1. Start Gazebo from a terminal
+>gazebo worlds/transporter.world
+1. Using the GUI, place an object, such as a sphere, at the origin.
+1. In a new terminal, run the transporter
+>./transporter
+1. The object should move to a new location
diff --git a/examples/stand_alone/transporter/transporter.cc b/examples/stand_alone/transporter/transporter.cc
new file mode 100644
index 0000000..ece3b68
--- /dev/null
+++ b/examples/stand_alone/transporter/transporter.cc
@@ -0,0 +1,46 @@
+/*
+ * 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/gazebo.hh>
+#include <gazebo/transport/transport.hh>
+#include <gazebo/msgs/msgs.hh>
+
+/////////////////////////////////////////////////
+int main(int _argc, char **_argv)
+{
+ // Load gazebo
+ gazebo::setupClient(_argc, _argv);
+
+ // Create our node for communication
+ gazebo::transport::NodePtr node(new gazebo::transport::Node());
+ node->Init();
+
+ // Publish to a Gazebo topic
+ gazebo::transport::PublisherPtr pub =
+ node->Advertise<gazebo::msgs::GzString>("~/transporter");
+
+ // Wait for a subscriber to connect
+ pub->WaitForConnection();
+
+ // Tell pad1 to activate
+ gazebo::msgs::GzString msg;
+ msg.set_data("pad1");
+ pub->Publish(msg);
+
+ // Make sure to shut everything down.
+ gazebo::shutdown();
+}
diff --git a/gazebo/CMakeLists.txt b/gazebo/CMakeLists.txt
index fcf8795..5bcaa5f 100644
--- a/gazebo/CMakeLists.txt
+++ b/gazebo/CMakeLists.txt
@@ -4,6 +4,9 @@ include_directories(SYSTEM
${Boost_INCLUDE_DIRS}
${PROTOBUF_INCLUDE_DIR}
${SDFormat_INCLUDE_DIRS}
+ ${IGNITION-MATH_INCLUDE_DIRS}
+ ${TBB_INCLUDEDIR}
+ ${tinyxml_INCLUDE_DIRS}
)
link_directories(
@@ -11,12 +14,22 @@ link_directories(
${Boost_LIBRARY_DIRS}
${SDFormat_LIBRARY_DIRS}
${PROJECT_BINARY_DIR}/test
+ ${TBB_LIBRARY_DIR}
+ ${CURL_LIBDIR}
)
+if (WIN32)
+ include_directories(${libdl_include_dir})
+endif()
+
if (HAVE_BULLET)
link_directories(${BULLET_LIBRARY_DIRS})
endif()
+if (WIN32)
+ add_definitions(-DBUILDING_DLL)
+endif()
+
add_subdirectory(msgs)
add_subdirectory(math)
add_subdirectory(transport)
@@ -26,6 +39,7 @@ add_subdirectory(gui)
add_subdirectory(physics)
add_subdirectory(sensors)
add_subdirectory(util)
+add_subdirectory(test)
add_dependencies(gazebo_physics gazebo_msgs)
add_dependencies(gazebo_physics_ode gazebo_msgs)
@@ -44,10 +58,10 @@ if(HAVE_DART)
endif()
-gz_add_executable(gzserver server_main.cc Server.cc Master.cc gazebo.cc)
-gz_add_executable(gazebo gazebo_main.cc Server.cc Master.cc gazebo.cc)
+gz_add_executable(gzserver server_main.cc)
target_link_libraries(gzserver
+ libgazebo
gazebo_common
gazebo_util
gazebo_transport
@@ -55,31 +69,56 @@ target_link_libraries(gzserver
gazebo_sensors
gazebo_rendering
gazebo_msgs
- pthread
+ # Extra libs needed when linking statically (e.g., on WIN32)
+ gazebo_physics_ode
+ ${freeimage_LIBRARIES}
+ ${TBB_LIBRARIES}
)
-target_link_libraries(gazebo
- gazebo_common
- gazebo_util
- gazebo_transport
- gazebo_physics
- gazebo_sensors
- gazebo_rendering
- gazebo_msgs
- gazebo_gui
- pthread
-)
+if (HAVE_SIMBODY)
+ target_link_libraries(gzserver gazebo_physics_simbody)
+endif()
+
+if (HAVE_DART)
+ target_link_libraries(gzserver gazebo_physics_dart)
+endif()
+
+if (HAVE_BULLET)
+ target_link_libraries(gzserver gazebo_physics_bullet)
+endif()
+
+if (UNIX)
+ target_link_libraries(gzserver pthread)
+endif()
if ("${CMAKE_BUILD_TYPE}" STREQUAL "CHECK")
target_link_libraries(gzserver gcov)
endif()
gz_install_executable(gzserver)
-gz_install_executable(gazebo)
manpage(gzserver 1)
-manpage(gazebo 1)
-gz_add_library(libgazebo Server.cc Master.cc gazebo.cc)
+# gazebo executable doesn't yet work on Windows
+if (NOT WIN32)
+ gz_add_executable(gazebo gazebo_main.cc)
+ target_link_libraries(gazebo
+ libgazebo
+ libgazebo_client
+ gazebo_common
+ gazebo_util
+ gazebo_transport
+ gazebo_physics
+ gazebo_sensors
+ gazebo_rendering
+ gazebo_msgs
+ gazebo_gui
+ )
+
+ gz_install_executable(gazebo)
+ manpage(gazebo 1)
+endif()
+
+gz_add_library(libgazebo Server.cc Master.cc gazebo.cc gazebo_shared.cc)
set_target_properties(libgazebo PROPERTIES OUTPUT_NAME "gazebo")
target_link_libraries(libgazebo
@@ -91,11 +130,31 @@ target_link_libraries(libgazebo
gazebo_rendering
gazebo_msgs
gazebo_math
- pthread
)
+if (UNIX)
+ target_link_libraries(libgazebo pthread)
+endif()
add_dependencies(libgazebo gazebo_msgs)
gz_install_library(libgazebo)
-set(headers Server.hh Master.hh gazebo.hh gazebo_core.hh)
+gz_add_library(libgazebo_client gazebo_client.cc gazebo_shared.cc)
+set_target_properties(libgazebo_client PROPERTIES OUTPUT_NAME "gazebo_client")
+target_link_libraries(libgazebo_client
+ gazebo_common
+ gazebo_transport
+ gazebo_util
+)
+if (UNIX)
+ target_link_libraries(libgazebo_client pthread)
+endif()
+gz_install_library(libgazebo_client)
+
+set(headers
+ gazebo_client.hh
+ gazebo_core.hh
+ gazebo.hh
+ Master.hh
+ Server.hh
+)
gz_install_includes("" ${headers})
diff --git a/gazebo/Master.cc b/gazebo/Master.cc
index c27583e..e1e1c35 100644
--- a/gazebo/Master.cc
+++ b/gazebo/Master.cc
@@ -364,7 +364,7 @@ void Master::RunOnce()
for (iter = this->connections.begin();
iter != this->connections.end();)
{
- if (iter->second->IsOpen())
+ if (iter->second && iter->second->IsOpen())
{
iter->second->ProcessWriteQueue();
++iter;
diff --git a/gazebo/Server.cc b/gazebo/Server.cc
index 4c12d7a..2348f6d 100644
--- a/gazebo/Server.cc
+++ b/gazebo/Server.cc
@@ -14,6 +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>
+ #define snprintf _snprintf
+#endif
+
#include <stdio.h>
#include <signal.h>
#include <boost/lexical_cast.hpp>
@@ -40,6 +48,7 @@
#include "gazebo/physics/PhysicsFactory.hh"
#include "gazebo/physics/PhysicsIface.hh"
+#include "gazebo/physics/PresetManager.hh"
#include "gazebo/physics/World.hh"
#include "gazebo/physics/Base.hh"
@@ -55,6 +64,8 @@ bool Server::stop = true;
Server::Server()
{
this->initialized = false;
+ this->systemPluginsArgc = 0;
+ this->systemPluginsArgv = NULL;
}
/////////////////////////////////////////////////
@@ -104,7 +115,9 @@ bool Server::ParseArgs(int _argc, char **_argv)
("iters", po::value<unsigned int>(), "Number of iterations to simulate.")
("minimal_comms", "Reduce the TCP/IP traffic output by gzserver")
("server-plugin,s", po::value<std::vector<std::string> >(),
- "Load a plugin.");
+ "Load a plugin.")
+ ("profile,o", po::value<std::string>(),
+ "Physics preset profile name from the options in the world file.");
po::options_description hiddenDesc("Hidden options");
hiddenDesc.add_options()
@@ -234,7 +247,11 @@ bool Server::ParseArgs(int _argc, char **_argv)
<< " Gazebo Version: "
<< util::LogPlay::Instance()->GetGazeboVersion() << "\n"
<< " Random Seed: "
- << util::LogPlay::Instance()->GetRandSeed() << "\n";
+ << util::LogPlay::Instance()->GetRandSeed() << "\n"
+ << " Log Start Time: "
+ << util::LogPlay::Instance()->GetLogStartTime() << "\n"
+ << " Log End Time: "
+ << util::LogPlay::Instance()->GetLogEndTime() << "\n";
// Get the SDF world description from the log file
std::string sdfString;
@@ -261,6 +278,22 @@ bool Server::ParseArgs(int _argc, char **_argv)
// Load the server
if (!this->LoadFile(configFilename, physics))
return false;
+
+ if (this->vm.count("profile"))
+ {
+ std::string profileName = this->vm["profile"].as<std::string>();
+ if (physics::get_world()->GetPresetManager()->HasProfile(profileName))
+ {
+ physics::get_world()->GetPresetManager()->CurrentProfile(profileName);
+ gzmsg << "Setting physics profile to [" << profileName << "]."
+ << std::endl;
+ }
+ else
+ {
+ gzerr << "Specified profile [" << profileName << "] was not found."
+ << std::endl;
+ }
+ }
}
this->ProcessParams();
@@ -301,7 +334,7 @@ bool Server::LoadFile(const std::string &_filename,
return false;
}
- return this->LoadImpl(sdf->root, _physics);
+ return this->LoadImpl(sdf->Root(), _physics);
}
/////////////////////////////////////////////////
@@ -321,7 +354,7 @@ bool Server::LoadString(const std::string &_sdfString)
return false;
}
- return this->LoadImpl(sdf->root);
+ return this->LoadImpl(sdf->Root());
}
/////////////////////////////////////////////////
@@ -430,12 +463,14 @@ void Server::Fini()
/////////////////////////////////////////////////
void Server::Run()
{
+#ifndef _WIN32
// Now that we're about to run, install a signal handler to allow for
// graceful shutdown on Ctrl-C.
struct sigaction sigact;
sigact.sa_handler = Server::SigInt;
if (sigaction(SIGINT, &sigact, NULL))
std::cerr << "sigaction(2) failed while setting up for SIGINT" << std::endl;
+#endif
if (this->stop)
return;
@@ -673,7 +708,7 @@ bool Server::OpenWorld(const std::string & /*_filename*/)
gazebo::transport::clear_buffers();
- sdf::ElementPtr worldElem = sdf->root->GetElement("world");
+ sdf::ElementPtr worldElem = sdf->Root()->GetElement("world");
physics::WorldPtr world = physics::create_world();
diff --git a/gazebo/common/Animation.cc b/gazebo/common/Animation.cc
index 0114531..81a1f21 100644
--- a/gazebo/common/Animation.cc
+++ b/gazebo/common/Animation.cc
@@ -16,8 +16,9 @@
*/
#include <algorithm>
-#include "gazebo/math/Spline.hh"
-#include "gazebo/math/RotationSpline.hh"
+#include <ignition/math/Spline.hh>
+#include <ignition/math/RotationSpline.hh>
+
#include "gazebo/common/Console.hh"
#include "gazebo/common/KeyFrame.hh"
#include "gazebo/common/Animation.hh"
@@ -66,7 +67,7 @@ void Animation::SetLength(double _len)
/////////////////////////////////////////////////
void Animation::SetTime(double _time)
{
- if (!math::equal(_time, this->timePos))
+ if (!ignition::math::equal(_time, this->timePos))
{
this->timePos = _time;
if (this->loop)
@@ -163,7 +164,7 @@ double Animation::GetKeyFramesAtTime(double _time, KeyFrame **_kf1,
*_kf1 = *iter;
t1 = (*_kf1)->GetTime();
- if (math::equal(t1, t2))
+ if (ignition::math::equal(t1, t2))
return 0.0;
else
return (_time - t1) / (t2 - t1);
@@ -208,13 +209,13 @@ PoseKeyFrame *PoseAnimation::CreateKeyFrame(double _time)
void PoseAnimation::BuildInterpolationSplines() const
{
if (!this->positionSpline)
- this->positionSpline = new math::Spline();
+ this->positionSpline = new ignition::math::Spline();
if (!this->rotationSpline)
- this->rotationSpline = new math::RotationSpline();
+ this->rotationSpline = new ignition::math::RotationSpline();
- this->positionSpline->SetAutoCalculate(false);
- this->rotationSpline->SetAutoCalculate(false);
+ this->positionSpline->AutoCalculate(false);
+ this->rotationSpline->AutoCalculate(false);
this->positionSpline->Clear();
this->rotationSpline->Clear();
@@ -223,8 +224,8 @@ void PoseAnimation::BuildInterpolationSplines() const
iter != this->keyFrames.end(); ++iter)
{
PoseKeyFrame *pkey = reinterpret_cast<PoseKeyFrame*>(*iter);
- this->positionSpline->AddPoint(pkey->GetTranslation());
- this->rotationSpline->AddPoint(pkey->GetRotation());
+ this->positionSpline->AddPoint(pkey->Translation());
+ this->rotationSpline->AddPoint(pkey->Rotation());
}
this->positionSpline->RecalcTangents();
@@ -253,15 +254,15 @@ void PoseAnimation::GetInterpolatedKeyFrame(double _time,
k1 = reinterpret_cast<PoseKeyFrame*>(kBase1);
- if (math::equal(t, 0.0))
+ if (ignition::math::equal(t, 0.0))
{
- _kf.SetTranslation(k1->GetTranslation());
- _kf.SetRotation(k1->GetRotation());
+ _kf.Translation(k1->Translation());
+ _kf.Rotation(k1->Rotation());
}
else
{
- _kf.SetTranslation(this->positionSpline->Interpolate(firstKeyIndex, t));
- _kf.SetRotation(this->rotationSpline->Interpolate(firstKeyIndex, t));
+ _kf.Translation(this->positionSpline->Interpolate(firstKeyIndex, t));
+ _kf.Rotation(this->rotationSpline->Interpolate(firstKeyIndex, t));
}
}
@@ -312,7 +313,7 @@ void NumericAnimation::GetInterpolatedKeyFrame(NumericKeyFrame &_kf) const
k1 = reinterpret_cast<NumericKeyFrame*>(kBase1);
k2 = reinterpret_cast<NumericKeyFrame*>(kBase2);
- if (math::equal(t, 0.0))
+ if (ignition::math::equal(t, 0.0))
{
// Just use k1
_kf.SetValue(k1->GetValue());
diff --git a/gazebo/common/Animation.hh b/gazebo/common/Animation.hh
index 50eaead..03f95f3 100644
--- a/gazebo/common/Animation.hh
+++ b/gazebo/common/Animation.hh
@@ -21,14 +21,17 @@
#include <vector>
#include "gazebo/util/system.hh"
-namespace gazebo
+namespace ignition
{
namespace math
{
class Spline;
class RotationSpline;
}
+}
+namespace gazebo
+{
/// \ingroup gazebo_common
/// \brief Common namespace
@@ -44,7 +47,7 @@ namespace gazebo
/// \class Animation Animation.hh common/common.hh
/// \brief Manages an animation, which is a collection of keyframes and
/// the ability to interpolate between the keyframes
- class GAZEBO_VISIBLE Animation
+ class GZ_COMMON_VISIBLE Animation
{
/// \brief Constructor
/// \param[in] _name Name of the animation, should be unique
@@ -122,7 +125,7 @@ namespace gazebo
/// \{
/// \brief A pose animation.
- class GAZEBO_VISIBLE PoseAnimation : public Animation
+ class GZ_COMMON_VISIBLE PoseAnimation : public Animation
{
/// \brief Constructor
/// \param[in] _name String name of the animation. This should be unique.
@@ -153,10 +156,10 @@ namespace gazebo
protected: void BuildInterpolationSplines() const;
/// \brief smooth interpolation for position
- private: mutable math::Spline *positionSpline;
+ private: mutable ignition::math::Spline *positionSpline;
/// \brief smooth interpolation for rotation
- private: mutable math::RotationSpline *rotationSpline;
+ private: mutable ignition::math::RotationSpline *rotationSpline;
};
/// \}
@@ -164,7 +167,7 @@ namespace gazebo
/// \{
/// \brief A numeric animation.
- class GAZEBO_VISIBLE NumericAnimation : public Animation
+ class GZ_COMMON_VISIBLE NumericAnimation : public Animation
{
/// \brief Constructor
/// \param[in] _name String name of the animation. This should be unique.
diff --git a/gazebo/common/Animation_TEST.cc b/gazebo/common/Animation_TEST.cc
index ec50fb0..2a70c0c 100644
--- a/gazebo/common/Animation_TEST.cc
+++ b/gazebo/common/Animation_TEST.cc
@@ -52,18 +52,18 @@ TEST_F(AnimationTest, PoseAnimation)
anim.SetLength(10.0);
EXPECT_DOUBLE_EQ(10.0, anim.GetLength());
- key->SetTranslation(math::Vector3(0, 0, 0));
- EXPECT_TRUE(key->GetTranslation() == math::Vector3(0, 0, 0));
+ key->Translation(ignition::math::Vector3d(0, 0, 0));
+ EXPECT_TRUE(key->Translation() == ignition::math::Vector3d(0, 0, 0));
- key->SetRotation(math::Quaternion(0, 0, 0));
- EXPECT_TRUE(key->GetRotation() == math::Quaternion(0, 0, 0));
+ key->Rotation(ignition::math::Quaterniond(0, 0, 0));
+ EXPECT_TRUE(key->Rotation() == ignition::math::Quaterniond(0, 0, 0));
key = anim.CreateKeyFrame(10.0);
- key->SetTranslation(math::Vector3(10, 20, 30));
- EXPECT_TRUE(key->GetTranslation() == math::Vector3(10, 20, 30));
+ key->Translation(ignition::math::Vector3d(10, 20, 30));
+ EXPECT_TRUE(key->Translation() == ignition::math::Vector3d(10, 20, 30));
- key->SetRotation(math::Quaternion(0.1, 0.2, 0.3));
- EXPECT_TRUE(key->GetRotation() == math::Quaternion(0.1, 0.2, 0.3));
+ key->Rotation(ignition::math::Quaterniond(0.1, 0.2, 0.3));
+ EXPECT_TRUE(key->Rotation() == ignition::math::Quaterniond(0.1, 0.2, 0.3));
anim.AddTime(5.0);
EXPECT_DOUBLE_EQ(5.0, anim.GetTime());
@@ -73,10 +73,10 @@ TEST_F(AnimationTest, PoseAnimation)
common::PoseKeyFrame interpolatedKey(-1.0);
anim.GetInterpolatedKeyFrame(interpolatedKey);
- EXPECT_TRUE(interpolatedKey.GetTranslation() ==
- math::Vector3(3.76, 7.52, 11.28));
- EXPECT_TRUE(interpolatedKey.GetRotation() ==
- math::Quaternion(0.0302776, 0.0785971, 0.109824));
+ EXPECT_TRUE(interpolatedKey.Translation() ==
+ ignition::math::Vector3d(3.76, 7.52, 11.28));
+ EXPECT_TRUE(interpolatedKey.Rotation() ==
+ ignition::math::Quaterniond(0.0302776, 0.0785971, 0.109824));
}
TEST_F(AnimationTest, NumericAnimation)
diff --git a/gazebo/common/AudioDecoder.cc b/gazebo/common/AudioDecoder.cc
index 4b251cd..96d167d 100644
--- a/gazebo/common/AudioDecoder.cc
+++ b/gazebo/common/AudioDecoder.cc
@@ -88,14 +88,14 @@ bool AudioDecoder::Decode(uint8_t **_outBuffer, unsigned int *_outBufferSize)
if (!decodedFrame)
{
- if (!(decodedFrame = avcodec_alloc_frame()))
+ if (!(decodedFrame = common::AVFrameAlloc()))
{
gzerr << "Audio decoder out of memory\n";
result = false;
}
}
else
- avcodec_get_frame_defaults(decodedFrame);
+ common::AVFrameUnref(decodedFrame);
av_init_packet(&packet);
while (av_read_frame(this->formatCtx, &packet) == 0)
diff --git a/gazebo/common/AudioDecoder.hh b/gazebo/common/AudioDecoder.hh
index 4a678e9..4ef45e4 100644
--- a/gazebo/common/AudioDecoder.hh
+++ b/gazebo/common/AudioDecoder.hh
@@ -35,7 +35,7 @@ namespace gazebo
/// \class AudioDecoder AudioDecoder.hh common/common.hh
/// \brief An audio decoder based on FFMPEG.
- class GAZEBO_VISIBLE AudioDecoder
+ class GZ_COMMON_VISIBLE AudioDecoder
{
/// \brief Constructor.
public: AudioDecoder();
diff --git a/gazebo/common/BVHLoader.cc b/gazebo/common/BVHLoader.cc
index 0400765..8ea7664 100644
--- a/gazebo/common/BVHLoader.cc
+++ b/gazebo/common/BVHLoader.cc
@@ -19,6 +19,8 @@
#include <sstream>
#include <boost/algorithm/string.hpp>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Matrix4.hh>
#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/BVHLoader.hh"
@@ -26,8 +28,6 @@
#include "gazebo/common/Skeleton.hh"
#include "gazebo/common/SkeletonAnimation.hh"
#include "gazebo/common/Console.hh"
-#include "gazebo/math/Matrix3.hh"
-#include "gazebo/math/Angle.hh"
using namespace gazebo;
using namespace common;
@@ -92,26 +92,27 @@ Skeleton *BVHLoader::Load(const std::string &_filename, double _scale)
file.close();
return NULL;
}
- math::Vector3 offset = math::Vector3(
- math::parseFloat(words[1]) * _scale,
- math::parseFloat(words[2]) * _scale,
- math::parseFloat(words[3]) * _scale);
- math::Matrix4 transform(math::Matrix4::IDENTITY);
- transform.SetTranslate(offset);
+ ignition::math::Vector3d offset = ignition::math::Vector3d(
+ ignition::math::parseFloat(words[1]) * _scale,
+ ignition::math::parseFloat(words[2]) * _scale,
+ ignition::math::parseFloat(words[3]) * _scale);
+ ignition::math::Matrix4d transform(
+ ignition::math::Matrix4d::Identity);
+ transform.Translate(offset);
node->SetTransform(transform);
}
else
if (words[0] == "CHANNELS")
{
if (words.size() < 3 ||
- static_cast<size_t>(math::parseInt(words[1]) + 2) >
+ static_cast<size_t>(ignition::math::parseInt(words[1]) + 2) >
words.size())
{
file.close();
return NULL;
}
nodeChannels.push_back(words);
- totalChannels += math::parseInt(words[1]);
+ totalChannels += ignition::math::parseInt(words[1]);
}
else
if (words[0] == "{")
@@ -152,7 +153,7 @@ Skeleton *BVHLoader::Load(const std::string &_filename, double _scale)
return NULL;
}
else
- frameCount = math::parseInt(words[1]);
+ frameCount = ignition::math::parseInt(words[1]);
getline(file, line);
words.clear();
@@ -165,7 +166,7 @@ Skeleton *BVHLoader::Load(const std::string &_filename, double _scale)
return NULL;
}
else
- frameTime = math::parseFloat(words[2]);
+ frameTime = ignition::math::parseFloat(words[2]);
double time = 0.0;
unsigned int frameNo = 0;
@@ -187,63 +188,72 @@ Skeleton *BVHLoader::Load(const std::string &_filename, double _scale)
}
unsigned int cursor = 0;
- for (unsigned int i = 0; i < nodes.size(); i++)
+ for (unsigned int i = 0; i < nodes.size(); ++i)
{
SkeletonNode *node = nodes[i];
std::vector<std::string> channels = nodeChannels[i];
- math::Vector3 translation = node->GetTransform().GetTranslation();
- math::Vector3 xAxis(1, 0, 0);
- math::Vector3 yAxis(0, 1, 0);
- math::Vector3 zAxis(0, 0, 1);
+ ignition::math::Vector3d translation = node->Transform().Translation();
+ ignition::math::Vector3d xAxis(1, 0, 0);
+ ignition::math::Vector3d yAxis(0, 1, 0);
+ ignition::math::Vector3d zAxis(0, 0, 1);
double xAngle = 0.0;
double yAngle = 0.0;
double zAngle = 0.0;
- math::Matrix4 transform(math::Matrix4::IDENTITY);
- std::vector<math::Matrix4> mats;
- unsigned int chanCount = math::parseInt(channels[1]);
- for (unsigned int j = 2; j < (2 + chanCount); j++)
+ ignition::math::Matrix4d transform(ignition::math::Matrix4d::Identity);
+ std::vector<ignition::math::Matrix4d> mats;
+ unsigned int chanCount = ignition::math::parseInt(channels[1]);
+ for (unsigned int j = 2; j < (2 + chanCount); ++j)
{
- double value = math::parseFloat(words[cursor]);
+ double value = ignition::math::parseFloat(words[cursor]);
cursor++;
std::string channel = channels[j];
if (channel == "Xposition")
- translation.x = value * _scale;
+ translation.X(value * _scale);
else
if (channel == "Yposition")
- translation.y = value * _scale;
+ translation.Y(value * _scale);
else
+ {
if (channel == "Zposition")
{
- translation.z = value * _scale;
+ translation.Z(value * _scale);
}
else
+ {
if (channel == "Zrotation")
{
zAngle = GZ_DTOR(value);
- mats.push_back(math::Quaternion(zAxis, zAngle).GetAsMatrix4());
+ mats.push_back(ignition::math::Matrix4d(
+ ignition::math::Quaterniond(zAxis, zAngle)));
}
else
+ {
if (channel == "Xrotation")
{
xAngle = GZ_DTOR(value);
- mats.push_back(
- math::Quaternion(xAxis, xAngle).GetAsMatrix4());
+ mats.push_back(ignition::math::Matrix4d(
+ ignition::math::Quaterniond(xAxis, xAngle)));
}
else
+ {
if (channel == "Yrotation")
{
yAngle = GZ_DTOR(value);
- mats.push_back(
- math::Quaternion(yAxis, yAngle).GetAsMatrix4());
+ mats.push_back(ignition::math::Matrix4d(
+ ignition::math::Quaterniond(yAxis, yAngle)));
}
+ }
+ }
+ }
+ }
}
while (!mats.empty())
{
transform = mats.back() * transform;
mats.pop_back();
}
- math::Matrix4 pos(math::Matrix4::IDENTITY);
- pos.SetTranslate(translation);
+ ignition::math::Matrix4d pos(ignition::math::Matrix4d::Identity);
+ pos.Translate(translation);
transform = pos * transform;
animation->AddKeyFrame(node->GetName(), time, transform);
}
diff --git a/gazebo/common/BVHLoader.hh b/gazebo/common/BVHLoader.hh
index 7425d62..f7a6e7b 100644
--- a/gazebo/common/BVHLoader.hh
+++ b/gazebo/common/BVHLoader.hh
@@ -14,14 +14,11 @@
* limitations under the License.
*
*/
-#ifndef BVHLOADER_HH
-#define BVHLOADER_HH
+#ifndef _GAZEBO_BVHLOADER_HH_
+#define _GAZEBO_BVHLOADER_HH_
-#include <vector>
-#include <map>
#include <string>
-#include "gazebo/math/Pose.hh"
#include "gazebo/util/system.hh"
#define X_POSITION 0
@@ -42,7 +39,7 @@ namespace gazebo
/// \class BVHLoader BVHLoader.hh common/common.hh
/// \brief Handles loading BVH animation files
- class GAZEBO_VISIBLE BVHLoader
+ class GZ_COMMON_VISIBLE BVHLoader
{
/// \brief Constructor
public: BVHLoader();
diff --git a/gazebo/common/Base64.hh b/gazebo/common/Base64.hh
index 496e005..e4e525d 100644
--- a/gazebo/common/Base64.hh
+++ b/gazebo/common/Base64.hh
@@ -24,7 +24,7 @@
/// \param[in] _bytesToEncode String of bytes to encode.
/// \param[in] _len Length of _bytesToEncode.
/// \param[out] _result Based64 string is appended to this string.
-GAZEBO_VISIBLE
+GZ_COMMON_VISIBLE
void Base64Encode(const char *_bytesToEncode, unsigned int _len,
std::string &_result);
@@ -32,6 +32,6 @@ void Base64Encode(const char *_bytesToEncode, unsigned int _len,
/// \brief Decode a base64 string.
/// \param[in] _encodedString A base 64 encoded string.
/// \return The decoded string.
-GAZEBO_VISIBLE
+GZ_COMMON_VISIBLE
std::string Base64Decode(const std::string &_encodedString);
#endif
diff --git a/gazebo/common/CMakeLists.txt b/gazebo/common/CMakeLists.txt
index 135d4e0..4d57017 100644
--- a/gazebo/common/CMakeLists.txt
+++ b/gazebo/common/CMakeLists.txt
@@ -1,5 +1,9 @@
include (${gazebo_cmake_dir}/GazeboUtils.cmake)
+if (WIN32)
+ add_definitions(-DBUILDING_DLL_GZ_COMMON)
+endif()
+
if (HAVE_OPENAL)
include_directories(${OPENAL_INCLUDE_DIR})
endif()
@@ -14,6 +18,17 @@ if (HAVE_GDAL)
include_directories(${GDAL_INCLUDE_DIR})
endif()
+if (CURL_FOUND)
+ include_directories(${CURL_INCLUDEDIR})
+ link_directories(${CURL_LIBDIR})
+ if (WIN32)
+ add_definitions(-DCURL_STATICLIB)
+ endif()
+endif()
+
+include_directories(${tinyxml_INCLUDE_DIRS})
+link_directories(${tinyxml_LIBRARY_DIRS})
+
set (sources
Animation.cc
Assert.cc
@@ -38,17 +53,29 @@ set (sources
MeshLoader.cc
MeshManager.cc
ModelDatabase.cc
+ MouseEvent.cc
PID.cc
SkeletonAnimation.cc
Skeleton.cc
SphericalCoordinates.cc
STLLoader.cc
SystemPaths.cc
+ SVGLoader.cc
Time.cc
Timer.cc
Video.cc
+ ffmpeg_inc.cc
)
+if (NOT USE_EXTERNAL_TINYXML)
+ include_directories (${CMAKE_SOURCE_DIR}/deps/win/tinyxml)
+ set (sources ${sources}
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinystr.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxml.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxmlerror.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxmlparser.cpp)
+endif()
+
set (headers
Animation.hh
Assert.hh
@@ -84,6 +111,7 @@ set (headers
SphericalCoordinates.hh
STLLoader.hh
SystemPaths.hh
+ SVGLoader.hh
Time.hh
Timer.hh
UpdateInfo.hh
@@ -91,19 +119,6 @@ set (headers
ffmpeg_inc.h
)
-if (HAVE_GTS)
-set (sources
- ${sources}
- GTSMeshUtils.cc
- MeshCSG.cc
-)
-set (headers
- ${headers}
- GTSMeshUtils.hh
- MeshCSG.hh
-)
-endif()
-
set (gtest_sources
Animation_TEST.cc
ColladaExporter_TEST.cc
@@ -118,12 +133,41 @@ set (gtest_sources
ImageHeightmap_TEST.cc
Material_TEST.cc
Mesh_TEST.cc
+ MeshManager_TEST.cc
MovingWindowFilter_TEST.cc
SphericalCoordinates_TEST.cc
SystemPaths_TEST.cc
+ SVGLoader_TEST.cc
Time_TEST.cc
)
+# Timer test fails on OSX
+# See issue #1626
+if (NOT APPLE)
+ set (gtest_sources
+ ${gtest_sources}
+ Timer_TEST.cc
+ )
+endif()
+
+if (HAVE_GTS)
+set (sources
+ ${sources}
+ GTSMeshUtils.cc
+ MeshCSG.cc
+)
+set (headers
+ ${headers}
+ GTSMeshUtils.hh
+ MeshCSG.hh
+)
+
+set (gtest_sources
+ ${gtest_sources}
+ GTSMeshUtils_TEST.cc
+)
+endif()
+
if (HAVE_FFMPEG)
set (gtest_sources ${gtest_sources}
AudioDecoder_TEST.cc)
@@ -138,6 +182,12 @@ foreach (hdr ${headers})
endforeach()
configure_file (${CMAKE_CURRENT_SOURCE_DIR}/common.hh.in ${CMAKE_CURRENT_BINARY_DIR}/common.hh )
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_COMMON")
+if (WIN32)
+ # 0x601 = Windows 7. TBB needs to know this.
+ set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D_WIN32_WINNT=0x601")
+endif ()
+
gz_add_library(gazebo_common ${sources})
set_property(
@@ -151,12 +201,9 @@ set_property(
OGRE_RESOURCE_PATH="${OGRE_RESOURCE_PATH}"
)
-link_directories(
- ${tinyxml_LIBRARY_DIRS}
-)
-
target_link_libraries(gazebo_common
gazebo_math
+ ${IGNITION-MATH_LIBRARIES}
${libdl_library}
${libtool_library}
${Boost_LIBRARIES}
@@ -170,11 +217,14 @@ target_link_libraries(gazebo_common
${libtar_LIBRARIES}
${TBB_LIBRARIES}
${SDFormat_LIBRARIES}
- pthread
)
-if (NOT APPLE)
- # rt is used for clock_gettime, which is not available on apple
+if (UNIX)
+ target_link_libraries(gazebo_common pthread)
+endif()
+
+if (UNIX AND NOT APPLE)
+ # rt is used for clock_gettime, which is not available on apple or windows
target_link_libraries(gazebo_common rt)
endif()
diff --git a/gazebo/common/ColladaExporter.cc b/gazebo/common/ColladaExporter.cc
index 73c756e..e7eb42a 100644
--- a/gazebo/common/ColladaExporter.cc
+++ b/gazebo/common/ColladaExporter.cc
@@ -17,6 +17,7 @@
#include <tinyxml.h>
#include <boost/filesystem.hpp>
+#include <ignition/math/Vector3.hh>
#include "gazebo/common/Material.hh"
#include "gazebo/common/Mesh.hh"
@@ -24,6 +25,10 @@
#include "gazebo/common/ColladaExporterPrivate.hh"
#include "gazebo/common/ColladaExporter.hh"
+#ifdef _WIN32
+ #define snprintf _snprintf
+#endif
+
using namespace gazebo;
using namespace common;
@@ -166,11 +171,11 @@ void ColladaExporter::ExportGeometrySource(
snprintf(sourceId, sizeof(sourceId), "%s-Positions", _meshID);
count = _subMesh->GetVertexCount();
stride = 3;
- gazebo::math::Vector3 vertex;
+ ignition::math::Vector3d vertex;
for (unsigned int i = 0; i < count; ++i)
{
- vertex = _subMesh->GetVertex(i);
- fillData << vertex.x << " " << vertex.y << " " << vertex.z << " ";
+ vertex = _subMesh->Vertex(i);
+ fillData << vertex.X() << " " << vertex.Y() << " " << vertex.Z() << " ";
}
}
if (_type == NORMAL)
@@ -178,11 +183,11 @@ void ColladaExporter::ExportGeometrySource(
snprintf(sourceId, sizeof(sourceId), "%s-Normals", _meshID);
count = _subMesh->GetNormalCount();
stride = 3;
- gazebo::math::Vector3 normal;
+ ignition::math::Vector3d normal;
for (unsigned int i = 0; i < count; ++i)
{
- normal = _subMesh->GetNormal(i);
- fillData << normal.x << " " << normal.y << " " << normal.z << " ";
+ normal = _subMesh->Normal(i);
+ fillData << normal.X() << " " << normal.Y() << " " << normal.Z() << " ";
}
}
if (_type == UVMAP)
@@ -190,11 +195,11 @@ void ColladaExporter::ExportGeometrySource(
snprintf(sourceId, sizeof(sourceId), "%s-UVMap", _meshID);
count = _subMesh->GetVertexCount();
stride = 2;
- gazebo::math::Vector2d inTexCoord;
+ ignition::math::Vector2d inTexCoord;
for (unsigned int i = 0; i < count; ++i)
{
- inTexCoord = _subMesh->GetTexCoord(i);
- fillData << inTexCoord.x << " " << 1-inTexCoord.y << " ";
+ inTexCoord = _subMesh->TexCoord(i);
+ fillData << inTexCoord.X() << " " << 1-inTexCoord.Y() << " ";
}
}
TiXmlElement *sourceXml = new TiXmlElement("source");
diff --git a/gazebo/common/ColladaExporter.hh b/gazebo/common/ColladaExporter.hh
index a6284f3..06e4317 100644
--- a/gazebo/common/ColladaExporter.hh
+++ b/gazebo/common/ColladaExporter.hh
@@ -23,7 +23,6 @@
#include <vector>
#include "gazebo/common/MeshExporter.hh"
-#include "gazebo/math/MathTypes.hh"
#include "gazebo/util/system.hh"
class TiXmlElement;
@@ -40,7 +39,7 @@ namespace gazebo
/// \class ColladaExporter ColladaExporter.hh common/common.hh
/// \brief Class used to export Collada mesh files
- class GAZEBO_VISIBLE ColladaExporter : public MeshExporter
+ class GZ_COMMON_VISIBLE ColladaExporter : public MeshExporter
{
/// \brief Geometry types
public: enum GeometryType {POSITION, NORMAL, UVMAP};
diff --git a/gazebo/common/ColladaExporter_TEST.cc b/gazebo/common/ColladaExporter_TEST.cc
index e27774a..04de0fe 100644
--- a/gazebo/common/ColladaExporter_TEST.cc
+++ b/gazebo/common/ColladaExporter_TEST.cc
@@ -69,8 +69,8 @@ TEST_F(ColladaExporter, ExportBox)
const common::Mesh *meshReloaded = loader.Load(filenameOut);
EXPECT_EQ(meshOriginal->GetName(), meshReloaded->GetName());
- EXPECT_EQ(meshOriginal->GetMax(), meshReloaded->GetMax());
- EXPECT_EQ(meshOriginal->GetMin(), meshReloaded->GetMin());
+ EXPECT_EQ(meshOriginal->Max(), meshReloaded->Max());
+ EXPECT_EQ(meshOriginal->Min(), meshReloaded->Min());
EXPECT_EQ(meshOriginal->GetSubMeshCount(), meshReloaded->GetSubMeshCount());
EXPECT_EQ(meshOriginal->GetMaterialCount(),
meshReloaded->GetMaterialCount());
@@ -83,18 +83,18 @@ TEST_F(ColladaExporter, ExportBox)
{
for (unsigned int j = 0; j < meshOriginal->GetVertexCount(); ++j)
{
- EXPECT_EQ(meshOriginal->GetSubMesh(i)->GetVertex(j),
- meshReloaded->GetSubMesh(i)->GetVertex(j));
+ EXPECT_EQ(meshOriginal->GetSubMesh(i)->Vertex(j),
+ meshReloaded->GetSubMesh(i)->Vertex(j));
}
for (unsigned int j = 0; j < meshOriginal->GetNormalCount(); ++j)
{
- EXPECT_EQ(meshOriginal->GetSubMesh(i)->GetNormal(j),
- meshReloaded->GetSubMesh(i)->GetNormal(j));
+ EXPECT_EQ(meshOriginal->GetSubMesh(i)->Normal(j),
+ meshReloaded->GetSubMesh(i)->Normal(j));
}
for (unsigned int j = 0; j < meshOriginal->GetTexCoordCount(); ++j)
{
- EXPECT_EQ(meshOriginal->GetSubMesh(i)->GetTexCoord(j),
- meshReloaded->GetSubMesh(i)->GetTexCoord(j));
+ EXPECT_EQ(meshOriginal->GetSubMesh(i)->TexCoord(j),
+ meshReloaded->GetSubMesh(i)->TexCoord(j));
}
}
@@ -150,8 +150,8 @@ TEST_F(ColladaExporter, ExportCordlessDrill)
"/tmp/cordless_drill_exported/meshes/cordless_drill_exported.dae");
EXPECT_EQ(meshOriginal->GetName(), meshReloaded->GetName());
- EXPECT_EQ(meshOriginal->GetMax(), meshReloaded->GetMax());
- EXPECT_EQ(meshOriginal->GetMin(), meshReloaded->GetMin());
+ EXPECT_EQ(meshOriginal->Max(), meshReloaded->Max());
+ EXPECT_EQ(meshOriginal->Min(), meshReloaded->Min());
EXPECT_EQ(meshOriginal->GetSubMeshCount(), meshReloaded->GetSubMeshCount());
EXPECT_EQ(meshOriginal->GetMaterialCount(),
meshReloaded->GetMaterialCount());
@@ -164,18 +164,18 @@ TEST_F(ColladaExporter, ExportCordlessDrill)
{
for (unsigned int j = 0; j < meshOriginal->GetVertexCount(); ++j)
{
- EXPECT_EQ(meshOriginal->GetSubMesh(i)->GetVertex(j),
- meshReloaded->GetSubMesh(i)->GetVertex(j));
+ EXPECT_EQ(meshOriginal->GetSubMesh(i)->Vertex(j),
+ meshReloaded->GetSubMesh(i)->Vertex(j));
}
for (unsigned int j = 0; j < meshOriginal->GetNormalCount(); ++j)
{
- EXPECT_EQ(meshOriginal->GetSubMesh(i)->GetNormal(j),
- meshReloaded->GetSubMesh(i)->GetNormal(j));
+ EXPECT_EQ(meshOriginal->GetSubMesh(i)->Normal(j),
+ meshReloaded->GetSubMesh(i)->Normal(j));
}
for (unsigned int j = 0; j < meshOriginal->GetTexCoordCount(); ++j)
{
- EXPECT_EQ(meshOriginal->GetSubMesh(i)->GetTexCoord(j),
- meshReloaded->GetSubMesh(i)->GetTexCoord(j));
+ EXPECT_EQ(meshOriginal->GetSubMesh(i)->TexCoord(j),
+ meshReloaded->GetSubMesh(i)->TexCoord(j));
}
}
diff --git a/gazebo/common/ColladaLoader.cc b/gazebo/common/ColladaLoader.cc
index 6e4c3f2..bfdd17e 100644
--- a/gazebo/common/ColladaLoader.cc
+++ b/gazebo/common/ColladaLoader.cc
@@ -42,26 +42,28 @@ using namespace gazebo;
using namespace common;
/////////////////////////////////////////////////
-struct Vector3Hash : std::unary_function<const math::Vector3, std::size_t>
+struct Vector3Hash : std::unary_function<const ignition::math::Vector3d,
+ std::size_t>
{
- std::size_t operator()(const math::Vector3 _v) const
+ std::size_t operator()(const ignition::math::Vector3d &_v) const
{
std::size_t seed = 0;
- boost::hash_combine(seed, _v.x);
- boost::hash_combine(seed, _v.y);
- boost::hash_combine(seed, _v.z);
+ boost::hash_combine(seed, _v.X());
+ boost::hash_combine(seed, _v.Y());
+ boost::hash_combine(seed, _v.Z());
return seed;
}
};
/////////////////////////////////////////////////
-struct Vector2dHash : std::unary_function<const math::Vector2d, std::size_t>
+struct Vector2dHash : std::unary_function<const ignition::math::Vector2d,
+ std::size_t>
{
- std::size_t operator()(const math::Vector2d _v) const
+ std::size_t operator()(const ignition::math::Vector2d &_v) const
{
std::size_t seed = 0;
- boost::hash_combine(seed, _v.x);
- boost::hash_combine(seed, _v.y);
+ boost::hash_combine(seed, _v.X());
+ boost::hash_combine(seed, _v.Y());
return seed;
}
};
@@ -120,7 +122,8 @@ Mesh *ColladaLoader::Load(const std::string &_filename)
{
TiXmlElement *unitXml = assetXml->FirstChildElement("unit");
if (unitXml && unitXml->Attribute("meter"))
- this->dataPtr->meter = math::parseFloat(unitXml->Attribute("meter"));
+ this->dataPtr->meter = ignition::math::parseFloat(
+ unitXml->Attribute("meter"));
}
Mesh *mesh = new Mesh();
@@ -153,19 +156,19 @@ void ColladaLoader::LoadScene(Mesh *_mesh)
TiXmlElement *nodeXml = visSceneXml->FirstChildElement("node");
while (nodeXml)
{
- this->LoadNode(nodeXml, _mesh, math::Matrix4::IDENTITY);
+ this->LoadNode(nodeXml, _mesh, ignition::math::Matrix4d::Identity);
nodeXml = nodeXml->NextSiblingElement("node");
}
}
/////////////////////////////////////////////////
void ColladaLoader::LoadNode(TiXmlElement *_elem, Mesh *_mesh,
- const math::Matrix4 &_transform)
+ const ignition::math::Matrix4d &_transform)
{
TiXmlElement *nodeXml;
TiXmlElement *instGeomXml;
- math::Matrix4 transform = this->LoadNodeTransform(_elem);
+ ignition::math::Matrix4d transform = this->LoadNodeTransform(_elem);
transform = _transform * transform;
if (_elem->Attribute("name"))
@@ -262,16 +265,16 @@ void ColladaLoader::LoadNode(TiXmlElement *_elem, Mesh *_mesh,
}
/////////////////////////////////////////////////
-math::Matrix4 ColladaLoader::LoadNodeTransform(TiXmlElement *_elem)
+ignition::math::Matrix4d ColladaLoader::LoadNodeTransform(TiXmlElement *_elem)
{
- math::Matrix4 transform(math::Matrix4::IDENTITY);
+ ignition::math::Matrix4d transform(ignition::math::Matrix4d::Identity);
if (_elem->FirstChildElement("matrix"))
{
std::string matrixStr = _elem->FirstChildElement("matrix")->GetText();
std::istringstream iss(matrixStr);
std::vector<double> values(16);
- for (unsigned int i = 0; i < 16; i++)
+ for (unsigned int i = 0; i < 16; ++i)
iss >> values[i];
transform.Set(values[0], values[1], values[2], values[3],
values[4], values[5], values[6], values[7],
@@ -283,27 +286,29 @@ math::Matrix4 ColladaLoader::LoadNodeTransform(TiXmlElement *_elem)
if (_elem->FirstChildElement("translate"))
{
std::string transStr = _elem->FirstChildElement("translate")->GetText();
- math::Vector3 translate;
- translate = boost::lexical_cast<math::Vector3>(transStr);
+ ignition::math::Vector3d translate;
+ translate = boost::lexical_cast<ignition::math::Vector3d>(transStr);
// translate *= this->dataPtr->meter;
- transform.SetTranslate(translate);
+ transform.Translate(translate);
}
TiXmlElement *rotateXml = _elem->FirstChildElement("rotate");
while (rotateXml)
{
- math::Matrix3 mat;
- math::Vector3 axis;
+ ignition::math::Matrix3d mat;
+ ignition::math::Vector3d axis;
double angle;
std::string rotateStr = rotateXml->GetText();
std::istringstream iss(rotateStr);
- iss >> axis.x >> axis.y >> axis.z;
+ iss >> axis.X() >> axis.Y() >> axis.Z();
iss >> angle;
- mat.SetFromAxis(axis, GZ_DTOR(angle));
+ mat.Axis(axis, IGN_DTOR(angle));
+ ignition::math::Matrix4d mat4(ignition::math::Matrix4d::Identity);
+ mat4 = mat;
- transform = transform * mat;
+ transform = transform * mat4;
rotateXml = rotateXml->NextSiblingElement("rotate");
}
@@ -311,10 +316,10 @@ math::Matrix4 ColladaLoader::LoadNodeTransform(TiXmlElement *_elem)
if (_elem->FirstChildElement("scale"))
{
std::string scaleStr = _elem->FirstChildElement("scale")->GetText();
- math::Vector3 scale;
- scale = boost::lexical_cast<math::Vector3>(scaleStr);
- math::Matrix4 scaleMat;
- scaleMat.SetScale(scale);
+ ignition::math::Vector3d scale;
+ scale = boost::lexical_cast<ignition::math::Vector3d>(scaleStr);
+ ignition::math::Matrix4d scaleMat;
+ scaleMat.Scale(scale);
transform = transform * scaleMat;
}
}
@@ -324,7 +329,8 @@ math::Matrix4 ColladaLoader::LoadNodeTransform(TiXmlElement *_elem)
/////////////////////////////////////////////////
void ColladaLoader::LoadController(TiXmlElement *_contrXml,
- TiXmlElement *_skelXml, const math::Matrix4 &_transform, Mesh *_mesh)
+ TiXmlElement *_skelXml,
+ const ignition::math::Matrix4d &_transform, Mesh *_mesh)
{
Skeleton *skeleton = new Skeleton(this->LoadSkeletonNodes(_skelXml, NULL));
_mesh->SetSkeleton(skeleton);
@@ -338,12 +344,12 @@ void ColladaLoader::LoadController(TiXmlElement *_contrXml,
TiXmlElement *skinXml = _contrXml->FirstChildElement("skin");
std::string geomURL = skinXml->Attribute("source");
- math::Matrix4 bindTrans;
+ ignition::math::Matrix4d bindTrans;
std::string matrixStr =
skinXml->FirstChildElement("bind_shape_matrix")->GetText();
std::istringstream iss(matrixStr);
std::vector<double> values(16);
- for (unsigned int i = 0; i < 16; i++)
+ for (unsigned int i = 0; i < 16; ++i)
iss >> values[i];
bindTrans.Set(values[0], values[1], values[2], values[3],
values[4], values[5], values[6], values[7],
@@ -395,18 +401,26 @@ void ColladaLoader::LoadController(TiXmlElement *_contrXml,
std::vector<std::string> strs;
boost::split(strs, posesStr, boost::is_any_of(" "));
- for (unsigned int i = 0; i < joints.size(); i++)
+ for (unsigned int i = 0; i < joints.size(); ++i)
{
unsigned int id = i * 16;
- math::Matrix4 mat;
- mat.Set(math::parseFloat(strs[id + 0]), math::parseFloat(strs[id + 1]),
- math::parseFloat(strs[id + 2]), math::parseFloat(strs[id + 3]),
- math::parseFloat(strs[id + 4]), math::parseFloat(strs[id + 5]),
- math::parseFloat(strs[id + 6]), math::parseFloat(strs[id + 7]),
- math::parseFloat(strs[id + 8]), math::parseFloat(strs[id + 9]),
- math::parseFloat(strs[id + 10]), math::parseFloat(strs[id + 11]),
- math::parseFloat(strs[id + 12]), math::parseFloat(strs[id + 13]),
- math::parseFloat(strs[id + 14]), math::parseFloat(strs[id + 15]));
+ ignition::math::Matrix4d mat;
+ mat.Set(ignition::math::parseFloat(strs[id + 0]),
+ ignition::math::parseFloat(strs[id + 1]),
+ ignition::math::parseFloat(strs[id + 2]),
+ ignition::math::parseFloat(strs[id + 3]),
+ ignition::math::parseFloat(strs[id + 4]),
+ ignition::math::parseFloat(strs[id + 5]),
+ ignition::math::parseFloat(strs[id + 6]),
+ ignition::math::parseFloat(strs[id + 7]),
+ ignition::math::parseFloat(strs[id + 8]),
+ ignition::math::parseFloat(strs[id + 9]),
+ ignition::math::parseFloat(strs[id + 10]),
+ ignition::math::parseFloat(strs[id + 11]),
+ ignition::math::parseFloat(strs[id + 12]),
+ ignition::math::parseFloat(strs[id + 13]),
+ ignition::math::parseFloat(strs[id + 14]),
+ ignition::math::parseFloat(strs[id + 15]));
skeleton->GetNodeByName(joints[i])->SetInverseBindTransform(mat);
}
@@ -442,7 +456,7 @@ void ColladaLoader::LoadController(TiXmlElement *_contrXml,
boost::split(wStrs, wString, boost::is_any_of(" "));
std::vector<float> weights;
- for (unsigned int i = 0; i < wStrs.size(); i++)
+ for (unsigned int i = 0; i < wStrs.size(); ++i)
weights.push_back(math::parseFloat(wStrs[i]));
std::string cString = vertWeightsXml->FirstChildElement("vcount")->GetText();
@@ -456,18 +470,18 @@ void ColladaLoader::LoadController(TiXmlElement *_contrXml,
std::vector<unsigned int> vCount;
std::vector<unsigned int> v;
- for (unsigned int i = 0; i < vCountStrs.size(); i++)
+ for (unsigned int i = 0; i < vCountStrs.size(); ++i)
vCount.push_back(math::parseInt(vCountStrs[i]));
- for (unsigned int i = 0; i < vStrs.size(); i++)
+ for (unsigned int i = 0; i < vStrs.size(); ++i)
v.push_back(math::parseInt(vStrs[i]));
skeleton->SetNumVertAttached(vCount.size());
unsigned int vIndex = 0;
- for (unsigned int i = 0; i < vCount.size(); i++)
+ for (unsigned int i = 0; i < vCount.size(); ++i)
{
- for (unsigned int j = 0; j < vCount[i]; j++)
+ for (unsigned int j = 0; j < vCount[i]; ++j)
{
skeleton->AddVertNodeWeight(i, joints[v[vIndex + jOffset]],
weights[v[vIndex + wOffset]]);
@@ -545,11 +559,11 @@ void ColladaLoader::LoadAnimationSet(TiXmlElement *_xml, Skeleton *_skel)
if (sep == '(')
{
std::string idx1Str = idxStr.substr(0, 1);
- idx1 = math::parseInt(idx1Str);
+ idx1 = ignition::math::parseInt(idx1Str);
if (idxStr.length() > 4)
{
std::string idx2Str = idxStr.substr(3, 1);
- idx2 = math::parseInt(idx2Str);
+ idx2 = ignition::math::parseInt(idx2Str);
}
}
@@ -578,7 +592,7 @@ void ColladaLoader::LoadAnimationSet(TiXmlElement *_xml, Skeleton *_skel)
boost::split(timeStrs, timeStr, boost::is_any_of(" "));
std::vector<double> times;
- for (unsigned int i = 0; i < timeStrs.size(); i++)
+ for (unsigned int i = 0; i < timeStrs.size(); ++i)
times.push_back(math::parseFloat(timeStrs[i]));
TiXmlElement *output = frameTransXml->FirstChildElement("float_array");
@@ -587,16 +601,17 @@ void ColladaLoader::LoadAnimationSet(TiXmlElement *_xml, Skeleton *_skel)
boost::split(outputStrs, outputStr, boost::is_any_of(" "));
std::vector<double> values;
- for (unsigned int i = 0; i < outputStrs.size(); i++)
+ for (unsigned int i = 0; i < outputStrs.size(); ++i)
values.push_back(math::parseFloat(outputStrs[i]));
TiXmlElement *accessor =
frameTransXml->FirstChildElement("technique_common");
accessor = accessor->FirstChildElement("accessor");
- unsigned int stride = math::parseInt(accessor->Attribute("stride"));
+ unsigned int stride =
+ ignition::math::parseInt(accessor->Attribute("stride"));
- for (unsigned int i = 0; i < times.size(); i++)
+ for (unsigned int i = 0; i < times.size(); ++i)
{
if (animation[targetBone].find(times[i]) == animation[targetBone].end())
animation[targetBone][times[i]] =
@@ -604,7 +619,7 @@ void ColladaLoader::LoadAnimationSet(TiXmlElement *_xml, Skeleton *_skel)
std::vector<NodeTransform> *frame = &animation[targetBone][times[i]];
- for (unsigned int j = 0; j < (*frame).size(); j++)
+ for (unsigned int j = 0; j < (*frame).size(); ++j)
{
NodeTransform *nt = &((*frame)[j]);
if (nt->GetSID() == targetTrans)
@@ -634,8 +649,8 @@ void ColladaLoader::LoadAnimationSet(TiXmlElement *_xml, Skeleton *_skel)
for (RawNodeAnim::iterator niter = iter->second.begin();
niter != iter->second.end(); ++niter)
{
- math::Matrix4 transform(math::Matrix4::IDENTITY);
- for (unsigned int i = 0; i < niter->second.size(); i++)
+ ignition::math::Matrix4d transform(ignition::math::Matrix4d::Identity);
+ for (unsigned int i = 0; i < niter->second.size(); ++i)
{
niter->second[i].RecalculateMatrix();
transform = transform * niter->second[i]();
@@ -676,14 +691,14 @@ SkeletonNode* ColladaLoader::LoadSkeletonNodes(TiXmlElement *_xml,
void ColladaLoader::SetSkeletonNodeTransform(TiXmlElement *_elem,
SkeletonNode *_node)
{
- math::Matrix4 transform(math::Matrix4::IDENTITY);
+ ignition::math::Matrix4d transform(ignition::math::Matrix4d::Identity);
if (_elem->FirstChildElement("matrix"))
{
std::string matrixStr = _elem->FirstChildElement("matrix")->GetText();
std::istringstream iss(matrixStr);
std::vector<double> values(16);
- for (unsigned int i = 0; i < 16; i++)
+ for (unsigned int i = 0; i < 16; ++i)
iss >> values[i];
transform.Set(values[0], values[1], values[2], values[3],
values[4], values[5], values[6], values[7],
@@ -701,10 +716,10 @@ void ColladaLoader::SetSkeletonNodeTransform(TiXmlElement *_elem,
if (_elem->FirstChildElement("translate"))
{
std::string transStr = _elem->FirstChildElement("translate")->GetText();
- math::Vector3 translate;
- translate = boost::lexical_cast<math::Vector3>(transStr);
+ ignition::math::Vector3d translate;
+ translate = boost::lexical_cast<ignition::math::Vector3d>(transStr);
// translate *= this->dataPtr->meter;
- transform.SetTranslate(translate);
+ transform.Translate(translate);
NodeTransform nt(transform);
if (_elem->FirstChildElement("translate")->Attribute("sid"))
@@ -717,18 +732,18 @@ void ColladaLoader::SetSkeletonNodeTransform(TiXmlElement *_elem,
TiXmlElement *rotateXml = _elem->FirstChildElement("rotate");
while (rotateXml)
{
- math::Matrix3 mat;
- math::Vector3 axis;
+ ignition::math::Matrix3d mat;
+ ignition::math::Vector3d axis;
double angle;
std::string rotateStr = rotateXml->GetText();
std::istringstream iss(rotateStr);
- iss >> axis.x >> axis.y >> axis.z;
+ iss >> axis.X() >> axis.Y() >> axis.Z();
iss >> angle;
- mat.SetFromAxis(axis, GZ_DTOR(angle));
+ mat.Axis(axis, IGN_DTOR(angle));
- math::Matrix4 mat4(math::Matrix4::IDENTITY);
+ ignition::math::Matrix4d mat4(ignition::math::Matrix4d::Identity);
mat4 = mat;
NodeTransform nt(mat4);
if (rotateXml->Attribute("sid"))
@@ -737,7 +752,7 @@ void ColladaLoader::SetSkeletonNodeTransform(TiXmlElement *_elem,
nt.SetSourceValues(axis, angle);
_node->AddRawTransform(nt);
- transform = transform * mat;
+ transform = transform * mat4;
rotateXml = rotateXml->NextSiblingElement("rotate");
}
@@ -745,10 +760,10 @@ void ColladaLoader::SetSkeletonNodeTransform(TiXmlElement *_elem,
if (_elem->FirstChildElement("scale"))
{
std::string scaleStr = _elem->FirstChildElement("scale")->GetText();
- math::Vector3 scale;
- scale = boost::lexical_cast<math::Vector3>(scaleStr);
- math::Matrix4 scaleMat;
- scaleMat.SetScale(scale);
+ ignition::math::Vector3d scale;
+ scale = boost::lexical_cast<ignition::math::Vector3d>(scaleStr);
+ ignition::math::Matrix4d scaleMat;
+ scaleMat.Scale(scale);
NodeTransform nt(scaleMat);
if (_elem->FirstChildElement("matrix")->Attribute("sid"))
@@ -766,7 +781,7 @@ void ColladaLoader::SetSkeletonNodeTransform(TiXmlElement *_elem,
/////////////////////////////////////////////////
void ColladaLoader::LoadGeometry(TiXmlElement *_xml,
- const math::Matrix4 &_transform, Mesh *_mesh)
+ const ignition::math::Matrix4d &_transform, Mesh *_mesh)
{
TiXmlElement *meshXml = _xml->FirstChildElement("mesh");
TiXmlElement *childXml;
@@ -836,9 +851,9 @@ TiXmlElement *ColladaLoader::GetElementId(TiXmlElement *_parent,
/////////////////////////////////////////////////
void ColladaLoader::LoadVertices(const std::string &_id,
- const math::Matrix4 &_transform,
- std::vector<math::Vector3> &_verts,
- std::vector<math::Vector3> &_norms)
+ const ignition::math::Matrix4d &_transform,
+ std::vector<ignition::math::Vector3d> &_verts,
+ std::vector<ignition::math::Vector3d> &_norms)
{
std::map<unsigned int, unsigned int> vertDup;
std::map<unsigned int, unsigned int> normDup;
@@ -847,9 +862,9 @@ void ColladaLoader::LoadVertices(const std::string &_id,
/////////////////////////////////////////////////
void ColladaLoader::LoadVertices(const std::string &_id,
- const math::Matrix4 &_transform,
- std::vector<math::Vector3> &_verts,
- std::vector<math::Vector3> &_norms,
+ const ignition::math::Matrix4d &_transform,
+ std::vector<ignition::math::Vector3d> &_verts,
+ std::vector<ignition::math::Vector3d> &_norms,
std::map<unsigned int, unsigned int> &_vertDups,
std::map<unsigned int, unsigned int> &_normDups)
{
@@ -882,8 +897,8 @@ void ColladaLoader::LoadVertices(const std::string &_id,
/////////////////////////////////////////////////
void ColladaLoader::LoadPositions(const std::string &_id,
- const math::Matrix4 &_transform,
- std::vector<math::Vector3> &_values,
+ const ignition::math::Matrix4d &_transform,
+ std::vector<ignition::math::Vector3d> &_values,
std::map<unsigned int, unsigned int> &_duplicates)
{
if (this->dataPtr->positionIds.find(_id) != this->dataPtr->positionIds.end())
@@ -931,7 +946,8 @@ void ColladaLoader::LoadPositions(const std::string &_id,
}
std::string valueStr = floatArrayXml->GetText();
- boost::unordered_map<math::Vector3, unsigned int, Vector3Hash> unique;
+ boost::unordered_map<ignition::math::Vector3d,
+ unsigned int, Vector3Hash> unique;
std::vector<std::string> strs;
std::vector<std::string>::iterator iter, end;
@@ -940,8 +956,10 @@ void ColladaLoader::LoadPositions(const std::string &_id,
end = strs.end();
for (iter = strs.begin(); iter != end; iter += 3)
{
- math::Vector3 vec(math::parseFloat(*iter), math::parseFloat(*(iter+1)),
- math::parseFloat(*(iter+2)));
+ ignition::math::Vector3d vec(math::parseFloat(*iter),
+ ignition::math::parseFloat(*(iter+1)),
+ ignition::math::parseFloat(*(iter+2)));
+
vec = _transform * vec;
_values.push_back(vec);
@@ -958,8 +976,8 @@ void ColladaLoader::LoadPositions(const std::string &_id,
/////////////////////////////////////////////////
void ColladaLoader::LoadNormals(const std::string &_id,
- const math::Matrix4 &_transform,
- std::vector<math::Vector3> &_values,
+ const ignition::math::Matrix4d &_transform,
+ std::vector<ignition::math::Vector3d> &_values,
std::map<unsigned int, unsigned int> &_duplicates)
{
if (this->dataPtr->normalIds.find(_id) != this->dataPtr->normalIds.end())
@@ -969,8 +987,8 @@ void ColladaLoader::LoadNormals(const std::string &_id,
return;
}
- math::Matrix4 rotMat = _transform;
- rotMat.SetTranslate(math::Vector3::Zero);
+ ignition::math::Matrix4d rotMat = _transform;
+ rotMat.Translate(ignition::math::Vector3d::Zero);
TiXmlElement *normalsXml = this->GetElementId("source", _id);
if (!normalsXml)
@@ -1009,14 +1027,15 @@ void ColladaLoader::LoadNormals(const std::string &_id,
return;
}
- boost::unordered_map<math::Vector3, unsigned int, Vector3Hash> unique;
+ boost::unordered_map<ignition::math::Vector3d,
+ unsigned int, Vector3Hash> unique;
std::string valueStr = floatArrayXml->GetText();
std::istringstream iss(valueStr);
do
{
- math::Vector3 vec;
- iss >> vec.x >> vec.y >> vec.z;
+ ignition::math::Vector3d vec;
+ iss >> vec.X() >> vec.Y() >> vec.Z();
if (iss)
{
vec = rotMat * vec;
@@ -1037,7 +1056,7 @@ void ColladaLoader::LoadNormals(const std::string &_id,
/////////////////////////////////////////////////
void ColladaLoader::LoadTexCoords(const std::string &_id,
- std::vector<math::Vector2d> &_values,
+ std::vector<ignition::math::Vector2d> &_values,
std::map<unsigned int, unsigned int> &_duplicates)
{
if (this->dataPtr->texcoordIds.find(_id) != this->dataPtr->texcoordIds.end())
@@ -1156,7 +1175,8 @@ void ColladaLoader::LoadTexCoords(const std::string &_id,
if (totCount == 0)
return;
- boost::unordered_map<math::Vector2d, unsigned int, Vector2dHash> unique;
+ boost::unordered_map<ignition::math::Vector2d,
+ unsigned int, Vector2dHash> unique;
// Read the raw texture values, and split them on spaces.
std::string valueStr = floatArrayXml->GetText();
@@ -1167,7 +1187,7 @@ void ColladaLoader::LoadTexCoords(const std::string &_id,
for (int i = 0; i < totCount; i += stride)
{
// We only handle 2D texture coordinates right now.
- math::Vector2d vec(boost::lexical_cast<double>(values[i]),
+ ignition::math::Vector2d vec(boost::lexical_cast<double>(values[i]),
1.0 - boost::lexical_cast<double>(values[i+1]));
_values.push_back(vec);
@@ -1351,7 +1371,7 @@ void ColladaLoader::LoadColorOrTexture(TiXmlElement *_elem,
/////////////////////////////////////////////////
void ColladaLoader::LoadPolylist(TiXmlElement *_polylistXml,
- const math::Matrix4 &_transform,
+ const ignition::math::Matrix4d &_transform,
Mesh *_mesh)
{
// This function parses polylist types in collada into
@@ -1388,9 +1408,9 @@ void ColladaLoader::LoadPolylist(TiXmlElement *_polylistXml,
TiXmlElement *polylistInputXml = _polylistXml->FirstChildElement("input");
- std::vector<math::Vector3> verts;
- std::vector<math::Vector3> norms;
- std::vector<math::Vector2d> texcoords;
+ std::vector<ignition::math::Vector3d> verts;
+ std::vector<ignition::math::Vector3d> norms;
+ std::vector<ignition::math::Vector2d> texcoords;
const unsigned int VERTEX = 0;
const unsigned int NORMAL = 1;
@@ -1405,9 +1425,9 @@ void ColladaLoader::LoadPolylist(TiXmlElement *_polylistXml,
std::map<unsigned int, unsigned int> normalDupMap;
std::map<unsigned int, unsigned int> positionDupMap;
- math::Matrix4 bindShapeMat(math::Matrix4::IDENTITY);
+ ignition::math::Matrix4d bindShapeMat(ignition::math::Matrix4d::Identity);
if (_mesh->HasSkeleton())
- bindShapeMat = _mesh->GetSkeleton()->GetBindShapeTransform();
+ bindShapeMat = _mesh->GetSkeleton()->BindShapeTransform();
// read input elements
std::map<const unsigned int, int> inputs;
@@ -1423,25 +1443,25 @@ void ColladaLoader::LoadPolylist(TiXmlElement *_polylistXml,
positionDupMap, normalDupMap);
if (norms.size() > count)
combinedVertNorms = true;
- inputs[VERTEX] = math::parseInt(offset);
+ inputs[VERTEX] = ignition::math::parseInt(offset);
hasVertices = true;
}
else if (semantic == "NORMAL")
{
this->LoadNormals(source, _transform, norms, normalDupMap);
combinedVertNorms = false;
- inputs[NORMAL] = math::parseInt(offset);
+ inputs[NORMAL] = ignition::math::parseInt(offset);
hasNormals = true;
}
else if (semantic == "TEXCOORD")
{
this->LoadTexCoords(source, texcoords, texDupMap);
- inputs[TEXCOORD] = math::parseInt(offset);
+ inputs[TEXCOORD] = ignition::math::parseInt(offset);
hasTexcoords = true;
}
else
{
- inputs[otherSemantics++] = math::parseInt(offset);
+ inputs[otherSemantics++] = ignition::math::parseInt(offset);
gzwarn << "Polylist input semantic: '" << semantic << "' is currently"
<< " not supported" << std::endl;
}
@@ -1495,9 +1515,9 @@ void ColladaLoader::LoadPolylist(TiXmlElement *_polylistXml,
if (j == 2)
triangle_index = (k)*inputs.size();
- for (unsigned int i = 0; i < inputs.size(); i++)
+ for (unsigned int i = 0; i < inputs.size(); ++i)
{
- values[i] = math::parseInt(strs_iter[triangle_index+i]);
+ values[i] = ignition::math::parseInt(strs_iter[triangle_index+i]);
/*gzerr << "debug parsing "
<< " poly-i[" << l
<< "] tri-end-index[" << k
@@ -1599,7 +1619,7 @@ void ColladaLoader::LoadPolylist(TiXmlElement *_polylistXml,
if (_mesh->HasSkeleton())
{
subMesh->SetVertex(newVertIndex, bindShapeMat *
- subMesh->GetVertex(newVertIndex));
+ subMesh->Vertex(newVertIndex));
Skeleton *skel = _mesh->GetSkeleton();
for (unsigned int i = 0;
i < skel->GetNumVertNodeWeights(daeVertIndex); ++i)
@@ -1632,8 +1652,8 @@ void ColladaLoader::LoadPolylist(TiXmlElement *_polylistXml,
inputRemappedTexcoordIndex =
texDupMap[inputRemappedTexcoordIndex];
}
- subMesh->AddTexCoord(texcoords[inputRemappedTexcoordIndex].x,
- texcoords[inputRemappedTexcoordIndex].y);
+ subMesh->AddTexCoord(texcoords[inputRemappedTexcoordIndex].X(),
+ texcoords[inputRemappedTexcoordIndex].Y());
input.texcoordIndex = inputRemappedTexcoordIndex;
}
@@ -1655,7 +1675,7 @@ void ColladaLoader::LoadPolylist(TiXmlElement *_polylistXml,
/////////////////////////////////////////////////
void ColladaLoader::LoadTriangles(TiXmlElement *_trianglesXml,
- const math::Matrix4 &_transform,
+ const ignition::math::Matrix4d &_transform,
Mesh *_mesh)
{
SubMesh *subMesh = new SubMesh;
@@ -1687,9 +1707,9 @@ void ColladaLoader::LoadTriangles(TiXmlElement *_trianglesXml,
TiXmlElement *trianglesInputXml = _trianglesXml->FirstChildElement("input");
- std::vector<math::Vector3> verts;
- std::vector<math::Vector3> norms;
- std::vector<math::Vector2d> texcoords;
+ std::vector<ignition::math::Vector3d> verts;
+ std::vector<ignition::math::Vector3d> norms;
+ std::vector<ignition::math::Vector2d> texcoords;
const unsigned int VERTEX = 0;
const unsigned int NORMAL = 1;
@@ -1718,26 +1738,26 @@ void ColladaLoader::LoadTriangles(TiXmlElement *_trianglesXml,
positionDupMap, normalDupMap);
if (norms.size() > count)
combinedVertNorms = true;
- inputs[VERTEX] = math::parseInt(offset);
+ inputs[VERTEX] = ignition::math::parseInt(offset);
hasVertices = true;
}
else if (semantic == "NORMAL")
{
this->LoadNormals(source, _transform, norms, normalDupMap);
combinedVertNorms = false;
- inputs[NORMAL] = math::parseInt(offset);
+ inputs[NORMAL] = ignition::math::parseInt(offset);
hasNormals = true;
}
else if (semantic == "TEXCOORD" && !hasTexcoords)
{
// we currently only support one set of UVs
this->LoadTexCoords(source, texcoords, texDupMap);
- inputs[TEXCOORD] = math::parseInt(offset);
+ inputs[TEXCOORD] = ignition::math::parseInt(offset);
hasTexcoords = true;
}
else
{
- inputs[otherSemantics++] = math::parseInt(offset);
+ inputs[otherSemantics++] = ignition::math::parseInt(offset);
gzwarn << "Triangle input semantic: '" << semantic << "' is currently"
<< " not supported" << std::endl;
}
@@ -1796,7 +1816,7 @@ void ColladaLoader::LoadTriangles(TiXmlElement *_trianglesXml,
for (unsigned int j = 0; j < strs.size(); j += offsetSize)
{
for (unsigned int i = 0; i < offsetSize; ++i)
- values[i] = math::parseInt(strs[j+i]);
+ values[i] = ignition::math::parseInt(strs[j+i]);
unsigned int daeVertIndex = 0;
bool addIndex = !hasVertices;
@@ -1911,8 +1931,8 @@ void ColladaLoader::LoadTriangles(TiXmlElement *_trianglesXml,
unsigned int inputRemappedTexcoordIndex = values[inputs[TEXCOORD]];
if (texDupMap.find(inputRemappedTexcoordIndex) != texDupMap.end())
inputRemappedTexcoordIndex = texDupMap[inputRemappedTexcoordIndex];
- subMesh->AddTexCoord(texcoords[inputRemappedTexcoordIndex].x,
- texcoords[inputRemappedTexcoordIndex].y);
+ subMesh->AddTexCoord(texcoords[inputRemappedTexcoordIndex].X(),
+ texcoords[inputRemappedTexcoordIndex].Y());
input.texcoordIndex = inputRemappedTexcoordIndex;
}
@@ -1932,7 +1952,7 @@ void ColladaLoader::LoadTriangles(TiXmlElement *_trianglesXml,
/////////////////////////////////////////////////
void ColladaLoader::LoadLines(TiXmlElement *_xml,
- const math::Matrix4 &_transform,
+ const ignition::math::Matrix4d &_transform,
Mesh *_mesh)
{
SubMesh *subMesh = new SubMesh;
@@ -1943,8 +1963,8 @@ void ColladaLoader::LoadLines(TiXmlElement *_xml,
// std::string semantic = inputXml->Attribute("semantic");
std::string source = inputXml->Attribute("source");
- std::vector<math::Vector3> verts;
- std::vector<math::Vector3> norms;
+ std::vector<ignition::math::Vector3d> verts;
+ std::vector<ignition::math::Vector3d> norms;
this->LoadVertices(source, _transform, verts, norms);
TiXmlElement *pXml = _xml->FirstChildElement("p");
@@ -1974,7 +1994,8 @@ float ColladaLoader::LoadFloat(TiXmlElement *_elem)
if (_elem->FirstChildElement("float"))
{
- value = math::parseFloat(_elem->FirstChildElement("float")->GetText());
+ value =
+ ignition::math::parseFloat(_elem->FirstChildElement("float")->GetText());
}
return value;
diff --git a/gazebo/common/ColladaLoader.hh b/gazebo/common/ColladaLoader.hh
index 5e680ac..922616a 100644
--- a/gazebo/common/ColladaLoader.hh
+++ b/gazebo/common/ColladaLoader.hh
@@ -15,15 +15,18 @@
*
*/
-#ifndef _COLLADALOADER_HH_
-#define _COLLADALOADER_HH_
+#ifndef _GAZEBO_COLLADALOADER_HH_
+#define _GAZEBO_COLLADALOADER_HH_
#include <string>
#include <vector>
#include <map>
+#include <ignition/math/Matrix4.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Vector2.hh>
+
#include "gazebo/common/MeshLoader.hh"
-#include "gazebo/math/MathTypes.hh"
#include "gazebo/util/system.hh"
class TiXmlElement;
@@ -40,7 +43,7 @@ namespace gazebo
/// \class ColladaLoader ColladaLoader.hh common/common.hh
/// \brief Class used to load Collada mesh files
- class GAZEBO_VISIBLE ColladaLoader : public MeshLoader
+ class GZ_COMMON_VISIBLE ColladaLoader : public MeshLoader
{
/// \brief Constructor
public: ColladaLoader();
@@ -59,7 +62,8 @@ namespace gazebo
/// \param[in] _transform A tranform to apply
/// \param[in,out] _mesh The mesh being loaded
private: void LoadController(TiXmlElement *_contrXml,
- TiXmlElement *_skelXml, const math::Matrix4 &_transform, Mesh *_mesh);
+ TiXmlElement *_skelXml,
+ const ignition::math::Matrix4d &_transform, Mesh *_mesh);
/// \brief Load animations for a skeleton
/// \param[in] _xml Animation XML instance
@@ -88,7 +92,7 @@ namespace gazebo
/// \param[in] _tranform Transform to apply to the loaded geometry
/// \param[in,out] _mesh Pointer to the mesh currently being loaded
private: void LoadGeometry(TiXmlElement *_xml,
- const math::Matrix4 &_transform, Mesh *_mesh);
+ const ignition::math::Matrix4d &_transform, Mesh *_mesh);
/// \brief Get an XML element by ID
/// \param[in] _parent The parent element
@@ -109,13 +113,12 @@ namespace gazebo
/// \param[in,out] _mesh Pointer to the current mesh
/// \param[out] _transform Transform to apply to the node
private: void LoadNode(TiXmlElement *_elem, Mesh *_mesh,
- const math::Matrix4 &_transform);
+ const ignition::math::Matrix4d &_transform);
/// \brief Load a transform
/// \param[in] _elem Pointer to the transform XML instance
/// \return A Matrix4 transform
- private: math::Matrix4 LoadNodeTransform(TiXmlElement *_elem);
-
+ private: ignition::math::Matrix4d LoadNodeTransform(TiXmlElement *_elem);
/// \brief Load vertices
/// \param[in] _id String id of the vertices XML node
@@ -123,9 +126,9 @@ namespace gazebo
/// \param[out] _verts Holds the resulting vertices
/// \param[out] _norms Holds the resulting normals
private: void LoadVertices(const std::string &_id,
- const math::Matrix4 &_transform,
- std::vector<math::Vector3> &_verts,
- std::vector<math::Vector3> &_norms);
+ const ignition::math::Matrix4d &_transform,
+ std::vector<ignition::math::Vector3d> &_verts,
+ std::vector<ignition::math::Vector3d> &_norms);
/// \brief Load vertices
/// \param[in] _id String id of the vertices XML node
@@ -135,9 +138,9 @@ namespace gazebo
/// \param[out] _vertDup Holds a map of duplicate position indices
/// \param[out] _normDup Holds a map of duplicate normal indices
private: void LoadVertices(const std::string &_id,
- const math::Matrix4 &_transform,
- std::vector<math::Vector3> &_verts,
- std::vector<math::Vector3> &_norms,
+ const ignition::math::Matrix4d &_transform,
+ std::vector<ignition::math::Vector3d> &_verts,
+ std::vector<ignition::math::Vector3d> &_norms,
std::map<unsigned int, unsigned int> &_vertDup,
std::map<unsigned int, unsigned int> &_normDup);
@@ -147,8 +150,8 @@ namespace gazebo
/// \param[out] _values Holds the resulting position values
/// \param[out] _values Holds a map of duplicate position indices
private: void LoadPositions(const std::string &_id,
- const math::Matrix4 &_transform,
- std::vector<math::Vector3> &_values,
+ const ignition::math::Matrix4d &_transform,
+ std::vector<ignition::math::Vector3d> &_values,
std::map<unsigned int, unsigned int> &_duplicates);
/// \brief Load normals
@@ -157,8 +160,8 @@ namespace gazebo
/// \param[out] _values Holds the resulting normal values
/// \param[out] _values Holds a map of duplicate normal indices
private: void LoadNormals(const std::string &_id,
- const math::Matrix4 &_transform,
- std::vector<math::Vector3> &_values,
+ const ignition::math::Matrix4d &_transform,
+ std::vector<ignition::math::Vector3d> &_values,
std::map<unsigned int, unsigned int> &_duplicates);
/// \brief Load texture coordinates
@@ -166,7 +169,7 @@ namespace gazebo
/// \param[out] _values Holds the resulting uv values
/// \param[out] _values Holds a map of duplicate uv indices
private: void LoadTexCoords(const std::string &_id,
- std::vector<math::Vector2d> &_values,
+ std::vector<ignition::math::Vector2d> &_values,
std::map<unsigned int, unsigned int> &_duplicates);
/// \brief Load a material
@@ -187,7 +190,7 @@ namespace gazebo
/// \param[in] _transform Transform to apply to all triangles
/// \param[out] _mesh Mesh that is currently being loaded
private: void LoadTriangles(TiXmlElement *_trianglesXml,
- const math::Matrix4 &_transform,
+ const ignition::math::Matrix4d &_transform,
Mesh *_mesh);
/// \brief Load a polygon list
@@ -195,7 +198,7 @@ namespace gazebo
/// \param[in] _transform Transform to apply to each polygon
/// \param[out] _mesh Mesh that is currently being loaded
private: void LoadPolylist(TiXmlElement *_polylistXml,
- const math::Matrix4 &_transform,
+ const ignition::math::Matrix4d &_transform,
Mesh *_mesh);
/// \brief Load lines
@@ -203,7 +206,7 @@ namespace gazebo
/// \param[in] _transform Transform to apply
/// \param[out] _mesh Mesh that is currently being loaded
private: void LoadLines(TiXmlElement *_xml,
- const math::Matrix4 &_transform,
+ const ignition::math::Matrix4d &_transform,
Mesh *_mesh);
/// \brief Load an entire scene
diff --git a/gazebo/common/ColladaLoaderPrivate.hh b/gazebo/common/ColladaLoaderPrivate.hh
index a783c23..d9643ae 100644
--- a/gazebo/common/ColladaLoaderPrivate.hh
+++ b/gazebo/common/ColladaLoaderPrivate.hh
@@ -14,14 +14,15 @@
* limitations under the License.
*
*/
-
-#ifndef _COLLADALOADER_PRIVATE_HH_
-#define _COLLADALOADER_PRIVATE_HH_
+#ifndef _GAZEBO_COLLADALOADER_PRIVATE_HH_
+#define _GAZEBO_COLLADALOADER_PRIVATE_HH_
#include <map>
#include <string>
#include <vector>
+#include <ignition/math/Vector3.hh>
+
class TiXmlElement;
namespace gazebo
@@ -52,13 +53,16 @@ namespace gazebo
public: std::string currentNodeName;
/// \brief Map of collada POSITION ids to list of vectors.
- public: std::map<std::string, std::vector<math::Vector3> > positionIds;
+ public: std::map<std::string,
+ std::vector<ignition::math::Vector3d> > positionIds;
/// \brief Map of collada NORMAL ids to list of normals.
- public: std::map<std::string, std::vector<math::Vector3> > normalIds;
+ public: std::map<std::string,
+ std::vector<ignition::math::Vector3d> > normalIds;
/// \brief Map of collada TEXCOORD ids to list of texture coordinates.
- public: std::map<std::string, std::vector<math::Vector2d> >texcoordIds;
+ public: std::map<std::string,
+ std::vector<ignition::math::Vector2d> >texcoordIds;
/// \brief Map of collada Material ids to Gazebo materials.
public: std::map<std::string, Material *> materialIds;
diff --git a/gazebo/common/ColladaLoader_TEST.cc b/gazebo/common/ColladaLoader_TEST.cc
index 95fc1f4..3e703d3 100644
--- a/gazebo/common/ColladaLoader_TEST.cc
+++ b/gazebo/common/ColladaLoader_TEST.cc
@@ -34,8 +34,8 @@ TEST_F(ColladaLoader, LoadBox)
std::string(PROJECT_SOURCE_PATH) + "/test/data/box.dae");
EXPECT_STREQ("unknown", mesh->GetName().c_str());
- EXPECT_EQ(math::Vector3(1, 1, 1), mesh->GetMax());
- EXPECT_EQ(math::Vector3(-1, -1, -1), mesh->GetMin());
+ EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), mesh->Max());
+ EXPECT_EQ(ignition::math::Vector3d(-1, -1, -1), mesh->Min());
// 36 vertices, 24 unique, 12 shared.
EXPECT_EQ(24u, mesh->GetVertexCount());
EXPECT_EQ(24u, mesh->GetNormalCount());
@@ -78,15 +78,15 @@ TEST_F(ColladaLoader, ShareVertices)
const common::SubMesh *subMesh = mesh->GetSubMesh(i);
for (unsigned int j = 0; j < subMesh->GetVertexCount(); ++j)
{
- math::Vector3 v = subMesh->GetVertex(j);
- math::Vector3 n = subMesh->GetNormal(j);
+ ignition::math::Vector3d v = subMesh->Vertex(j);
+ ignition::math::Vector3d n = subMesh->Normal(j);
// Verify there is no other vertex with the same position AND normal
for (unsigned int k = j+1; k < subMesh->GetVertexCount(); ++k)
{
- if (v == subMesh->GetVertex(k))
+ if (v == subMesh->Vertex(k))
{
- EXPECT_TRUE(n != subMesh->GetNormal(k));
+ EXPECT_TRUE(n != subMesh->Normal(k));
}
}
}
diff --git a/gazebo/common/Color.cc b/gazebo/common/Color.cc
index 98ecf61..ffd59ca 100644
--- a/gazebo/common/Color.cc
+++ b/gazebo/common/Color.cc
@@ -14,11 +14,6 @@
* limitations under the License.
*
*/
-/* Desc: Color class
- * Author: Nate Koenig
- * Date: 08 May 2009
- */
-
#include <math.h>
#include <algorithm>
@@ -87,7 +82,7 @@ void Color::SetFromHSV(float _h, float _s, float _v)
_h = static_cast<int>(_h) % 360;
- if (math::equal(_s, 0.0f))
+ if (ignition::math::equal(_s, 0.0f))
{
// acromatic (grey)
this->r = this->g = this->b = _v;
@@ -146,75 +141,68 @@ void Color::SetFromHSV(float _h, float _s, float _v)
//////////////////////////////////////////////////
math::Vector3 Color::GetAsHSV() const
{
- math::Vector3 hsv;
+ return this->HSV();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d Color::HSV() const
+{
+ ignition::math::Vector3d hsv;
float x, v, f, i;
x = std::min(this->r, std::min(this->g, this->b));
v = std::max(this->r, std::max(this->g, this->b));
- if (math::equal(v, x))
+ if (ignition::math::equal(v, x))
{
gzerr << "rgb to hsv undefined\n";
return hsv;
}
- if (math::equal(r, x))
+ if (ignition::math::equal(r, x))
f = this->g - this->b;
- else if (math::equal(this->g, x))
+ else if (ignition::math::equal(this->g, x))
f = this->b - this->r;
else
f = this->r - this->g;
- if (math::equal(this->r, x))
+ if (ignition::math::equal(this->r, x))
i = 3;
- else if (math::equal(this->g, x))
+ else if (ignition::math::equal(this->g, x))
i = 5;
else
i = 1;
- hsv.x = i - f /(v - x);
- hsv.y = (v - x)/v;
- hsv.z = v;
+ hsv.X(i - f /(v - x));
+ hsv.Y((v - x)/v);
+ hsv.Z(v);
return hsv;
}
-
//////////////////////////////////////////////////
math::Vector3 Color::GetAsYUV() const
{
- math::Vector3 yuv;
-
- yuv.x = 0.299*this->r + 0.587*this->g + 0.114*this->b;
- yuv.y = -0.1679*this->r - 0.332*this->g + 0.5*this->b + 0.5;
- yuv.z = 0.5*this->r - 0.4189*this->g - 0.08105*this->b + 0.5;
-
- yuv.x = yuv.x < 0 ? 0: yuv.x;
- yuv.x = yuv.x > 255 ? 255.0: yuv.x;
-
- yuv.y = yuv.y < 0 ? 0: yuv.y;
- yuv.y = yuv.y > 255 ? 255.0: yuv.y;
-
- yuv.z = yuv.z < 0 ? 0: yuv.z;
- yuv.z = yuv.z > 255 ? 255.0: yuv.z;
+ return this->YUV();
+}
+//////////////////////////////////////////////////
+ignition::math::Vector3d Color::YUV() const
+{
+ ignition::math::Vector3d yuv;
+ yuv.X(0.299*this->r + 0.587*this->g + 0.114*this->b);
+ yuv.Y(-0.1679*this->r - 0.332*this->g + 0.5*this->b + 0.5);
+ yuv.Z(0.5*this->r - 0.4189*this->g - 0.08105*this->b + 0.5);
- /*if (yuv.x > 255)
- yuv.x = 255;
- if (yuv.x < 0)
- yuv.x = 0;
+ yuv.X(yuv.X() < 0 ? 0: yuv.X());
+ yuv.X(yuv.X() > 255 ? 255.0: yuv.X());
- if (yuv.y > 255)
- yuv.y = 255;
- if (yuv.y < 0)
- yuv.y = 0;
+ yuv.Y(yuv.Y() < 0 ? 0: yuv.Y());
+ yuv.Y(yuv.Y() > 255 ? 255.0: yuv.Y());
- if (yuv.z > 255)
- yuv.z = 255;
- if (yuv.z < 0)
- yuv.z = 0;
- */
+ yuv.Z(yuv.Z() < 0 ? 0: yuv.Z());
+ yuv.Z(yuv.Z() > 255 ? 255.0: yuv.Z());
return yuv;
}
@@ -512,10 +500,10 @@ const Color &Color::operator*=(const Color &pt)
//////////////////////////////////////////////////
bool Color::operator ==(const Color &_pt) const
{
- return math::equal(this->r, _pt.r) &&
- math::equal(this->g, _pt.g) &&
- math::equal(this->b, _pt.b) &&
- math::equal(this->a, _pt.a);
+ return ignition::math::equal(this->r, _pt.r) &&
+ ignition::math::equal(this->g, _pt.g) &&
+ ignition::math::equal(this->b, _pt.b) &&
+ ignition::math::equal(this->a, _pt.a);
}
//////////////////////////////////////////////////
diff --git a/gazebo/common/Color.hh b/gazebo/common/Color.hh
index d046e30..0f936b9 100644
--- a/gazebo/common/Color.hh
+++ b/gazebo/common/Color.hh
@@ -14,15 +14,12 @@
* limitations under the License.
*
*/
-/* Desc: Color class
- * Author: Nate Koenig
- * Date: 08 May 2009
- */
-
#ifndef _GAZEBO_COLOR_HH_
#define _GAZEBO_COLOR_HH_
#include <iostream>
+#include <ignition/math/Vector3.hh>
+
#include "gazebo/common/CommonTypes.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/util/system.hh"
@@ -36,7 +33,7 @@ namespace gazebo
/// \class Color Color.hh common/common.hh
/// \brief Defines a color
- class GAZEBO_VISIBLE Color
+ class GZ_COMMON_VISIBLE Color
{
/// \brief (1, 1, 1)
public: static const Color White;
@@ -98,7 +95,13 @@ namespace gazebo
/// \brief Get the color in HSV colorspace
/// \return HSV values in a math::Vector3 format
- public: math::Vector3 GetAsHSV() const;
+ /// \deprecated See HSV() function that returns an
+ /// ignition::math::Vector3d object.
+ public: math::Vector3 GetAsHSV() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the color in HSV colorspace
+ /// \return HSV values in an ignition::math::Vector3d format.
+ public: ignition::math::Vector3d HSV() const;
/// \brief Set a color based on HSV values
/// \param[in] _h Hue(0..360)
@@ -108,8 +111,14 @@ namespace gazebo
/// \brief Get the color in YUV colorspace
/// \return the YUV color
+ /// \deprecated See YUV() function that returns an
+ /// ignition::math::Vector3d object.
public: math::Vector3 GetAsYUV() const;
+ /// \brief Get the color in YUV colorspace
+ /// \return The YUV color in an ignition::math::Vector3d format.
+ public: ignition::math::Vector3d YUV() const;
+
/// \brief Set from yuv
/// \param[in] _y value
/// \param[in] _u value
diff --git a/gazebo/common/Color_TEST.cc b/gazebo/common/Color_TEST.cc
index f8cd69d..4e9abb6 100644
--- a/gazebo/common/Color_TEST.cc
+++ b/gazebo/common/Color_TEST.cc
@@ -44,7 +44,7 @@ TEST_F(Color, Color)
EXPECT_FLOAT_EQ(0.5f, clr.b);
EXPECT_FLOAT_EQ(0.0f, clr.a);
- EXPECT_TRUE(clr.GetAsHSV() == math::Vector3(6, 0.5, 1));
+ EXPECT_TRUE(clr.HSV() == ignition::math::Vector3d(6, 0.5, 1));
clr.SetFromHSV(60, 0.0, 1.0);
EXPECT_FLOAT_EQ(1.0f, clr.r);
@@ -123,30 +123,31 @@ TEST_F(Color, Color)
clr.SetFromYUV(0.5, 0.2, 0.8);
- EXPECT_TRUE(math::equal(0.00553f, clr.r, 1e-3f));
- EXPECT_TRUE(math::equal(0.0f, clr.g));
- EXPECT_TRUE(math::equal(0.9064f, clr.b, 1e-3f));
- EXPECT_TRUE(math::equal(0.04f, clr.a));
+ EXPECT_TRUE(ignition::math::equal(0.00553f, clr.r, 1e-3f));
+ EXPECT_TRUE(ignition::math::equal(0.0f, clr.g));
+ EXPECT_TRUE(ignition::math::equal(0.9064f, clr.b, 1e-3f));
+ EXPECT_TRUE(ignition::math::equal(0.04f, clr.a));
- EXPECT_TRUE(clr.GetAsYUV() == math::Vector3(0.104985, 0.95227, 0.429305));
+ EXPECT_TRUE(clr.YUV() ==
+ ignition::math::Vector3d(0.104985, 0.95227, 0.429305));
clr = common::Color(1.0, 0.0, 0.5, 1.0) + common::Color(0.1, 0.3, 0.4, 1.0);
- EXPECT_TRUE(math::equal(0.00431373f, clr.r));
- EXPECT_TRUE(math::equal(0.3f, clr.g));
- EXPECT_TRUE(math::equal(0.9f, clr.b));
- EXPECT_TRUE(math::equal(2.0f, clr.a));
+ EXPECT_TRUE(ignition::math::equal(0.00431373f, clr.r));
+ EXPECT_TRUE(ignition::math::equal(0.3f, clr.g));
+ EXPECT_TRUE(ignition::math::equal(0.9f, clr.b));
+ EXPECT_TRUE(ignition::math::equal(2.0f, clr.a));
clr = common::Color(1.0, 0.0, 0.5, 1.0) - common::Color(0.1, 0.3, 0.4, 1.0);
- EXPECT_TRUE(math::equal(0.9f, clr.r));
- EXPECT_TRUE(math::equal(0.0f, clr.g));
- EXPECT_TRUE(math::equal(0.1f, clr.b));
- EXPECT_TRUE(math::equal(0.0f, clr.a));
+ EXPECT_TRUE(ignition::math::equal(0.9f, clr.r));
+ EXPECT_TRUE(ignition::math::equal(0.0f, clr.g));
+ EXPECT_TRUE(ignition::math::equal(0.1f, clr.b));
+ EXPECT_TRUE(ignition::math::equal(0.0f, clr.a));
clr = common::Color(0.5, 0.2, 0.4, 0.6) / 2.0;
- EXPECT_TRUE(math::equal(0.25f, clr.r));
- EXPECT_TRUE(math::equal(0.1f, clr.g));
- EXPECT_TRUE(math::equal(0.2f, clr.b));
- EXPECT_TRUE(math::equal(0.3f, clr.a));
+ EXPECT_TRUE(ignition::math::equal(0.25f, clr.r));
+ EXPECT_TRUE(ignition::math::equal(0.1f, clr.g));
+ EXPECT_TRUE(ignition::math::equal(0.2f, clr.b));
+ EXPECT_TRUE(ignition::math::equal(0.3f, clr.a));
}
diff --git a/gazebo/common/CommonIface.cc b/gazebo/common/CommonIface.cc
index bf7f8dd..cb3464e 100644
--- a/gazebo/common/CommonIface.cc
+++ b/gazebo/common/CommonIface.cc
@@ -14,6 +14,12 @@
* limitations under the License.
*
*/
+#ifdef _WIN32
+ #include <Windows.h>
+#endif
+
+#include <cstdlib>
+
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>
@@ -74,3 +80,18 @@ std::string common::find_file_path(const std::string &_file)
return filepath.substr(0, index);
}
}
+
+/////////////////////////////////////////////////
+const char *common::getEnv(const char *_name)
+{
+#ifdef _WIN32
+ const DWORD buffSize = 65535;
+ static char buffer[buffSize];
+ if (GetEnvironmentVariable(_name, buffer, buffSize))
+ return buffer;
+ else
+ return NULL;
+#else
+ return getenv(_name);
+#endif
+}
diff --git a/gazebo/common/CommonIface.hh b/gazebo/common/CommonIface.hh
index e6d227e..afd23e3 100644
--- a/gazebo/common/CommonIface.hh
+++ b/gazebo/common/CommonIface.hh
@@ -34,18 +34,18 @@ namespace gazebo
/// \{
/// \brief Load the common library.
- GAZEBO_VISIBLE
+ GZ_COMMON_VISIBLE
void load();
/// \brief add path sufix to common::SystemPaths
/// \param[in] _suffix The suffix to add.
- GAZEBO_VISIBLE
+ GZ_COMMON_VISIBLE
void add_search_path_suffix(const std::string &_suffix);
/// \brief search for file in common::SystemPaths
/// \param[in] _file Name of the file to find.
/// \return The path containing the file.
- GAZEBO_VISIBLE
+ GZ_COMMON_VISIBLE
std::string find_file(const std::string &_file);
/// \brief search for file in common::SystemPaths
@@ -53,14 +53,14 @@ namespace gazebo
/// \param[in] _searchLocalPath True to search in the current working
/// directory.
/// \return The path containing the file.
- GAZEBO_VISIBLE
+ GZ_COMMON_VISIBLE
std::string find_file(const std::string &_file,
bool _searchLocalPath);
/// \brief search for a file in common::SystemPaths
/// \param[in] _file the file name to look for.
/// \return The path containing the file.
- GAZEBO_VISIBLE
+ GZ_COMMON_VISIBLE
std::string find_file_path(const std::string &_file);
/// \brief Compute the SHA1 hash of an array of bytes.
@@ -68,16 +68,19 @@ namespace gazebo
/// function are std::string and any STL container.
/// \return The string representation (40 character) of the SHA1 hash.
template<typename T>
- GAZEBO_VISIBLE
std::string get_sha1(const T &_buffer);
+ /// \brief Cross platform retrieval of an environment variable.
+ /// \param[in] _name Name of the environment variable to get.
+ /// \return Environment variable contents, or NULL on error.
+ GZ_COMMON_VISIBLE
+ const char *getEnv(const char *_name);
/// \}
}
///////////////////////////////////////////////
// Implementation of get_sha1
template<typename T>
- GAZEBO_VISIBLE
std::string common::get_sha1(const T &_buffer)
{
boost::uuids::detail::sha1 sha1;
diff --git a/gazebo/common/CommonTypes.hh b/gazebo/common/CommonTypes.hh
index 0b079f7..11d6797 100644
--- a/gazebo/common/CommonTypes.hh
+++ b/gazebo/common/CommonTypes.hh
@@ -37,8 +37,11 @@
#if defined(__GNUC__)
#define GAZEBO_DEPRECATED(version) __attribute__((deprecated))
#define GAZEBO_FORCEINLINE __attribute__((always_inline))
-#elif defined(MSVC)
-#define GAZEBO_DEPRECATED(version) ()
+#elif defined(_WIN32)
+// GAZEBO_DEPRECATED should be defined as something like
+// __declspec(deprecated), but it needs to go *before* the function name,
+// and we're putting GAZEBO_DEPRECATED *after* the function.
+#define GAZEBO_DEPRECATED(version)
#define GAZEBO_FORCEINLINE __forceinline
#else
#define GAZEBO_DEPRECATED(version) ()
diff --git a/gazebo/common/Console.cc b/gazebo/common/Console.cc
index 985bd5d..17762ad 100644
--- a/gazebo/common/Console.cc
+++ b/gazebo/common/Console.cc
@@ -93,7 +93,15 @@ Logger::Buffer::Buffer(LogType _type, int _color)
/////////////////////////////////////////////////
Logger::Buffer::~Buffer()
{
- this->pubsync();
+ // Can't throw from a destructor
+ try
+ {
+ this->pubsync();
+ }
+ catch(...)
+ {
+ std::cerr << "Exception thrown while pubsync'ing Buffer" << std::endl;
+ }
}
/////////////////////////////////////////////////
@@ -108,11 +116,19 @@ int Logger::Buffer::sync()
{
if (this->type == Logger::STDOUT)
{
- std::cout << "\033[1;" << this->color << "m" << this->str() << "\033[0m";
+ #ifndef _WIN32
+ std::cout << "\033[1;" << this->color << "m" << this->str() << "\033[0m";
+ #else
+ std::cout << this->str();
+ #endif
}
else
{
- std::cerr << "\033[1;" << this->color << "m" << this->str() << "\033[0m";
+ #ifndef _WIN32
+ std::cerr << "\033[1;" << this->color << "m" << this->str() << "\033[0m";
+ #else
+ std::cerr << this->str();
+ #endif
}
}
@@ -251,8 +267,15 @@ FileLogger::Buffer::Buffer(const std::string &_filename)
/////////////////////////////////////////////////
FileLogger::Buffer::~Buffer()
{
- if (this->stream)
- static_cast<std::ofstream*>(this->stream)->close();
+ try
+ {
+ if (this->stream)
+ static_cast<std::ofstream*>(this->stream)->close();
+ }
+ catch(...)
+ {
+ std::cerr << "Exception thrown while closing Buffer" << std::endl;
+ }
}
/////////////////////////////////////////////////
diff --git a/gazebo/common/Console.hh b/gazebo/common/Console.hh
index fcf06c2..b375898 100644
--- a/gazebo/common/Console.hh
+++ b/gazebo/common/Console.hh
@@ -66,7 +66,7 @@ namespace gazebo
/// \class FileLogger FileLogger.hh common/common.hh
/// \brief A logger that outputs messages to a file.
- class GAZEBO_VISIBLE FileLogger : public std::ostream
+ class GZ_COMMON_VISIBLE FileLogger : public std::ostream
{
/// \brief Constructor.
/// \param[in] _filename Filename to write into. If empty,
@@ -132,7 +132,7 @@ namespace gazebo
/// \class Logger Logger.hh common/common.hh
/// \brief Terminal logger.
- class GAZEBO_VISIBLE Logger : public std::ostream
+ class GZ_COMMON_VISIBLE Logger : public std::ostream
{
/// \enum LogType.
/// \brief Output destination type.
@@ -201,7 +201,7 @@ namespace gazebo
/// \class Console Console.hh common/common.hh
/// \brief Container for loggers, and global logging options
/// (such as verbose vs. quiet output).
- class GAZEBO_VISIBLE Console
+ class GZ_COMMON_VISIBLE Console
{
/// \brief Set quiet output.
/// \param[in] q True to prevent warning.
diff --git a/gazebo/common/Dem.cc b/gazebo/common/Dem.cc
index 19f9ede..71d7529 100644
--- a/gazebo/common/Dem.cc
+++ b/gazebo/common/Dem.cc
@@ -20,7 +20,10 @@
#include <gazebo/gazebo_config.h>
#ifdef HAVE_GDAL
-# include <gdal/ogr_spatialref.h>
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wfloat-equal"
+# include <ogr_spatialref.h>
+# pragma GCC diagnostic pop
#endif
#include "gazebo/common/CommonIface.hh"
@@ -29,7 +32,6 @@
#include "gazebo/common/DemPrivate.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/SphericalCoordinates.hh"
-#include "gazebo/math/Angle.hh"
using namespace gazebo;
using namespace common;
@@ -63,8 +65,8 @@ int Dem::Load(const std::string &_filename)
unsigned int height;
int xSize, ySize;
double upLeftX, upLeftY, upRightX, upRightY, lowLeftX, lowLeftY;
- math::Angle upLeftLat, upLeftLong, upRightLat, upRightLong;
- math::Angle lowLeftLat, lowLeftLong;
+ ignition::math::Angle upLeftLat, upLeftLong, upRightLat, upRightLong;
+ ignition::math::Angle lowLeftLat, lowLeftLong;
// Sanity check
std::string fullName = _filename;
@@ -125,15 +127,15 @@ int Dem::Load(const std::string &_filename)
lowLeftLat, lowLeftLong);
// Set the terrain's side (the terrain will be squared after the padding)
- if (math::isPowerOfTwo(ySize - 1))
+ if (ignition::math::isPowerOfTwo(ySize - 1))
height = ySize;
else
- height = math::roundUpPowerOfTwo(ySize) + 1;
+ height = ignition::math::roundUpPowerOfTwo(ySize) + 1;
- if (math::isPowerOfTwo(xSize - 1))
+ if (ignition::math::isPowerOfTwo(xSize - 1))
width = xSize;
else
- width = math::roundUpPowerOfTwo(xSize) + 1;
+ width = ignition::math::roundUpPowerOfTwo(xSize) + 1;
this->dataPtr->side = std::max(width, height);
@@ -177,7 +179,7 @@ float Dem::GetMaxElevation() const
//////////////////////////////////////////////////
void Dem::GetGeoReference(double _x, double _y,
- math::Angle &_latitude, math::Angle &_longitude)
+ ignition::math::Angle &_latitude, ignition::math::Angle &_longitude) const
{
double geoTransf[6];
if (this->dataPtr->dataSet->GetGeoTransform(geoTransf) == CE_None)
@@ -198,8 +200,8 @@ void Dem::GetGeoReference(double _x, double _y,
cT->Transform(1, &xGeoDeg, &yGeoDeg);
- _latitude.SetFromDegree(yGeoDeg);
- _longitude.SetFromDegree(xGeoDeg);
+ _latitude.Degree(yGeoDeg);
+ _longitude.Degree(xGeoDeg);
}
else
gzthrow("Unable to obtain the georeferenced values for coordinates ("
@@ -209,6 +211,16 @@ void Dem::GetGeoReference(double _x, double _y,
//////////////////////////////////////////////////
void Dem::GetGeoReferenceOrigin(math::Angle &_latitude, math::Angle &_longitude)
{
+ ignition::math::Angle lat, lon;
+ this->GetGeoReferenceOrigin(lat, lon);
+ _latitude = lat;
+ _longitude = lon;
+}
+
+//////////////////////////////////////////////////
+void Dem::GetGeoReferenceOrigin(ignition::math::Angle &_latitude,
+ ignition::math::Angle &_longitude) const
+{
return this->GetGeoReference(0, 0, _latitude, _longitude);
}
@@ -241,6 +253,16 @@ void Dem::FillHeightMap(int _subSampling, unsigned int _vertSize,
const math::Vector3 &_size, const math::Vector3 &_scale,
bool _flipY, std::vector<float> &_heights)
{
+ this->FillHeightMap(_subSampling, _vertSize, _size.Ign(), _scale.Ign(),
+ _flipY, _heights);
+}
+
+//////////////////////////////////////////////////
+void Dem::FillHeightMap(int _subSampling, unsigned int _vertSize,
+ const ignition::math::Vector3d &_size,
+ const ignition::math::Vector3d &_scale,
+ bool _flipY, std::vector<float> &_heights)
+{
if (_subSampling <= 0)
{
gzerr << "Illegal subsampling value (" << _subSampling << ")\n";
@@ -278,16 +300,16 @@ void Dem::FillHeightMap(int _subSampling, unsigned int _vertSize,
float h2 = (px3 - ((px3 - px4) * dx));
float h = (h1 - ((h1 - h2) * dy) - std::max(0.0f,
- this->GetMinElevation())) * _scale.z;
+ this->GetMinElevation())) * _scale.Z();
// Invert pixel definition so 1=ground, 0=full height,
// if the terrain size has a negative z component
// this is mainly for backward compatibility
- if (_size.z < 0)
+ if (_size.Z() < 0)
h *= -1;
// Convert to 0 if a NODATA value is found
- if (_size.z >= 0 && h < 0)
+ if (_size.Z() >= 0 && h < 0)
h = 0;
// Store the height for future use
diff --git a/gazebo/common/Dem.hh b/gazebo/common/Dem.hh
index cd8d7d3..6f5fbff 100644
--- a/gazebo/common/Dem.hh
+++ b/gazebo/common/Dem.hh
@@ -18,6 +18,9 @@
#ifndef _GAZEBO_DEM_HH_
#define _GAZEBO_DEM_HH_
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Angle.hh>
+
#include <gazebo/gazebo_config.h>
#include <gazebo/util/system.hh>
@@ -39,7 +42,7 @@ namespace gazebo
/// \class DEM DEM.hh common/common.hh
/// \brief Encapsulates a DEM (Digital Elevation Model) file.
- class GAZEBO_VISIBLE Dem : public HeightmapData
+ class GZ_COMMON_VISIBLE Dem : public HeightmapData
{
/// \brief Constructor.
public: Dem();
@@ -70,8 +73,17 @@ namespace gazebo
/// origin in WGS84.
/// \param[out] _latitude Georeferenced latitude.
/// \param[out] _longitude Georeferenced longitude.
+ /// \deprecated See GetGeoReferenceOrigin() that accepts
+ /// ignition::math::Angle objects.
public: void GetGeoReferenceOrigin(math::Angle &_latitude,
- math::Angle &_longitude);
+ math::Angle &_longitude) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the georeferenced coordinates (lat, long) of the terrain's
+ /// origin in WGS84.
+ /// \param[out] _latitude Georeferenced latitude.
+ /// \param[out] _longitude Georeferenced longitude.
+ public: void GetGeoReferenceOrigin(ignition::math::Angle &_latitude,
+ ignition::math::Angle &_longitude) const;
/// \brief Get the terrain's height. Due to the Ogre constrains, this
/// value will be a power of two plus one. The value returned might be
@@ -109,9 +121,28 @@ namespace gazebo
/// \param[in] _flipY If true, it inverts the order in which the vector
/// is filled.
/// \param[out] _heights Vector containing the terrain heights.
+ /// \deprecated See FillHeightMap() function that accepts
+ /// ignition::math objects.
public: void FillHeightMap(int _subSampling, unsigned int _vertSize,
const math::Vector3 &_size, const math::Vector3 &_scale, bool _flipY,
- std::vector<float> &_heights);
+ std::vector<float> &_heights) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Create a lookup table of the terrain's height.
+ /// \param[in] _subsampling Multiplier used to increase the resolution.
+ /// Ex: A subsampling of 2 in a terrain of 129x129 means that the height
+ /// vector will be 257 * 257.
+ /// \param[in] _vertSize Number of points per row.
+ /// \param[in] _size Real dimmensions of the terrain in meters.
+ /// \param[in] _scale Vector3 used to scale the height.
+ /// \param[in] _flipY If true, it inverts the order in which the vector
+ /// is filled.
+ /// \param[out] _heights Vector containing the terrain heights.
+ public: void FillHeightMap(const int _subSampling,
+ const unsigned int _vertSize,
+ const ignition::math::Vector3d &_size,
+ const ignition::math::Vector3d &_scale,
+ const bool _flipY,
+ std::vector<float> &_heights);
/// \brief Get the georeferenced coordinates (lat, long) of a terrain's
/// pixel in WGS84.
@@ -120,8 +151,8 @@ namespace gazebo
/// \param[out] _latitude Georeferenced latitude.
/// \param[out] _longitude Georeferenced longitude.
private: void GetGeoReference(double _x, double _y,
- math::Angle &_latitude,
- math::Angle &_longitude);
+ ignition::math::Angle &_latitude,
+ ignition::math::Angle &_longitude) const;
/// \brief Get the terrain file as a data array. Due to the Ogre
/// constrains, the data might be stored in a bigger vector representing
diff --git a/gazebo/common/DemPrivate.hh b/gazebo/common/DemPrivate.hh
index a4833dd..2e0e372 100644
--- a/gazebo/common/DemPrivate.hh
+++ b/gazebo/common/DemPrivate.hh
@@ -22,7 +22,7 @@
#include <gazebo/util/system.hh>
#ifdef HAVE_GDAL
-# include <gdal/gdal_priv.h>
+# include <gdal_priv.h>
# include <vector>
namespace gazebo
@@ -34,7 +34,7 @@ namespace gazebo
/// \class DemPrivate DemPrivate.hh common/common.hh
/// \brief Private data for the Dem class.
- class GAZEBO_VISIBLE DemPrivate
+ class GZ_COMMON_VISIBLE DemPrivate
{
/// \brief A set of associated raster bands.
public: GDALDataset *dataSet;
diff --git a/gazebo/common/Dem_TEST.cc b/gazebo/common/Dem_TEST.cc
index 0817123..be0155a 100644
--- a/gazebo/common/Dem_TEST.cc
+++ b/gazebo/common/Dem_TEST.cc
@@ -17,6 +17,8 @@
#include <boost/filesystem.hpp>
#include <gtest/gtest.h>
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Vector3.hh>
#include "gazebo/common/Dem.hh"
#include "gazebo/math/Angle.hh"
@@ -117,7 +119,7 @@ TEST_F(DemTest, BasicAPI)
ASSERT_ANY_THROW(dem.GetElevation(width, height));
// Check GetGeoReferenceOrigin()
- math::Angle latitude, longitude;
+ ignition::math::Angle latitude, longitude;
dem.GetGeoReferenceOrigin(latitude, longitude);
EXPECT_FLOAT_EQ(38.001667, latitude.Degree());
EXPECT_FLOAT_EQ(-122.22278, longitude.Degree());
@@ -135,22 +137,23 @@ TEST_F(DemTest, FillHeightmap)
// Use FillHeightMap() to retrieve a vector<float> after some transformations
int subsampling;
unsigned vertSize;
- math::Vector3 size;
- math::Vector3 scale;
+ ignition::math::Vector3d size;
+ ignition::math::Vector3d scale;
bool flipY;
std::vector<float> elevations;
subsampling = 2;
vertSize = (dem.GetWidth() * subsampling) - 1;
- size.x = dem.GetWorldWidth();
- size.y = dem.GetWorldHeight();
- size.z = dem.GetMaxElevation() - dem.GetMinElevation();
- scale.x = size.x / vertSize;
- scale.y = size.y / vertSize;
- if (math::equal(dem.GetMaxElevation(), 0.0f))
- scale.z = fabs(size.z);
+ size.X(dem.GetWorldWidth());
+ size.Y(dem.GetWorldHeight());
+ size.Z(dem.GetMaxElevation() - dem.GetMinElevation());
+ scale.X(size.X() / vertSize);
+ scale.Y(size.Y() / vertSize);
+
+ if (ignition::math::equal(dem.GetMaxElevation(), 0.0f))
+ scale.Z(fabs(size.Z()));
else
- scale.z = fabs(size.z) / dem.GetMaxElevation();
+ scale.Z(fabs(size.Z()) / dem.GetMaxElevation());
flipY = false;
dem.FillHeightMap(subsampling, vertSize, size, scale, flipY, elevations);
diff --git a/gazebo/common/Event.hh b/gazebo/common/Event.hh
index e369da6..86ac5d7 100644
--- a/gazebo/common/Event.hh
+++ b/gazebo/common/Event.hh
@@ -15,17 +15,19 @@
*
*/
-#ifndef _EVENT_HH_
-#define _EVENT_HH_
+#ifndef _GAZEBO_EVENT_HH_
+#define _GAZEBO_EVENT_HH_
+#include <atomic>
#include <iostream>
#include <vector>
#include <map>
+#include <memory>
+#include <mutex>
+#include <list>
#include <boost/function.hpp>
#include <boost/bind.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/thread/mutex.hpp>
#include <gazebo/gazebo_config.h>
#include <gazebo/common/Time.hh>
@@ -45,7 +47,7 @@ namespace gazebo
/// \internal
// Private data members for Event class.
// This must be in the header due to templatization.
- class GAZEBO_VISIBLE EventPrivate
+ class GZ_COMMON_VISIBLE EventPrivate
{
// \brief Constructor
public: EventPrivate();
@@ -56,7 +58,7 @@ namespace gazebo
/// \class Event Event.hh common/common.hh
/// \brief Base class for all events
- class GAZEBO_VISIBLE Event
+ class GZ_COMMON_VISIBLE Event
{
/// \brief Constructor
public: Event();
@@ -86,7 +88,7 @@ namespace gazebo
/// \internal
// Private data members for Connection class.
- class GAZEBO_VISIBLE ConnectionPrivate
+ class GZ_COMMON_VISIBLE ConnectionPrivate
{
/// \brief Constructor.
public: ConnectionPrivate();
@@ -107,7 +109,7 @@ namespace gazebo
};
/// \brief A class that encapsulates a connection.
- class GAZEBO_VISIBLE Connection
+ class GZ_COMMON_VISIBLE Connection
{
/// \brief Constructor.
public: Connection();
@@ -132,33 +134,49 @@ namespace gazebo
};
/// \internal
+ template<typename T>
+ class EventConnection
+ {
+ /// \brief Constructor
+ public: EventConnection(const bool _on,
+ boost::function<T> *_cb)
+ : on(_on), callback(_cb)
+ {
+ }
+
+ /// \brief On/off value for the event callback
+ public: std::atomic_bool on;
+
+ /// \brief Callback function
+ public: std::shared_ptr<boost::function<T> > callback;
+ };
+
+ /// \internal
// Private data members for EventT<T> class.
template< typename T>
- class GAZEBO_VISIBLE EventTPrivate : public EventPrivate
+ class GZ_COMMON_VISIBLE EventTPrivate : public EventPrivate
{
/// \def EvtConnectionMap
/// \brief Event Connection map typedef.
- typedef std::map<int, boost::function<T>*> EvtConnectionMap;
+ typedef std::map<int, std::shared_ptr<EventConnection<T> > >
+ EvtConnectionMap;
/// \brief Array of connection callbacks.
public: EvtConnectionMap connections;
- /// \brief Set of connections to erased.
- public: std::vector<int> connectionsToErase;
-
/// \brief A thread lock.
- public: boost::mutex connectionsEraseMutex;
+ public: std::mutex mutex;
+
+ /// \brief List of connections to remove
+ public: std::list<typename EvtConnectionMap::const_iterator>
+ connectionsToRemove;
};
/// \class EventT Event.hh common/common.hh
/// \brief A class for event processing.
template< typename T>
- class GAZEBO_VISIBLE EventT : public Event
+ class EventT : public Event
{
- /// \def EvtConnectionMap.
- /// \brief Event Connection map typedef.
- typedef std::map<int, boost::function<T>*> EvtConnectionMap;
-
/// \brief Constructor.
public: EventT();
@@ -187,31 +205,22 @@ namespace gazebo
public: void operator()()
{this->Signal();}
- /// \brief Signal the event for all subscribers.
- public: void Signal()
- {
- this->myDataPtr->signaled = true;
- this->Cleanup();
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- (*iter->second)();
- }
- }
-
/// \brief Signal the event with one parameter.
/// \param[in] _p the parameter.
public: template< typename P >
void operator()(const P &_p)
- { this->Signal(_p); }
+ {
+ this->Signal(_p);
+ }
/// \brief Signal the event with two parameters.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
public: template< typename P1, typename P2 >
void operator()(const P1 &_p1, const P2 &_p2)
- { this->Signal(_p1, _p2); }
+ {
+ this->Signal(_p1, _p2);
+ }
/// \brief Signal the event with three parameters.
/// \param[in] _p1 the first parameter.
@@ -219,7 +228,9 @@ namespace gazebo
/// \param[in] _p3 the second parameter.
public: template< typename P1, typename P2, typename P3 >
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3)
- { this->Signal(_p1, _p2, _p3); }
+ {
+ this->Signal(_p1, _p2, _p3);
+ }
/// \brief Signal the event with four parameters.
/// \param[in] _p1 the first parameter.
@@ -229,7 +240,9 @@ namespace gazebo
public: template< typename P1, typename P2, typename P3, typename P4 >
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4)
- { this->Signal(_p1, _p2, _p3, _p4); }
+ {
+ this->Signal(_p1, _p2, _p3, _p4);
+ }
/// \brief Signal the event with five parameters.
/// \param[in] _p1 the first parameter.
@@ -241,7 +254,9 @@ namespace gazebo
typename P5 >
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5)
- { this->Signal(_p1, _p2, _p3, _p4, _p5); }
+ {
+ this->Signal(_p1, _p2, _p3, _p4, _p5);
+ }
/// \brief Signal the event with six parameters.
/// \param[in] _p1 the first parameter.
@@ -254,7 +269,9 @@ namespace gazebo
typename P5, typename P6 >
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6)
- { this->Signal(_p1, _p2, _p3, _p4, _p5, _p6); }
+ {
+ this->Signal(_p1, _p2, _p3, _p4, _p5, _p6);
+ }
/// \brief Signal the event with seven parameters.
/// \param[in] _p1 the first parameter.
@@ -269,7 +286,9 @@ namespace gazebo
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7)
- { this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7); }
+ {
+ this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7);
+ }
/// \brief Signal the event with eight parameters.
/// \param[in] _p1 the first parameter.
@@ -285,7 +304,9 @@ namespace gazebo
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7, const P8 &_p8)
- { this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8); }
+ {
+ this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8);
+ }
/// \brief Signal the event with nine parameters.
/// \param[in] _p1 the first parameter.
@@ -303,7 +324,9 @@ namespace gazebo
void operator()(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7, const P8 &_p8, const P9 &_p9)
- { this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9); }
+ {
+ this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9);
+ }
/// \brief Signal the event with ten parameters.
/// \param[in] _p1 the first parameter.
@@ -323,40 +346,53 @@ namespace gazebo
const P4 &_p4, const P5 &_p5, const P6 &_p6,
const P7 &_p7, const P8 &_p8, const P9 &_p9,
const P10 &_p10)
- {
- this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9, _p10);
- }
+ {
+ this->Signal(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9, _p10);
+ }
+
+ /// \brief Signal the event for all subscribers.
+ public: void Signal()
+ {
+ this->Cleanup();
+
+ this->myDataPtr->signaled = true;
+ for (auto iter: this->myDataPtr->connections)
+ {
+ if (iter.second->on)
+ (*iter.second->callback)();
+ }
+ }
/// \brief Signal the event with one parameter.
/// \param[in] _p parameter.
public: template< typename P >
void Signal(const P &_p)
- {
- this->myDataPtr->signaled = true;
- this->Cleanup();
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- (*iter->second)(_p);
- }
- }
+ {
+ this->Cleanup();
+
+ this->myDataPtr->signaled = true;
+ for (auto iter: this->myDataPtr->connections)
+ {
+ if (iter.second->on)
+ (*iter.second->callback)(_p);
+ }
+ }
/// \brief Signal the event with two parameter.
/// \param[in] _p1 the first parameter.
/// \param[in] _p2 the second parameter.
public: template< typename P1, typename P2 >
void Signal(const P1 &_p1, const P2 &_p2)
- {
- this->myDataPtr->signaled = true;
- this->Cleanup();
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- (*iter->second)(_p1, _p2);
- }
- }
+ {
+ this->Cleanup();
+
+ this->myDataPtr->signaled = true;
+ for (auto iter: this->myDataPtr->connections)
+ {
+ if (iter.second->on)
+ (*iter.second->callback)(_p1, _p2);
+ }
+ }
/// \brief Signal the event with three parameter.
/// \param[in] _p1 the first parameter.
@@ -364,16 +400,16 @@ namespace gazebo
/// \param[in] _p3 the second parameter.
public: template< typename P1, typename P2, typename P3 >
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3)
- {
- this->myDataPtr->signaled = true;
- this->Cleanup();
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- (*iter->second)(_p1, _p2, _p3);
- }
- }
+ {
+ this->Cleanup();
+
+ this->myDataPtr->signaled = true;
+ for (auto iter: this->myDataPtr->connections)
+ {
+ if (iter.second->on)
+ (*iter.second->callback)(_p1, _p2, _p3);
+ }
+ }
/// \brief Signal the event with four parameter.
/// \param[in] _p1 the first parameter.
@@ -383,16 +419,16 @@ namespace gazebo
public: template<typename P1, typename P2, typename P3, typename P4>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4)
- {
- this->myDataPtr->signaled = true;
- this->Cleanup();
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- (*iter->second)(_p1, _p2, _p3, _p4);
- }
- }
+ {
+ this->Cleanup();
+
+ this->myDataPtr->signaled = true;
+ for (auto iter: this->myDataPtr->connections)
+ {
+ if (iter.second->on)
+ (*iter.second->callback)(_p1, _p2, _p3, _p4);
+ }
+ }
/// \brief Signal the event with five parameter.
/// \param[in] _p1 the first parameter.
@@ -404,17 +440,16 @@ namespace gazebo
typename P5>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5)
- {
- this->myDataPtr->signaled = true;
- this->Cleanup();
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- (*iter->second)(_p1, _p2, _p3, _p4, _p5);
- }
- }
+ {
+ this->Cleanup();
+ this->myDataPtr->signaled = true;
+ for (auto iter: this->myDataPtr->connections)
+ {
+ if (iter.second->on)
+ (*iter.second->callback)(_p1, _p2, _p3, _p4, _p5);
+ }
+ }
/// \brief Signal the event with six parameter.
/// \param[in] _p1 the first parameter.
@@ -427,16 +462,16 @@ namespace gazebo
typename P5, typename P6>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6)
- {
- this->myDataPtr->signaled = true;
- this->Cleanup();
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- (*iter->second)(_p1, _p2, _p3, _p4, _p5, _p6);
- }
- }
+ {
+ this->Cleanup();
+
+ this->myDataPtr->signaled = true;
+ for (auto iter: this->myDataPtr->connections)
+ {
+ if (iter.second->on)
+ (*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6);
+ }
+ }
/// \brief Signal the event with seven parameter.
/// \param[in] _p1 the first parameter.
@@ -450,16 +485,16 @@ namespace gazebo
typename P5, typename P6, typename P7>
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7)
- {
- this->myDataPtr->signaled = true;
- this->Cleanup();
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- (*iter->second)(_p1, _p2, _p3, _p4, _p5, _p6, _p7);
- }
- }
+ {
+ this->Cleanup();
+
+ this->myDataPtr->signaled = true;
+ for (auto iter: this->myDataPtr->connections.begin())
+ {
+ if (iter.second->on)
+ (*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6, _p7);
+ }
+ }
/// \brief Signal the event with eight parameter.
/// \param[in] _p1 the first parameter.
@@ -475,16 +510,18 @@ namespace gazebo
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7,
const P8 &_p8)
- {
- this->myDataPtr->signaled = true;
- this->Cleanup();
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- (*iter->second)(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8);
- }
- }
+ {
+ this->Cleanup();
+
+ this->myDataPtr->signaled = true;
+ for (auto iter: this->myDataPtr->connections)
+ {
+ if (iter.second->on)
+ {
+ (*iter.second->callback)(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8);
+ }
+ }
+ }
/// \brief Signal the event with nine parameter.
/// \param[in] _p1 the first parameter.
@@ -502,16 +539,19 @@ namespace gazebo
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7,
const P8 &_p8, const P9 &_p9)
+ {
+ this->Cleanup();
+
+ this->myDataPtr->signaled = true;
+ for (auto iter: this->myDataPtr->connections)
+ {
+ if (iter.second->on)
{
- this->myDataPtr->signaled = true;
- this->Cleanup();
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- (*iter->second)(_p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9);
- }
+ (*iter.second->callback)(
+ _p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9);
}
+ }
+ }
/// \brief Signal the event with ten parameter.
/// \param[in] _p1 the first parameter.
@@ -530,19 +570,23 @@ namespace gazebo
void Signal(const P1 &_p1, const P2 &_p2, const P3 &_p3,
const P4 &_p4, const P5 &_p5, const P6 &_p6, const P7 &_p7,
const P8 &_p8, const P9 &_p9, const P10 &_p10)
- {
- this->myDataPtr->signaled = true;
- this->Cleanup();
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- (*iter->second)(_p1, _p2, _p3, _p4, _p5,
- _p6, _p7, _p8, _p9, _p10);
- }
- }
-
- /// \brief Cleanup disconnected connections.
+ {
+ this->Cleanup();
+
+ this->myDataPtr->signaled = true;
+ for (auto iter: this->myDataPtr->connections)
+ {
+ if (iter.second->on)
+ {
+ (*iter.second->callback)(
+ _p1, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9, _p10);
+ }
+ }
+ }
+
+ /// \internal
+ /// \brief Removes queued connections.
+ /// We assume that this function is called from a Signal function.
private: void Cleanup();
/// \brief Private data pointer.
@@ -561,13 +605,6 @@ namespace gazebo
template<typename T>
EventT<T>::~EventT()
{
- for (typename EvtConnectionMap::iterator iter =
- this->myDataPtr->connections.begin();
- iter != this->myDataPtr->connections.end(); ++iter)
- {
- delete iter->second;
- }
-
this->myDataPtr->connections.clear();
}
@@ -579,11 +616,11 @@ namespace gazebo
int index = 0;
if (!this->myDataPtr->connections.empty())
{
- typename EvtConnectionMap::reverse_iterator iter =
- this->myDataPtr->connections.rbegin();
+ auto const &iter = this->myDataPtr->connections.rbegin();
index = iter->first + 1;
}
- this->myDataPtr->connections[index] = new boost::function<T>(_subscriber);
+ this->myDataPtr->connections[index].reset(new EventConnection<T>(true,
+ new boost::function<T>(_subscriber)));
return ConnectionPtr(new Connection(this, index));
}
@@ -613,32 +650,27 @@ namespace gazebo
template<typename T>
void EventT<T>::Disconnect(int _id)
{
- boost::mutex::scoped_lock lock(this->myDataPtr->connectionsEraseMutex);
- this->myDataPtr->connectionsToErase.push_back(_id);
+ // Find the connection
+ auto const &it = this->myDataPtr->connections.find(_id);
+
+ if (it != this->myDataPtr->connections.end())
+ {
+ it->second->on = false;
+ this->myDataPtr->connectionsToRemove.push_back(it);
+ }
}
- /// \brief Cleanup disconnected connections.
+ /////////////////////////////////////////////
template<typename T>
void EventT<T>::Cleanup()
{
- if (this->myDataPtr->connectionsToErase.empty())
- return;
- boost::mutex::scoped_lock lock(this->myDataPtr->connectionsEraseMutex);
-
- for (std::vector<int>::iterator iter =
- this->myDataPtr->connectionsToErase.begin();
- iter != this->myDataPtr->connectionsToErase.end(); ++iter)
- {
- typename EvtConnectionMap::iterator iter2 =
- this->myDataPtr->connections.find(*iter);
- if (iter2 != this->myDataPtr->connections.end())
- {
- delete iter2->second;
- this->myDataPtr->connections.erase(iter2);
- }
- }
- this->myDataPtr->connectionsToErase.clear();
+ std::lock_guard<std::mutex> lock(this->myDataPtr->mutex);
+ // Remove all queue connections.
+ for (auto &conn : this->myDataPtr->connectionsToRemove)
+ this->myDataPtr->connections.erase(conn);
+ this->myDataPtr->connectionsToRemove.clear();
}
+
/// \}
}
}
diff --git a/gazebo/common/Event_TEST.cc b/gazebo/common/Event_TEST.cc
index e0022af..090df60 100644
--- a/gazebo/common/Event_TEST.cc
+++ b/gazebo/common/Event_TEST.cc
@@ -26,6 +26,9 @@ class EventTest : public gazebo::testing::AutoLogFixture { };
int g_callback = 0;
int g_callback1 = 0;
+event::EventT<void ()> g_event;
+event::ConnectionPtr g_conn;
+event::ConnectionPtr g_conn2;
/////////////////////////////////////////////////
void callback()
@@ -40,6 +43,40 @@ void callback1()
}
/////////////////////////////////////////////////
+// Used by the CallbackDisconnect test.
+void callbackDisconnect()
+{
+ // Remove both connections in the callback, which should not cause
+ // a segfault.
+ g_event.Disconnect(g_conn);
+ g_event.Disconnect(g_conn2);
+}
+
+/////////////////////////////////////////////////
+// Used by the CallbackDisconnect test.
+void callbackDisconnect2()
+{
+ // This function should still be called, even though it was disconnected
+ // in the callDisconnect function. The mutex in Event.hh prevents
+ // a callback from deleting active connections until the event is
+ // complete.
+ ASSERT_TRUE(true);
+}
+
+/////////////////////////////////////////////////
+// Make sure that calling disconnect in an event callback does not produce
+// a segfault.
+TEST_F(EventTest, CallbackDisconnect)
+{
+ // Create two connections
+ g_conn = g_event.Connect(boost::bind(&callbackDisconnect));
+ g_conn2 = g_event.Connect(boost::bind(&callbackDisconnect2));
+
+ // Call the event. See the callback functions for more info.
+ g_event();
+}
+
+/////////////////////////////////////////////////
TEST_F(EventTest, SignalOnce)
{
g_callback = 0;
diff --git a/gazebo/common/Events.hh b/gazebo/common/Events.hh
index 8bccc30..0d54b20 100644
--- a/gazebo/common/Events.hh
+++ b/gazebo/common/Events.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class Events Events.hh common/common.hh
/// \brief An Event class to get notifications for simulator events
- class GAZEBO_VISIBLE Events
+ class GZ_COMMON_VISIBLE Events
{
//////////////////////////////////////////////////////////////////////////
/// \brief Connect a boost::slot the the pause signal
diff --git a/gazebo/common/Exception.hh b/gazebo/common/Exception.hh
index 568cd47..f029b04 100644
--- a/gazebo/common/Exception.hh
+++ b/gazebo/common/Exception.hh
@@ -42,7 +42,7 @@ namespace gazebo
/// \class Exception Exception.hh common/common.hh
/// \brief Class for generating exceptions
- class GAZEBO_VISIBLE Exception
+ class GZ_COMMON_VISIBLE Exception
{
/// \brief Constructor
public: Exception();
@@ -92,7 +92,7 @@ namespace gazebo
/// \brief Class for generating Internal Gazebo Errors:
/// those errors which should never happend and
/// represent programming bugs.
- class GAZEBO_VISIBLE InternalError : public Exception
+ class GZ_COMMON_VISIBLE InternalError : public Exception
{
/// \brief Constructor
public: InternalError();
@@ -113,7 +113,7 @@ namespace gazebo
/// gazebo assertions. They include information about the
/// assertion expression violated, function where problem
/// appeared and assertion debug message.
- class GAZEBO_VISIBLE AssertionInternalError : public InternalError
+ class GZ_COMMON_VISIBLE AssertionInternalError : public InternalError
{
/// \brief Constructor for assertions
/// \param[in] _file File name
diff --git a/gazebo/common/GTSMeshUtils.cc b/gazebo/common/GTSMeshUtils.cc
index f45178a..a4abcfe 100644
--- a/gazebo/common/GTSMeshUtils.cc
+++ b/gazebo/common/GTSMeshUtils.cc
@@ -43,21 +43,11 @@ static void FillFace(GtsTriangle *_t, gpointer *_data)
{
SubMesh *subMesh = reinterpret_cast<SubMesh *>(_data[0]);
GHashTable *vIndex = reinterpret_cast<GHashTable *>(_data[2]);
- int *x = reinterpret_cast<int*>(_data[3]);
GtsVertex *v1, *v2, *v3;
gts_triangle_vertices(_t, &v1, &v2, &v3);
- if (*x == 0)
- {
- subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v1))+*x);
- subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v3))+*x);
- subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v2))+*x);
- }
- else
- {
- subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v1))+*x);
- subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v2))+*x);
- subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v3))+*x);
- }
+ subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v1)));
+ subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v3)));
+ subMesh->AddIndex(GPOINTER_TO_UINT(g_hash_table_lookup(vIndex, v2)));
}
//////////////////////////////////////////////////////////////////////////
@@ -67,125 +57,203 @@ static void AddConstraint(GtsConstraint *_c, GtsSurface *_s)
}
//////////////////////////////////////////////////
-bool GTSMeshUtils::CreateExtrudedPolyline(
- const std::vector<math::Vector2d> &_vertices,
- const double &_height, SubMesh *_subMesh)
+static void Intersection(GtsEdge *_c, gpointer *_data)
{
- if (_vertices.size() < 3)
+ double x = *reinterpret_cast<double *>(_data[0]);
+ double y = *reinterpret_cast<double *>(_data[1]);
+ int *intersection = reinterpret_cast<int *>(_data[2]);
+
+ GtsVertex *v1, *v2;
+ v1 = _c->segment.v1;
+ v2 = _c->segment.v2;
+
+ double x1 = v1->p.x;
+ double x2 = v2->p.x;
+ double y1 = v1->p.y;
+ double y2 = v2->p.y;
+
+ double xmin = std::min(x1, x2);
+ double xmax = (x1 + x2) - xmin;
+ double ymin = std::min(y1, y2);
+ double ymax = (y1 + y2) - ymin;
+ double xBound = xmax+1;
+
+ if (y < ymax && y >= ymin)
{
- gzerr << "Unable to create an extruded polyline mesh with "
- << "less than 3 vertices\n";
- return false;
+ double xdiff1, ydiff1, xdiff2, ydiff2;
+ xdiff1 = x2 - x1;
+ ydiff1 = y2 - y1;
+ xdiff2 = xBound - x;
+ ydiff2 = 0;
+
+ double s, t;
+ s = (-ydiff1 * (x1 - x) + xdiff1 * (y1 - y)) /
+ (-xdiff2 * ydiff1 + xdiff1 * ydiff2);
+ t = (xdiff2 * (y1 - y) - ydiff2 * (x1 - x)) /
+ (-xdiff2 * ydiff1 + xdiff1 * ydiff2);
+
+ if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
+ (*intersection)++;
}
+}
- if (!_subMesh)
- _subMesh = new SubMesh();
+//////////////////////////////////////////////////
+bool TriangleIsHole(GtsTriangle *_t, GtsFifo *_edgeList)
+{
+ GtsEdge *e1, *e2, *e3;
+ GtsVertex *v1, *v2, *v3;
- int i, k;
+ gts_triangle_vertices_edges(_t, NULL, &v1, &v2, &v3, &e1, &e2, &e3);
- int numSides = _vertices.size();
+ double xCenter = (v1->p.x + v2->p.x + v3->p.x) / 3.0;
+ double yCenter = (v1->p.y + v2->p.y + v3->p.y) / 3.0;
- // Euler's Formula: numFaces = numEdges - numVertices + 2
- // = numSides + 2
- // # of SideFaces = numFaces - (upper face + lower face)
- // = numFaces - 2
- // = numSides
+ int intersections = 0;
+ gpointer data[3];
+ data[0] = &xCenter;
+ data[1] = &yCenter;
+ data[2] = &intersections;
+ gts_fifo_foreach(_edgeList, (GtsFunc) Intersection, data);
+ if (intersections % 2)
+ return false;
+ else
+ return true;
+}
+
+//////////////////////////////////////////////////
+GtsSurface *GTSMeshUtils::DelaunayTriangulation(
+ const std::vector<ignition::math::Vector2d> &_vertices,
+ const std::vector<ignition::math::Vector2i> &_edges)
+{
GSList *l, *verticesList = NULL;
- double z;
- for (k = 0; k < 2; ++k)
+
+ GtsSurface *surface;
+ verticesList = NULL;
+
+ for (const auto &vertex : _vertices)
{
- GtsSurface *surface;
- GtsVertex *v1, *v2, *v3;
- GtsFifo *edgeList;
- edgeList = gts_fifo_new();
- verticesList = NULL;
-
- if (k == 0)
- z = 0.0;
- else
- z = _height;
-
- // List the vertices and edges
- for (i = 0; i < numSides; ++i)
- {
- verticesList = g_slist_append(verticesList,
+ verticesList = g_slist_append(verticesList,
gts_vertex_new(gts_vertex_class(),
- _vertices[i].x, _vertices[i].y, z));
- if (i != 0)
- {
- gts_fifo_push(edgeList,
- gts_edge_new(GTS_EDGE_CLASS(gts_constraint_class()),
- reinterpret_cast<GtsVertex *>
- (g_slist_nth_data(verticesList, i)),
- reinterpret_cast<GtsVertex *>
- (g_slist_nth_data(verticesList, i-1))));
- }
- }
+ vertex.X(), vertex.Y(), 0));
+ }
+ GtsFifo *edgeList;
+ edgeList = gts_fifo_new();
+ for (const auto &edge : _edges)
+ {
gts_fifo_push(edgeList,
- gts_edge_new(GTS_EDGE_CLASS(gts_constraint_class()),
- reinterpret_cast<GtsVertex *>
- (g_slist_nth_data(verticesList, i-1)),
- reinterpret_cast<GtsVertex *>
- (g_slist_nth_data(verticesList, 0))));
-
- GtsTriangle *tri = gts_triangle_enclosing(
- gts_triangle_class(), verticesList, 100.);
- gts_triangle_vertices(tri, &v1, &v2, &v3);
-
- surface = gts_surface_new(gts_surface_class(),
- gts_face_class(),
- gts_edge_class(),
- gts_vertex_class());
-
- gts_surface_add_face(surface,
- gts_face_new(gts_face_class(),
- tri->e1, tri->e2, tri->e3));
-
- l = verticesList;
- while (l)
+ gts_edge_new(GTS_EDGE_CLASS(gts_constraint_class()),
+ reinterpret_cast<GtsVertex *>
+ (g_slist_nth_data(verticesList, edge.X())),
+ reinterpret_cast<GtsVertex *>
+ (g_slist_nth_data(verticesList, edge.Y()))));
+ }
+
+
+ GtsVertex *v1, *v2, *v3;
+ GtsTriangle *tri = gts_triangle_enclosing(
+ gts_triangle_class(), verticesList, 100.);
+ gts_triangle_vertices(tri, &v1, &v2, &v3);
+
+ surface = gts_surface_new(gts_surface_class(),
+ gts_face_class(),
+ gts_edge_class(),
+ gts_vertex_class());
+
+ gts_surface_add_face(surface,
+ gts_face_new(gts_face_class(),
+ tri->e1, tri->e2, tri->e3));
+
+ l = verticesList;
+ while (l)
+ {
+ GtsVertex *v_in = reinterpret_cast<GtsVertex *>(l->data);
+ GtsVertex *v_out = gts_delaunay_add_vertex(surface, v_in, NULL);
+ if (v_out != NULL)
{
- GtsVertex *v_in = reinterpret_cast<GtsVertex *>(l->data);
- GtsVertex *v_out = gts_delaunay_add_vertex(surface, v_in, NULL);
- if (v_out != NULL)
- {
- gts_vertex_replace(v_in, v_out);
- }
- l = l->next;
+ gts_vertex_replace(v_in, v_out);
}
+ l = l->next;
+ }
+
+ // add constraints
+ gts_fifo_foreach(edgeList, (GtsFunc) AddConstraint, surface);
- // add constraints
- gts_fifo_foreach(edgeList, (GtsFunc) AddConstraint, surface);
-
- // delete the enclosing triangle
- gts_allow_floating_vertices = TRUE;
- gts_object_destroy(GTS_OBJECT(v1));
- gts_object_destroy(GTS_OBJECT(v2));
- gts_object_destroy(GTS_OBJECT(v3));
- gts_allow_floating_vertices = FALSE;
-
- // Remove edges on the boundary which are not constraints
- gts_delaunay_remove_hull(surface);
-
- // fill the submesh with data generated by GTS
- unsigned int n2 = 0, m = numSides*k;
- gpointer data[4];
- GHashTable *vIndex = g_hash_table_new(NULL, NULL);
-
- data[0] = _subMesh;
- data[1] = &n2;
- data[2] = vIndex;
- data[3] = &m;
- gts_surface_foreach_vertex(surface, (GtsFunc) FillVertex, data);
- n2 = 0;
- gts_surface_foreach_face(surface, (GtsFunc) FillFace, data);
-
- g_hash_table_destroy(vIndex);
- gts_object_destroy(GTS_OBJECT(surface));
- gts_fifo_destroy(edgeList);
+ // delete the enclosing triangle
+ gts_allow_floating_vertices = true;
+ gts_object_destroy(GTS_OBJECT(v1));
+ gts_object_destroy(GTS_OBJECT(v2));
+ gts_object_destroy(GTS_OBJECT(v3));
+ gts_allow_floating_vertices = false;
+
+ // Remove edges on the boundary which are not constraints
+ gts_delaunay_remove_hull(surface);
+
+ // remove triangles that are inside holes
+ gts_surface_foreach_face_remove(surface, (GtsFunc) TriangleIsHole,
+ edgeList);
+
+ gts_fifo_destroy(edgeList);
+ return surface;
+}
+
+//////////////////////////////////////////////////
+bool GTSMeshUtils::DelaunayTriangulation(
+ const std::vector<math::Vector2d> &_vertices,
+ const std::vector<math::Vector2i> &_edges,
+ SubMesh *_subMesh)
+{
+ std::vector<ignition::math::Vector2d> vertices;
+ std::vector<ignition::math::Vector2i> edges;
+
+ for (auto const &vert : _vertices)
+ vertices.push_back(vert.Ign());
+
+ for (auto const &edge : _edges)
+ edges.push_back(edge.Ign());
+
+ return DelaunayTriangulation(vertices, edges, _subMesh);
+}
+
+//////////////////////////////////////////////////
+bool GTSMeshUtils::DelaunayTriangulation(
+ const std::vector<ignition::math::Vector2d> &_vertices,
+ const std::vector<ignition::math::Vector2i> &_edges,
+ SubMesh *_subMesh)
+{
+ if (_edges.empty() || _vertices.empty())
+ {
+ gzerr << "Unable to create an extruded outline mesh with "
+ << "no paths\n";
+ return false;
}
+ if (!_subMesh)
+ _subMesh = new SubMesh();
+
+ GtsSurface *surface = GTSMeshUtils::DelaunayTriangulation(_vertices, _edges);
+ // tip: to debug the triangulation, it is possible to save the suface
+ // to view it in Meshlab
+ // FILE *fp = fopen ("surface.gts", "wt");
+ // gts_surface_write (surface, fp);
+ // fclose(fp);
+
+ // fill the submesh with data generated by GTS
+ unsigned int n2 = 0;
+ gpointer data[3];
+ GHashTable *vIndex = g_hash_table_new(NULL, NULL);
+
+ data[0] = _subMesh;
+ data[1] = &n2;
+ data[2] = vIndex;
+ gts_surface_foreach_vertex(surface, (GtsFunc) FillVertex, data);
+ n2 = 0;
+ gts_surface_foreach_face(surface, (GtsFunc) FillFace, data);
+
+ g_hash_table_destroy(vIndex);
+ gts_object_destroy(GTS_OBJECT(surface));
return true;
}
+
diff --git a/gazebo/common/GTSMeshUtils.hh b/gazebo/common/GTSMeshUtils.hh
index fc39e60..8c0a2ae 100644
--- a/gazebo/common/GTSMeshUtils.hh
+++ b/gazebo/common/GTSMeshUtils.hh
@@ -14,13 +14,21 @@
* limitations under the License.
*
*/
+
#ifndef _GAZEBO_GTSMESHUTILS_HH_
#define _GAZEBO_GTSMESHUTILS_HH_
#include <vector>
+#include <ignition/math/Vector2.hh>
+
#include "gazebo/common/Mesh.hh"
#include "gazebo/math/Vector2d.hh"
+#include "gazebo/math/Vector2i.hh"
+
+struct _GtsSurface;
+typedef _GtsSurface GtsSurface;
+
namespace gazebo
{
@@ -33,18 +41,43 @@ namespace gazebo
/// \class GTSMeshUtils GTSMeshUtils.hh common/common.hh
/// \brief Creates GTS utilities for meshes
- class GAZEBO_VISIBLE GTSMeshUtils
+ class GZ_COMMON_VISIBLE GTSMeshUtils
{
- /// \brief Create an extruded Polyline submesh
- /// \param[in] _vertices the x y dimentions of eah vertex in meter
- /// \param[in] _height the height of the polyline
+ /// \brief Perform delaunay triangulation on input vertices.
+ /// \param[in] _vertices A list of all vertices
+ /// \param[in] _edges A list of edges. Each edge is made of 2 vertex
+ /// indices from _vertices
/// \param[out] _submesh A submesh that will be populated with the
- /// extruded polyline.
+ /// resulting triangles.
/// \return True on success.
- public: static bool CreateExtrudedPolyline(
+ /// \deprecated See DelaunayTriangulation function that accepts
+ /// ignition::math objects.
+ public: static bool DelaunayTriangulation(
const std::vector<math::Vector2d> &_vertices,
- const double &_height,
+ const std::vector<math::Vector2i> &_edges,
+ SubMesh *_submesh) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Perform delaunay triangulation on input vertices.
+ /// \param[in] _vertices A list of all vertices
+ /// \param[in] _edges A list of edges. Each edge is made of 2 vertex
+ /// indices from _vertices
+ /// \param[out] _submesh A submesh that will be populated with the
+ /// resulting triangles.
+ /// \return True on success.
+ public: static bool DelaunayTriangulation(
+ const std::vector<ignition::math::Vector2d> &_vertices,
+ const std::vector<ignition::math::Vector2i> &_edges,
SubMesh *_submesh);
+
+
+ /// \brief Perform delaunay triangulation on input vertices.
+ /// \param[in] _vertices A list of all vertices
+ /// \param[in] _edges A list of edges. Each edge is made of 2 vertex
+ /// indices from _vertices
+ /// \return Triangulated GTS surface.
+ private: static GtsSurface *DelaunayTriangulation(
+ const std::vector<ignition::math::Vector2d> &_vertices,
+ const std::vector<ignition::math::Vector2i> &_edges);
};
}
}
diff --git a/gazebo/common/GTSMeshUtils_TEST.cc b/gazebo/common/GTSMeshUtils_TEST.cc
new file mode 100644
index 0000000..096fa46
--- /dev/null
+++ b/gazebo/common/GTSMeshUtils_TEST.cc
@@ -0,0 +1,80 @@
+/*
+ * 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/GTSMeshUtils.hh"
+#include "test/util.hh"
+
+using namespace gazebo;
+
+class GTSMeshUtils : public gazebo::testing::AutoLogFixture { };
+
+/////////////////////////////////////////////////
+TEST_F(GTSMeshUtils, DelaunayTriangulation)
+{
+ // test triangulation of a path with two subpaths:
+ // a smaller square inside a bigger square.
+ // The smaller square should be treated as a hole inside the bigger square.
+
+ std::vector<ignition::math::Vector2d> vertices;
+ // outside square
+ vertices.push_back(ignition::math::Vector2d(0, 0));
+ vertices.push_back(ignition::math::Vector2d(1, 0));
+ vertices.push_back(ignition::math::Vector2d(1, 1));
+ vertices.push_back(ignition::math::Vector2d(0, 1));
+ // inside square
+ vertices.push_back(ignition::math::Vector2d(0.25, 0.25));
+ vertices.push_back(ignition::math::Vector2d(0.25, 0.75));
+ vertices.push_back(ignition::math::Vector2d(0.75, 0.75));
+ vertices.push_back(ignition::math::Vector2d(0.75, 0.25));
+
+ std::vector<ignition::math::Vector2i> edges;
+ edges.push_back(ignition::math::Vector2i(0, 1));
+ edges.push_back(ignition::math::Vector2i(1, 2));
+ edges.push_back(ignition::math::Vector2i(2, 3));
+ edges.push_back(ignition::math::Vector2i(3, 0));
+
+ edges.push_back(ignition::math::Vector2i(4, 5));
+ edges.push_back(ignition::math::Vector2i(5, 6));
+ edges.push_back(ignition::math::Vector2i(6, 7));
+ edges.push_back(ignition::math::Vector2i(7, 4));
+
+ common::Mesh *mesh = new common::Mesh();
+ mesh->SetName("extruded");
+ common::SubMesh *subMesh = new common::SubMesh();
+ mesh->AddSubMesh(subMesh);
+
+ bool result = common::GTSMeshUtils::DelaunayTriangulation(vertices,
+ edges,
+ subMesh);
+ EXPECT_TRUE(result);
+
+ // same as number of vertices in the path
+ EXPECT_EQ(subMesh->GetVertexCount(), 8u);
+
+ // there should be 8 triangles.
+ EXPECT_EQ(subMesh->GetIndexCount() / 3u, 8u);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/common/HeightmapData.hh b/gazebo/common/HeightmapData.hh
index 9af96d3..cbfb138 100644
--- a/gazebo/common/HeightmapData.hh
+++ b/gazebo/common/HeightmapData.hh
@@ -19,6 +19,9 @@
#define _GAZEBO_HEIGHTMAPDATA_HH_
#include <vector>
+#include <ignition/math/Vector3.hh>
+
+#include "gazebo/common/CommonTypes.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/util/system.hh"
@@ -31,7 +34,7 @@ namespace gazebo
/// \class HeightmapData HeightmapData.hh common/common.hh
/// \brief Encapsulates a generic heightmap data file.
- class GAZEBO_VISIBLE HeightmapData
+ class GZ_COMMON_VISIBLE HeightmapData
{
/// \brief Destructor.
public: virtual ~HeightmapData() {}
@@ -46,9 +49,26 @@ namespace gazebo
/// \param[in] _flipY If true, it inverts the order in which the vector
/// is filled.
/// \param[out] _heights Vector containing the terrain heights.
+ /// \deprecated See FillHeightMap() that accepts
+ /// ignition::math::Vector3d objects.
public: virtual void FillHeightMap(int _subSampling,
unsigned int _vertSize, const math::Vector3 &_size,
const math::Vector3 &_scale, bool _flipY,
+ std::vector<float> &_heights) GAZEBO_DEPRECATED(6.0) = 0;
+
+ /// \brief Create a lookup table of the terrain's height.
+ /// \param[in] _subsampling Multiplier used to increase the resolution.
+ /// Ex: A subsampling of 2 in a terrain of 129x129 means that the height
+ /// vector will be 257 * 257.
+ /// \param[in] _vertSize Number of points per row.
+ /// \param[in] _size Real dimmensions of the terrain.
+ /// \param[in] _scale Vector3 used to scale the height.
+ /// \param[in] _flipY If true, it inverts the order in which the vector
+ /// is filled.
+ /// \param[out] _heights Vector containing the terrain heights.
+ public: virtual void FillHeightMap(int _subSampling,
+ unsigned int _vertSize, const ignition::math::Vector3d &_size,
+ const ignition::math::Vector3d &_scale, bool _flipY,
std::vector<float> &_heights) = 0;
/// \brief Get the terrain's height.
diff --git a/gazebo/common/Image.cc b/gazebo/common/Image.cc
index ace7edf..84ba0b7 100644
--- a/gazebo/common/Image.cc
+++ b/gazebo/common/Image.cc
@@ -369,8 +369,12 @@ Color Image::GetMaxColor() const
//////////////////////////////////////////////////
void Image::Rescale(int _width, int _height)
{
+#ifndef _WIN32
this->bitmap = FreeImage_Rescale(this->bitmap, _width, _height,
FILTER_LANCZOS3);
+#else
+ gzerr << "Image::Rescale is not implemented on Windows.\n";
+#endif
}
//////////////////////////////////////////////////
diff --git a/gazebo/common/Image.hh b/gazebo/common/Image.hh
index 7a5699c..f44e189 100644
--- a/gazebo/common/Image.hh
+++ b/gazebo/common/Image.hh
@@ -65,7 +65,7 @@ namespace gazebo
/// \class Image Image.hh common/common.hh
/// \brief Encapsulates an image
- class GAZEBO_VISIBLE Image
+ class GZ_COMMON_VISIBLE Image
{
/// \brief Pixel formats enumeration
public: enum PixelFormat
diff --git a/gazebo/common/ImageHeightmap.cc b/gazebo/common/ImageHeightmap.cc
index 566254c..cc0601d 100644
--- a/gazebo/common/ImageHeightmap.cc
+++ b/gazebo/common/ImageHeightmap.cc
@@ -45,6 +45,16 @@ void ImageHeightmap::FillHeightMap(int _subSampling,
const math::Vector3 &_scale, bool _flipY,
std::vector<float> &_heights)
{
+ this->FillHeightMap(_subSampling, _vertSize, _size.Ign(), _scale.Ign(),
+ _flipY, _heights);
+}
+
+//////////////////////////////////////////////////
+void ImageHeightmap::FillHeightMap(int _subSampling,
+ unsigned int _vertSize, const ignition::math::Vector3d &_size,
+ const ignition::math::Vector3d &_scale, bool _flipY,
+ std::vector<float> &_heights)
+{
// Resize the vector to match the size of the vertices.
_heights.resize(_vertSize * _vertSize);
@@ -91,12 +101,12 @@ void ImageHeightmap::FillHeightMap(int _subSampling,
double px4 = static_cast<int>(data[y2 * pitch + x2 * bpp]) / 255.0;
float h2 = (px3 - ((px3 - px4) * dx));
- float h = (h1 - ((h1 - h2) * dy)) * _scale.z;
+ float h = (h1 - ((h1 - h2) * dy)) * _scale.Z();
// invert pixel definition so 1=ground, 0=full height,
// if the terrain size has a negative z component
// this is mainly for backward compatibility
- if (_size.z < 0)
+ if (_size.Z() < 0)
h = 1.0 - h;
// Store the height for future use
diff --git a/gazebo/common/ImageHeightmap.hh b/gazebo/common/ImageHeightmap.hh
index 505b576..dd369ad 100644
--- a/gazebo/common/ImageHeightmap.hh
+++ b/gazebo/common/ImageHeightmap.hh
@@ -15,11 +15,13 @@
*
*/
-#ifndef _IMAGE_HEIGHTMAP_DATA_HH_
-#define _IMAGE_HEIGHTMAP_DATA_HH_
+#ifndef _GAZEBO_IMAGE_HEIGHTMAP_DATA_HH_
+#define _GAZEBO_IMAGE_HEIGHTMAP_DATA_HH_
#include <string>
#include <vector>
+#include <ignition/math/Vector3.hh>
+
#include "gazebo/common/HeightmapData.hh"
#include "gazebo/common/Image.hh"
#include "gazebo/math/Vector3.hh"
@@ -34,7 +36,7 @@ namespace gazebo
/// \class ImageHeightmap ImageHeightmap.hh common/common.hh
/// \brief Encapsulates an image that will be interpreted as a heightmap.
- class GAZEBO_VISIBLE ImageHeightmap
+ class GZ_COMMON_VISIBLE ImageHeightmap
: public gazebo::common::HeightmapData
{
/// \brief Constructor
@@ -51,6 +53,12 @@ namespace gazebo
const math::Vector3 &_size, const math::Vector3 &_scale, bool _flipY,
std::vector<float> &_heights);
+ // Documentation inherited.
+ public: void FillHeightMap(int _subSampling, unsigned int _vertSize,
+ const ignition::math::Vector3d &_size,
+ const ignition::math::Vector3d &_scale, bool _flipY,
+ std::vector<float> &_heights);
+
/// \brief Get the full filename of the image
/// \return The filename used to load the image
public: std::string GetFilename() const;
diff --git a/gazebo/common/ImageHeightmap_TEST.cc b/gazebo/common/ImageHeightmap_TEST.cc
index 3cba952..535a98d 100644
--- a/gazebo/common/ImageHeightmap_TEST.cc
+++ b/gazebo/common/ImageHeightmap_TEST.cc
@@ -74,22 +74,22 @@ TEST_F(ImageHeightmapTest, FillHeightmap)
// Use FillHeightMap() to retrieve a vector<float> after some transformations
int subsampling;
unsigned vertSize;
- math::Vector3 size;
- math::Vector3 scale;
+ ignition::math::Vector3d size;
+ ignition::math::Vector3d scale;
bool flipY;
std::vector<float> elevations;
subsampling = 2;
vertSize = (img.GetWidth() * subsampling) - 1;
- size.x = 129;
- size.y = 129;
- size.z = 10;
- scale.x = size.x / vertSize;
- scale.y = size.y / vertSize;
- if (math::equal(img.GetMaxElevation(), 0.0f))
- scale.z = fabs(size.z);
+ size.X(129);
+ size.Y(129);
+ size.Z(10);
+ scale.X(size.X() / vertSize);
+ scale.Y(size.Y() / vertSize);
+ if (ignition::math::equal(img.GetMaxElevation(), 0.0f))
+ scale.Z(fabs(size.Z()));
else
- scale.z = fabs(size.z) / img.GetMaxElevation();
+ scale.Z(fabs(size.Z()) / img.GetMaxElevation());
flipY = false;
img.FillHeightMap(subsampling, vertSize, size, scale, flipY, elevations);
diff --git a/gazebo/common/KeyEvent.hh b/gazebo/common/KeyEvent.hh
index 2c596b8..f3e0f29 100644
--- a/gazebo/common/KeyEvent.hh
+++ b/gazebo/common/KeyEvent.hh
@@ -29,7 +29,7 @@ namespace gazebo
/// \class KeyEvent KeyEvent.hh common/common.hh
/// \brief Generic description of a keyboard event.
- class GAZEBO_VISIBLE KeyEvent
+ class GZ_COMMON_VISIBLE KeyEvent
{
/// \brief Constructor.
public: KeyEvent() : type(NO_EVENT), key(0), text(""), control(false),
diff --git a/gazebo/common/KeyFrame.cc b/gazebo/common/KeyFrame.cc
index 9fe0218..2eeb66c 100644
--- a/gazebo/common/KeyFrame.cc
+++ b/gazebo/common/KeyFrame.cc
@@ -19,68 +19,104 @@
using namespace gazebo;
using namespace common;
- KeyFrame::KeyFrame(double _time)
+/////////////////////////////////////////////////
+KeyFrame::KeyFrame(double _time)
: time(_time)
{
}
+/////////////////////////////////////////////////
KeyFrame::~KeyFrame()
{
}
+/////////////////////////////////////////////////
double KeyFrame::GetTime() const
{
return this->time;
}
-
- PoseKeyFrame::PoseKeyFrame(double _time)
+/////////////////////////////////////////////////
+PoseKeyFrame::PoseKeyFrame(double _time)
: KeyFrame(_time)
{
}
+/////////////////////////////////////////////////
PoseKeyFrame::~PoseKeyFrame()
{
}
+/////////////////////////////////////////////////
void PoseKeyFrame::SetTranslation(const math::Vector3 &_trans)
{
+ this->translate.Set(_trans.x, _trans.y, _trans.z);
+}
+
+/////////////////////////////////////////////////
+void PoseKeyFrame::Translation(const ignition::math::Vector3d &_trans)
+{
this->translate = _trans;
}
-const math::Vector3 &PoseKeyFrame::GetTranslation() const
+/////////////////////////////////////////////////
+math::Vector3 PoseKeyFrame::GetTranslation() const
+{
+ return gazebo::math::Vector3(this->translate.X(), this->translate.Y(),
+ this->translate.Z());
+}
+
+/////////////////////////////////////////////////
+ignition::math::Vector3d PoseKeyFrame::Translation() const
{
return this->translate;
}
+/////////////////////////////////////////////////
void PoseKeyFrame::SetRotation(const math::Quaternion &_rot)
{
+ this->rotate.Set(_rot.w, _rot.x, _rot.y, _rot.z);
+}
+
+/////////////////////////////////////////////////
+void PoseKeyFrame::Rotation(const ignition::math::Quaterniond &_rot)
+{
this->rotate = _rot;
}
-const math::Quaternion &PoseKeyFrame::GetRotation() const
+/////////////////////////////////////////////////
+math::Quaternion PoseKeyFrame::GetRotation() const
+{
+ return gazebo::math::Quaternion(this->rotate.W(),
+ this->rotate.X(), this->rotate.Y(), this->rotate.Z());
+}
+
+/////////////////////////////////////////////////
+ignition::math::Quaterniond PoseKeyFrame::Rotation() const
{
return this->rotate;
}
- NumericKeyFrame::NumericKeyFrame(double _time)
+/////////////////////////////////////////////////
+NumericKeyFrame::NumericKeyFrame(double _time)
: KeyFrame(_time)
{
}
+/////////////////////////////////////////////////
NumericKeyFrame::~NumericKeyFrame()
{
}
+/////////////////////////////////////////////////
void NumericKeyFrame::SetValue(const double &_value)
{
this->value = _value;
}
+/////////////////////////////////////////////////
const double &NumericKeyFrame::GetValue() const
{
return this->value;
}
-
-
diff --git a/gazebo/common/KeyFrame.hh b/gazebo/common/KeyFrame.hh
index c8e203a..ba17247 100644
--- a/gazebo/common/KeyFrame.hh
+++ b/gazebo/common/KeyFrame.hh
@@ -17,6 +17,10 @@
#ifndef _KEYFRAME_HH_
#define _KEYFRAME_HH_
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Quaternion.hh>
+
+#include "gazebo/common/CommonTypes.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Quaternion.hh"
#include "gazebo/util/system.hh"
@@ -30,7 +34,7 @@ namespace gazebo
/// \class KeyFrame KeyFrame.hh common/common.hh
/// \brief A key frame in an animation
- class GAZEBO_VISIBLE KeyFrame
+ class GZ_COMMON_VISIBLE KeyFrame
{
/// \brief Constructor
/// \param[in] _time Time of the keyframe in seconds
@@ -48,7 +52,7 @@ namespace gazebo
};
/// \brief A keyframe for a PoseAnimation
- class GAZEBO_VISIBLE PoseKeyFrame : public KeyFrame
+ class GZ_COMMON_VISIBLE PoseKeyFrame : public KeyFrame
{
/// \brief Constructor
/// \param[in] _time of the keyframe
@@ -59,29 +63,53 @@ namespace gazebo
/// \brief Set the translation for the keyframe
/// \param[in] _trans Translation amount
- public: void SetTranslation(const math::Vector3 &_trans);
+ /// \deprecated See Translation function that accepts ignition::math
+ public: void SetTranslation(
+ const math::Vector3 &_trans) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set the translation for the keyframe
+ /// \param[in] _trans Translation amount
+ public: void Translation(const ignition::math::Vector3d &_trans);
+
+ /// \brief Get the translation of the keyframe
+ /// \return The translation amount
+ /// \deprecated See Translation function that returns igntion::math
+ public: math::Vector3 GetTranslation() const
+ GAZEBO_DEPRECATED(6.0);
/// \brief Get the translation of the keyframe
/// \return The translation amount
- public: const math::Vector3 &GetTranslation() const;
+ public: ignition::math::Vector3d Translation() const;
/// \brief Set the rotation for the keyframe
/// \param[in] _rot Rotation amount
- public: void SetRotation(const math::Quaternion &_rot);
+ /// \deprecated See Rotation function that accepts ignition::math
+ public: void SetRotation(const math::Quaternion &_rot)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set the rotation for the keyframe
+ /// \param[in] _rot Rotation amount
+ public: void Rotation(const ignition::math::Quaterniond &_rot);
+
+ /// \brief Get the rotation of the keyframe
+ /// \return The rotation amount
+ /// \deprecated See Rotation function that returns ignition::math
+ public: math::Quaternion GetRotation() const
+ GAZEBO_DEPRECATED(6.0);
/// \brief Get the rotation of the keyframe
/// \return The rotation amount
- public: const math::Quaternion &GetRotation() const;
+ public: ignition::math::Quaterniond Rotation() const;
/// \brief the translation vector
- protected: math::Vector3 translate;
+ protected: ignition::math::Vector3d translate;
/// \brief the rotation quaternion
- protected: math::Quaternion rotate;
+ protected: ignition::math::Quaterniond rotate;
};
/// \brief A keyframe for a NumericAnimation
- class GAZEBO_VISIBLE NumericKeyFrame : public KeyFrame
+ class GZ_COMMON_VISIBLE NumericKeyFrame : public KeyFrame
{
/// \brief Constructor
/// \param[in] _time Time of the keyframe
diff --git a/gazebo/common/Material.hh b/gazebo/common/Material.hh
index d8be05e..84747d1 100644
--- a/gazebo/common/Material.hh
+++ b/gazebo/common/Material.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class Material Material.hh common/common.hh
/// \brief Encapsulates description of a material
- class GAZEBO_VISIBLE Material
+ class GZ_COMMON_VISIBLE Material
{
public: enum ShadeMode {FLAT, GOURAUD, PHONG, BLINN, SHADE_COUNT};
public: static std::string ShadeModeStr[SHADE_COUNT];
diff --git a/gazebo/common/Mesh.cc b/gazebo/common/Mesh.cc
index 9d69364..2e71eea 100644
--- a/gazebo/common/Mesh.cc
+++ b/gazebo/common/Mesh.cc
@@ -85,22 +85,28 @@ std::string Mesh::GetName() const
//////////////////////////////////////////////////
math::Vector3 Mesh::GetMax() const
{
- math::Vector3 max;
+ return this->Max();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d Mesh::Max() const
+{
+ ignition::math::Vector3d max;
std::vector<SubMesh*>::const_iterator iter;
- max.x = -FLT_MAX;
- max.y = -FLT_MAX;
- max.z = -FLT_MAX;
+ max.X(-FLT_MAX);
+ max.Y(-FLT_MAX);
+ max.Z(-FLT_MAX);
for (iter = this->submeshes.begin(); iter != this->submeshes.end(); ++iter)
{
if ((*iter)->GetVertexCount() <= 2)
continue;
- math::Vector3 smax = (*iter)->GetMax();
- max.x = std::max(max.x, smax.x);
- max.y = std::max(max.y, smax.y);
- max.z = std::max(max.z, smax.z);
+ ignition::math::Vector3d smax = (*iter)->Max();
+ max.X(std::max(max.X(), smax.X()));
+ max.Y(std::max(max.Y(), smax.Y()));
+ max.Z(std::max(max.Z(), smax.Z()));
}
return max;
@@ -109,22 +115,28 @@ math::Vector3 Mesh::GetMax() const
//////////////////////////////////////////////////
math::Vector3 Mesh::GetMin() const
{
- math::Vector3 min;
+ return this->Min();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d Mesh::Min() const
+{
+ ignition::math::Vector3d min;
std::vector<SubMesh *>::const_iterator iter;
- min.x = FLT_MAX;
- min.y = FLT_MAX;
- min.z = FLT_MAX;
+ min.X(FLT_MAX);
+ min.Y(FLT_MAX);
+ min.Z(FLT_MAX);
for (iter = this->submeshes.begin(); iter != this->submeshes.end(); ++iter)
{
if ((*iter)->GetVertexCount() <= 2)
continue;
- math::Vector3 smin = (*iter)->GetMin();
- min.x = std::min(min.x, smin.x);
- min.y = std::min(min.y, smin.y);
- min.z = std::min(min.z, smin.z);
+ ignition::math::Vector3d smin = (*iter)->Min();
+ min.X(std::min(min.X(), smin.X()));
+ min.Y(std::min(min.Y(), smin.Y()));
+ min.Z(std::min(min.Z(), smin.Z()));
}
return min;
@@ -311,7 +323,7 @@ void Mesh::FillArrays(float **_vertArr, int **_indArr) const
memcpy(vPtr, vertTmp, sizeof(vertTmp[0])*(*iter)->GetVertexCount()*3);
- for (unsigned int i = 0; i < (*iter)->GetIndexCount(); i++)
+ for (unsigned int i = 0; i < (*iter)->GetIndexCount(); ++i)
{
(*_indArr)[index++] = (*iter)->GetIndex(i) + offset;
}
@@ -365,6 +377,12 @@ void Mesh::Scale(double _factor)
//////////////////////////////////////////////////
void Mesh::SetScale(const math::Vector3 &_factor)
{
+ this->SetScale(_factor.Ign());
+}
+
+//////////////////////////////////////////////////
+void Mesh::SetScale(const ignition::math::Vector3d &_factor)
+{
std::vector<SubMesh*>::iterator iter;
for (iter = this->submeshes.begin(); iter != this->submeshes.end(); ++iter)
(*iter)->SetScale(_factor);
@@ -373,6 +391,12 @@ void Mesh::SetScale(const math::Vector3 &_factor)
//////////////////////////////////////////////////
void Mesh::GenSphericalTexCoord(const math::Vector3 &_center)
{
+ this->GenSphericalTexCoord(_center.Ign());
+}
+
+//////////////////////////////////////////////////
+void Mesh::GenSphericalTexCoord(const ignition::math::Vector3d &_center)
+{
std::vector<SubMesh*>::iterator siter;
for (siter = this->submeshes.begin(); siter != this->submeshes.end(); ++siter)
(*siter)->GenSphericalTexCoord(_center);
@@ -381,9 +405,15 @@ void Mesh::GenSphericalTexCoord(const math::Vector3 &_center)
//////////////////////////////////////////////////
void Mesh::Center(const math::Vector3 &_center)
{
- math::Vector3 min, max, half;
- min = this->GetMin();
- max = this->GetMax();
+ this->Center(_center.Ign());
+}
+
+//////////////////////////////////////////////////
+void Mesh::Center(const ignition::math::Vector3d &_center)
+{
+ ignition::math::Vector3d min, max, half;
+ min = this->Min();
+ max = this->Max();
half = (max - min) * 0.5;
this->Translate(_center - (min + half));
@@ -392,6 +422,12 @@ void Mesh::Center(const math::Vector3 &_center)
//////////////////////////////////////////////////
void Mesh::Translate(const math::Vector3 &_vec)
{
+ this->Translate(_vec.Ign());
+}
+
+//////////////////////////////////////////////////
+void Mesh::Translate(const ignition::math::Vector3d &_vec)
+{
std::vector<SubMesh*>::iterator iter;
for (iter = this->submeshes.begin(); iter != this->submeshes.end(); ++iter)
@@ -416,6 +452,12 @@ SubMesh::SubMesh()
//////////////////////////////////////////////////
SubMesh::SubMesh(const SubMesh *_mesh)
{
+ if (!_mesh)
+ {
+ gzerr << "Submesh is NULL." << std::endl;
+ return;
+ }
+
this->name = _mesh->name;
this->materialIndex = _mesh->materialIndex;
this->primitiveType = _mesh->primitiveType;
@@ -456,6 +498,15 @@ SubMesh::PrimitiveType SubMesh::GetPrimitiveType() const
//////////////////////////////////////////////////
void SubMesh::CopyVertices(const std::vector<math::Vector3> &_verts)
{
+ std::vector<ignition::math::Vector3d> verts;
+ for (auto const &vert : _verts)
+ verts.push_back(vert.Ign());
+ this->CopyVertices(verts);
+}
+
+//////////////////////////////////////////////////
+void SubMesh::CopyVertices(const std::vector<ignition::math::Vector3d> &_verts)
+{
this->vertices.clear();
this->vertices.resize(_verts.size());
std::copy(_verts.begin(), _verts.end(), this->vertices.begin());
@@ -464,13 +515,22 @@ void SubMesh::CopyVertices(const std::vector<math::Vector3> &_verts)
//////////////////////////////////////////////////
void SubMesh::CopyNormals(const std::vector<math::Vector3> &_norms)
{
+ std::vector<ignition::math::Vector3d> norms;
+ for (auto const &norm : _norms)
+ norms.push_back(norm.Ign());
+ this->CopyNormals(norms);
+}
+
+//////////////////////////////////////////////////
+void SubMesh::CopyNormals(const std::vector<ignition::math::Vector3d> &_norms)
+{
this->normals.clear();
this->normals.resize(_norms.size());
- for (unsigned int i = 0; i < _norms.size(); i++)
+ for (unsigned int i = 0; i < _norms.size(); ++i)
{
this->normals[i] = _norms[i];
this->normals[i].Normalize();
- if (math::equal(this->normals[i].GetLength(), 0.0))
+ if (ignition::math::equal(this->normals[i].Length(), 0.0))
{
this->normals[i].Set(0, 0, 1);
}
@@ -510,31 +570,43 @@ void SubMesh::AddIndex(unsigned int _i)
//////////////////////////////////////////////////
void SubMesh::AddVertex(const math::Vector3 &_v)
{
+ this->AddVertex(_v.Ign());
+}
+
+//////////////////////////////////////////////////
+void SubMesh::AddVertex(const ignition::math::Vector3d &_v)
+{
this->vertices.push_back(_v);
}
//////////////////////////////////////////////////
void SubMesh::AddVertex(double _x, double _y, double _z)
{
- this->AddVertex(math::Vector3(_x, _y, _z));
+ this->AddVertex(ignition::math::Vector3d(_x, _y, _z));
}
//////////////////////////////////////////////////
void SubMesh::AddNormal(const math::Vector3 &_n)
{
+ this->AddNormal(_n.Ign());
+}
+
+//////////////////////////////////////////////////
+void SubMesh::AddNormal(const ignition::math::Vector3d &_n)
+{
this->normals.push_back(_n);
}
//////////////////////////////////////////////////
void SubMesh::AddNormal(double _x, double _y, double _z)
{
- this->AddNormal(math::Vector3(_x, _y, _z));
+ this->AddNormal(ignition::math::Vector3d(_x, _y, _z));
}
//////////////////////////////////////////////////
void SubMesh::AddTexCoord(double _u, double _v)
{
- this->texCoords.push_back(math::Vector2d(_u, _v));
+ this->texCoords.push_back(ignition::math::Vector2d(_u, _v));
}
//////////////////////////////////////////////////
@@ -552,6 +624,12 @@ void SubMesh::AddNodeAssignment(unsigned int _vertex, unsigned int _node,
//////////////////////////////////////////////////
math::Vector3 SubMesh::GetVertex(unsigned int _i) const
{
+ return this->Vertex(_i);
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d SubMesh::Vertex(unsigned int _i) const
+{
if (_i >= this->vertices.size())
gzthrow("Index too large");
@@ -561,6 +639,12 @@ math::Vector3 SubMesh::GetVertex(unsigned int _i) const
//////////////////////////////////////////////////
void SubMesh::SetVertex(unsigned int _i, const math::Vector3 &_v)
{
+ this->SetVertex(_i, _v.Ign());
+}
+
+//////////////////////////////////////////////////
+void SubMesh::SetVertex(unsigned int _i, const ignition::math::Vector3d &_v)
+{
if (_i >= this->vertices.size())
gzthrow("Index too large");
@@ -570,6 +654,12 @@ void SubMesh::SetVertex(unsigned int _i, const math::Vector3 &_v)
//////////////////////////////////////////////////
math::Vector3 SubMesh::GetNormal(unsigned int _i) const
{
+ return this->Normal(_i);
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d SubMesh::Normal(unsigned int _i) const
+{
if (_i >= this->normals.size())
gzthrow("Index too large");
@@ -579,6 +669,12 @@ math::Vector3 SubMesh::GetNormal(unsigned int _i) const
//////////////////////////////////////////////////
void SubMesh::SetNormal(unsigned int _i, const math::Vector3 &_n)
{
+ this->SetNormal(_i, _n.Ign());
+}
+
+//////////////////////////////////////////////////
+void SubMesh::SetNormal(unsigned int _i, const ignition::math::Vector3d &_n)
+{
if (_i >= this->normals.size())
gzthrow("Index too large");
@@ -588,6 +684,12 @@ void SubMesh::SetNormal(unsigned int _i, const math::Vector3 &_n)
//////////////////////////////////////////////////
math::Vector2d SubMesh::GetTexCoord(unsigned int _i) const
{
+ return this->TexCoord(_i);
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector2d SubMesh::TexCoord(unsigned int _i) const
+{
if (_i >= this->texCoords.size())
gzthrow("Index too large");
@@ -606,6 +708,12 @@ NodeAssignment SubMesh::GetNodeAssignment(unsigned int _i) const
//////////////////////////////////////////////////
void SubMesh::SetTexCoord(unsigned int _i, const math::Vector2d &_t)
{
+ return this->SetTexCoord(_i, _t.Ign());
+}
+
+//////////////////////////////////////////////////
+void SubMesh::SetTexCoord(unsigned int _i, const ignition::math::Vector2d &_t)
+{
if (_i >= this->texCoords.size())
gzthrow("Index too large");
@@ -624,18 +732,24 @@ unsigned int SubMesh::GetIndex(unsigned int _i) const
//////////////////////////////////////////////////
math::Vector3 SubMesh::GetMax() const
{
- math::Vector3 max;
- std::vector<math::Vector3>::const_iterator iter;
+ return this->Max();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d SubMesh::Max() const
+{
+ ignition::math::Vector3d max;
+ std::vector<ignition::math::Vector3d>::const_iterator iter;
- max.x = -FLT_MAX;
- max.y = -FLT_MAX;
- max.z = -FLT_MAX;
+ max.X(-FLT_MAX);
+ max.Y(-FLT_MAX);
+ max.Z(-FLT_MAX);
for (iter = this->vertices.begin(); iter != this->vertices.end(); ++iter)
{
- max.x = std::max(max.x, (*iter).x);
- max.y = std::max(max.y, (*iter).y);
- max.z = std::max(max.z, (*iter).z);
+ max.X(std::max(max.X(), (*iter).X()));
+ max.Y(std::max(max.Y(), (*iter).Y()));
+ max.Z(std::max(max.Z(), (*iter).Z()));
}
return max;
@@ -644,18 +758,24 @@ math::Vector3 SubMesh::GetMax() const
//////////////////////////////////////////////////
math::Vector3 SubMesh::GetMin() const
{
- math::Vector3 min;
- std::vector<math::Vector3>::const_iterator iter;
+ return this->Min();
+}
- min.x = FLT_MAX;
- min.y = FLT_MAX;
- min.z = FLT_MAX;
+//////////////////////////////////////////////////
+ignition::math::Vector3d SubMesh::Min() const
+{
+ ignition::math::Vector3d min;
+ std::vector<ignition::math::Vector3d>::const_iterator iter;
+
+ min.X(FLT_MAX);
+ min.Y(FLT_MAX);
+ min.Z(FLT_MAX);
for (iter = this->vertices.begin(); iter != this->vertices.end(); ++iter)
{
- min.x = std::min(min.x, (*iter).x);
- min.y = std::min(min.y, (*iter).y);
- min.z = std::min(min.z, (*iter).z);
+ min.X(std::min(min.X(), (*iter).X()));
+ min.Y(std::min(min.Y(), (*iter).Y()));
+ min.Z(std::min(min.Z(), (*iter).Z()));
}
return min;
@@ -718,7 +838,13 @@ unsigned int SubMesh::GetMaterialIndex() const
//////////////////////////////////////////////////
bool SubMesh::HasVertex(const math::Vector3 &_v) const
{
- std::vector< math::Vector3 >::const_iterator iter;
+ return this->HasVertex(_v.Ign());
+}
+
+//////////////////////////////////////////////////
+bool SubMesh::HasVertex(const ignition::math::Vector3d &_v) const
+{
+ std::vector< ignition::math::Vector3d >::const_iterator iter;
for (iter = this->vertices.begin(); iter != this->vertices.end(); ++iter)
if (_v.Equal(*iter))
@@ -730,7 +856,13 @@ bool SubMesh::HasVertex(const math::Vector3 &_v) const
//////////////////////////////////////////////////
unsigned int SubMesh::GetVertexIndex(const math::Vector3 &_v) const
{
- std::vector< math::Vector3 >::const_iterator iter;
+ return this->GetVertexIndex(_v.Ign());
+}
+
+//////////////////////////////////////////////////
+unsigned int SubMesh::GetVertexIndex(const ignition::math::Vector3d &_v) const
+{
+ std::vector< ignition::math::Vector3d >::const_iterator iter;
for (iter = this->vertices.begin(); iter != this->vertices.end(); ++iter)
if (_v.Equal(*iter))
@@ -745,8 +877,8 @@ void SubMesh::FillArrays(float **_vertArr, int **_indArr) const
if (this->vertices.empty() || this->indices.empty())
gzerr << "No vertices or indices\n";
- std::vector< math::Vector3 >::const_iterator viter;
- std::vector< unsigned int >::const_iterator iiter;
+ std::vector<ignition::math::Vector3d>::const_iterator viter;
+ std::vector<unsigned int>::const_iterator iiter;
unsigned int i;
if (*_vertArr)
@@ -761,14 +893,16 @@ void SubMesh::FillArrays(float **_vertArr, int **_indArr) const
for (viter = this->vertices.begin(), i = 0; viter != this->vertices.end();
++viter)
{
- (*_vertArr)[i++] = static_cast<float>((*viter).x);
- (*_vertArr)[i++] = static_cast<float>((*viter).y);
- (*_vertArr)[i++] = static_cast<float>((*viter).z);
+ (*_vertArr)[i++] = static_cast<float>((*viter).X());
+ (*_vertArr)[i++] = static_cast<float>((*viter).Y());
+ (*_vertArr)[i++] = static_cast<float>((*viter).Z());
}
for (iiter = this->indices.begin(), i = 0;
iiter != this->indices.end(); ++iiter)
+ {
(*_indArr)[i++] = (*iiter);
+ }
}
//////////////////////////////////////////////////
@@ -779,7 +913,7 @@ void SubMesh::RecalculateNormals()
return;
// Reset all the normals
- for (i = 0; i < this->normals.size(); i++)
+ for (i = 0; i < this->normals.size(); ++i)
this->normals[i].Set(0, 0, 0);
if (this->normals.size() != this->vertices.size())
@@ -788,12 +922,12 @@ void SubMesh::RecalculateNormals()
// For each face, which is defined by three indices, calculate the normals
for (i = 0; i < this->indices.size(); i+= 3)
{
- math::Vector3 v1 = this->vertices[this->indices[i]];
- math::Vector3 v2 = this->vertices[this->indices[i+1]];
- math::Vector3 v3 = this->vertices[this->indices[i+2]];
- math::Vector3 n = math::Vector3::GetNormal(v1, v2, v3);
+ ignition::math::Vector3d v1 = this->vertices[this->indices[i]];
+ ignition::math::Vector3d v2 = this->vertices[this->indices[i+1]];
+ ignition::math::Vector3d v3 = this->vertices[this->indices[i+2]];
+ ignition::math::Vector3d n = ignition::math::Vector3d::Normal(v1, v2, v3);
- for (unsigned int j = 0; j< this->vertices.size(); j++)
+ for (unsigned int j = 0; j< this->vertices.size(); ++j)
{
if (this->vertices[j] == v1 ||
this->vertices[j] == v2 ||
@@ -805,56 +939,74 @@ void SubMesh::RecalculateNormals()
}
// Normalize the results
- for (i = 0; i < this->normals.size(); i++)
+ for (i = 0; i < this->normals.size(); ++i)
{
this->normals[i].Normalize();
}
}
//////////////////////////////////////////////////
-void Mesh::GetAABB(math::Vector3 &_center, math::Vector3 &_min_xyz,
- math::Vector3 &_max_xyz) const
+void Mesh::GetAABB(math::Vector3 &_center, math::Vector3 &_minXYZ,
+ math::Vector3 &_maxXYZ) const
+{
+ ignition::math::Vector3d center, min, max;
+ this->GetAABB(center, min, max);
+ _center = center;
+ _minXYZ = min;
+ _maxXYZ = max;
+}
+
+//////////////////////////////////////////////////
+void Mesh::GetAABB(ignition::math::Vector3d &_center,
+ ignition::math::Vector3d &_minXYZ,
+ ignition::math::Vector3d &_maxXYZ) const
{
// find aabb center
- _min_xyz.x = 1e15;
- _max_xyz.x = -1e15;
- _min_xyz.y = 1e15;
- _max_xyz.y = -1e15;
- _min_xyz.z = 1e15;
- _max_xyz.z = -1e15;
- _center.x = 0;
- _center.y = 0;
- _center.z = 0;
+ _minXYZ.X(1e15);
+ _maxXYZ.X(-1e15);
+ _minXYZ.Y(1e15);
+ _maxXYZ.Y(-1e15);
+ _minXYZ.Z(1e15);
+ _maxXYZ.Z(-1e15);
+ _center.X(0);
+ _center.Y(0);
+ _center.Z(0);
std::vector<SubMesh*>::const_iterator siter;
for (siter = this->submeshes.begin(); siter != this->submeshes.end(); ++siter)
{
- math::Vector3 max = (*siter)->GetMax();
- math::Vector3 min = (*siter)->GetMin();
- _min_xyz.x = std::min(_min_xyz.x, min.x);
- _max_xyz.x = std::max(_max_xyz.x, max.x);
- _min_xyz.y = std::min(_min_xyz.y, min.y);
- _max_xyz.y = std::max(_max_xyz.y, max.y);
- _min_xyz.z = std::min(_min_xyz.z, min.z);
- _max_xyz.z = std::max(_max_xyz.z, max.z);
+ ignition::math::Vector3d max = (*siter)->Max();
+ ignition::math::Vector3d min = (*siter)->Min();
+
+ _minXYZ.X(std::min(_minXYZ.X(), min.X()));
+ _maxXYZ.X(std::max(_maxXYZ.X(), max.X()));
+ _minXYZ.Y(std::min(_minXYZ.Y(), min.Y()));
+ _maxXYZ.Y(std::max(_maxXYZ.Y(), max.Y()));
+ _minXYZ.Z(std::min(_minXYZ.Z(), min.Z()));
+ _maxXYZ.Z(std::max(_maxXYZ.Z(), max.Z()));
}
- _center.x = 0.5*(_min_xyz.x+_max_xyz.x);
- _center.y = 0.5*(_min_xyz.y+_max_xyz.y);
- _center.z = 0.5*(_min_xyz.z+_max_xyz.z);
+ _center.X(0.5 * (_minXYZ.X() + _maxXYZ.X()));
+ _center.Y(0.5 * (_minXYZ.Y() + _maxXYZ.Y()));
+ _center.Z(0.5 * (_minXYZ.Z() + _maxXYZ.Z()));
}
-
//////////////////////////////////////////////////
void SubMesh::GenSphericalTexCoord(const math::Vector3 &_center)
{
- std::vector<math::Vector3>::const_iterator viter;
+ this->GenSphericalTexCoord(_center.Ign());
+}
+
+//////////////////////////////////////////////////
+void SubMesh::GenSphericalTexCoord(const ignition::math::Vector3d &_center)
+{
+ std::vector<ignition::math::Vector3d>::const_iterator viter;
for (viter = this->vertices.begin(); viter != this->vertices.end(); ++viter)
{
// generate projected texture coordinates, projected from center
// get x, y, z for computing texture coordinate projections
- double x = (*viter).x - _center.x;
- double y = (*viter).y - _center.y;
- double z = (*viter).z - _center.z;
+ double x = (*viter).X() - _center.X();
+ double y = (*viter).Y() - _center.Y();
+ double z = (*viter).Z() - _center.Z();
double r = std::max(0.000001, sqrt(x*x+y*y+z*z));
double s = std::min(1.0, std::max(-1.0, z/r));
@@ -868,31 +1020,35 @@ void SubMesh::GenSphericalTexCoord(const math::Vector3 &_center)
//////////////////////////////////////////////////
void SubMesh::Scale(double _factor)
{
- for (std::vector<math::Vector3>::iterator iter = this->vertices.begin();
- iter != this->vertices.end(); ++iter)
- {
- (*iter) *= _factor;
- }
+ for (auto &vert : this->vertices)
+ vert *= _factor;
}
//////////////////////////////////////////////////
void SubMesh::SetScale(const math::Vector3 &_factor)
{
- for (std::vector<math::Vector3>::iterator iter = this->vertices.begin();
- iter != this->vertices.end(); ++iter)
- {
- (*iter).x *= _factor.x;
- (*iter).y *= _factor.y;
- (*iter).z *= _factor.z;
- }
+ this->SetScale(_factor.Ign());
+}
+
+//////////////////////////////////////////////////
+void SubMesh::SetScale(const ignition::math::Vector3d &_factor)
+{
+ for (auto &vert : this->vertices)
+ vert *= _factor;
}
//////////////////////////////////////////////////
void SubMesh::Center(const math::Vector3 &_center)
{
- math::Vector3 min, max, half;
- min = this->GetMin();
- max = this->GetMax();
+ this->Center(_center.Ign());
+}
+
+//////////////////////////////////////////////////
+void SubMesh::Center(const ignition::math::Vector3d &_center)
+{
+ ignition::math::Vector3d min, max, half;
+ min = this->Min();
+ max = this->Max();
half = (max - min) * 0.5;
this->Translate(_center - (min + half));
@@ -901,11 +1057,14 @@ void SubMesh::Center(const math::Vector3 &_center)
//////////////////////////////////////////////////
void SubMesh::Translate(const math::Vector3 &_vec)
{
- for (std::vector<math::Vector3>::iterator iter = this->vertices.begin();
- iter != this->vertices.end(); ++iter)
- {
- (*iter) += _vec;
- }
+ this->Translate(_vec.Ign());
+}
+
+//////////////////////////////////////////////////
+void SubMesh::Translate(const ignition::math::Vector3d &_vec)
+{
+ for (auto &vert : this->vertices)
+ vert += _vec;
}
//////////////////////////////////////////////////
diff --git a/gazebo/common/Mesh.hh b/gazebo/common/Mesh.hh
index 1caa066..2af60e4 100644
--- a/gazebo/common/Mesh.hh
+++ b/gazebo/common/Mesh.hh
@@ -20,8 +20,12 @@
#include <vector>
#include <string>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Vector2.hh>
+
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Vector2d.hh"
+#include "gazebo/common/CommonTypes.hh"
#include "gazebo/util/system.hh"
namespace gazebo
@@ -37,7 +41,7 @@ namespace gazebo
/// \class Mesh Mesh.hh common/common.hh
/// \brief A 3D mesh
- class GAZEBO_VISIBLE Mesh
+ class GZ_COMMON_VISIBLE Mesh
{
/// \brief Constructor
public: Mesh();
@@ -63,11 +67,21 @@ namespace gazebo
/// \brief Get the maximun X, Y, Z values
/// \return the upper bounds of the bounding box
- public: math::Vector3 GetMax() const;
+ /// \deprecated See Max function that returns ignition::math::Vector3d
+ public: math::Vector3 GetMax() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the maximun X, Y, Z values
+ /// \return the upper bounds of the bounding box
+ public: ignition::math::Vector3d Max() const;
/// \brief Get the minimum X, Y, Z values
/// \return the lower bounds of the bounding box
- public: math::Vector3 GetMin() const;
+ /// \deprecated See Min function that returns ignition::math::Vector3d
+ public: math::Vector3 GetMin() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the minimum X, Y, Z values
+ /// \return the lower bounds of the bounding box
+ public: ignition::math::Vector3d Min() const;
/// \brief Return the number of vertices
/// \return the count
@@ -137,13 +151,33 @@ namespace gazebo
/// \param[out] _center of the bounding box
/// \param[out] _min_xyz bounding box minimum values
/// \param[out] _max_xyz bounding box maximum values
+ /// \deprecated See GetAABB function that accepts
+ /// ignition::math::Vector3d
public: void GetAABB(math::Vector3 &_center, math::Vector3 &_min_xyz,
- math::Vector3 &_max_xyz) const;
+ math::Vector3 &_max_xyz) const
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get AABB coordinate
+ /// \param[out] _center of the bounding box
+ /// \param[out] _minXYZ bounding box minimum values
+ /// \param[out] _maxXYZ bounding box maximum values
+ public: void GetAABB(ignition::math::Vector3d &_center,
+ ignition::math::Vector3d &_minXYZ,
+ ignition::math::Vector3d &_maxXYZ) const;
/// \brief Generate texture coordinates using spherical projection
/// from center
/// \param[in] _center the center of the projection
- public: void GenSphericalTexCoord(const math::Vector3 &_center);
+ /// \deprecated See GenSphericalTexCoord that accepts
+ /// ignition::math::Vector3d.
+ public: void GenSphericalTexCoord(const math::Vector3 &_center)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Generate texture coordinates using spherical projection
+ /// from center
+ /// \param[in] _center the center of the projection
+ public: void GenSphericalTexCoord(
+ const ignition::math::Vector3d &_center);
/// \brief Get the skeleton to which this mesh is attached.
/// \return pointer to skeleton, or NULL if none is present.
@@ -161,16 +195,38 @@ namespace gazebo
/// \brief Scale all vertices by the _factor vector
/// \param[in] _factor Scaling vector
- public: void SetScale(const math::Vector3 &_factor);
+ /// \deprecated See SetScale function that accepts
+ /// ignition::math::Vector3d
+ public: void SetScale(const math::Vector3 &_factor)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Scale all vertices by the _factor vector
+ /// \param[in] _factor Scaling vector
+ public: void SetScale(const ignition::math::Vector3d &_factor);
+
+ /// \brief Move the center of the mesh to the given coordinate. This
+ /// will move all the vertices in all submeshes.
+ /// \param[in] _center Location of the mesh center.
+ /// \deprecated See Center function that accepts
+ /// ignition::math::Vector3
+ public: void Center(const math::Vector3 &_center = math::Vector3::Zero)
+ GAZEBO_DEPRECATED(6.0);
/// \brief Move the center of the mesh to the given coordinate. This
/// will move all the vertices in all submeshes.
/// \param[in] _center Location of the mesh center.
- public: void Center(const math::Vector3 &_center = math::Vector3::Zero);
+ public: void Center(const ignition::math::Vector3d &_center =
+ ignition::math::Vector3d::Zero);
/// \brief Move all vertices in all submeshes by _vec.
/// \param[in] _vec Amount to translate vertices.
- public: void Translate(const math::Vector3 &_vec);
+ /// \deprecated See Translate function that accepts
+ /// ignition::math::Vector3d.
+ public: void Translate(const math::Vector3 &_vec) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Move all vertices in all submeshes by _vec.
+ /// \param[in] _vec Amount to translate vertices.
+ public: void Translate(const ignition::math::Vector3d &_vec);
/// \brief The name of the mesh
private: std::string name;
@@ -190,7 +246,7 @@ namespace gazebo
/// \brief Vertex to node weighted assignement for skeleton animation
/// visualization
- class GAZEBO_VISIBLE NodeAssignment
+ class GZ_COMMON_VISIBLE NodeAssignment
{
/// \brief Constructor.
public: NodeAssignment();
@@ -206,7 +262,7 @@ namespace gazebo
};
/// \brief A child mesh
- class GAZEBO_VISIBLE SubMesh
+ class GZ_COMMON_VISIBLE SubMesh
{
/// \brief An enumeration of the geometric mesh primitives
public: enum PrimitiveType {POINTS, LINES, LINESTRIPS, TRIANGLES,
@@ -239,11 +295,27 @@ namespace gazebo
/// \brief Copy vertices from a vector
/// \param[in] _verts the vertices to copy from
- public: void CopyVertices(const std::vector<math::Vector3> &_verts);
+ /// \deprecated See CopyVertices that accepts
+ /// ignition::math::Vector3d.
+ public: void CopyVertices(const std::vector<math::Vector3> &_verts)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Copy vertices from a vector
+ /// \param[in] _verts the vertices to copy from
+ public: void CopyVertices(
+ const std::vector<ignition::math::Vector3d> &_verts);
/// \brief Copy normals from a vector
/// \param[in] _norms to copy from
- public: void CopyNormals(const std::vector<math::Vector3> &_norms);
+ /// \deprecated See CopyNormals that accepts
+ /// ignition::math::Vector3d.
+ public: void CopyNormals(const std::vector<math::Vector3> &_norms)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Copy normals from a vector
+ /// \param[in] _norms to copy from
+ public: void CopyNormals(
+ const std::vector<ignition::math::Vector3d> &_norms);
/// \brief Resize the vertex array
/// \param[in] _count the new size of the array
@@ -267,7 +339,13 @@ namespace gazebo
/// \brief Add a vertex to the mesh
/// \param[in] _v the new position
- public: void AddVertex(const math::Vector3 &_v);
+ /// \deprecated See AddVertex function that accepts
+ /// ignition::math::Vector3d.
+ public: void AddVertex(const math::Vector3 &_v) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Add a vertex to the mesh
+ /// \param[in] _v the new position
+ public: void AddVertex(const ignition::math::Vector3d &_v);
/// \brief Add a vertex to the mesh
/// \param[in] _x position along x
@@ -277,7 +355,13 @@ namespace gazebo
/// \brief Add a normal to the mesh
/// \param[in] _n the normal
- public: void AddNormal(const math::Vector3 &_n);
+ /// \deprecated See AddNormal function that accepts
+ /// ignition::math::Vector3d.
+ public: void AddNormal(const math::Vector3 &_n) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Add a normal to the mesh
+ /// \param[in] _n the normal
+ public: void AddNormal(const ignition::math::Vector3d &_n);
/// \brief Add a normal to the mesh
/// \param[in] _x position along x
@@ -300,27 +384,69 @@ namespace gazebo
/// \brief Get a vertex
/// \param[in] _i the vertex index
/// \return the position or throws an exception
- public: math::Vector3 GetVertex(unsigned int _i) const;
+ /// \deprecated See Vertex function that return an
+ /// ignition::math::Vector3d.
+ public: math::Vector3 GetVertex(unsigned int _i) const
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get a vertex
+ /// \param[in] _i the vertex index
+ /// \return the position or throws an exception
+ public: ignition::math::Vector3d Vertex(unsigned int _i) const;
/// \brief Set a vertex
/// \param[in] _i the index
/// \param[in] _v the position
- public: void SetVertex(unsigned int _i, const math::Vector3 &_v);
+ /// \deprecated See SetVertex function that accepts
+ /// ignition::math::Vector3d.
+ public: void SetVertex(unsigned int _i, const math::Vector3 &_v)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set a vertex
+ /// \param[in] _i the index
+ /// \param[in] _v the position
+ public: void SetVertex(unsigned int _i,
+ const ignition::math::Vector3d &_v);
+
+ /// \brief Get a normal
+ /// \param[in] _i the normal index
+ /// \return the orientation of the normal, or throws an exception
+ /// \deprecated See Normal function that return
+ /// ignition::math::Vector3d.
+ public: math::Vector3 GetNormal(unsigned int _i) const
+ GAZEBO_DEPRECATED(6.0);
/// \brief Get a normal
/// \param[in] _i the normal index
/// \return the orientation of the normal, or throws an exception
- public: math::Vector3 GetNormal(unsigned int _i) const;
+ public: ignition::math::Vector3d Normal(unsigned int _i) const;
+
+ /// \brief Set a normal
+ /// \param[in] _i the normal index
+ /// \param[in] _n the normal direction
+ /// \deprecated See SetNormal function that accepts
+ /// ignition::math::Vector3d.
+ public: void SetNormal(unsigned int _i, const math::Vector3 &_n)
+ GAZEBO_DEPRECATED(6.0);
/// \brief Set a normal
/// \param[in] _i the normal index
/// \param[in] _n the normal direction
- public: void SetNormal(unsigned int _i, const math::Vector3 &_n);
+ public: void SetNormal(unsigned int _i,
+ const ignition::math::Vector3d &_n);
+
+ /// \brief Get a tex coord
+ /// \param[in] _i the texture index
+ /// \return the texture coordinates
+ /// \deprecated See TexCoord function that return
+ /// ignition::math::Vector2d.
+ public: math::Vector2d GetTexCoord(unsigned int _i) const
+ GAZEBO_DEPRECATED(6.0);
/// \brief Get a tex coord
/// \param[in] _i the texture index
/// \return the texture coordinates
- public: math::Vector2d GetTexCoord(unsigned int _i) const;
+ public: ignition::math::Vector2d TexCoord(unsigned int _i) const;
/// \brief Get a vertex - skeleton node assignment
/// \param[in] _i the index of the assignment
@@ -329,19 +455,38 @@ namespace gazebo
/// \brief Set a tex coord
/// \param[in] _i
/// \param[in] _t
- public: void SetTexCoord(unsigned int _i, const math::Vector2d &_t);
+ /// \deprecated See SetTexCoord function that accepts
+ /// ignition::math::Vector2d.
+ public: void SetTexCoord(unsigned int _i, const math::Vector2d &_t)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set a tex coord
+ /// \param[in] _i
+ /// \param[in] _t
+ public: void SetTexCoord(unsigned int _i,
+ const ignition::math::Vector2d &_t);
/// \brief Get an index
/// \param[in] _i
public: unsigned int GetIndex(unsigned int _i) const;
- /// \brief Get the maximun X, Y, Z values
- /// \return
- public: math::Vector3 GetMax() const;
+ /// \brief Get the maximum X, Y, Z values
+ /// \return Max X,Y,Z values from all vertices in submesh
+ /// \deprecated See Max function that returns ignition::math::Vector3d.
+ public: math::Vector3 GetMax() const GAZEBO_DEPRECATED(6.0);
- /// \brief Get the minimum X, Y, Z values
- /// \return
- public: math::Vector3 GetMin() const;
+ /// \brief Get the maximum X, Y, Z values from all the vertices
+ /// \return Max X,Y,Z values from all vertices in submesh
+ public: ignition::math::Vector3d Max() const;
+
+ /// \brief Get the minimum X, Y, Z values from all the vertices
+ /// \return Min X,Y,Z values from all vertices in submesh
+ /// \deprecated See Min function that returns ignition::math::Vector3d.
+ public: math::Vector3 GetMin() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the minimum X, Y, Z values from all the vertices
+ /// \return Min X,Y,Z values from all vertices in submesh
+ public: ignition::math::Vector3d Min() const;
/// \brief Return the number of vertices
public: unsigned int GetVertexCount() const;
@@ -371,11 +516,29 @@ namespace gazebo
/// \brief Return true if this submesh has the vertex
/// \param[in] _v
- public: bool HasVertex(const math::Vector3 &_v) const;
+ /// \return Return true if this submesh has the vertex
+ /// \deprecated See HasVertex function that accepts
+ /// ignition::math::Vector3d.
+ public: bool HasVertex(const math::Vector3 &_v) const
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Return true if this submesh has the vertex
+ /// \param[in] _v
+ /// \return Return true if this submesh has the vertex
+ public: bool HasVertex(const ignition::math::Vector3d &_v) const;
/// \brief Get the index of the vertex
/// \param[in] _v
- public: unsigned int GetVertexIndex(const math::Vector3 &_v) const;
+ /// \deprecated See GetVertexIndex function that accepts
+ /// ignition::math::Vector3d.
+ public: unsigned int GetVertexIndex(const math::Vector3 &_v) const
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the index of the vertex
+ /// \param[in] _v Vertex to check
+ /// \return Index of the vertex that matches _v.
+ public: unsigned int GetVertexIndex(
+ const ignition::math::Vector3d &_v) const;
/// \brief Put all the data into flat arrays
/// \param[in] _verArr
@@ -385,14 +548,19 @@ namespace gazebo
/// \brief Recalculate all the normals.
public: void RecalculateNormals();
- /// \brief Reset mesh center to geometric center
+ /// \brief Generate texture coordinates using spherical projection
+ /// from center
/// \param[in] _center
- public: void SetSubMeshCenter(math::Vector3 _center);
+ /// \deprecated See GenSphericalTexCoord that accepts
+ /// ignition::math::Vector3d.
+ public: void GenSphericalTexCoord(const math::Vector3 &_center)
+ GAZEBO_DEPRECATED(6.0);
/// \brief Generate texture coordinates using spherical projection
/// from center
/// \param[in] _center
- public: void GenSphericalTexCoord(const math::Vector3 &_center);
+ public: void GenSphericalTexCoord(
+ const ignition::math::Vector3d &_center);
/// \brief Scale all vertices by _factor
/// \param[in] _factor Scaling factor
@@ -401,24 +569,46 @@ namespace gazebo
/// \brief Move the center of the submesh to the given coordinate. This
/// will move all the vertices.
/// \param[in] _center Location of the mesh center.
- public: void Center(const math::Vector3 &_center = math::Vector3::Zero);
+ /// \deprecated See Center function that accepts
+ /// ignition::math::Vector3d.
+ public: void Center(const math::Vector3 &_center = math::Vector3::Zero)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Move the center of the submesh to the given coordinate. This
+ /// will move all the vertices.
+ /// \param[in] _center Location of the mesh center.
+ public: void Center(const ignition::math::Vector3d &_center =
+ ignition::math::Vector3d::Zero);
+
+ /// \brief Move all vertices by _vec.
+ /// \param[in] _vec Amount to translate vertices.
+ /// \deprecated See Translate function that accepts
+ /// ignition::math::Vector3d.
+ public: void Translate(const math::Vector3 &_vec) GAZEBO_DEPRECATED(6.0);
/// \brief Move all vertices by _vec.
/// \param[in] _vec Amount to translate vertices.
- public: void Translate(const math::Vector3 &_vec);
+ public: void Translate(const ignition::math::Vector3d &_vec);
+
+ /// \brief Scale all vertices by the _factor vector
+ /// \param[in] _factor Scaling vector
+ /// \deprecated See SetScale function accepts
+ /// ignition::math::Vector3d.
+ public: void SetScale(const math::Vector3 &_factor)
+ GAZEBO_DEPRECATED(6.0);
/// \brief Scale all vertices by the _factor vector
/// \param[in] _factor Scaling vector
- public: void SetScale(const math::Vector3 &_factor);
+ public: void SetScale(const ignition::math::Vector3d &_factor);
/// \brief the vertex array
- private: std::vector< math::Vector3 > vertices;
+ private: std::vector<ignition::math::Vector3d> vertices;
/// \brief the normal array
- private: std::vector< math::Vector3 > normals;
+ private: std::vector<ignition::math::Vector3d> normals;
/// \brief the texture coordinate array
- private: std::vector< math::Vector2d > texCoords;
+ private: std::vector<ignition::math::Vector2d> texCoords;
/// \brief the vertex index array
private: std::vector<unsigned int> indices;
diff --git a/gazebo/common/MeshCSG.cc b/gazebo/common/MeshCSG.cc
index 70d9ecd..f455edb 100644
--- a/gazebo/common/MeshCSG.cc
+++ b/gazebo/common/MeshCSG.cc
@@ -53,7 +53,7 @@ void MeshCSG::MergeVertices(GPtrArray * _vertices, double _epsilon)
// for each vertex, do a bbox kdtree search to find nearby vertices within
// _epsilon, merge vertices if they are within the bbox
- for (unsigned int i = 0; i < _vertices->len; i++)
+ for (unsigned int i = 0; i < _vertices->len; ++i)
{
GtsVertex *v = reinterpret_cast<GtsVertex *>(verticesData[i]);
@@ -139,6 +139,13 @@ static void FillFace(GtsTriangle *_t, gpointer *_data)
Mesh *MeshCSG::CreateBoolean(const Mesh *_m1, const Mesh *_m2, int _operation,
const math::Pose &_offset)
{
+ return this->CreateBoolean(_m1, _m2, _operation, _offset.Ign());
+}
+
+//////////////////////////////////////////////////
+Mesh *MeshCSG::CreateBoolean(const Mesh *_m1, const Mesh *_m2, int _operation,
+ const ignition::math::Pose3d &_offset)
+{
GtsSurface *s1, *s2, *s3;
GtsSurfaceInter *si;
GNode *tree1, *tree2;
@@ -156,7 +163,7 @@ Mesh *MeshCSG::CreateBoolean(const Mesh *_m1, const Mesh *_m2, int _operation,
this->ConvertMeshToGTS(_m1, s1);
- if (_offset != math::Pose::Zero)
+ if (_offset != ignition::math::Pose3d::Zero)
{
Mesh *m2 = new Mesh();
for (unsigned int i = 0; i < _m2->GetSubMeshCount(); ++i)
@@ -169,7 +176,8 @@ Mesh *MeshCSG::CreateBoolean(const Mesh *_m1, const Mesh *_m2, int _operation,
continue;
for (unsigned int j = 0; j < subMesh->GetVertexCount(); ++j)
{
- m2SubMesh->AddVertex(_offset.pos + _offset.rot*subMesh->GetVertex(j));
+ m2SubMesh->AddVertex(_offset.Pos() +
+ _offset.Rot()*subMesh->Vertex(j));
}
for (unsigned int j = 0; j < subMesh->GetIndexCount(); ++j)
{
@@ -287,9 +295,9 @@ void MeshCSG::ConvertMeshToGTS(const Mesh *_mesh, GtsSurface *_surface)
for (unsigned int j = 0; j < subMesh->GetVertexCount(); ++j)
{
- math::Vector3 vertex = subMesh->GetVertex(j);
- g_ptr_array_add(vertices, gts_vertex_new(gts_vertex_class(), vertex.x,
- vertex.y, vertex.z));
+ ignition::math::Vector3d vertex = subMesh->Vertex(j);
+ g_ptr_array_add(vertices, gts_vertex_new(gts_vertex_class(), vertex.X(),
+ vertex.Y(), vertex.Z()));
}
// merge duplicate vertices, otherwise gts produces undesirable results
diff --git a/gazebo/common/MeshCSG.hh b/gazebo/common/MeshCSG.hh
index 1c3ae65..2dcb51e 100644
--- a/gazebo/common/MeshCSG.hh
+++ b/gazebo/common/MeshCSG.hh
@@ -14,8 +14,10 @@
* limitations under the License.
*
*/
-#ifndef _MESHCSG_HH_
-#define _MESHCSG_HH_
+#ifndef _GAZEBO_MESHCSG_HH_
+#define _GAZEBO_MESHCSG_HH_
+
+#include <ignition/math/Pose3.hh>
#include "gazebo/math/Pose.hh"
#include "gazebo/util/system.hh"
@@ -36,7 +38,7 @@ namespace gazebo
/// \class MeshCSG MeshCSG.hh common/common.hh
/// \brief Creates CSG meshes
- class GAZEBO_VISIBLE MeshCSG
+ class GZ_COMMON_VISIBLE MeshCSG
{
/// \brief An enumeration of the boolean operations
public: enum BooleanOperation {UNION, INTERSECTION, DIFFERENCE};
@@ -53,8 +55,21 @@ namespace gazebo
/// \param[in] _operation the boolean operation applied to the two meshes
/// \param[in] _offset _m2's pose offset from _m1
/// \return a pointer to the created mesh
+ /// \deprecated See CreateBoolean function that accepts ignition math
+ /// objects
+ public: Mesh *CreateBoolean(const Mesh *_m1, const Mesh *_m2,
+ const int _operation, const math::Pose &_offset = math::Pose::Zero)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Create a boolean mesh from two meshes
+ /// \param[in] _m1 the parent mesh in the boolean operation
+ /// \param[in] _m2 the child mesh in the boolean operation
+ /// \param[in] _operation the boolean operation applied to the two meshes
+ /// \param[in] _offset _m2's pose offset from _m1
+ /// \return a pointer to the created mesh
public: Mesh *CreateBoolean(const Mesh *_m1, const Mesh *_m2,
- const int _operation, const math::Pose &_offset = math::Pose::Zero);
+ const int _operation,
+ const ignition::math::Pose3d &_offset = ignition::math::Pose3d::Zero);
/// \brief Helper method for converting Mesh to GTS Surface
private: void ConvertMeshToGTS(const Mesh *mesh, GtsSurface *surface);
diff --git a/gazebo/common/MeshExporter.hh b/gazebo/common/MeshExporter.hh
index c75dbb1..c72174f 100644
--- a/gazebo/common/MeshExporter.hh
+++ b/gazebo/common/MeshExporter.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class MeshExporter MeshExporter.hh common/common.hh
/// \brief Base class for exporting meshes
- class GAZEBO_VISIBLE MeshExporter
+ class GZ_COMMON_VISIBLE MeshExporter
{
/// \brief Constructor
public: MeshExporter();
diff --git a/gazebo/common/MeshLoader.hh b/gazebo/common/MeshLoader.hh
index 72a2ec1..5d79a9c 100644
--- a/gazebo/common/MeshLoader.hh
+++ b/gazebo/common/MeshLoader.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class MeshLoader MeshLoader.hh common/common.hh
/// \brief Base class for loading meshes
- class GAZEBO_VISIBLE MeshLoader
+ class GZ_COMMON_VISIBLE MeshLoader
{
/// \brief Constructor
public: MeshLoader();
diff --git a/gazebo/common/MeshManager.cc b/gazebo/common/MeshManager.cc
index 9a28485..0212271 100644
--- a/gazebo/common/MeshManager.cc
+++ b/gazebo/common/MeshManager.cc
@@ -16,10 +16,12 @@
*/
#include <sys/stat.h>
#include <string>
+#include <map>
#include "gazebo/math/Plane.hh"
#include "gazebo/math/Matrix3.hh"
#include "gazebo/math/Matrix4.hh"
+#include "gazebo/math/Vector2i.hh"
#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/Exception.hh"
@@ -49,16 +51,17 @@ MeshManager::MeshManager()
// Create some basic shapes
this->CreatePlane("unit_plane",
- math::Plane(math::Vector3(0, 0, 1), math::Vector2d(1, 1), 0),
- math::Vector2d(1, 1),
- math::Vector2d(1, 1));
+ ignition::math::Planed(
+ ignition::math::Vector3d(0, 0, 1), ignition::math::Vector2d(1, 1), 0),
+ ignition::math::Vector2d(1, 1),
+ ignition::math::Vector2d(1, 1));
this->CreateSphere("unit_sphere", 0.5, 32, 32);
this->CreateSphere("joint_anchor", 0.01, 32, 32);
- this->CreateBox("body_cg", math::Vector3(0.014, 0.014, 0.014),
- math::Vector2d(0.014, 0.014));
- this->CreateBox("unit_box", math::Vector3(1, 1, 1),
- math::Vector2d(1, 1));
+ this->CreateBox("body_cg", ignition::math::Vector3d(0.014, 0.014, 0.014),
+ ignition::math::Vector2d(0.014, 0.014));
+ this->CreateBox("unit_box", ignition::math::Vector3d(1, 1, 1),
+ ignition::math::Vector2d(1, 1));
this->CreateCylinder("unit_cylinder", 0.5, 1.0, 1, 32);
this->CreateCone("unit_cone", 0.5, 1.0, 5, 32);
this->CreateCamera("unit_camera", 0.5);
@@ -191,18 +194,36 @@ bool MeshManager::IsValidFilename(const std::string &_filename)
extension) != this->fileExtensions.end();
}
-
//////////////////////////////////////////////////
void MeshManager::GetMeshAABB(const Mesh *_mesh, math::Vector3 &_center,
- math::Vector3 &_min_xyz, math::Vector3 &_max_xyz)
+ math::Vector3 &_minXYZ, math::Vector3 &_maxXYZ)
+{
+ ignition::math::Vector3d center, minXYZ, maxXYZ;
+ this->GetMeshAABB(_mesh, center, minXYZ, maxXYZ);
+ _center = center;
+ _minXYZ = minXYZ;
+ _maxXYZ = maxXYZ;
+}
+
+//////////////////////////////////////////////////
+void MeshManager::GetMeshAABB(const Mesh *_mesh,
+ ignition::math::Vector3d &_center,
+ ignition::math::Vector3d &_minXYZ, ignition::math::Vector3d &_maxXYZ)
{
if (this->HasMesh(_mesh->GetName()))
- this->meshes[_mesh->GetName()]->GetAABB(_center, _min_xyz, _max_xyz);
+ this->meshes[_mesh->GetName()]->GetAABB(_center, _minXYZ, _maxXYZ);
}
//////////////////////////////////////////////////
void MeshManager::GenSphericalTexCoord(const Mesh *_mesh, math::Vector3 _center)
{
+ this->GenSphericalTexCoord(_mesh, _center.Ign());
+}
+
+//////////////////////////////////////////////////
+void MeshManager::GenSphericalTexCoord(const Mesh *_mesh,
+ const ignition::math::Vector3d &_center)
+{
if (this->HasMesh(_mesh->GetName()))
this->meshes[_mesh->GetName()]->GenSphericalTexCoord(_center);
}
@@ -251,7 +272,7 @@ void MeshManager::CreateSphere(const std::string &name, float radius,
int ring, seg;
float deltaSegAngle = (2.0 * M_PI / segments);
float deltaRingAngle = (M_PI / rings);
- math::Vector3 vert, norm;
+ ignition::math::Vector3d vert, norm;
unsigned int verticeIndex = 0;
Mesh *mesh = new Mesh();
@@ -262,18 +283,17 @@ void MeshManager::CreateSphere(const std::string &name, float radius,
mesh->AddSubMesh(subMesh);
// Generate the group of rings for the sphere
- for (ring = 0; ring <= rings; ring++)
+ for (ring = 0; ring <= rings; ++ring)
{
float r0 = radius * sinf(ring * deltaRingAngle);
- vert.y = radius * cosf(ring * deltaRingAngle);
+ vert.Y() = radius * cosf(ring * deltaRingAngle);
// Generate the group of segments for the current ring
- for (seg = 0; seg <= segments; seg++)
+ for (seg = 0; seg <= segments; ++seg)
{
- vert.x = r0 * sinf(seg * deltaSegAngle);
- vert.z = r0 * cosf(seg * deltaSegAngle);
+ vert.X() = r0 * sinf(seg * deltaSegAngle);
+ vert.Z() = r0 * cosf(seg * deltaSegAngle);
- // TODO: Don't think these normals are correct.
norm = vert;
norm.Normalize();
@@ -298,60 +318,81 @@ void MeshManager::CreateSphere(const std::string &name, float radius,
}
}
}
+}
- mesh->RecalculateNormals();
+//////////////////////////////////////////////////
+void MeshManager::CreatePlane(const std::string &_name,
+ const math::Plane &_plane,
+ const math::Vector2d &_segments,
+ const math::Vector2d &_uvTile)
+{
+ this->CreatePlane(_name, _plane.Ign(), _segments.Ign(), _uvTile.Ign());
}
//////////////////////////////////////////////////
-void MeshManager::CreatePlane(const std::string &name, const math::Plane &plane,
- const math::Vector2d &segments,
- const math::Vector2d &uvTile)
+void MeshManager::CreatePlane(const std::string &_name,
+ const ignition::math::Planed &_plane,
+ const ignition::math::Vector2d &_segments,
+ const ignition::math::Vector2d &_uvTile)
{
- this->CreatePlane(name, plane.normal, plane.d, plane.size, segments, uvTile);
+ this->CreatePlane(_name, _plane.Normal(), _plane.Offset(), _plane.Size(),
+ _segments, _uvTile);
}
//////////////////////////////////////////////////
-void MeshManager::CreatePlane(const std::string &name,
- const math::Vector3 &normal, double d, const math::Vector2d &size,
- const math::Vector2d &segments, const math::Vector2d &uvTile)
+void MeshManager::CreatePlane(const std::string &_name,
+ const math::Vector3 &_normal, double _d, const math::Vector2d &_size,
+ const math::Vector2d &_segments, const math::Vector2d &_uvTile)
{
- if (this->HasMesh(name))
+ this->CreatePlane(_name, _normal.Ign(), _d, _size.Ign(), _segments.Ign(),
+ _uvTile.Ign());
+}
+
+//////////////////////////////////////////////////
+void MeshManager::CreatePlane(const std::string &_name,
+ const ignition::math::Vector3d &_normal,
+ const double _d,
+ const ignition::math::Vector2d &_size,
+ const ignition::math::Vector2d &_segments,
+ const ignition::math::Vector2d &_uvTile)
+{
+ if (this->HasMesh(_name))
{
return;
}
Mesh *mesh = new Mesh();
- mesh->SetName(name);
- this->meshes.insert(std::make_pair(name, mesh));
+ mesh->SetName(_name);
+ this->meshes.insert(std::make_pair(_name, mesh));
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
- math::Vector3 zAxis, yAxis, xAxis;
- zAxis = normal;
+ ignition::math::Vector3d zAxis, yAxis, xAxis;
+ zAxis = _normal;
zAxis.Normalize();
- yAxis = zAxis.GetPerpendicular();
+ yAxis = zAxis.Perpendicular();
xAxis = yAxis.Cross(zAxis);
- math::Matrix4 xlate, xform, rot;
- xlate = rot = math::Matrix4::IDENTITY;
+ ignition::math::Matrix4d xlate, xform, rot;
+ xlate = rot = ignition::math::Matrix4d::Identity;
- math::Matrix3 rot3;
- rot3.SetFromAxes(xAxis, yAxis, zAxis);
+ ignition::math::Matrix3d rot3;
+ rot3.Axes(xAxis, yAxis, zAxis);
rot = rot3;
- xlate.SetTranslate(normal * -d);
+ xlate.Translate(_normal * -_d);
xform = xlate * rot;
- math::Vector3 vec;
- math::Vector3 norm(0, 0, 1);
- double xSpace = size.x / segments.x;
- double ySpace = size.y / segments.y;
- double halfWidth = size.x / 2.0;
- double halfHeight = size.y / 2.0;
- double xTex = uvTile.x / segments.x;
- double yTex = uvTile.y / segments.y;
+ ignition::math::Vector3d vec;
+ ignition::math::Vector3d norm(0, 0, 1);
+ double xSpace = _size.X() / _segments.X();
+ double ySpace = _size.Y() / _segments.Y();
+ double halfWidth = _size.X() / 2.0;
+ double halfHeight = _size.Y() / 2.0;
+ double xTex = _uvTile.X() / _segments.X();
+ double yTex = _uvTile.Y() / _segments.Y();
// Give it some thickness to reduce shadow artifacts.
double thickness = 0.01;
@@ -359,14 +400,14 @@ void MeshManager::CreatePlane(const std::string &name,
for (int i = 0; i <= 1; ++i)
{
double z = i*thickness;
- for (int y = 0; y <= segments.y; ++y)
+ for (int y = 0; y <= _segments.Y(); ++y)
{
- for (int x = 0; x <= segments.x; ++x)
+ for (int x = 0; x <= _segments.X(); ++x)
{
// Compute the position of the vertex
- vec.x = (x * xSpace) - halfWidth;
- vec.y = (y * ySpace) - halfHeight;
- vec.z = -z;
+ vec.X() = (x * xSpace) - halfWidth;
+ vec.Y() = (y * ySpace) - halfHeight;
+ vec.Z() = -z;
vec = xform.TransformAffine(vec);
subMesh->AddVertex(vec);
@@ -380,23 +421,31 @@ void MeshManager::CreatePlane(const std::string &name,
}
}
- this->Tesselate2DMesh(subMesh, segments.x + 1, segments.y + 1, false);
+ this->Tesselate2DMesh(subMesh, _segments.X() + 1, _segments.Y() + 1, false);
}
//////////////////////////////////////////////////
-void MeshManager::CreateBox(const std::string &name, const math::Vector3 &sides,
- const math::Vector2d &uvCoords)
+void MeshManager::CreateBox(const std::string &_name,
+ const math::Vector3 &_sides, const math::Vector2d &_uvCoords)
+{
+ this->CreateBox(_name, _sides.Ign(), _uvCoords.Ign());
+}
+
+//////////////////////////////////////////////////
+void MeshManager::CreateBox(const std::string &_name,
+ const ignition::math::Vector3d &_sides,
+ const ignition::math::Vector2d &_uvCoords)
{
int i, k;
- if (this->HasMesh(name))
+ if (this->HasMesh(_name))
{
return;
}
Mesh *mesh = new Mesh();
- mesh->SetName(name);
- this->meshes.insert(std::make_pair(name, mesh));
+ mesh->SetName(_name);
+ this->meshes.insert(std::make_pair(_name, mesh));
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
@@ -404,27 +453,32 @@ void MeshManager::CreateBox(const std::string &name, const math::Vector3 &sides,
// Vertex values
float v[8][3] =
{
- {-1, -1, -1}, {-1, -1, +1}, {+1, -1, +1}, {+1, -1, -1},
- {-1, +1, -1}, {-1, +1, +1}, {+1, +1, +1}, {+1, +1, -1}
+ {-1, -1, -1},
+ {-1, -1, +1},
+ {+1, -1, +1},
+ {+1, -1, -1},
+ {-1, +1, -1},
+ {-1, +1, +1},
+ {+1, +1, +1},
+ {+1, +1, -1}
};
- // Normals for each vertex
- float n[8][3]=
+ // Normals for each face
+ float n[6][3]=
{
- {-0.577350, -0.577350, -0.577350},
- {-0.577350, -0.577350, 0.577350},
- {0.577350, -0.577350, 0.577350},
- {0.577350, -0.577350, -0.577350},
- {-0.577350, 0.577350, -0.577350},
- {-0.577350, 0.577350, 0.577350},
- {0.577350, 0.577350, 0.577350},
- {0.577350, 0.577350, -0.577350}
+ {+0, -1, +0},
+ {+0, +1, +0},
+ {+0, +0, +1},
+ {-1, +0, +0},
+ {+0, +0, -1},
+ {+1, +0, +0},
};
// Texture coords
double t[4][2] =
{
- {uvCoords.x, 0}, {0, 0}, {0, uvCoords.y}, {uvCoords.x, uvCoords.y}
+ {_uvCoords.X(), 0}, {0, 0}, {0, _uvCoords.Y()},
+ {_uvCoords.X(), _uvCoords.Y()}
};
// Vertices
@@ -453,47 +507,83 @@ void MeshManager::CreateBox(const std::string &name, const math::Vector3 &sides,
};
// Compute the vertices
- for (i = 0; i < 8; i++)
+ for (i = 0; i < 8; ++i)
{
- v[i][0] *= sides.x * 0.5;
- v[i][1] *= sides.y * 0.5;
- v[i][2] *= sides.z * 0.5;
+ v[i][0] *= _sides.X() * 0.5;
+ v[i][1] *= _sides.Y() * 0.5;
+ v[i][2] *= _sides.Z() * 0.5;
}
// For each face
- for (i = 0; i < 6; i++)
+ for (i = 0; i < 6; ++i)
{
// For each vertex in the face
for (k = 0; k < 4; k++)
{
- subMesh->AddVertex(v[faces[i][k]][0], v[faces[i][k]][1],
- v[faces[i][k]][2]);
- subMesh->AddNormal(n[faces[i][k]][0], n[faces[i][k]][1],
- n[faces[i][k]][2]);
+ subMesh->AddVertex(v[faces[i][k]][0],
+ v[faces[i][k]][1],
+ v[faces[i][k]][2]);
+ subMesh->AddNormal(n[i][0], n[i][1], n[i][2]);
subMesh->AddTexCoord(t[k][0], t[k][1]);
}
}
// Set the indices
- for (i = 0; i < 36; i++)
+ for (i = 0; i < 36; ++i)
subMesh->AddIndex(ind[i]);
+}
+
+//////////////////////////////////////////////////
+void MeshManager::CreateExtrudedPolyline(const std::string &_name,
+ const std::vector<std::vector<math::Vector2d> > &_polys, double _height)
+{
+ std::vector<std::vector<ignition::math::Vector2d> > polys;
+ for (auto const &polyVec : _polys)
+ {
+ std::vector<ignition::math::Vector2d> newVec;
+ for (auto const &poly : polyVec)
+ newVec.push_back(poly.Ign());
+ polys.push_back(newVec);
+ }
- subMesh->RecalculateNormals();
+ this->CreateExtrudedPolyline(_name, polys, _height);
}
//////////////////////////////////////////////////
void MeshManager::CreateExtrudedPolyline(const std::string &_name,
- const std::vector<math::Vector2d> &_vertices,
- const double &_height, const math::Vector2d & /*_uvCoords*/)
+ const std::vector<std::vector<ignition::math::Vector2d> > &_polys,
+ double _height)
{
+ // distance tolerence between 2 points. This is used when creating a list
+ // of distinct points in the polylines.
+ double tol = 1e-4;
+ #if !HAVE_GTS
+ gzerr << "GTS library not found. Can not extrude polyline" << std::endl;
+ return;
+ #endif
+ auto polys = _polys;
+ // close all the loops
+ for (auto &poly : polys)
+ {
+ // does the poly ends with the first point?
+ auto first = poly[0];
+ auto last = poly[poly.size()-1];
+ double d = (first.X() - last.X()) * (first.X() - last.X());
+ d += (first.Y() - last.Y()) * (first.Y() - last.Y());
+
+ // within range
+ if (d > tol * tol)
+ {
+ // add the first point at the end
+ poly.push_back(first);
+ }
+ }
+
if (this->HasMesh(_name))
{
return;
}
- int i;
- int numSides = _vertices.size();
-
Mesh *mesh = new Mesh();
mesh->SetName(_name);
this->meshes.insert(std::make_pair(_name, mesh));
@@ -501,108 +591,176 @@ void MeshManager::CreateExtrudedPolyline(const std::string &_name,
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
+ std::vector<ignition::math::Vector2d> vertices;
+ std::vector<ignition::math::Vector2i> edges;
+ MeshManager::ConvertPolylinesToVerticesAndEdges(polys,
+ tol,
+ vertices,
+ edges);
#if HAVE_GTS
+ if (!GTSMeshUtils::DelaunayTriangulation(vertices, edges, subMesh))
{
- if (!GTSMeshUtils::CreateExtrudedPolyline(_vertices, _height, subMesh))
- {
- gzerr << "Unable to create extruded polyline.\n";
- delete mesh;
- return;
- }
+ gzerr << "Unable to triangulate polyline." << std::endl;
+ delete mesh;
+ return;
}
- #else
- {
- gzerr << "GTS library not found.\n" <<
- "Polylines rendered may be incorrect, if concave\n";
+ #endif
- // Add the vertices
- for (i = 0; i < numSides; ++i)
+ std::vector<ignition::math::Vector3d> normals;
+ for (unsigned int i = 0; i < edges.size(); ++i)
+ {
+ // we retrieve each edge's coordinates
+ int i0 = edges[i][0];
+ int i1 = edges[i][1];
+ ignition::math::Vector2d edgeV0 = vertices[i0];
+ ignition::math::Vector2d edgeV1 = vertices[i1];
+
+ // we look for those points in the subMesh (where indices may have changed)
+ for (unsigned int j = 0; j < subMesh->GetIndexCount(); j+=3)
{
- subMesh->AddVertex(_vertices[i].x, _vertices[i].y, 0.0);
- subMesh->AddVertex(_vertices[i].x, _vertices[i].y, _height);
- }
+ ignition::math::Vector3d v0 = subMesh->Vertex(subMesh->GetIndex(j));
+ ignition::math::Vector3d v1 = subMesh->Vertex(subMesh->GetIndex(j+1));
+ ignition::math::Vector3d v2 = subMesh->Vertex(subMesh->GetIndex(j+2));
- // Euler's Formula: numFaces = numEdges - numVertices + 2
- // = numSides + 2
- // # of SideFaces = numFaces - (upper face + lower face)
- // = numFaces - 2
- // = numSides
-
- // for lower face
- int startVert = 0;
- int endVert = numSides * 2 - 2;
- subMesh->AddIndex(startVert);
- startVert += 2;
- subMesh->AddIndex(startVert);
- subMesh->AddIndex(endVert);
- for (i = 1; i < numSides-2; ++i)
- {
- if (i%2)
- {
- subMesh->AddIndex(startVert);
- startVert += 2;
- subMesh->AddIndex(startVert);
- subMesh->AddIndex(endVert);
- }
- else
- {
- subMesh->AddIndex(endVert);
- endVert -= 2;
- subMesh->AddIndex(startVert);
- subMesh->AddIndex(endVert);
- }
- }
+ std::vector<ignition::math::Vector3d> triangle;
+ triangle.push_back(v0);
+ triangle.push_back(v1);
+ triangle.push_back(v2);
- // for upper face
- startVert = 1;
- endVert = numSides*2-1;
- subMesh->AddIndex(startVert);
- startVert += 2;
- subMesh->AddIndex(endVert);
- subMesh->AddIndex(startVert);
- for (i = 1; i < numSides-2; ++i)
- {
- if (!i%2)
+ int ev0 = -1;
+ for (unsigned int k = 0; k < triangle.size(); ++k)
{
- subMesh->AddIndex(startVert);
- startVert += 2;
- subMesh->AddIndex(startVert);
- subMesh->AddIndex(endVert);
+ if (ignition::math::Vector2d(triangle[k].X(), triangle[k].Y()) ==
+ edgeV0)
+ {
+ // found a vertex in triangle that matches the vertex of the edge
+ ev0 = k;
+ break;
+ }
}
- else
+ if (ev0 >=0)
{
- subMesh->AddIndex(endVert);
- endVert -= 2;
- subMesh->AddIndex(endVert);
- subMesh->AddIndex(startVert);
+ int ev1 = -1;
+ int ev2 = -1;
+ for (unsigned int k = 0; k < triangle.size()-1; ++k)
+ {
+ int index = (ev0 + k + 1) % triangle.size();
+ ignition::math::Vector3d triV = triangle[index];
+ if (math::Vector2d(triV.X(), triV.Y()) == edgeV1)
+ {
+ // found another vertex in triangle that matches the vertex of the
+ // other edge.
+ ev1 = index;
+ // Store the index of the third triangle vertex.
+ // It's either 0, 1, or 2. Find it using simple bitwise operation.
+ ev2 = ~(ev1 | ev0) & 0x03;
+ break;
+ }
+ }
+ if (ev1 >= 0 && ev2 >= 0 && ev0 != ev1 && ev0 != ev2)
+ {
+ // Found an edge in triangle that matches the exterior edge.
+ // Now find its normal.
+
+ ignition::math::Vector3d edgeVec = triangle[ev0] - triangle[ev1];
+ edgeVec.Normalize();
+ ignition::math::Vector3d normal(edgeVec.Y(), -edgeVec.X(), 0);
+
+ ignition::math::Vector3d otherEdgeVec = triangle[ev0] - triangle[ev2];
+ otherEdgeVec.Normalize();
+ double angle0 = otherEdgeVec.Dot(normal);
+ double angle1 = otherEdgeVec.Dot(-normal);
+
+ if (angle0 > angle1)
+ {
+ if (angle0 >= 0)
+ normals.push_back(normal);
+ }
+ else
+ {
+ if (angle1 >= 0)
+ normals.push_back(-normal);
+ }
+ }
}
}
}
- #endif
- // for each sideface
- for (i = 0; i < numSides; ++i)
+ // number of exterior edge normals found should be equal to the number of
+ // exterior edges
+ if (normals.size() != edges.size())
{
- subMesh->AddVertex(_vertices[i].x, _vertices[i].y, 0.0);
- subMesh->AddVertex(_vertices[i].x, _vertices[i].y, _height);
+ gzerr << "Unable to extrude mesh. Triangulation failed" << std::endl;
+ return;
}
- subMesh->AddVertex(_vertices[0].x, _vertices[0].y, 0.0);
- subMesh->AddVertex(_vertices[0].x, _vertices[0].y, _height);
- for (i = 0; i < numSides; ++i)
- {
- subMesh->AddIndex(i*2+numSides*2);
- subMesh->AddIndex(i*2+1+numSides*2);
- subMesh->AddIndex(i*2+2+numSides*2);
+ unsigned int numVertices = subMesh->GetVertexCount();
+
+ // add normal for bottom face
+ for (unsigned int i = 0; i < numVertices; ++i)
+ subMesh->AddNormal(-ignition::math::Vector3d::UnitZ);
- subMesh->AddIndex(i*2+2+numSides*2);
- subMesh->AddIndex(i*2+1+numSides*2);
- subMesh->AddIndex(i*2+3+numSides*2);
+ // create the top face
+ for (unsigned int i = 0; i < numVertices; ++i)
+ {
+ ignition::math::Vector3d v = subMesh->Vertex(i);
+ subMesh->AddVertex(v.X(), v.Y(), _height);
+ subMesh->AddNormal(ignition::math::Vector3d::UnitZ);
}
+ unsigned int numIndices = subMesh->GetIndexCount();
+ for (unsigned int i = 0; i < numIndices; i+=3)
+ {
+ unsigned int i0 = subMesh->GetIndex(i);
+ unsigned int i1 = subMesh->GetIndex(i+1);
+ unsigned int i2 = subMesh->GetIndex(i+2);
+ subMesh->AddIndex(numVertices+i0);
+ subMesh->AddIndex(numVertices+i2);
+ subMesh->AddIndex(numVertices+i1);
+ }
+
+ // create the side faces
+ for (unsigned int i = 0; i < edges.size(); ++i)
+ {
+ // we retrieve each edge's coordinates
+ int i0 = edges[i][0];
+ int i1 = edges[i][1];
+ ignition::math::Vector2d v0 = vertices[i0];
+ ignition::math::Vector2d v1 = vertices[i1];
- if (subMesh->GetNormalCount() != subMesh->GetVertexCount())
- subMesh->SetNormalCount(subMesh->GetVertexCount());
- subMesh->RecalculateNormals();
+ ignition::math::Vector2d edge2d = v1 - v0;
+ ignition::math::Vector3d edge(edge2d.X(), edge2d.Y(), 0);
+ ignition::math::Vector3d cross = edge.Cross(normals[i]);
+
+ unsigned int vCount = subMesh->GetVertexCount();
+
+ subMesh->AddVertex(ignition::math::Vector3d(v0.X(), v0.Y(), 0));
+ if (cross.Z() >0)
+ {
+ subMesh->AddVertex(ignition::math::Vector3d(v0.X(), v0.Y(), _height));
+ subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), _height));
+ }
+ else
+ {
+ subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), _height));
+ subMesh->AddVertex(ignition::math::Vector3d(v0.X(), v0.Y(), _height));
+ }
+ subMesh->AddVertex(ignition::math::Vector3d(v0.X(), v0.Y(), 0));
+ if (cross.Z() >0)
+ {
+ subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), _height));
+ subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), 0));
+ }
+ else
+ {
+ subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), 0));
+ subMesh->AddVertex(ignition::math::Vector3d(v1.X(), v1.Y(), _height));
+ }
+ for (unsigned int j = 0; j < 6; ++j)
+ {
+ subMesh->AddIndex(vCount++);
+ subMesh->AddNormal(normals[i]);
+ }
+ }
}
//////////////////////////////////////////////////
@@ -645,7 +803,7 @@ void MeshManager::CreateCamera(const std::string &_name, float _scale)
// Texture coords
/*float t[4][2] =
{
- {uvCoords.x, 0}, {0, 0}, {0, uvCoords.y}, {uvCoords.x, uvCoords.y}
+ {uvCoords.X(), 0}, {0, 0}, {0, uvCoords.Y()}, {uvCoords.X(), uvCoords.Y()}
};*/
// Vertices
@@ -674,7 +832,7 @@ void MeshManager::CreateCamera(const std::string &_name, float _scale)
};
// Compute the vertices
- for (i = 0; i < 8; i++)
+ for (i = 0; i < 8; ++i)
{
v[i][0] *= _scale * 0.5;
v[i][1] *= _scale * 0.5;
@@ -682,7 +840,7 @@ void MeshManager::CreateCamera(const std::string &_name, float _scale)
}
// For each face
- for (i = 0; i < 6; i++)
+ for (i = 0; i < 6; ++i)
{
// For each vertex in the face
for (k = 0; k < 4; k++)
@@ -696,7 +854,7 @@ void MeshManager::CreateCamera(const std::string &_name, float _scale)
}
// Set the indices
- for (i = 0; i < 36; i++)
+ for (i = 0; i < 36; ++i)
subMesh->AddIndex(ind[i]);
mesh->RecalculateNormals();
@@ -706,9 +864,8 @@ void MeshManager::CreateCamera(const std::string &_name, float _scale)
void MeshManager::CreateCylinder(const std::string &name, float radius,
float height, int rings, int segments)
{
- math::Vector3 vert, norm;
+ ignition::math::Vector3d vert, norm;
unsigned int verticeIndex = 0;
- unsigned int i, j;
int ring, seg;
float deltaSegAngle = (2.0 * M_PI / segments);
@@ -724,20 +881,20 @@ void MeshManager::CreateCylinder(const std::string &name, float radius,
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
-
- // Generate the group of rings for the sphere
- for (ring = 0; ring <= rings; ring++)
+ // Generate the group of rings for the cylinder
+ for (ring = 0; ring <= rings; ++ring)
{
- vert.z = ring * height/rings - height/2.0;
+ vert.Z() = ring * height/rings - height/2.0;
// Generate the group of segments for the current ring
- for (seg = 0; seg <= segments; seg++)
+ for (seg = 0; seg <= segments; ++seg)
{
- vert.y = radius * cosf(seg * deltaSegAngle);
- vert.x = radius * sinf(seg * deltaSegAngle);
+ vert.Y() = radius * cosf(seg * deltaSegAngle);
+ vert.X() = radius * sinf(seg * deltaSegAngle);
// TODO: Don't think these normals are correct.
norm = vert;
+ norm.Z() = 0;
norm.Normalize();
// Add one vertex to the strip which makes up the sphere
@@ -761,57 +918,70 @@ void MeshManager::CreateCylinder(const std::string &name, float radius,
}
}
- /// The top cap vertex
- subMesh->AddVertex(0, 0, height/2.0);
- subMesh->AddNormal(0, 0, 1);
- subMesh->AddTexCoord(0, 0);
-
- // The bottom cap vertex
- subMesh->AddVertex(0, 0, -height/2.0);
- subMesh->AddNormal(0, 0, -1);
- subMesh->AddTexCoord(0, 0);
-
- // Create the top fan
- verticeIndex += segments + 1;
- for (seg = 0; seg < segments; seg++)
+ // This block generates the top cap
{
- subMesh->AddIndex(verticeIndex);
- subMesh->AddIndex(verticeIndex - segments + seg);
- subMesh->AddIndex(verticeIndex - segments + seg - 1);
- }
+ vert.Z() = height/2.0;
+ // Generate the group of segments for the top ring
+ for (seg = 0; seg <= segments; ++seg)
+ {
+ vert.Y() = radius * cosf(seg * deltaSegAngle);
+ vert.X() = radius * sinf(seg * deltaSegAngle);
+ subMesh->AddVertex(vert);
+ subMesh->AddNormal(0, 0, 1);
+ subMesh->AddTexCoord(
+ static_cast<float>(seg) / static_cast<float>(segments), 1.0);
+ }
- // Create the bottom fan
- verticeIndex++;
- for (seg = 0; seg < segments; seg++)
- {
- subMesh->AddIndex(verticeIndex);
- subMesh->AddIndex(seg);
- subMesh->AddIndex(seg+1);
+ // The top-middle cap vertex
+ subMesh->AddVertex(0, 0, height/2.0);
+ subMesh->AddNormal(0, 0, 1);
+ subMesh->AddTexCoord(0, 0);
+
+ // Create the top fan
+ verticeIndex = subMesh->GetVertexCount()-1;
+ for (seg = 0; seg < segments; seg++)
+ {
+ subMesh->AddIndex(verticeIndex);
+ subMesh->AddIndex(verticeIndex - segments + seg);
+ subMesh->AddIndex(verticeIndex - segments + seg - 1);
+ }
}
- // Fix all the normals
- for (i = 0; i+3 < subMesh->GetIndexCount(); i+= 3)
+ // This block generates the bottom cap
{
- norm.Set();
+ vert.Z() = -height/2.0;
+ // Generate the group of segments for the bottom ring
+ for (seg = 0; seg <= segments; ++seg)
+ {
+ vert.Y() = radius * cosf(seg * deltaSegAngle);
+ vert.X() = radius * sinf(seg * deltaSegAngle);
+ subMesh->AddVertex(vert);
+ subMesh->AddNormal(0, 0, -1);
+ subMesh->AddTexCoord(
+ static_cast<float>(seg) / static_cast<float>(segments), 0.0);
+ }
- for (j = 0; j < 3; j++)
- norm += subMesh->GetNormal(subMesh->GetIndex(i+j));
+ // The bottom-middle cap vertex
+ subMesh->AddVertex(0, 0, -height/2.0);
+ subMesh->AddNormal(0, 0, -1);
+ subMesh->AddTexCoord(0, 0);
- norm /= 3;
- norm.Normalize();
-
- for (j = 0; j < 3; j++)
- subMesh->SetNormal(subMesh->GetIndex(i+j), norm);
+ // Create the bottom fan
+ verticeIndex = subMesh->GetVertexCount()-1;
+ for (seg = 0; seg < segments; seg++)
+ {
+ subMesh->AddIndex(verticeIndex);
+ subMesh->AddIndex(verticeIndex - segments + seg - 1);
+ subMesh->AddIndex(verticeIndex - segments + seg);
+ }
}
-
- mesh->RecalculateNormals();
}
//////////////////////////////////////////////////
void MeshManager::CreateCone(const std::string &name, float radius,
float height, int rings, int segments)
{
- math::Vector3 vert, norm;
+ ignition::math::Vector3d vert, norm;
unsigned int verticeIndex = 0;
unsigned int i, j;
int ring, seg;
@@ -836,15 +1006,15 @@ void MeshManager::CreateCone(const std::string &name, float radius,
// Generate the group of rings for the cone
for (ring = 0; ring < rings; ring++)
{
- vert.z = ring * height/rings - height/2.0;
+ vert.Z() = ring * height/rings - height/2.0;
- double ringRadius = ((height - (vert.z+height/2.0)) / height) * radius;
+ double ringRadius = ((height - (vert.Z()+height/2.0)) / height) * radius;
// Generate the group of segments for the current ring
for (seg = 0; seg <= segments; seg++)
{
- vert.y = ringRadius * cosf(seg * deltaSegAngle);
- vert.x = ringRadius * sinf(seg * deltaSegAngle);
+ vert.Y() = ringRadius * cosf(seg * deltaSegAngle);
+ vert.X() = ringRadius * sinf(seg * deltaSegAngle);
// TODO: Don't think these normals are correct.
norm = vert;
@@ -904,13 +1074,13 @@ void MeshManager::CreateCone(const std::string &name, float radius,
{
norm.Set();
- for (j = 0; j < 3; j++)
- norm += subMesh->GetNormal(subMesh->GetIndex(i+j));
+ for (j = 0; j < 3; ++j)
+ norm += subMesh->Normal(subMesh->GetIndex(i+j));
norm /= 3;
norm.Normalize();
- for (j = 0; j < 3; j++)
+ for (j = 0; j < 3; ++j)
subMesh->SetNormal(subMesh->GetIndex(i+j), norm);
}
@@ -918,42 +1088,42 @@ void MeshManager::CreateCone(const std::string &name, float radius,
}
//////////////////////////////////////////////////
-void MeshManager::CreateTube(const std::string &name, float innerRadius,
- float outterRadius, float height, int rings,
- int segments)
+void MeshManager::CreateTube(const std::string &_name, float _innerRadius,
+ float _outerRadius, float _height, int _rings, int _segments, double _arc)
{
- math::Vector3 vert, norm;
+ ignition::math::Vector3d vert, norm;
unsigned int verticeIndex = 0;
int ring, seg;
- float deltaSegAngle = (2.0 * M_PI / segments);
- // Needs at lest 2 rings, and 3 segments
- rings = std::max(rings, 1);
- segments = std::max(segments, 3);
+ // Needs at lest 1 ring, and 3 segments
+ int rings = std::max(_rings, 1);
+ int segments = std::max(_segments, 3);
+
+ float deltaSegAngle = (_arc / segments);
float radius = 0;
- radius = outterRadius;
+ radius = _outerRadius;
- if (this->HasMesh(name))
+ if (this->HasMesh(_name))
return;
Mesh *mesh = new Mesh();
- mesh->SetName(name);
- this->meshes.insert(std::make_pair(name, mesh));
+ mesh->SetName(_name);
+ this->meshes.insert(std::make_pair(_name, mesh));
SubMesh *subMesh = new SubMesh();
mesh->AddSubMesh(subMesh);
// Generate the group of rings for the outsides of the cylinder
- for (ring = 0; ring <= rings; ring++)
+ for (ring = 0; ring <= rings; ++ring)
{
- vert.z = ring * height/rings - height/2.0;
+ vert.Z() = ring * _height/rings - _height/2.0;
// Generate the group of segments for the current ring
- for (seg = 0; seg <= segments; seg++)
+ for (seg = 0; seg <= segments; ++seg)
{
- vert.y = radius * cosf(seg * deltaSegAngle);
- vert.x = radius * sinf(seg * deltaSegAngle);
+ vert.Y() = radius * cosf(seg * deltaSegAngle);
+ vert.X() = radius * sinf(seg * deltaSegAngle);
// TODO: Don't think these normals are correct.
norm = vert;
@@ -966,33 +1136,43 @@ void MeshManager::CreateTube(const std::string &name, float innerRadius,
static_cast<float>(seg) / static_cast<float>(segments),
static_cast<float>(ring) / static_cast<float>(rings));
+ // outer triangles connecting ring [ring] to ring [ring + 1]
if (ring != rings)
{
- // each vertex (except the last) has six indices
- subMesh->AddIndex(verticeIndex + segments + 1);
- subMesh->AddIndex(verticeIndex);
- subMesh->AddIndex(verticeIndex + segments);
- subMesh->AddIndex(verticeIndex + segments + 1);
- subMesh->AddIndex(verticeIndex + 1);
- subMesh->AddIndex(verticeIndex);
+ if (seg != 0)
+ {
+ subMesh->AddIndex(verticeIndex + segments + 1);
+ subMesh->AddIndex(verticeIndex);
+ subMesh->AddIndex(verticeIndex + segments);
+ }
+ if (seg != segments)
+ {
+ subMesh->AddIndex(verticeIndex + segments + 1);
+ subMesh->AddIndex(verticeIndex + 1);
+ subMesh->AddIndex(verticeIndex);
+ }
}
- else
+ // ring [rings] is the edge of the top cap
+ else if (seg != segments)
{
- // This indices form the top cap
+ // These indices form the top cap
subMesh->AddIndex(verticeIndex);
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex+1);
+
subMesh->AddIndex(verticeIndex+1);
subMesh->AddIndex(verticeIndex + segments + 1);
subMesh->AddIndex(verticeIndex + segments + 2);
}
- // There indices form the bottom cap
+ // ring [0] is the edge of the bottom cap
if (ring == 0 && seg < segments)
{
+ // These indices form the bottom cap
subMesh->AddIndex(verticeIndex+1);
subMesh->AddIndex(verticeIndex + (segments+1) * (((rings+1)*2)-1));
subMesh->AddIndex(verticeIndex);
+
subMesh->AddIndex(verticeIndex + (segments+1) * (((rings+1)*2)-1) + 1);
subMesh->AddIndex(verticeIndex + (segments+1) * (((rings+1)*2)-1));
subMesh->AddIndex(verticeIndex+1);
@@ -1003,16 +1183,16 @@ void MeshManager::CreateTube(const std::string &name, float innerRadius,
}
// Generate the group of rings for the inside of the cylinder
- radius = innerRadius;
- for (ring = 0; ring <= rings; ring++)
+ radius = _innerRadius;
+ for (ring = 0; ring <= rings; ++ring)
{
- vert.z = (height/2.0) - (ring * height/rings);
+ vert.Z() = (_height/2.0) - (ring * _height/rings);
// Generate the group of segments for the current ring
- for (seg = 0; seg <= segments; seg++)
+ for (seg = 0; seg <= segments; ++seg)
{
- vert.y = radius * cosf(seg * deltaSegAngle);
- vert.x = radius * sinf(seg * deltaSegAngle);
+ vert.Y() = radius * cosf(seg * deltaSegAngle);
+ vert.X() = radius * sinf(seg * deltaSegAngle);
// TODO: Don't think these normals are correct.
norm = vert;
@@ -1025,21 +1205,52 @@ void MeshManager::CreateTube(const std::string &name, float innerRadius,
static_cast<float>(seg) / static_cast<float>(segments),
static_cast<float>(ring) / static_cast<float>(rings));
+ // inner triangles connecting ring [ring] to ring [ring + 1]
if (ring != rings)
{
- // each vertex (except the last) has six indices
- subMesh->AddIndex(verticeIndex + segments + 1);
- subMesh->AddIndex(verticeIndex);
- subMesh->AddIndex(verticeIndex + segments);
-
- subMesh->AddIndex(verticeIndex + segments + 1);
- subMesh->AddIndex(verticeIndex + 1);
- subMesh->AddIndex(verticeIndex);
+ // each vertex has six indices (2 triangles)
+ if (seg != 0)
+ {
+ subMesh->AddIndex(verticeIndex + segments + 1);
+ subMesh->AddIndex(verticeIndex);
+ subMesh->AddIndex(verticeIndex + segments);
+ }
+ if (seg != segments)
+ {
+ subMesh->AddIndex(verticeIndex + segments + 1);
+ subMesh->AddIndex(verticeIndex + 1);
+ subMesh->AddIndex(verticeIndex);
+ }
}
verticeIndex++;
}
}
+ // Close ends in case it's not a full circle
+ if (!ignition::math::equal(_arc, 2.0 * M_PI))
+ {
+ for (ring = 0; ring < rings; ++ring)
+ {
+ // Close beginning
+ subMesh->AddIndex((segments+1)*(ring+1));
+ subMesh->AddIndex((segments+1)*ring);
+ subMesh->AddIndex((segments+1)*((rings+1)*2-2-ring));
+
+ subMesh->AddIndex((segments+1)*((rings+1)*2-2-ring));
+ subMesh->AddIndex((segments+1)*ring);
+ subMesh->AddIndex((segments+1)*((rings+1)*2-1-ring));
+
+ // Close end
+ subMesh->AddIndex((segments+1)*((rings+1)*2-2-ring)+segments);
+ subMesh->AddIndex((segments+1)*((rings+1)*2-1-ring)+segments);
+ subMesh->AddIndex((segments+1)*(ring+1)+segments);
+
+ subMesh->AddIndex((segments+1)*(ring+1)+segments);
+ subMesh->AddIndex((segments+1)*((rings+1)*2-1-ring)+segments);
+ subMesh->AddIndex((segments+1)*ring+segments);
+ }
+ }
+
mesh->RecalculateNormals();
}
@@ -1114,6 +1325,13 @@ void MeshManager::Tesselate2DMesh(SubMesh *sm, int meshWidth, int meshHeight,
void MeshManager::CreateBoolean(const std::string &_name, const Mesh *_m1,
const Mesh *_m2, int _operation, const math::Pose &_offset)
{
+ this->CreateBoolean(_name, _m1, _m2, _operation, _offset.Ign());
+}
+
+//////////////////////////////////////////////////
+void MeshManager::CreateBoolean(const std::string &_name, const Mesh *_m1,
+ const Mesh *_m2, int _operation, const ignition::math::Pose3d &_offset)
+{
if (this->HasMesh(_name))
return;
@@ -1124,3 +1342,54 @@ void MeshManager::CreateBoolean(const std::string &_name, const Mesh *_m1,
}
#endif
+//////////////////////////////////////////////////
+size_t MeshManager::AddUniquePointToVerticesTable(
+ std::vector<ignition::math::Vector2d> &_vertices,
+ const ignition::math::Vector2d &_p,
+ double _tol)
+{
+ double sqrTol = _tol * _tol;
+ for (auto i = 0u; i != _vertices.size(); ++i)
+ {
+ auto v = _vertices[i] - _p;
+ double d = (v.X() * v.X() + v.Y() * v.Y());
+ if ( d < sqrTol)
+ {
+ return i;
+ }
+ }
+ _vertices.push_back(_p);
+ size_t r = _vertices.size() -1;
+ return r;
+}
+
+//////////////////////////////////////////////////
+void MeshManager::ConvertPolylinesToVerticesAndEdges(
+ const std::vector<std::vector<ignition::math::Vector2d> > &_polys,
+ double _tol,
+ std::vector<ignition::math::Vector2d> &_vertices,
+ std::vector<ignition::math::Vector2i> &edges)
+{
+ for (auto poly : _polys)
+ {
+ ignition::math::Vector2d previous = poly[0];
+ for (auto i = 1u; i != poly.size(); ++i)
+ {
+ auto p = poly[i];
+ auto startPointIndex = AddUniquePointToVerticesTable(_vertices,
+ previous, _tol);
+ auto endPointIndex = AddUniquePointToVerticesTable(_vertices,
+ p, _tol);
+ // current end point is now the starting point for the next edge
+ previous = p;
+ if (startPointIndex == endPointIndex)
+ {
+ gzwarn << "Ignoring edge without 2 distinct vertices" << std::endl;
+ continue;
+ }
+ // add the new edge
+ ignition::math::Vector2i e(startPointIndex, endPointIndex);
+ edges.push_back(e);
+ }
+ }
+}
diff --git a/gazebo/common/MeshManager.hh b/gazebo/common/MeshManager.hh
index a4062cf..95654dd 100644
--- a/gazebo/common/MeshManager.hh
+++ b/gazebo/common/MeshManager.hh
@@ -14,19 +14,27 @@
* limitations under the License.
*
*/
-#ifndef _MESHMANAGER_HH_
-#define _MESHMANAGER_HH_
+#ifndef _GAZEBO_MESHMANAGER_HH_
+#define _GAZEBO_MESHMANAGER_HH_
#include <map>
+#include <utility>
#include <string>
#include <vector>
#include <boost/thread/mutex.hpp>
+#include <ignition/math/Plane.hh>
+#include <ignition/math/Matrix3.hh>
+#include <ignition/math/Matrix4.hh>
+#include <ignition/math/Vector2.hh>
+
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Vector2d.hh"
+#include "gazebo/math/Vector2i.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/math/Plane.hh"
#include "gazebo/common/SingletonT.hh"
+#include "gazebo/common/CommonTypes.hh"
#include "gazebo/util/system.hh"
namespace gazebo
@@ -45,7 +53,7 @@ namespace gazebo
/// \class MeshManager MeshManager.hh common/common.hh
/// \brief Maintains and manages all meshes
- class GAZEBO_VISIBLE MeshManager : public SingletonT<MeshManager>
+ class GZ_COMMON_VISIBLE MeshManager : public SingletonT<MeshManager>
{
/// \brief Constructor
private: MeshManager();
@@ -76,17 +84,37 @@ namespace gazebo
/// \brief Get mesh aabb and center.
/// \param[in] _mesh the mesh
/// \param[out] _center the AAB center position
+ /// \param[out] _minXYZ the bounding box minimum
+ /// \param[out] _maxXYZ the bounding box maximum
+ /// \deprecated See GetMeshAABB that accepts ignition::math::Vector3d.
+ public: void GetMeshAABB(const Mesh *_mesh,
+ math::Vector3 &_center,
+ math::Vector3 &_minXYZ,
+ math::Vector3 &_maxXYZ) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get mesh aabb and center.
+ /// \param[in] _mesh the mesh
+ /// \param[out] _center the AAB center position
/// \param[out] _min_xyz the bounding box minimum
/// \param[out] _max_xyz the bounding box maximum
public: void GetMeshAABB(const Mesh *_mesh,
- math::Vector3 &_center,
- math::Vector3 &_min_xyz,
- math::Vector3 &_max_xyz);
+ ignition::math::Vector3d &_center,
+ ignition::math::Vector3d &_min_xyz,
+ ignition::math::Vector3d &_max_xyz);
/// \brief generate spherical texture coordinates
+ /// \param[in] _mesh Pointer to the mesh
+ /// \param[in] _center Center of the mesh
+ /// \deprecated See GenSphericalTexCoord function that accepts
+ /// ignition::math::Vector3d.
public: void GenSphericalTexCoord(const Mesh *_mesh,
- math::Vector3 _center);
+ math::Vector3 _center) GAZEBO_DEPRECATED(6.0);
+ /// \brief generate spherical texture coordinates
+ /// \param[in] _mesh Pointer to the mesh
+ /// \param[in] _center Center of the mesh
+ public: void GenSphericalTexCoord(const Mesh *_mesh,
+ const ignition::math::Vector3d &_center);
/// \brief Add a mesh to the manager.
///
@@ -116,19 +144,53 @@ namespace gazebo
/// \param[in] _name the name of the new mesh
/// \param[in] _sides the x y x dimentions of eah side in meter
/// \param[in] _uvCoords the texture coordinates
+ /// \deprecated See CreateBox function that accepts
+ /// ignition::math::Vector3d and ignition::math::Vector2d.
public: void CreateBox(const std::string &_name,
- const math::Vector3 &_sides,
- const math::Vector2d &_uvCoords);
+ const math::Vector3 &_sides,
+ const math::Vector2d &_uvCoords) GAZEBO_DEPRECATED(6.0);
- /// \brief Create a Extruded Polyline mesh
+ /// \brief Create a Box mesh
/// \param[in] _name the name of the new mesh
- /// \param[in] _vertices the x y dimentions of eah vertex in meter
- /// \param[in] _height the height of the polyline
+ /// \param[in] _sides the x y x dimentions of eah side in meter
/// \param[in] _uvCoords the texture coordinates
+ public: void CreateBox(const std::string &_name,
+ const ignition::math::Vector3d &_sides,
+ const ignition::math::Vector2d &_uvCoords);
+
+ /// \brief Create an extruded mesh from polylines. The polylines are
+ /// assumed to be closed and non-intersecting. Delaunay triangulation is
+ /// applied to create the resulting mesh. If there is more than one
+ /// polyline, a ray casting algorithm will be used to identify the
+ /// exterior/interior edges and remove holes from the 2D shape before
+ /// extrusion.
+ /// \param[in] _name the name of the new mesh
+ /// \param[in] _vertices A multidimensional vector of polylines and their
+ /// vertices. Each element in the outer vector consists of a vector of
+ /// vertices that describe one polyline.
+ /// edges and remove the holes in the shape.
+ /// \param[in] _height the height of extrusion
+ /// \deprecated See CreateExtrudedPolyline that accepts
+ /// ignition::math::Vector2d.
public: void CreateExtrudedPolyline(const std::string &_name,
- const std::vector<math::Vector2d> &_vertices,
- const double &_height,
- const math::Vector2d &_uvCoords);
+ const std::vector<std::vector<math::Vector2d> > &_vertices,
+ double _height) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Create an extruded mesh from polylines. The polylines are
+ /// assumed to be closed and non-intersecting. Delaunay triangulation is
+ /// applied to create the resulting mesh. If there is more than one
+ /// polyline, a ray casting algorithm will be used to identify the
+ /// exterior/interior edges and remove holes from the 2D shape before
+ /// extrusion.
+ /// \param[in] _name the name of the new mesh
+ /// \param[in] _vertices A multidimensional vector of polylines and their
+ /// vertices. Each element in the outer vector consists of a vector of
+ /// vertices that describe one polyline.
+ /// edges and remove the holes in the shape.
+ /// \param[in] _height the height of extrusion
+ public: void CreateExtrudedPolyline(const std::string &_name,
+ const std::vector<std::vector<ignition::math::Vector2d> >
+ &_vertices, double _height);
/// \brief Create a cylinder mesh
/// \param[in] _name the name of the new mesh
@@ -164,22 +226,52 @@ namespace gazebo
/// \param[in] _height the height along z
/// \param[in] _rings the number of circles along the height
/// \param[in] _segments the number of segment per circle
+ /// \param[in] _arc the arc angle in radians
public: void CreateTube(const std::string &_name,
float _innerRadius,
float _outterRadius,
float _height,
int _rings,
- int _segments);
+ int _segments,
+ double _arc = 2.0 * M_PI);
/// \brief Create mesh for a plane
/// \param[in] _name
/// \param[in] _plane plane parameters
/// \param[in] _segments number of segments in x and y
/// \param[in] _uvTile the texture tile size in x and y
+ /// \deprecated See CreatePlane function that accepts ignition::math
+ /// objects.
public: void CreatePlane(const std::string &_name,
- const math::Plane &_plane,
- const math::Vector2d &_segments,
- const math::Vector2d &_uvTile);
+ const math::Plane &_plane,
+ const math::Vector2d &_segments,
+ const math::Vector2d &_uvTile) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Create mesh for a plane
+ /// \param[in] _name
+ /// \param[in] _plane plane parameters
+ /// \param[in] _segments number of segments in x and y
+ /// \param[in] _uvTile the texture tile size in x and y
+ public: void CreatePlane(const std::string &_name,
+ const ignition::math::Planed &_plane,
+ const ignition::math::Vector2d &_segments,
+ const ignition::math::Vector2d &_uvTile);
+
+ /// \brief Create mesh for a plane
+ /// \param[in] _name the name of the new mesh
+ /// \param[in] _normal the normal to the plane
+ /// \param[in] _d distance from the origin along normal
+ /// \param[in] _size the size of the plane in x and y
+ /// \param[in] _segments the number of segments in x and y
+ /// \param[in] _uvTile the texture tile size in x and y
+ /// \deprecated See CreatePlane function that accepts ignition::math
+ /// objects.
+ public: void CreatePlane(const std::string &_name,
+ const math::Vector3 &_normal,
+ double _d,
+ const math::Vector2d &_size,
+ const math::Vector2d &_segments,
+ const math::Vector2d &_uvTile) GAZEBO_DEPRECATED(6.0);
/// \brief Create mesh for a plane
/// \param[in] _name the name of the new mesh
@@ -189,11 +281,11 @@ namespace gazebo
/// \param[in] _segments the number of segments in x and y
/// \param[in] _uvTile the texture tile size in x and y
public: void CreatePlane(const std::string &_name,
- const math::Vector3 &_normal,
- double _d,
- const math::Vector2d &_size,
- const math::Vector2d &_segments,
- const math::Vector2d &_uvTile);
+ const ignition::math::Vector3d &_normal,
+ const double _d,
+ const ignition::math::Vector2d &_size,
+ const ignition::math::Vector2d &_segments,
+ const ignition::math::Vector2d &_uvTile);
/// \brief Tesselate a 2D mesh
///
@@ -219,11 +311,49 @@ namespace gazebo
/// \param[in] _m2 the child mesh in the boolean operation
/// \param[in] _operation the boolean operation applied to the two meshes
/// \param[in] _offset _m2's pose offset from _m1
+ /// \deprecated See CreateBoolean function that accepts
+ /// ignition::math::Pose3d.
public: void CreateBoolean(const std::string &_name, const Mesh *_m1,
const Mesh *_m2, const int _operation,
- const math::Pose &_offset = math::Pose::Zero);
+ const math::Pose &_offset = math::Pose::Zero) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Create a boolean mesh from two meshes
+ /// \param[in] _name the name of the new mesh
+ /// \param[in] _m1 the parent mesh in the boolean operation
+ /// \param[in] _m2 the child mesh in the boolean operation
+ /// \param[in] _operation the boolean operation applied to the two meshes
+ /// \param[in] _offset _m2's pose offset from _m1
+ public: void CreateBoolean(const std::string &_name, const Mesh *_m1,
+ const Mesh *_m2, const int _operation,
+ const ignition::math::Pose3d &_offset = ignition::math::Pose3d::Zero);
#endif
+ /// \brief Converts a vector of polylines into a table of vertices and
+ /// a list of edges (each made of 2 points from the table of vertices.
+ /// \param[in] _polys the polylines
+ /// \param[in] _tol tolerence for 2 vertices to be considered the same
+ /// \param[out] _vertices a table of unique vertices
+ /// \param[out] _edges a list of edges (made of start/end point indices
+ /// from the vertex table)
+ private: static void ConvertPolylinesToVerticesAndEdges(
+ const std::vector<std::vector<ignition::math::Vector2d> >
+ &_polys,
+ double _tol,
+ std::vector<ignition::math::Vector2d> &_vertices,
+ std::vector<ignition::math::Vector2i> &_edges);
+
+ /// \brief Check a point againts a list, and only adds it to the list
+ /// if it is not there already.
+ /// \param[in] _vertices the vertex table where points are stored
+ /// \param[in] _p the point coordinates
+ /// \param[in] _tol the maximum distance under which 2 points are
+ /// considered to be the same point.
+ /// \return the index of the point.
+ private: static size_t AddUniquePointToVerticesTable(
+ std::vector<ignition::math::Vector2d> &_vertices,
+ const ignition::math::Vector2d &_p,
+ double _tol);
+
/// \brief 3D mesh loader for COLLADA files
private: ColladaLoader *colladaLoader;
diff --git a/gazebo/common/MeshManager_TEST.cc b/gazebo/common/MeshManager_TEST.cc
new file mode 100644
index 0000000..66c1de0
--- /dev/null
+++ b/gazebo/common/MeshManager_TEST.cc
@@ -0,0 +1,253 @@
+/*
+ * 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/Mesh.hh"
+#include "gazebo/common/MeshManager.hh"
+#include "gazebo/gazebo_config.h"
+#include "test/util.hh"
+
+using namespace gazebo;
+
+class MeshManager : public gazebo::testing::AutoLogFixture { };
+
+#ifdef HAVE_GTS
+/////////////////////////////////////////////////
+TEST_F(MeshManager, CreateExtrudedPolyline)
+{
+ // test extrusion of a path with two subpaths:
+ // a smaller square inside a bigger square.
+ // The smaller square should be treated as a hole inside the bigger square.
+ std::vector<std::vector<ignition::math::Vector2d> > path;
+ std::vector<ignition::math::Vector2d> subpath01;
+ subpath01.push_back(ignition::math::Vector2d(0, 0));
+ subpath01.push_back(ignition::math::Vector2d(1, 0));
+ subpath01.push_back(ignition::math::Vector2d(1, 1));
+ subpath01.push_back(ignition::math::Vector2d(0, 1));
+ subpath01.push_back(ignition::math::Vector2d(0, 0));
+
+ std::vector<ignition::math::Vector2d> subpath02;
+ subpath02.push_back(ignition::math::Vector2d(0.25, 0.25));
+ subpath02.push_back(ignition::math::Vector2d(0.25, 0.75));
+ subpath02.push_back(ignition::math::Vector2d(0.75, 0.75));
+ subpath02.push_back(ignition::math::Vector2d(0.75, 0.25));
+ subpath02.push_back(ignition::math::Vector2d(0.25, 0.25));
+
+ path.push_back(subpath01);
+ path.push_back(subpath02);
+
+ std::string meshName = "extruded_path";
+ double height = 10.0;
+ common::MeshManager::Instance()->CreateExtrudedPolyline(
+ meshName, path, height);
+
+ // check mesh
+ EXPECT_TRUE(common::MeshManager::Instance()->HasMesh(meshName));
+ const common::Mesh *mesh = common::MeshManager::Instance()->GetMesh(meshName);
+ EXPECT_TRUE(mesh != NULL);
+
+ unsigned int submeshCount = mesh->GetSubMeshCount();
+ EXPECT_EQ(submeshCount, 1u);
+
+ // check submesh bounds
+ const common::SubMesh *submesh = mesh->GetSubMesh(0);
+ EXPECT_TRUE(submesh != NULL);
+ EXPECT_EQ(ignition::math::Vector3d(0, 0, 0), submesh->Min());
+ EXPECT_EQ(ignition::math::Vector3d(1.0, 1.0, 10.0), submesh->Max());
+
+ // check vertices
+ for (unsigned int i = 0; i < submesh->GetVertexCount(); ++i)
+ {
+ ignition::math::Vector3d v = submesh->Vertex(i);
+
+ // check no vertices are in the region of the hole
+ EXPECT_FALSE((v.X() > 0.25 && v.X() < 0.75));
+ EXPECT_FALSE((v.Y() > 0.25 && v.Y() < 0.75));
+
+ // check extruded height
+ EXPECT_TRUE((ignition::math::equal(v.Z(), 0.0) ||
+ ignition::math::equal(v.Z(), 10.0)));
+ }
+
+ // verify same number of normals and vertices
+ EXPECT_EQ(submesh->GetVertexCount(), submesh->GetNormalCount());
+
+ // check normals
+ for (unsigned int i = 0; i < submesh->GetNormalCount(); ++i)
+ {
+ ignition::math::Vector3d v = submesh->Vertex(i);
+ ignition::math::Vector3d n = submesh->Normal(i);
+
+ // vertex at 0 could be a bottom face or side face
+ if (ignition::math::equal(v.Z(), 0.0))
+ {
+ if (ignition::math::equal(n.Z(), 0.0))
+ {
+ // side face - check non-zero normal
+ EXPECT_TRUE(!(ignition::math::equal(n.X(), 0.0) &&
+ ignition::math::equal(n.Y(), 0.0)));
+ }
+ else
+ {
+ // bottom face - normal in -z direction
+ EXPECT_TRUE((n == -ignition::math::Vector3d::UnitZ) ||
+ (ignition::math::equal(n.Z(), 0.0)));
+ }
+ }
+
+ // vertex at height could be a top face or side face
+ if (ignition::math::equal(v.Z(), 10.0))
+ {
+ if (ignition::math::equal(n.Z(), 0.0))
+ {
+ // side face - check non-zero normal
+ EXPECT_TRUE(!(ignition::math::equal(n.X(), 0.0) &&
+ ignition::math::equal(n.Y(), 0.0)));
+ }
+ else
+ {
+ // top face - normal in +z direction
+ EXPECT_TRUE((n == ignition::math::Vector3d::UnitZ) ||
+ (ignition::math::equal(n.Z(), 0.0)));
+ }
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(MeshManager, CreateExtrudedPolylineClosedPath)
+{
+ // test extrusion of a path that has two closed subpaths, i.e.,
+ // first and last vertices are the same.
+ // The following two subpaths form the letter 'A'.
+ std::vector<std::vector<ignition::math::Vector2d> > path2;
+ std::vector<ignition::math::Vector2d> subpath03;
+ subpath03.push_back(ignition::math::Vector2d(2.27467, 1.0967));
+ subpath03.push_back(ignition::math::Vector2d(1.81094, 2.35418));
+ subpath03.push_back(ignition::math::Vector2d(2.74009, 2.35418));
+
+ std::vector<ignition::math::Vector2d> subpath04;
+ subpath04.push_back(ignition::math::Vector2d(2.08173, 0.7599));
+ subpath04.push_back(ignition::math::Vector2d(2.4693, 0.7599));
+ subpath04.push_back(ignition::math::Vector2d(3.4323, 3.28672));
+ subpath04.push_back(ignition::math::Vector2d(3.07689, 3.28672));
+ subpath04.push_back(ignition::math::Vector2d(2.84672, 2.63851));
+ subpath04.push_back(ignition::math::Vector2d(1.7077, 2.63851));
+ subpath04.push_back(ignition::math::Vector2d(1.47753, 3.28672));
+ subpath04.push_back(ignition::math::Vector2d(1.11704, 3.28672));
+
+ path2.push_back(subpath03);
+ path2.push_back(subpath04);
+
+ std::string meshName = "extruded_path_closed";
+ double height = 2.0;
+ common::MeshManager::Instance()->CreateExtrudedPolyline(
+ meshName, path2, height);
+
+ // check mesh
+ EXPECT_TRUE(common::MeshManager::Instance()->HasMesh(meshName));
+ const common::Mesh *mesh = common::MeshManager::Instance()->GetMesh(meshName);
+ EXPECT_TRUE(mesh != NULL);
+
+ unsigned int submeshCount = mesh->GetSubMeshCount();
+ EXPECT_EQ(submeshCount, 1u);
+
+ // check submesh bounds
+ const common::SubMesh *submesh = mesh->GetSubMesh(0);
+ EXPECT_TRUE(submesh != NULL);
+ EXPECT_EQ(submesh->Min(), ignition::math::Vector3d(1.11704, 0.7599, 0));
+ EXPECT_EQ(submesh->Max(), ignition::math::Vector3d(3.4323, 3.28672, 2.0));
+
+ for (unsigned int i = 0; i < submesh->GetVertexCount(); ++i)
+ {
+ ignition::math::Vector3d v = submesh->Vertex(i);
+
+ // check no vertices are in the region of the hole using a point-in-polygon
+ // algorithm
+ bool pointInPolygon = false;
+ for (unsigned int j = 0, k = subpath03.size()-1; j < subpath03.size();
+ k = ++j)
+ {
+ if (((subpath03[j].Y() > v.Y()) != (subpath03[k].Y() > v.Y())) &&
+ (v.X() < (subpath03[k].X()-subpath03[j].X()) *
+ (v.Y()-subpath03[j].Y()) /
+ (subpath03[k].Y()-subpath03[j].Y()) + subpath03[j].X()) )
+ {
+ pointInPolygon = !pointInPolygon;
+ }
+ }
+ EXPECT_FALSE(pointInPolygon);
+
+ // check extruded height
+ EXPECT_TRUE((ignition::math::equal(v.Z(), 0.0) ||
+ ignition::math::equal(v.Z(), 2.0)));
+ }
+
+ // verify same number of normals and vertices
+ EXPECT_EQ(submesh->GetVertexCount(), submesh->GetNormalCount());
+
+ // check normals
+ for (unsigned int i = 0; i < submesh->GetNormalCount(); ++i)
+ {
+ ignition::math::Vector3d v = submesh->Vertex(i);
+ ignition::math::Vector3d n = submesh->Normal(i);
+
+ // vertex at 0 could be a bottom face or side face
+ if (ignition::math::equal(v.Z(), 0.0))
+ {
+ if (ignition::math::equal(n.Z(), 0.0))
+ {
+ // side face - check non-zero normal
+ EXPECT_TRUE(!(ignition::math::equal(n.X(), 0.0) &&
+ ignition::math::equal(n.Y(), 0.0)));
+ }
+ else
+ {
+ // bottom face - normal in -z direction
+ EXPECT_TRUE((n == -ignition::math::Vector3d::UnitZ) ||
+ (ignition::math::equal(n.Z(), 0.0)));
+ }
+ }
+
+ // vertex at height could be a top face or side face
+ if (ignition::math::equal(v.Z(), 10.0))
+ {
+ if (ignition::math::equal(n.Z(), 0.0))
+ {
+ // side face - check non-zero normal
+ EXPECT_TRUE(!(ignition::math::equal(n.X(), 0.0) &&
+ ignition::math::equal(n.Y(), 0.0)));
+ }
+ else
+ {
+ // top face - normal in +z direction
+ EXPECT_TRUE((n == ignition::math::Vector3d::UnitZ) ||
+ (ignition::math::equal(n.Z(), 0.0)));
+ }
+ }
+ }
+}
+#endif
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/common/Mesh_TEST.cc b/gazebo/common/Mesh_TEST.cc
index 30c23ec..5811c00 100644
--- a/gazebo/common/Mesh_TEST.cc
+++ b/gazebo/common/Mesh_TEST.cc
@@ -140,12 +140,11 @@ TEST_F(MeshTest, Mesh)
EXPECT_EQ(static_cast<unsigned int>(24), mesh->GetTexCoordCount());
EXPECT_EQ(static_cast<unsigned int>(0), mesh->GetMaterialCount());
- math::Vector3 center, min, max;
+ ignition::math::Vector3d center, min, max;
mesh->GetAABB(center, min, max);
- EXPECT_TRUE(center == math::Vector3(0, 0, 0));
- EXPECT_TRUE(min == math::Vector3(-.5, -.5, -.5));
- EXPECT_TRUE(max == math::Vector3(.5, .5, .5));
-
+ EXPECT_TRUE(center == ignition::math::Vector3d(0, 0, 0));
+ EXPECT_TRUE(min == ignition::math::Vector3d(-.5, -.5, -.5));
+ EXPECT_TRUE(max == ignition::math::Vector3d(.5, .5, .5));
float *vertArray = NULL;
int *indArray = NULL;
@@ -189,26 +188,26 @@ TEST_F(MeshTest, Mesh)
common::SubMesh *subMesh = new common::SubMesh();
newMesh->AddSubMesh(subMesh);
- std::vector<math::Vector3> verts;
- std::vector<math::Vector3> norms;
+ std::vector<ignition::math::Vector3d> verts;
+ std::vector<ignition::math::Vector3d> norms;
EXPECT_THROW(mesh->GetSubMesh(1), common::Exception);
for (i = 0; i < 24; ++i)
{
- verts.push_back(mesh->GetSubMesh(0)->GetVertex(i));
- norms.push_back(mesh->GetSubMesh(0)->GetNormal(i));
+ verts.push_back(mesh->GetSubMesh(0)->Vertex(i));
+ norms.push_back(mesh->GetSubMesh(0)->Normal(i));
}
subMesh->CopyVertices(verts);
subMesh->CopyNormals(norms);
- EXPECT_TRUE(subMesh->HasVertex(math::Vector3(-.5, -.5, -.5)));
- EXPECT_FALSE(subMesh->HasVertex(math::Vector3(0, 0, 0)));
+ EXPECT_TRUE(subMesh->HasVertex(ignition::math::Vector3d(-.5, -.5, -.5)));
+ EXPECT_FALSE(subMesh->HasVertex(ignition::math::Vector3d(0, 0, 0)));
newMesh->GetAABB(center, min, max);
- EXPECT_TRUE(center == math::Vector3(0, 0, 0));
- EXPECT_TRUE(min == math::Vector3(-.5, -.5, -.5));
- EXPECT_TRUE(max == math::Vector3(.5, .5, .5));
+ EXPECT_TRUE(center == ignition::math::Vector3d(0, 0, 0));
+ EXPECT_TRUE(min == ignition::math::Vector3d(-.5, -.5, -.5));
+ EXPECT_TRUE(max == ignition::math::Vector3d(.5, .5, .5));
subMesh->SetVertexCount(1);
subMesh->SetIndexCount(1);
@@ -220,13 +219,13 @@ TEST_F(MeshTest, Mesh)
EXPECT_EQ(static_cast<unsigned int>(1), subMesh->GetNormalCount());
EXPECT_EQ(static_cast<unsigned int>(1), subMesh->GetTexCoordCount());
- subMesh->SetVertex(0, math::Vector3(1, 2, 3));
- EXPECT_TRUE(subMesh->GetVertex(0) == math::Vector3(1, 2, 3));
+ subMesh->SetVertex(0, ignition::math::Vector3d(1, 2, 3));
+ EXPECT_TRUE(subMesh->Vertex(0) == ignition::math::Vector3d(1, 2, 3));
- subMesh->SetTexCoord(0, math::Vector2d(.1, .2));
- EXPECT_TRUE(subMesh->GetTexCoord(0) == math::Vector2d(.1, .2));
+ subMesh->SetTexCoord(0, ignition::math::Vector2d(.1, .2));
+ EXPECT_TRUE(subMesh->TexCoord(0) == ignition::math::Vector2d(.1, .2));
- newMesh->GenSphericalTexCoord(math::Vector3(0, 0, 0));
+ newMesh->GenSphericalTexCoord(ignition::math::Vector3d(0, 0, 0));
delete newMesh;
std::ofstream stlFile((paths->GetDefaultTestPath() +
@@ -241,9 +240,9 @@ TEST_F(MeshTest, Mesh)
mesh = common::MeshManager::Instance()->Load(
paths->GetDefaultTestPath() + "/gazebo_stl_test.stl");
mesh->GetAABB(center, min, max);
- EXPECT_TRUE(center == math::Vector3(0.5, 0.5, 0.5));
- EXPECT_TRUE(min == math::Vector3(0, 0, 0));
- EXPECT_TRUE(max == math::Vector3(1, 1, 1));
+ EXPECT_TRUE(center == ignition::math::Vector3d(0.5, 0.5, 0.5));
+ EXPECT_TRUE(min == ignition::math::Vector3d(0, 0, 0));
+ EXPECT_TRUE(max == ignition::math::Vector3d(1, 1, 1));
// Cleanup test directory.
boost::filesystem::remove_all(paths->GetDefaultTestPath());
@@ -257,18 +256,18 @@ TEST_F(MeshTest, MeshMove)
common::Mesh *mesh = loader.Load(
std::string(PROJECT_SOURCE_PATH) + "/test/data/box_offset.dae");
- // The default location of the box_offest is not centered
- EXPECT_EQ(math::Vector3(5.46554, 2.18039, 4.8431), mesh->GetMax());
- EXPECT_EQ(math::Vector3(3.46555, 0.180391, 2.8431), mesh->GetMin());
+ // The default location of the box_offset is not centered
+ EXPECT_EQ(ignition::math::Vector3d(5.46554, 2.18039, 4.8431), mesh->Max());
+ EXPECT_EQ(ignition::math::Vector3d(3.46555, 0.180391, 2.8431), mesh->Min());
- mesh->Center();
+ mesh->Center(ignition::math::Vector3d::Zero);
- EXPECT_EQ(math::Vector3(1.0, 1.0, 1.0), mesh->GetMax());
- EXPECT_EQ(math::Vector3(-1.0, -1.0, -1.0), mesh->GetMin());
+ EXPECT_EQ(ignition::math::Vector3d(1.0, 1.0, 1.0), mesh->Max());
+ EXPECT_EQ(ignition::math::Vector3d(-1.0, -1.0, -1.0), mesh->Min());
- mesh->Translate(math::Vector3(1, 2, 3));
- EXPECT_EQ(math::Vector3(2.0, 3.0, 4.0), mesh->GetMax());
- EXPECT_EQ(math::Vector3(0.0, 1.0, 2.0), mesh->GetMin());
+ mesh->Translate(ignition::math::Vector3d(1, 2, 3));
+ EXPECT_EQ(ignition::math::Vector3d(2.0, 3.0, 4.0), mesh->Max());
+ EXPECT_EQ(ignition::math::Vector3d(0.0, 1.0, 2.0), mesh->Min());
}
/////////////////////////////////////////////////
@@ -280,23 +279,23 @@ TEST_F(MeshTest, SubMeshCenter)
std::string(PROJECT_SOURCE_PATH) + "/test/data/box_offset.dae");
// The default location of the box_offest is not centered
- EXPECT_EQ(math::Vector3(5.46554, 2.18039, 4.8431), mesh->GetMax());
- EXPECT_EQ(math::Vector3(3.46555, 0.180391, 2.8431), mesh->GetMin());
+ EXPECT_EQ(ignition::math::Vector3d(5.46554, 2.18039, 4.8431), mesh->Max());
+ EXPECT_EQ(ignition::math::Vector3d(3.46555, 0.180391, 2.8431), mesh->Min());
// Get the Cube submesh
common::SubMesh submesh(mesh->GetSubMesh("Cube"));
- submesh.Center(math::Vector3(1, 2, 3));
- EXPECT_EQ(math::Vector3(0, 1, 2), submesh.GetMin());
- EXPECT_EQ(math::Vector3(2, 3, 4), submesh.GetMax());
+ submesh.Center(ignition::math::Vector3d(1, 2, 3));
+ EXPECT_EQ(ignition::math::Vector3d(0, 1, 2), submesh.Min());
+ EXPECT_EQ(ignition::math::Vector3d(2, 3, 4), submesh.Max());
- submesh.Translate(math::Vector3(1, 2, 3));
- EXPECT_EQ(math::Vector3(1, 3, 5), submesh.GetMin());
- EXPECT_EQ(math::Vector3(3, 5, 7), submesh.GetMax());
+ submesh.Translate(ignition::math::Vector3d(1, 2, 3));
+ EXPECT_EQ(ignition::math::Vector3d(1, 3, 5), submesh.Min());
+ EXPECT_EQ(ignition::math::Vector3d(3, 5, 7), submesh.Max());
// The original mesh should not change
- EXPECT_EQ(math::Vector3(5.46554, 2.18039, 4.8431), mesh->GetMax());
- EXPECT_EQ(math::Vector3(3.46555, 0.180391, 2.8431), mesh->GetMin());
+ EXPECT_EQ(ignition::math::Vector3d(5.46554, 2.18039, 4.8431), mesh->Max());
+ EXPECT_EQ(ignition::math::Vector3d(3.46555, 0.180391, 2.8431), mesh->Min());
}
/////////////////////////////////////////////////
diff --git a/gazebo/common/ModelDatabase.cc b/gazebo/common/ModelDatabase.cc
index a897801..80da4d7 100644
--- a/gazebo/common/ModelDatabase.cc
+++ b/gazebo/common/ModelDatabase.cc
@@ -16,7 +16,9 @@
*/
#include <tinyxml.h>
+#ifndef _WIN32
#include <libtar.h>
+#endif
#include <curl/curl.h>
#include <fcntl.h>
#include <sys/stat.h>
@@ -290,7 +292,8 @@ void ModelDatabase::UpdateModelCache(bool _fetchImmediately)
else
{
boost::mutex::scoped_lock lock2(this->dataPtr->callbacksMutex);
-
+ if (this->dataPtr->stop)
+ break;
this->dataPtr->modelDBUpdated(this->dataPtr->modelCache);
}
this->dataPtr->updateCacheCompleteCondition.notify_all();
@@ -501,6 +504,7 @@ std::string ModelDatabase::GetModelPath(const std::string &_uri,
continue;
}
+#ifndef _WIN32
TAR *tar;
tar_open(&tar, const_cast<char*>(tarfilename.c_str()),
NULL, O_RDONLY, 0644, TAR_GNU);
@@ -512,6 +516,7 @@ std::string ModelDatabase::GetModelPath(const std::string &_uri,
path = outputPath + "/" + modelName;
ModelDatabase::DownloadDependencies(path);
+#endif
}
curl_easy_cleanup(curl);
diff --git a/gazebo/common/ModelDatabase.hh b/gazebo/common/ModelDatabase.hh
index 387f27c..9bf6edd 100644
--- a/gazebo/common/ModelDatabase.hh
+++ b/gazebo/common/ModelDatabase.hh
@@ -45,7 +45,7 @@ namespace gazebo
/// \class ModelDatabase ModelDatabase.hh common/common.hh
/// \brief Connects to model database, and has utility functions to find
/// models.
- class GAZEBO_VISIBLE ModelDatabase : public SingletonT<ModelDatabase>
+ class GZ_COMMON_VISIBLE ModelDatabase : public SingletonT<ModelDatabase>
{
/// \brief Constructor. This will update the model cache
private: ModelDatabase();
diff --git a/gazebo/common/ModelDatabasePrivate.hh b/gazebo/common/ModelDatabasePrivate.hh
index a6bf0e7..edcb343 100644
--- a/gazebo/common/ModelDatabasePrivate.hh
+++ b/gazebo/common/ModelDatabasePrivate.hh
@@ -32,7 +32,7 @@ namespace gazebo
namespace common
{
/// \brief Private class attributes for ModelDatabase.
- class GAZEBO_VISIBLE ModelDatabasePrivate
+ class GZ_COMMON_VISIBLE ModelDatabasePrivate
{
/// \brief Thread to update the model cache.
public: boost::thread *updateCacheThread;
diff --git a/gazebo/common/MouseEvent.cc b/gazebo/common/MouseEvent.cc
new file mode 100644
index 0000000..38cfa63
--- /dev/null
+++ b/gazebo/common/MouseEvent.cc
@@ -0,0 +1,203 @@
+/*
+ * 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/common/MouseEventPrivate.hh"
+#include "gazebo/common/MouseEvent.hh"
+
+using namespace gazebo;
+using namespace common;
+
+/////////////////////////////////////////////////
+MouseEvent::MouseEvent()
+ : dataPtr(new MouseEventPrivate)
+{
+}
+
+/////////////////////////////////////////////////
+MouseEvent::~MouseEvent()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+ignition::math::Vector2i MouseEvent::Pos() const
+{
+ return this->dataPtr->pos;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetPos(const ignition::math::Vector2i &_pos)
+{
+ this->dataPtr->pos = _pos;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetPos(const int _x, const int _y)
+{
+ this->dataPtr->pos.Set(_x, _y);
+}
+
+/////////////////////////////////////////////////
+ignition::math::Vector2i MouseEvent::PrevPos() const
+{
+ return this->dataPtr->prevPos;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetPrevPos(const ignition::math::Vector2i &_pos)
+{
+ this->dataPtr->prevPos = _pos;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetPrevPos(const int _x, const int _y)
+{
+ this->dataPtr->prevPos.Set(_x, _y);
+}
+
+/////////////////////////////////////////////////
+ignition::math::Vector2i MouseEvent::PressPos() const
+{
+ return this->dataPtr->pressPos;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetPressPos(const ignition::math::Vector2i &_pos)
+{
+ this->dataPtr->pressPos = _pos;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetPressPos(const int _x, const int _y)
+{
+ this->dataPtr->pressPos.Set(_x, _y);
+}
+
+/////////////////////////////////////////////////
+ignition::math::Vector2i MouseEvent::Scroll() const
+{
+ return this->dataPtr->scroll;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetScroll(const ignition::math::Vector2i &_scroll)
+{
+ this->dataPtr->scroll = _scroll;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetScroll(const int _x, const int _y)
+{
+ this->dataPtr->scroll.Set(_x, _y);
+}
+
+/////////////////////////////////////////////////
+float MouseEvent::MoveScale() const
+{
+ return this->dataPtr->moveScale;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetMoveScale(const float _scale)
+{
+ this->dataPtr->moveScale = _scale;
+}
+
+/////////////////////////////////////////////////
+bool MouseEvent::Dragging() const
+{
+ return this->dataPtr->dragging;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetDragging(const bool _dragging)
+{
+ this->dataPtr->dragging = _dragging;
+}
+
+/////////////////////////////////////////////////
+MouseEvent::EventType MouseEvent::Type() const
+{
+ return this->dataPtr->type;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetType(const EventType _type) const
+{
+ this->dataPtr->type = _type;
+}
+
+/////////////////////////////////////////////////
+MouseEvent::MouseButton MouseEvent::Button() const
+{
+ return this->dataPtr->button;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetButton(const MouseEvent::MouseButton _button) const
+{
+ this->dataPtr->button = _button;
+}
+
+/////////////////////////////////////////////////
+unsigned int MouseEvent::Buttons() const
+{
+ return this->dataPtr->buttons;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetButtons(const unsigned int &_buttons)
+{
+ this->dataPtr->buttons = _buttons;
+}
+
+/////////////////////////////////////////////////
+bool MouseEvent::Shift() const
+{
+ return this->dataPtr->shift;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetShift(const bool _shift) const
+{
+ this->dataPtr->shift = _shift;
+}
+
+/////////////////////////////////////////////////
+bool MouseEvent::Alt() const
+{
+ return this->dataPtr->alt;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetAlt(const bool _alt)
+{
+ this->dataPtr->alt = _alt;
+}
+
+/////////////////////////////////////////////////
+bool MouseEvent::Control() const
+{
+ return this->dataPtr->control;
+}
+
+/////////////////////////////////////////////////
+void MouseEvent::SetControl(const bool _control) const
+{
+ this->dataPtr->control = _control;
+}
diff --git a/gazebo/common/MouseEvent.hh b/gazebo/common/MouseEvent.hh
index 5847591..3c1e5df 100644
--- a/gazebo/common/MouseEvent.hh
+++ b/gazebo/common/MouseEvent.hh
@@ -14,74 +14,189 @@
* limitations under the License.
*
*/
-#ifndef _MOUSEEVENT_HH_
-#define _MOUSEEVENT_HH_
+#ifndef _GAZEBO_MOUSEEVENT_HH_
+#define _GAZEBO_MOUSEEVENT_HH_
-#include "gazebo/math/Vector2i.hh"
+#include <ignition/math/Vector2.hh>
#include "gazebo/util/system.hh"
namespace gazebo
{
namespace common
{
+ /// Forward declare private data
+ class MouseEventPrivate;
+
/// \addtogroup gazebo_common
/// \{
/// \class MouseEvent MouseEvent.hh common/common.hh
/// \brief Generic description of a mouse event.
- class GAZEBO_VISIBLE MouseEvent
+ class GZ_COMMON_VISIBLE MouseEvent
{
/// \brief Standard mouse buttons enumeration.
- public: enum Buttons {NO_BUTTON = 0x0, LEFT = 0x1, MIDDLE = 0x2,
- RIGHT = 0x4};
-
- /// \brief Mouse event types enumeration.
- public: enum EventType {NO_EVENT, MOVE, PRESS, RELEASE, SCROLL};
-
-
- /// \brief Constructor.
- public: MouseEvent()
- : pos(0, 0), prevPos(0, 0), pressPos(0, 0), scroll(0, 0),
- moveScale(0.01), dragging(false), type(NO_EVENT), button(0),
- buttons(NO_BUTTON), shift(false), alt(false), control(false)
- {}
+ public: enum MouseButton
+ {
+ /// \brief No button
+ NO_BUTTON = 0x0,
+ /// \brief Left button
+ LEFT = 0x1,
- /// \brief Mouse pointer position on the screen.
- public: math::Vector2i pos;
+ /// \brief Middle button
+ MIDDLE = 0x2,
- /// \brief Previous position.
- public: math::Vector2i prevPos;
+ /// \brief Right button
+ RIGHT = 0x4
+ };
- /// \brief Position of button press.
- public: math::Vector2i pressPos;
-
- /// \brief Scroll position.
- public: math::Vector2i scroll;
-
- /// \brief Scaling factor.
- public: float moveScale;
-
- /// \brief Flag for mouse drag motion
- public: bool dragging;
-
- /// \brief Event type.
- public: EventType type;
+ /// \brief Mouse event types enumeration.
+ public: enum EventType
+ {
+ /// \brief No event
+ NO_EVENT,
- /// \brief The button which caused the event.
- public: unsigned int button;
+ /// \brief Move event
+ MOVE,
- /// \brief State of the buttons when the event was generated.
- public: unsigned int buttons;
+ /// \brief Press event
+ PRESS,
- /// \brief Shift key press flag.
- public: bool shift;
+ /// \brief Release event
+ RELEASE,
- /// \brief Alt key press flag.
- public: bool alt;
+ /// \brief Scroll event
+ SCROLL
+ };
- /// \brief Control key press flag.
- public: bool control;
+ /// \brief Constructor.
+ public: MouseEvent();
+
+ /// \brief Destructor
+ public: virtual ~MouseEvent();
+
+ /// \brief Get mouse pointer position on the screen.
+ /// \return Mouse pointer position on the screen.
+ public: ignition::math::Vector2i Pos() const;
+
+ /// \brief Set mouse pointer position on the screen.
+ /// \param[in] _pos Mouse pointer position on the screen.
+ public: void SetPos(const ignition::math::Vector2i &_pos);
+
+ /// \brief Set mouse pointer position on the screen.
+ /// \param[in] _x Mouse pointer X position on the screen.
+ /// \param[in] _y Mouse pointer Y position on the screen.
+ public: void SetPos(const int _x, const int _y);
+
+ /// \brief Get the previous position.
+ /// \return The previous mouse position.
+ public: ignition::math::Vector2i PrevPos() const;
+
+ /// \brief Set the previous position.
+ /// \param[in] _pos Previous mouse pointer position on the screen.
+ public: void SetPrevPos(const ignition::math::Vector2i &_pos);
+
+ /// \brief Set the previous position.
+ /// \param[in] _x Previous mouse pointer X position on the screen.
+ /// \param[in] _y Previous mouse pointer Y position on the screen.
+ public: void SetPrevPos(const int _x, const int _y);
+
+ /// \brief Get the position of button press.
+ /// \return Screen position of a button press.
+ public: ignition::math::Vector2i PressPos() const;
+
+ /// \brief Set the position of button press.
+ /// \param[in] _pos Mouse pointer position on the screen.
+ public: void SetPressPos(const ignition::math::Vector2i &_pos);
+
+ /// \brief Set the position of button press.
+ /// \param[in] _x Mouse pointer X position on the screen.
+ /// \param[in] _y Mouse pointer Y position on the screen.
+ public: void SetPressPos(const int _x, const int _y);
+
+ /// \brief Get the scroll position.
+ /// \return The scroll position.
+ public: ignition::math::Vector2i Scroll() const;
+
+ /// \brief Set the scroll position.
+ /// \param[in] _scroll Scroll position.
+ public: void SetScroll(const ignition::math::Vector2i &_scroll);
+
+ /// \brief Set the scroll position.
+ /// \param[in] _x Scroll X position.
+ /// \param[in] _y Scroll Y position.
+ public: void SetScroll(const int _x, const int _y);
+
+ /// \brief Get the scaling factor.
+ /// \return The move scaling factor.
+ public: float MoveScale() const;
+
+ /// \brief Set the scaling factor.
+ /// \param[in] _scale The move scaling factor.
+ public: void SetMoveScale(const float _scale);
+
+ /// \brief Get the flag for mouse drag motion
+ /// \return True if dragging, usually indicating a mouse move with
+ /// mouse button pressed.
+ public: bool Dragging() const;
+
+ /// \brief Set the flag for mouse drag motion
+ /// \param[in] _dragging The dragging flag.
+ public: void SetDragging(const bool _dragging);
+
+ /// \brief Get the event type.
+ /// \return The EventType.
+ public: EventType Type() const;
+
+ /// \brief Set the event type.
+ /// \param[in] _type The EventType.
+ public: void SetType(const EventType _type) const;
+
+ /// \brief Get the button which caused this event.
+ /// \return The button which caused this event.
+ public: MouseEvent::MouseButton Button() const;
+
+ /// \brief Set the button which caused the event.
+ /// \param[in] _button The button which caused this event.
+ public: void SetButton(const MouseEvent::MouseButton _button) const;
+
+ /// \brief Get the state of the buttons when the event was generated.
+ /// \return The state of the buttons, which can be a bitwise
+ /// combination of MouseEvent::MouseButton.
+ public: unsigned int Buttons() const;
+
+ /// \brief Set the state of the buttons when the event was generated.
+ /// \param[in] _buttons The state of the buttons, which can be a bitwise
+ /// combination of MouseEvent::MouseButton.
+ public: void SetButtons(const unsigned int &_buttons);
+
+ /// \brief Get the shift key press flag.
+ /// \return True if the shift key is pressed.
+ public: bool Shift() const;
+
+ /// \brief Set the shift key press flag.
+ /// \param[in] _shift The shift key press flag.
+ public: void SetShift(const bool _shift) const;
+
+ /// \brief Get the alt key press flag.
+ /// \return True if the alt key is pressed.
+ public: bool Alt() const;
+
+ /// \brief Set the alt key press flag.
+ /// \param[in] _alt The alt key flag.
+ public: void SetAlt(const bool _alt);
+
+ /// \brief Get the control key press flag.
+ /// \return True if the control key is pressed.
+ public: bool Control() const;
+
+ /// \brief Set the control key press flag.
+ /// \param[in] _control The control key flag.
+ public: void SetControl(const bool _control) const;
+
+ /// \internal
+ /// \brief Private data pointer
+ private: MouseEventPrivate *dataPtr;
};
/// \}
}
diff --git a/gazebo/common/MouseEventPrivate.hh b/gazebo/common/MouseEventPrivate.hh
new file mode 100644
index 0000000..171b6fc
--- /dev/null
+++ b/gazebo/common/MouseEventPrivate.hh
@@ -0,0 +1,77 @@
+/*
+ * 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_MOUSEEVENT_PRIVATE_HH_
+#define _GAZEBO_MOUSEEVENT_PRIVATE_HH_
+
+#include <ignition/math/Vector2.hh>
+#include "gazebo/common/MouseEvent.hh"
+
+namespace gazebo
+{
+ namespace common
+ {
+ /// \internal
+ /// \brief Mouse event private data
+ class MouseEventPrivate
+ {
+ /// \brief Constructor.
+ public: MouseEventPrivate()
+ : pos(0, 0), prevPos(0, 0), pressPos(0, 0), scroll(0, 0),
+ moveScale(0.01), dragging(false), type(MouseEvent::NO_EVENT),
+ button(MouseEvent::NO_BUTTON), buttons(MouseEvent::NO_BUTTON),
+ shift(false), alt(false), control(false)
+ {}
+
+ /// \brief Mouse pointer position on the screen.
+ public: ignition::math::Vector2i pos;
+
+ /// \brief Previous position.
+ public: ignition::math::Vector2i prevPos;
+
+ /// \brief Position of button press.
+ public: ignition::math::Vector2i pressPos;
+
+ /// \brief Scroll position.
+ public: ignition::math::Vector2i scroll;
+
+ /// \brief Scaling factor.
+ public: float moveScale;
+
+ /// \brief Flag for mouse drag motion
+ public: bool dragging;
+
+ /// \brief Event type.
+ public: MouseEvent::EventType type;
+
+ /// \brief The button which caused the event.
+ public: MouseEvent::MouseButton button;
+
+ /// \brief State of the buttons when the event was generated.
+ public: unsigned int buttons;
+
+ /// \brief Shift key press flag.
+ public: bool shift;
+
+ /// \brief Alt key press flag.
+ public: bool alt;
+
+ /// \brief Control key press flag.
+ public: bool control;
+ };
+ }
+}
+#endif
diff --git a/gazebo/common/MovingWindowFilter_TEST.cc b/gazebo/common/MovingWindowFilter_TEST.cc
index 6f6fc33..feae7b7 100644
--- a/gazebo/common/MovingWindowFilter_TEST.cc
+++ b/gazebo/common/MovingWindowFilter_TEST.cc
@@ -16,8 +16,9 @@
*/
#include <gtest/gtest.h>
+#include <ignition/math/Vector3.hh>
+
#include "gazebo/common/MovingWindowFilter.hh"
-#include "gazebo/math/Vector3.hh"
using namespace gazebo;
@@ -39,7 +40,7 @@ TEST(MovingWindowFilterTest, FilterSomething)
{
common::MovingWindowFilter<double> doubleMWF;
common::MovingWindowFilter<double> doubleMWF2;
- common::MovingWindowFilter<math::Vector3> vectorMWF;
+ common::MovingWindowFilter<ignition::math::Vector3d> vectorMWF;
doubleMWF.SetWindowSize(10);
doubleMWF2.SetWindowSize(2);
@@ -49,9 +50,9 @@ TEST(MovingWindowFilterTest, FilterSomething)
{
doubleMWF.Update(static_cast<double>(i));
doubleMWF2.Update(static_cast<double>(i));
- math::Vector3 v(1.0*static_cast<double>(i),
- 2.0*static_cast<double>(i),
- 3.0*static_cast<double>(i));
+ ignition::math::Vector3d v(1.0*static_cast<double>(i),
+ 2.0*static_cast<double>(i),
+ 3.0*static_cast<double>(i));
vectorMWF.Update(v);
}
@@ -61,11 +62,11 @@ TEST(MovingWindowFilterTest, FilterSomething)
EXPECT_DOUBLE_EQ(doubleMWF.Get(), sum/10.0);
EXPECT_DOUBLE_EQ(doubleMWF2.Get(), (18.0+19.0)/2.0);
- math::Vector3 vsum;
+ ignition::math::Vector3d vsum;
for (unsigned int i = 0; i < 20; ++i)
- vsum += math::Vector3(1.0*static_cast<double>(i),
- 2.0*static_cast<double>(i),
- 3.0*static_cast<double>(i));
+ vsum += ignition::math::Vector3d(1.0*static_cast<double>(i),
+ 2.0*static_cast<double>(i),
+ 3.0*static_cast<double>(i));
EXPECT_EQ(vectorMWF.Get(), vsum / 20.0);
}
diff --git a/gazebo/common/PID.cc b/gazebo/common/PID.cc
index 63a4633..93a46bf 100644
--- a/gazebo/common/PID.cc
+++ b/gazebo/common/PID.cc
@@ -19,7 +19,7 @@
#include <cmath>
#include <stdio.h>
-#include "gazebo/math/Helpers.hh"
+#include <ignition/math/Helpers.hh>
#include "PID.hh"
using namespace gazebo;
@@ -112,8 +112,11 @@ double PID::Update(double _error, common::Time _dt)
double pTerm, dTerm, iTerm;
this->pErr = _error;
- if (_dt == common::Time(0, 0) || math::isnan(_error) || std::isinf(_error))
+ if (_dt == common::Time(0, 0) || ignition::math::isnan(_error) ||
+ std::isinf(_error))
+ {
return 0.0;
+ }
// Calculate proportional contribution to command
pTerm = this->pGain * this->pErr;
@@ -148,9 +151,9 @@ double PID::Update(double _error, common::Time _dt)
this->cmd = -pTerm - iTerm - dTerm;
// Check the command limits
- if (!math::equal(this->cmdMax, 0.0) && this->cmd > this->cmdMax)
+ if (!ignition::math::equal(this->cmdMax, 0.0) && this->cmd > this->cmdMax)
this->cmd = this->cmdMax;
- if (!math::equal(this->cmdMin, 0.0) && this->cmd < this->cmdMin)
+ if (!ignition::math::equal(this->cmdMin, 0.0) && this->cmd < this->cmdMin)
this->cmd = this->cmdMin;
return this->cmd;
diff --git a/gazebo/common/PID.hh b/gazebo/common/PID.hh
index 1ed910a..fab4bec 100644
--- a/gazebo/common/PID.hh
+++ b/gazebo/common/PID.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// Generic proportiolnal-integral-derivative controller class that
/// keeps track of PID-error states and control inputs given
/// the state of a system and a user specified target state.
- class GAZEBO_VISIBLE PID
+ class GZ_COMMON_VISIBLE PID
{
/// \brief Constructor, zeros out Pid values when created and
/// initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
diff --git a/gazebo/common/Plugin.hh b/gazebo/common/Plugin.hh
index b1f43c6..f1aaedd 100644
--- a/gazebo/common/Plugin.hh
+++ b/gazebo/common/Plugin.hh
@@ -17,7 +17,19 @@
#ifndef _GZ_PLUGIN_HH_
#define _GZ_PLUGIN_HH_
-#include <unistd.h>
+#ifdef _WIN32
+ // Ensure that Winsock2.h is included before Windows.h, which can get
+ // pulled in by anybody (e.g., Boost).
+ // This was put here because all the plugins are going to use it
+ // This doesn't guarantee something else won't cause it,
+ // but this saves putting this in every plugin
+#include <Winsock2.h>
+#endif
+
+#ifndef _WIN32
+ #include <unistd.h>
+#endif
+
#include <sys/types.h>
#include <sys/stat.h>
@@ -68,7 +80,7 @@ namespace gazebo
/// \class PluginT Plugin.hh common/common.hh
/// \brief A class which all plugins must inherit from
template<class T>
- class GAZEBO_VISIBLE PluginT
+ class GZ_COMMON_VISIBLE PluginT
{
/// \brief plugin pointer type definition
public: typedef boost::shared_ptr<T> TPtr;
@@ -205,7 +217,7 @@ namespace gazebo
/// \brief A plugin with access to physics::World. See
/// <a href="http://gazebosim.org/tutorials/?tut=plugins_world
/// &cat=write_plugin">reference</a>.
- class GAZEBO_VISIBLE WorldPlugin : public PluginT<WorldPlugin>
+ class GZ_COMMON_VISIBLE WorldPlugin : public PluginT<WorldPlugin>
{
/// \brief Constructor
public: WorldPlugin()
@@ -230,7 +242,7 @@ namespace gazebo
/// \brief A plugin with access to physics::Model. See
/// <a href="http://gazebosim.org/tutorials?tut=plugins_model
/// &cat=write_plugin">reference</a>.
- class GAZEBO_VISIBLE ModelPlugin : public PluginT<ModelPlugin>
+ class GZ_COMMON_VISIBLE ModelPlugin : public PluginT<ModelPlugin>
{
/// \brief Constructor
public: ModelPlugin()
@@ -259,7 +271,7 @@ namespace gazebo
/// \brief A plugin with access to physics::Sensor. See
/// <a href="http://gazebosim.org/tutorials?tut=plugins_hello_world
/// &cat=write_plugin">reference</a>.
- class GAZEBO_VISIBLE SensorPlugin : public PluginT<SensorPlugin>
+ class GZ_COMMON_VISIBLE SensorPlugin : public PluginT<SensorPlugin>
{
/// \brief Constructor
public: SensorPlugin()
@@ -288,7 +300,7 @@ namespace gazebo
/// <a href="http://gazebosim.org/tutorials?tut=system_plugin
/// &cat=write_plugin">reference</a>
/// @todo how to make doxygen reference to the file gazebo.cc#g_plugins?
- class GAZEBO_VISIBLE SystemPlugin : public PluginT<SystemPlugin>
+ class GZ_COMMON_VISIBLE SystemPlugin : public PluginT<SystemPlugin>
{
/// \brief Constructor
public: SystemPlugin()
@@ -316,7 +328,7 @@ namespace gazebo
/// \brief A plugin loaded within the gzserver on startup. See
/// <a href="http://gazebosim.org/tutorials?tut=plugins_hello_world
/// &cat=write_plugin">reference</a>.
- class GAZEBO_VISIBLE VisualPlugin : public PluginT<VisualPlugin>
+ class GZ_COMMON_VISIBLE VisualPlugin : public PluginT<VisualPlugin>
{
public: VisualPlugin()
{this->type = VISUAL_PLUGIN;}
@@ -347,8 +359,7 @@ namespace gazebo
/// to add the plugin to the registered list.
/// \return the name of the registered plugin
#define GZ_REGISTER_MODEL_PLUGIN(classname) \
- extern "C" GAZEBO_VISIBLE gazebo::ModelPlugin *RegisterPlugin(); \
- GAZEBO_VISIBLE \
+ extern "C" GZ_COMMON_VISIBLE gazebo::ModelPlugin *RegisterPlugin(); \
gazebo::ModelPlugin *RegisterPlugin() \
{\
return new classname();\
@@ -359,8 +370,8 @@ namespace gazebo
/// to add the plugin to the registered list.
/// \return the name of the registered plugin
#define GZ_REGISTER_WORLD_PLUGIN(classname) \
- extern "C" GAZEBO_VISIBLE gazebo::WorldPlugin *RegisterPlugin(); \
- GAZEBO_VISIBLE \
+ extern "C" GZ_COMMON_VISIBLE gazebo::WorldPlugin *RegisterPlugin(); \
+ GZ_COMMON_VISIBLE \
gazebo::WorldPlugin *RegisterPlugin() \
{\
return new classname();\
@@ -371,8 +382,8 @@ namespace gazebo
/// the plugin to the registered list.
/// \return the name of the registered plugin
#define GZ_REGISTER_SENSOR_PLUGIN(classname) \
- extern "C" GAZEBO_VISIBLE gazebo::SensorPlugin *RegisterPlugin(); \
- GAZEBO_VISIBLE \
+ extern "C" GZ_COMMON_VISIBLE gazebo::SensorPlugin *RegisterPlugin(); \
+ GZ_COMMON_VISIBLE \
gazebo::SensorPlugin *RegisterPlugin() \
{\
return new classname();\
@@ -383,8 +394,8 @@ namespace gazebo
/// library to add the plugin to the registered list.
/// \return the name of the registered plugin
#define GZ_REGISTER_SYSTEM_PLUGIN(classname) \
- extern "C" GAZEBO_VISIBLE gazebo::SystemPlugin *RegisterPlugin(); \
- GAZEBO_VISIBLE \
+ extern "C" GZ_COMMON_VISIBLE gazebo::SystemPlugin *RegisterPlugin(); \
+ GZ_COMMON_VISIBLE \
gazebo::SystemPlugin *RegisterPlugin() \
{\
return new classname();\
@@ -395,8 +406,8 @@ namespace gazebo
/// library to add the plugin to the registered list.
/// \return the name of the registered plugin
#define GZ_REGISTER_VISUAL_PLUGIN(classname) \
- extern "C" GAZEBO_VISIBLE gazebo::VisualPlugin *RegisterPlugin(); \
- GAZEBO_VISIBLE \
+ extern "C" GZ_COMMON_VISIBLE gazebo::VisualPlugin *RegisterPlugin(); \
+ GZ_COMMON_VISIBLE \
gazebo::VisualPlugin *RegisterPlugin() \
{\
return new classname();\
diff --git a/gazebo/common/STLLoader.cc b/gazebo/common/STLLoader.cc
index c6a1661..6699004 100644
--- a/gazebo/common/STLLoader.cc
+++ b/gazebo/common/STLLoader.cc
@@ -99,14 +99,14 @@ bool STLLoader::ReadAscii(FILE *_filein, Mesh *_mesh)
// FACET
if (this->Leqi(token, const_cast<char*>("facet")))
{
- math::Vector3 normal;
+ ignition::math::Vector3d normal;
// Get the XYZ coordinates of the normal vector to the face.
sscanf(next, "%*s %e %e %e", &r1, &r2, &r3);
- normal.x = r1;
- normal.y = r2;
- normal.z = r3;
+ normal.X(r1);
+ normal.Y(r2);
+ normal.Z(r3);
if (fgets (input, LINE_MAX_LEN, _filein) == NULL)
{
@@ -116,7 +116,7 @@ bool STLLoader::ReadAscii(FILE *_filein, Mesh *_mesh)
for (; result; )
{
- math::Vector3 vertex;
+ ignition::math::Vector3d vertex;
if (fgets (input, LINE_MAX_LEN, _filein) == NULL)
{
result = false;
@@ -128,9 +128,9 @@ bool STLLoader::ReadAscii(FILE *_filein, Mesh *_mesh)
if (count != 3)
break;
- vertex.x = r1;
- vertex.y = r2;
- vertex.z = r3;
+ vertex.X(r1);
+ vertex.Y(r2);
+ vertex.Z(r3);
subMesh->AddVertex(vertex);
subMesh->AddNormal(normal);
@@ -188,14 +188,14 @@ bool STLLoader::ReadBinary(FILE *_filein, Mesh *_mesh)
SubMesh *subMesh = new SubMesh();
// 80 byte Header.
- for (i = 0; i < 80; i++)
+ for (i = 0; i < 80; ++i)
static_cast<char>(fgetc(_filein));
// Number of faces.
face_num = this->LongIntRead(_filein);
- math::Vector3 normal;
- math::Vector3 vertex;
+ ignition::math::Vector3d normal;
+ ignition::math::Vector3d vertex;
// For each (triangular) face,
// components of normal vector,
@@ -203,39 +203,39 @@ bool STLLoader::ReadBinary(FILE *_filein, Mesh *_mesh)
// 2 byte "attribute".
for (iface = 0; iface < face_num; iface++)
{
- if (!this->FloatRead(_filein, normal.x))
+ if (!this->FloatRead(_filein, normal.X()))
return false;
- if (!this->FloatRead(_filein, normal.y))
+ if (!this->FloatRead(_filein, normal.Y()))
return false;
- if (!this->FloatRead(_filein, normal.z))
+ if (!this->FloatRead(_filein, normal.Z()))
return false;
- if (!this->FloatRead(_filein, vertex.x))
+ if (!this->FloatRead(_filein, vertex.X()))
return false;
- if (!this->FloatRead(_filein, vertex.y))
+ if (!this->FloatRead(_filein, vertex.Y()))
return false;
- if (!this->FloatRead(_filein, vertex.z))
+ if (!this->FloatRead(_filein, vertex.Z()))
return false;
subMesh->AddVertex(vertex);
subMesh->AddNormal(normal);
subMesh->AddIndex(subMesh->GetVertexCount()-1);
- if (!this->FloatRead(_filein, vertex.x))
+ if (!this->FloatRead(_filein, vertex.X()))
return false;
- if (!this->FloatRead(_filein, vertex.y))
+ if (!this->FloatRead(_filein, vertex.Y()))
return false;
- if (!this->FloatRead(_filein, vertex.z))
+ if (!this->FloatRead(_filein, vertex.Z()))
return false;
subMesh->AddVertex(vertex);
subMesh->AddNormal(normal);
subMesh->AddIndex(subMesh->GetVertexCount()-1);
- if (!this->FloatRead(_filein, vertex.x))
+ if (!this->FloatRead(_filein, vertex.X()))
return false;
- if (!this->FloatRead(_filein, vertex.y))
+ if (!this->FloatRead(_filein, vertex.Y()))
return false;
- if (!this->FloatRead(_filein, vertex.z))
+ if (!this->FloatRead(_filein, vertex.Z()))
return false;
subMesh->AddVertex(vertex);
subMesh->AddNormal(normal);
@@ -267,20 +267,20 @@ bool STLLoader::Leqi(char* _string1, char* _string2)
nchar = nchar2;
// The strings are not equal if they differ over their common length.
- for (i = 0; i < nchar; i++)
+ for (i = 0; i < nchar; ++i)
if (toupper (_string1[i]) != toupper (_string2[i]))
return false;
// The strings are not equal if the longer one includes nonblanks in the tail.
if (nchar1 > nchar)
{
- for (i = nchar; i < nchar1; i++)
+ for (i = nchar; i < nchar1; ++i)
if (_string1[i] != ' ')
return false;
}
else if (nchar2 > nchar)
{
- for (i = nchar; i < nchar2; i++)
+ for (i = nchar; i < nchar2; ++i)
if (_string2[i] != ' ')
return false;
}
@@ -297,11 +297,11 @@ int STLLoader::RcolFind(float _a[][COR3_MAX], int _m, int _n, float _r[])
icol = -1;
- for (j = 0; j < _n; j++)
+ for (j = 0; j < _n; ++j)
{
- for (i = 0; i < _m; i++)
+ for (i = 0; i < _m; ++i)
{
- if (!math::equal(_a[i][j], _r[i]))
+ if (!ignition::math::equal(_a[i][j], _r[i]))
break;
if (i == _m-1)
return j;
diff --git a/gazebo/common/STLLoader.hh b/gazebo/common/STLLoader.hh
index f72086c..90f7fd4 100644
--- a/gazebo/common/STLLoader.hh
+++ b/gazebo/common/STLLoader.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _STLLOADER_HH_
-#define _STLLOADER_HH_
+#ifndef _GAZEBO_STLLOADER_HH_
+#define _GAZEBO_STLLOADER_HH_
#include <stdint.h>
#include <string>
@@ -37,7 +37,7 @@ namespace gazebo
/// \class STLLoader STLLoader.hh common/common.hh
/// \brief Class used to load STL mesh files
- class GAZEBO_VISIBLE STLLoader : public MeshLoader
+ class GZ_COMMON_VISIBLE STLLoader : public MeshLoader
{
/// \brief Constructor
public: STLLoader();
diff --git a/gazebo/common/SVGLoader.cc b/gazebo/common/SVGLoader.cc
new file mode 100644
index 0000000..11514bd
--- /dev/null
+++ b/gazebo/common/SVGLoader.cc
@@ -0,0 +1,1003 @@
+/*
+ * 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 <algorithm>
+#include <tinyxml.h>
+#include <utility>
+
+#include <gazebo/common/Console.hh>
+#include <gazebo/common/Assert.hh>
+
+#include "SVGLoaderPrivate.hh"
+#include "SVGLoader.hh"
+
+using namespace gazebo;
+using namespace common;
+
+/////////////////////////////////////////////////
+std::string lowercase(const std::string &_in)
+{
+ std::string out = _in;
+ std::transform(out.begin(), out.end(), out.begin(), ::tolower);
+ return out;
+}
+
+/////////////////////////////////////////////////
+std::string lowercase(const char *_in)
+{
+ std::string ins = _in;
+ return lowercase(ins);
+}
+
+/////////////////////////////////////////////////
+std::vector<std::string> &split(const std::string &_s,
+ char _delim,
+ std::vector<std::string> &_elems)
+{
+ std::stringstream ss(_s);
+ std::string item;
+ while (std::getline(ss, item, _delim))
+ {
+ _elems.push_back(item);
+ }
+ return _elems;
+}
+
+/////////////////////////////////////////////////
+ignition::math::Vector2d bezierInterpolate(double _t,
+ const ignition::math::Vector2d &_p0,
+ const ignition::math::Vector2d &_p1,
+ const ignition::math::Vector2d &_p2,
+ const ignition::math::Vector2d &_p3)
+{
+ double t_1 = 1.0 - _t;
+ double t_1_2 = t_1 * t_1;
+ double t_1_3 = t_1_2 * t_1;
+ double t2 = _t * _t;
+ double t3 = t2 * _t;
+
+ ignition::math::Vector2d p;
+ p.X(t_1_3 * _p0.X() + 3 * _t * t_1_2 * _p1.X() + 3 * t2 * t_1 * _p2.X() +
+ t3 * _p3.X());
+ p.Y(t_1_3 * _p0.Y() + 3 * _t * t_1_2 * _p1.Y() + 3 * t2 * t_1 * _p2.Y() +
+ t3 * _p3.Y());
+ return p;
+}
+
+/////////////////////////////////////////////////
+void cubicBezier(const ignition::math::Vector2d &_p0,
+ const ignition::math::Vector2d &_p1,
+ const ignition::math::Vector2d &_p2,
+ const ignition::math::Vector2d &_p3,
+ double _step,
+ std::vector<ignition::math::Vector2d> &_points)
+{
+ // we don't start at t = 0, but t = step...
+ // so we assume that the first point is there (from the last move)
+ double t = _step;
+ while (t < 1.0)
+ {
+ auto p = bezierInterpolate(t, _p0, _p1, _p2, _p3);
+ _points.push_back(p);
+ t += _step;
+ }
+
+ // however we close the loop with the last point (t = 1)
+ _points.push_back(_p3);
+}
+
+/////////////////////////////////////////////////
+static double Sqr(float _x)
+{
+ return _x * _x;
+}
+
+/////////////////////////////////////////////////
+static float VecAng(float _ux, float _uy, float _vx, float _vy)
+{
+ // computes the angle between 2 vectors, using acos
+ double ux = _ux;
+ double uy = _uy;
+ double vx = _vx;
+ double vy = _vy;
+
+ double uMag = sqrt(ux * ux + uy * uy);
+ double vMag = sqrt(vx * vx + vy * vy);
+ double r = (ux * vx + uy * vy) / (uMag * vMag);
+
+ if (r < -1.0)
+ {
+ r = -1.0;
+ }
+ else if (r > 1.0)
+ {
+ r = 1.0;
+ }
+
+ double a = acos(r);
+ if (ux * vy < uy * vx)
+ {
+ return -a;
+ }
+ else
+ {
+ return a;
+ }
+}
+
+/////////////////////////////////////////////////
+void arcPath(const ignition::math::Vector2d &_p0,
+ const double _rx,
+ const double _ry,
+ const double _rotxDeg,
+ const size_t _largeArc,
+ const size_t _sweepDirection,
+ const ignition::math::Vector2d &_pEnd,
+ const double _step,
+ std::vector<ignition::math::Vector2d> &_points)
+{
+ // Ported from canvg (https://code.google.com/p/canvg/)
+ double rx = _rx;
+ double ry = _ry;
+ double rotx = _rotxDeg / 180.0 * M_PI;
+
+ double x1, y1, x2, y2, cx, cy, dx, dy, d;
+ double x1p, y1p, cxp, cyp, s, sa, sb;
+ double ux, uy, vx, vy, a1, da;
+ double px = 0, py = 0, ptanx = 0, ptany = 0, t[6];
+ double sinrx, cosrx;
+ double hda, kappa;
+
+ x1 = _p0.X();
+ y1 = _p0.Y();
+ x2 = _pEnd.X();
+ y2 = _pEnd.Y();
+
+ dx = x1 - x2;
+ dy = y1 - y2;
+ d = sqrt(dx*dx + dy*dy);
+ if (d < 1e-6 || rx < 1e-6 || ry < 1e-6)
+ {
+ // The arc degenerates to a line
+ _points.push_back(_pEnd);
+ return;
+ }
+
+ sinrx = sin(rotx);
+ cosrx = cos(rotx);
+
+ // Convert to center point parameterization.
+ // http://www.w3.org/TR/SVG11/implnote.html#ArcImplementationNotes
+ // 1) Compute x1', y1'
+ x1p = cosrx * dx / 2.0 + sinrx * dy / 2.0;
+ y1p = -sinrx * dx / 2.0 + cosrx * dy / 2.0;
+ d = Sqr(x1p) / Sqr(rx) + Sqr(y1p) / Sqr(ry);
+ if (d > 1)
+ {
+ d = sqrt(d);
+ rx *= d;
+ ry *= d;
+ }
+ // 2) Compute cx', cy'
+ s = 0.0;
+ sa = Sqr(rx) * Sqr(ry) - Sqr(rx) * Sqr(y1p) - Sqr(ry) * Sqr(x1p);
+ sb = Sqr(rx) * Sqr(y1p) + Sqr(ry) * Sqr(x1p);
+ if (sa < 0.0)
+ sa = 0.0;
+ if (sb > 0.0)
+ s = sqrt(sa / sb);
+
+ if (_largeArc == _sweepDirection)
+ {
+ s = -s;
+ }
+
+ cxp = s * rx * y1p / ry;
+ cyp = s * -ry * x1p / rx;
+
+ // 3) Compute cx,cy from cx',cy'
+ cx = (x1 + x2) / 2.0 + cosrx * cxp - sinrx * cyp;
+ cy = (y1 + y2) / 2.0 + sinrx * cxp + cosrx * cyp;
+
+ // 4) Calculate theta1, and delta theta.
+ ux = (x1p - cxp) / rx;
+ uy = (y1p - cyp) / ry;
+ vx = (-x1p - cxp) / rx;
+ vy = (-y1p - cyp) / ry;
+ // initial angle
+ a1 = VecAng(1.0, 0.0, ux, uy);
+ // delta angle
+ da = VecAng(ux, uy, vx, vy);
+
+ if (_largeArc)
+ {
+ // Choose large arc
+ if (da > 0.0)
+ da = da - 2 * M_PI;
+ else
+ da = 2 * M_PI + da;
+ }
+
+ // rounding errors for half circles
+ if (M_PI - fabs(da) < 0.001)
+ {
+ if (_sweepDirection)
+ da = M_PI;
+ else
+ da = -M_PI;
+ }
+
+ // Approximate the arc using cubic spline segments.
+ t[0] = cosrx;
+ t[1] = sinrx;
+ t[2] = -sinrx;
+ t[3] = cosrx;
+ t[4] = cx;
+ t[5] = cy;
+
+ // Split arc into max 90 degree segments.
+ // The loop assumes an iteration per end point
+ // (including start and end), this +1.
+ size_t ndivs = static_cast<int>(fabs(da) / (M_PI * 0.5) + 1.0);
+ hda = (da / ndivs) / 2.0;
+ kappa = fabs(4.0 / 3.0 * (1.0 - cos(hda)) / sin(hda));
+ if (da < 0.0)
+ kappa = -kappa;
+
+ for (size_t i = 0; i <= ndivs; ++i)
+ {
+ double x, y, tanx, tany, a;
+ a = a1 + da * (1.0 * i /ndivs);
+ dx = cos(a);
+ dy = sin(a);
+ // position xform point
+ double pox = dx * rx;
+ double poy = dy * ry;
+ x = pox * t[0] + poy * t[2] + t[4];
+ y = pox * t[1] + poy * t[3] + t[5];
+ // tangent xform vec
+ double tx = -dy * rx * kappa;
+ double ty = dx * ry * kappa;
+ tanx = tx * t[0] + ty * t[2];
+ tany = tx * t[1] + ty * t[3];
+
+ if (i > 0)
+ {
+ ignition::math::Vector2d p0(px, py);
+ ignition::math::Vector2d p1(px + ptanx, py + ptany);
+ ignition::math::Vector2d p2(x - tanx, y - tany);
+ ignition::math::Vector2d p3(x, y);
+ cubicBezier(p0, p1, p2, p3, _step, _points);
+ }
+ px = x;
+ py = y;
+ ptanx = tanx;
+ ptany = tany;
+ }
+}
+
+/////////////////////////////////////////////////
+SvgError::SvgError(const std::string &_what)
+ : std::runtime_error(_what)
+{
+}
+
+/////////////////////////////////////////////////
+ignition::math::Vector2d SVGLoader::SubpathToPolyline(
+ const std::vector<SVGCommand> &_subpath,
+ ignition::math::Vector2d _last,
+ std::vector<ignition::math::Vector2d> &_polyline)
+{
+ GZ_ASSERT(_polyline.size() == 0, "polyline not empty");
+ for (SVGCommand cmd: _subpath)
+ {
+ size_t i = 0;
+ size_t count = cmd.numbers.size();
+
+ switch (cmd.cmd)
+ {
+ case 'm':
+ case 'l':
+ while (i < count)
+ {
+ ignition::math::Vector2d p;
+ p.X(cmd.numbers[i+0]);
+ p.Y(cmd.numbers[i+1]);
+ // m and l cmds are relative to the last point
+ p.X() += _last.X();
+ p.Y() += _last.Y();
+ _polyline.push_back(p);
+ _last = p;
+ i += 2;
+ }
+ break;
+ case 'M':
+ case 'L':
+ while (i < count)
+ {
+ ignition::math::Vector2d p;
+ p.X(cmd.numbers[i+0]);
+ p.Y(cmd.numbers[i+1]);
+ _polyline.push_back(p);
+ _last = p;
+ i += 2;
+ }
+ break;
+ case 'C':
+ while (i < count)
+ {
+ ignition::math::Vector2d p0 = _last;
+ ignition::math::Vector2d p1, p2, p3;
+ p1.X(cmd.numbers[i+0]);
+ p1.Y(cmd.numbers[i+1]);
+ p2.X(cmd.numbers[i+2]);
+ p2.Y(cmd.numbers[i+3]);
+ p3.X(cmd.numbers[i+4]);
+ p3.Y(cmd.numbers[i+5]);
+ cubicBezier(p0, p1, p2, p3, this->dataPtr->resolution, _polyline);
+ _last = p3;
+ i += 6;
+ }
+ break;
+ case 'c':
+ while (i < count)
+ {
+ ignition::math::Vector2d p0 = _last;
+ ignition::math::Vector2d p1, p2, p3;
+ p1.X(cmd.numbers[i+0] + _last.X());
+ p1.Y(cmd.numbers[i+1] + _last.Y());
+ p2.X(cmd.numbers[i+2] + _last.X());
+ p2.Y(cmd.numbers[i+3] + _last.Y());
+ p3.X(cmd.numbers[i+4] + _last.X());
+ p3.Y(cmd.numbers[i+5] + _last.Y());
+ cubicBezier(p0, p1, p2, p3, this->dataPtr->resolution, _polyline);
+ _last = p3;
+ i += 6;
+ }
+ break;
+ case 'A':
+ while (i < count)
+ {
+ ignition::math::Vector2d p0 = _last;
+ double rx = cmd.numbers[i+0];
+ double ry = cmd.numbers[i+1];
+ double xRot = cmd.numbers[i+2];
+ unsigned int arc(cmd.numbers[i+3]);
+ unsigned int sweep(cmd.numbers[i+4]);
+ ignition::math::Vector2d pEnd;
+ pEnd.X(cmd.numbers[i+5]);
+ pEnd.Y(cmd.numbers[i+6]);
+ arcPath(p0, rx, ry, xRot, arc, sweep, pEnd,
+ this->dataPtr->resolution, _polyline);
+ _last = pEnd;
+ i += 7;
+ }
+ break;
+ case 'a':
+ while (i < count)
+ {
+ ignition::math::Vector2d p0 = _last;
+ double rx = cmd.numbers[i+0];
+ double ry = cmd.numbers[i+1];
+ double xRot = cmd.numbers[i+2];
+ unsigned int arc(cmd.numbers[i+3]);
+ unsigned int sweep(cmd.numbers[i+4]);
+ ignition::math::Vector2d pEnd;
+ pEnd.X(cmd.numbers[i+5] + _last.X());
+ pEnd.Y(cmd.numbers[i+6] + _last.Y());
+ arcPath(p0, rx, ry, xRot, arc, sweep, pEnd,
+ this->dataPtr->resolution, _polyline);
+ _last = pEnd;
+ i += 7;
+ }
+ // Z and z indicate closed path.
+ // just add the first point to the list
+ case 'Z':
+ case 'z':
+ {
+ auto &p = _polyline.front();
+ if (_polyline.back().Distance(p) > 1e-5)
+ {
+ gzerr << "Zz" << _polyline.back().Distance(p) << std::endl;
+ _polyline.push_back(p);
+ }
+ break;
+ }
+ default:
+ gzerr << "Unexpected SVGCommand value: " << cmd.cmd << std::endl;
+ }
+ }
+ return _last;
+}
+
+/////////////////////////////////////////////////
+SVGLoader::SVGLoader(unsigned int _samples)
+{
+ this->dataPtr = new SVGLoaderPrivate();
+ if (_samples == 0)
+ {
+ std::string m("The number of samples cannot be 0");
+ SvgError e(m);
+ throw e;
+ }
+ this->dataPtr->resolution = 1.0/_samples;
+}
+
+/////////////////////////////////////////////////
+SVGLoader::~SVGLoader()
+{
+ delete(this->dataPtr);
+}
+
+/////////////////////////////////////////////////
+void SVGLoader::SplitSubpaths(const std::vector<SVGCommand> &_cmds,
+ std::vector< std::vector<SVGCommand> > &_subpaths)
+{
+ if (_cmds.empty())
+ {
+ std::ostringstream os;
+ os << "SVGPath has no commands";
+ SvgError x(os.str());
+ throw x;
+ }
+
+ for (SVGCommand cmd: _cmds)
+ {
+ if (tolower(cmd.cmd) == 'm')
+ {
+ // the path contains a subpath
+ std::vector<SVGCommand> sub;
+ _subpaths.push_back(sub);
+ }
+ // get a reference to the latest subpath
+ std::vector<SVGCommand> &subpath = _subpaths.back();
+ // give the cmd to the latest
+ subpath.push_back(cmd);
+ }
+}
+
+/////////////////////////////////////////////////
+void SVGLoader::ExpandCommands(
+ const std::vector< std::vector<SVGCommand> > &_subpaths,
+ SVGPath &_path)
+{
+ for (std::vector<SVGCommand> compressedSubpath :_subpaths)
+ {
+ // add new subpath
+ _path.subpaths.push_back(std::vector<SVGCommand>());
+ // get a reference
+ std::vector<SVGCommand> &subpath = _path.subpaths.back();
+ // copy the cmds with repeating commands, grouping the numbers
+ for (SVGCommand xCmd : compressedSubpath)
+ {
+ unsigned int numberCount = 0;
+ if (tolower(xCmd.cmd) == 'a')
+ numberCount = 7;
+ if (tolower(xCmd.cmd) == 'c')
+ numberCount = 6;
+ if (tolower(xCmd.cmd) == 'm')
+ numberCount = 2;
+ if (tolower(xCmd.cmd) == 'l')
+ numberCount = 2;
+ if (tolower(xCmd.cmd) == 'v')
+ numberCount = 1;
+ if (tolower(xCmd.cmd) == 'h')
+ numberCount = 1;
+ if (tolower(xCmd.cmd) == 'z')
+ subpath.push_back(xCmd);
+ // group numbers together and repeat the command
+ // for each group
+ unsigned int n = 0;
+ size_t size = xCmd.numbers.size();
+ while (n < size)
+ {
+ subpath.push_back(SVGCommand());
+ SVGCommand &cmd = subpath.back();
+ cmd.cmd = xCmd.cmd;
+ for (size_t i = 0; i < numberCount; ++i)
+ {
+ cmd.numbers.push_back(xCmd.numbers[i+n]);
+ }
+ n += numberCount;
+ }
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void SVGLoader::GetPathCommands(const std::vector<std::string> &_tokens,
+ SVGPath &_path)
+{
+ std::vector <SVGCommand> cmds;
+ std::string lookup = "aAcCmMqQlLvVhHzZ";
+ char lastCmd = 'x';
+ std::vector<double> numbers;
+
+ for (std::string token: _tokens)
+ {
+ // new command?
+ if (lookup.find(token[0]) == std::string::npos)
+ {
+ // its just numbers
+ std::vector<std::string> numberStrs;
+ split(token, ',', numberStrs);
+ for (std::string numberStr : numberStrs)
+ {
+ double f = atof(numberStr.c_str());
+ numbers.push_back(f);
+ }
+ }
+ else
+ {
+ if (lastCmd != 'x')
+ {
+ SVGCommand c;
+ c.cmd = lastCmd;
+ c.numbers = numbers;
+ cmds.push_back(c);
+ }
+ // its new command
+ lastCmd = token[0];
+ numbers.resize(0);
+ }
+ }
+ // the last command
+ if (lastCmd != 'x')
+ {
+ SVGCommand c;
+ c.cmd = lastCmd;
+ c.numbers = numbers;
+ cmds.push_back(c);
+ }
+ // split the commands into sub_paths
+ std::vector< std::vector< SVGCommand> > subpaths;
+ this->SplitSubpaths(cmds, subpaths);
+ this->ExpandCommands(subpaths, _path);
+ // the starting point for the subpath
+ // it is the end point of the previous one
+ ignition::math::Vector2d p;
+ for (std::vector<SVGCommand> subpath : subpaths)
+ {
+ _path.polylines.push_back(std::vector<ignition::math::Vector2d>());
+ std::vector<ignition::math::Vector2d> &polyline = _path.polylines.back();
+ p = this->SubpathToPolyline(subpath, p, polyline);
+ }
+}
+
+/////////////////////////////////////////////////
+void SVGLoader::GetPathAttribs(TiXmlElement *_pElement, SVGPath &_path)
+{
+ GZ_ASSERT(_pElement, "empty XML element where a path was expected");
+ TiXmlAttribute *pAttrib = _pElement->FirstAttribute();
+ while (pAttrib)
+ {
+ std::string name = lowercase(pAttrib->Name());
+ std::string value = pAttrib->Value();
+ if (name == "style")
+ {
+ _path.style = value;
+ }
+ else if (name == "id")
+ {
+ _path.id = value;
+ }
+ else if (name == "transform")
+ {
+ _path.transform = value;
+ gzwarn << "transform attribute \"" << name
+ << "\" not implemented yet" << std::endl;
+ }
+ else if (name == "d")
+ {
+ // this attribute contains a list of coordinates
+ std::vector<std::string> tokens;
+ split(value, ' ', tokens);
+ this->GetPathCommands(tokens, _path);
+ }
+ else
+ {
+ gzwarn << "Ignoring attribute \"" << name << "\" in path" << std::endl;
+ }
+ pAttrib = pAttrib->Next();
+ }
+}
+
+/////////////////////////////////////////////////
+void SVGLoader::GetSvgPaths(TiXmlNode *_pParent, std::vector<SVGPath> &_paths)
+{
+ if (!_pParent)
+ return;
+
+ TiXmlNode *pChild;
+ int t = _pParent->Type();
+ std::string name;
+ if ( t == TiXmlNode::TINYXML_ELEMENT)
+ {
+ name = lowercase(_pParent->Value());
+ if (name == "path")
+ {
+ TiXmlElement *element = _pParent->ToElement();
+ SVGPath p;
+ this->GetPathAttribs(element, p);
+ _paths.push_back(p);
+ }
+ // skip defs node that can contain path
+ // elements that are not actual paths.
+ if (name == "defs")
+ {
+ return;
+ }
+ }
+
+ for (pChild = _pParent->FirstChild();
+ pChild != 0;
+ pChild = pChild->NextSibling())
+ {
+ this->GetSvgPaths(pChild, _paths);
+ }
+}
+
+/////////////////////////////////////////////////
+bool SVGLoader::Parse(const std::string &_filename,
+ std::vector<SVGPath> &_paths)
+{
+ try
+ {
+ // load the named file and dump its structure to STDOUT
+ TiXmlDocument doc(_filename.c_str());
+ if (!doc.LoadFile())
+ {
+ std::ostringstream os;
+ gzerr << "Failed to load file " << _filename << std::endl;
+ gzerr << os.str() << std::endl;
+ return false;
+ }
+ this->GetSvgPaths(&doc, _paths);
+ return true;
+ }
+ catch(SvgError &e)
+ {
+ gzerr << e.what() << std::endl;
+ }
+ return false;
+}
+
+/////////////////////////////////////////////////
+void SVGLoader::DumpPaths(const std::vector<SVGPath> &_paths,
+ std::ostream &_out) const
+{
+ // this prints an html document that allows to debug
+ // SVG parsing issues. The points are generated in
+ // a loop between the header and footer.
+ std::string header = R"***(
+<!DOCTYPE html>
+<html>
+
+<script type="text/javascript">
+
+)***";
+ std::string footer = R"***(
+</script>
+
+<script>
+
+var x0 = 0;
+var y0 = 0;
+var scale = 1.;
+
+function xx(x)
+{
+ var r = x0 + scale * x;
+ return r;
+}
+
+function yy(y)
+{
+ var r = - (y0 + scale * (-y) );
+ return r;
+}
+
+function drawPoint(ctx, x, y)
+{
+ ctx.beginPath();
+ ctx.arc(x, y, 5, 0, 2 * Math.PI, true);
+ ctx.strokeStyle= style;
+ ctx.stroke();
+}
+
+function drawPath(ctx, path, style, x0, y0, scale, showCtrlPoints )
+{
+ console.log('drawPath ' + path.name);
+
+ ctx.beginPath();
+ for (var j = 0; j < path.subpaths.length; ++j)
+ {
+ var points = path.subpaths[j];
+ console.log(points.length + ' points in subpath, (' + style + ')');
+ if (points.length < 2)
+ {
+ console.log("not enough points in subpath " + j);
+ return;
+ }
+ ctx.moveTo(xx(points[0][0]), yy(points[0][1]));
+ for (var i = 1; i < points.length; ++i)
+ {
+ var x= xx(points[i][0]);
+ var y= yy(points[i][1]);
+ ctx.lineTo(x, y);
+ }
+ ctx.strokeStyle= style;
+ ctx.stroke();
+
+ // draw points
+ if (showCtrlPoints)
+ {
+ var styles = ["black", "orange", "grey"];
+ for (var i = 0; i < points.length; ++i)
+ {
+ var x= xx(points[i][0]);
+ var y= yy(points[i][1]);
+ var m = " [" + points[i][0] + ", " + points[i][1];
+ m += "] [" + x + ", " + y + "]";
+ console.log(m);
+ ctx.beginPath();
+ if (i == 0)
+ {
+ ctx.arc(x, y, 4, 0, 2 * Math.PI, true);
+ ctx.strokeStyle = "red";
+ ctx.fill();
+ }
+ else if (i == 1)
+ {
+ ctx.arc(x, y, 2, 0, 2 * Math.PI, true);
+ ctx.strokeStyle= "red";
+ }
+ else
+ {
+ ctx.arc(x, y, 2, 0, 2 * Math.PI, true);
+ ctx.strokeStyle= styles[i % styles.length ];
+ }
+ ctx.stroke();
+ }
+ }
+ }
+}
+
+
+function draw(showCtrlPoints)
+{
+ var canvas = document.getElementById("myCanvas");
+ var ctx = canvas.getContext("2d");
+ var styles = ["red", "green", "blue"];
+
+ ctx.clearRect(0, 0, canvas.width, canvas.height);
+ x0 = Number(document.getElementsByName("xoff_in")[0].value);
+ y0 = Number(document.getElementsByName("yoff_in")[0].value);
+ scale = Number(document.getElementsByName("scale_in")[0].value);
+
+ for (var i =0; i < svg.length; ++i)
+ {
+ var path = svg[i];
+ console.log("path: " + path.name);
+ drawPath(ctx, path, styles[i%3], x0, y0, scale, showCtrlPoints);
+ }
+}
+
+ console.log("number of paths: " + svg.length);
+
+ document.addEventListener("DOMContentLoaded", function(event)
+ {
+ draw();
+ });
+
+</script>
+
+
+<body>
+
+ <div>
+
+ Xoff: <input type="text" name="xoff_in" value="0"><br>
+ Yoff: <input type="text" name="yoff_in" value="0"><br>
+ Scale: <input type="text" name="scale_in" value="1.0"><br>
+
+ <button onclick="draw(true);">Draw</button>
+ </div>
+
+ <canvas
+ id="myCanvas"
+ width="1024"
+ height="768"
+ style="border:1px solid #d3d3d3;">
+ Your browser does not support the canvas element.
+ </canvas>
+
+</body>
+</html>
+
+)***";
+
+ _out << header << std::endl;
+ _out << "var svg = [];" << std::endl;
+ for (SVGPath path : _paths)
+ {
+ _out << "svg.push({name:\"" << path.id;
+ _out << "\", subpaths:[], style: \"";
+ _out << path.style << "\"}); " << std::endl;
+ _out << "svg[svg.length-1].subpaths = [";
+ char psep = ' ';
+
+ for (unsigned int i = 0; i < path.polylines.size(); ++i)
+ {
+ std::vector<ignition::math::Vector2d> poly = path.polylines[i];
+ _out << psep << "[" << std::endl;
+ psep = ',';
+ char sep = ' ';
+ for (ignition::math::Vector2d p : poly)
+ {
+ _out << " " << sep << " [" << p.X() << ", "
+ << p.Y() << "]" <<std::endl;
+ sep = ',';
+ }
+ _out << " ] " << std::endl;
+ }
+ _out << "];" << std::endl;
+ _out << "\n\n";
+ }
+ _out << footer << std::endl;
+}
+
+/////////////////////////////////////////////////
+bool Vector2dCompare(const ignition::math::Vector2d &_a,
+ const ignition::math::Vector2d &_b,
+ double _tol)
+{
+ double x = _a.X() - _b.X();
+ double y = _a.Y() - _b.Y();
+ // is squared distance smaller than squared tolerance?
+ return (x*x + y*y < _tol * _tol);
+}
+
+/////////////////////////////////////////////////
+void SVGLoader::PathsToClosedPolylines(
+ const std::vector<common::SVGPath> &_paths,
+ double _tol,
+ std::vector< std::vector<math::Vector2d> > &_closedPolys,
+ std::vector< std::vector<math::Vector2d> > &_openPolys)
+{
+ std::vector<std::vector<ignition::math::Vector2d>> closedPolys;
+ std::vector<std::vector<ignition::math::Vector2d>> openPolys;
+
+ for (auto const &closed : _closedPolys)
+ {
+ std::vector<ignition::math::Vector2d> vecs;
+ for (auto const &v : closed)
+ vecs.push_back(v.Ign());
+ closedPolys.push_back(vecs);
+ }
+
+ for (auto const &open : _openPolys)
+ {
+ std::vector<ignition::math::Vector2d> vecs;
+ for (auto const &v : open)
+ vecs.push_back(v.Ign());
+ openPolys.push_back(vecs);
+ }
+
+ PathsToClosedPolylines(_paths, _tol, closedPolys, openPolys);
+}
+
+/////////////////////////////////////////////////
+void SVGLoader::PathsToClosedPolylines(
+ const std::vector<common::SVGPath> &_paths,
+ double _tol,
+ std::vector< std::vector<ignition::math::Vector2d> > &_closedPolys,
+ std::vector< std::vector<ignition::math::Vector2d> > &_openPolys)
+{
+ // first we extract all polyline into a vector of line segments
+ std::list<std::pair<ignition::math::Vector2d,
+ ignition::math::Vector2d> > segments;
+
+ for (auto const &path : _paths)
+ {
+ for (auto const &poly : path.polylines)
+ {
+ ignition::math::Vector2d startPoint = poly[0];
+ for (unsigned int i =1; i < poly.size(); ++i)
+ {
+ const ignition::math::Vector2d &endPoint = poly[i];
+ double length = endPoint.Distance(startPoint);
+ if (length < _tol)
+ {
+ gzmsg << "Ignoring short segment (length: "
+ << length << ")" <<std::endl;
+ }
+ else
+ {
+ segments.push_back(std::make_pair(startPoint, endPoint));
+ startPoint = endPoint;
+ }
+ }
+ }
+ }
+
+ // then we remove segments until there are none left
+ while (!segments.empty())
+ {
+ // start a new polyline, made from the 2 points of
+ // the next available segment.
+ std::vector<ignition::math::Vector2d> polyline;
+ auto &s = segments.front();
+ polyline.push_back(s.first);
+ polyline.push_back(s.second);
+ // remove the segment from the list
+ segments.pop_front();
+ // this flag will be false if the polyline has no
+ // new segment
+ bool segmentFound = true;
+ // this flag is true when the polyline is closed
+ bool loopClosed = false;
+ while (segmentFound && !loopClosed)
+ {
+ // find the segment in the polyline
+ segmentFound = false;
+ for (auto it = segments.begin(); it != segments.end(); ++it)
+ {
+ auto seg = *it;
+ ignition::math::Vector2d nextPoint;
+ if (Vector2dCompare(polyline.back(), seg.first, _tol))
+ {
+ nextPoint = seg.second;
+ segmentFound = true;
+ }
+ if (Vector2dCompare(polyline.back(), seg.second, _tol))
+ {
+ nextPoint = seg.first;
+ segmentFound = true;
+ }
+ if (segmentFound)
+ {
+ // remove the segment from the list of all remaining segments
+ segments.erase(it);
+ // add the new point to the polyline
+ polyline.push_back(nextPoint);
+ // verify if the polyline is closed
+ if (Vector2dCompare(nextPoint, polyline[0], _tol))
+ {
+ // the loop is closed, we don't need another segment
+ loopClosed = true;
+ }
+ // the segment has been found
+ // get out of the for loop.
+ break;
+ }
+ }
+ }
+ // the new polyline is complete
+ if (loopClosed)
+ {
+ _closedPolys.push_back(polyline);
+ }
+ else
+ {
+ gzmsg << "Line segments that are not part of a closed paths have"
+ << " been found with the current minimum distance of " << _tol
+ << " between 2 points." << std::endl << std::endl;
+ _openPolys.push_back(polyline);
+ }
+ }
+}
diff --git a/gazebo/common/SVGLoader.hh b/gazebo/common/SVGLoader.hh
new file mode 100644
index 0000000..f50cae6
--- /dev/null
+++ b/gazebo/common/SVGLoader.hh
@@ -0,0 +1,188 @@
+/*
+ * 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_SVGLOADER_HH_
+#define _GAZEBO_SVGLOADER_HH_
+
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <ignition/math/Vector2.hh>
+#include <ignition/math/Matrix3.hh>
+
+#include <gazebo/common/Console.hh>
+#include <gazebo/math/Vector2d.hh>
+#include <gazebo/math/Matrix3.hh>
+
+class TiXmlElement;
+class TiXmlNode;
+
+namespace gazebo
+{
+ namespace common
+ {
+ class SVGLoaderPrivate;
+
+ /// \brief Handles errors during SVG parsing
+ class GZ_COMMON_VISIBLE SvgError: public std::runtime_error
+ {
+ /// \brief constructor
+ /// \param[in] _what The error description
+ public: SvgError(const std::string &_what);
+ };
+
+ /// \brief SVG command data structure
+ class GZ_COMMON_VISIBLE SVGCommand
+ {
+ /// \brief Constructor
+ public: SVGCommand() : cmd(' ') {}
+
+ /// \brief Destructor
+ public: virtual ~SVGCommand() = default;
+
+ /// \brief A letter that describe the segment
+ public: char cmd;
+
+ /// \brief Coordinates for the command
+ public: std::vector<double> numbers;
+ };
+
+ /// \brief An SVG path element data structure
+ struct GZ_COMMON_VISIBLE SVGPath
+ {
+ /// \brief An id or name
+ std::string id;
+
+ /// \brief The style (i.e. stroke style, color, thickness etc)
+ std::string style;
+
+ /// \brief A 2D transform (or a list of transforms)
+ std::string transform;
+
+ /// \brief A list of subpaths (as lists of commands)
+ std::vector< std::vector<SVGCommand> > subpaths;
+
+ /// \brief The polylines described by the commands
+ std::vector< std::vector<ignition::math::Vector2d> > polylines;
+ };
+
+ /// \brief A loader for SVG files
+ class GZ_COMMON_VISIBLE SVGLoader
+ {
+ /// \brief Constructor
+ /// \param[in] _samples The number of points for cubic spline segments
+ public: SVGLoader(unsigned int _samples);
+
+ /// \brief destructor
+ public: ~SVGLoader();
+
+ /// \brief Reads an SVG file and loads all the paths
+ /// \param[in] _filename The SVG file
+ /// \param[out] _paths Vector that receives path datai
+ /// \return false when the file cannot be processed
+ public: bool Parse(const std::string &_filename,
+ std::vector<SVGPath> &_paths);
+
+ /// \brief Reads in paths and outputs closed polylines and open polylines
+ /// \param[in] _paths The input paths
+ /// \param[in] _tol Tolerence when comparing distance between 2 points.
+ /// \param[out] _closedPolys A vector to collect new closed loops
+ /// \param[out] _openPolys A vector to collect non closed paths
+ /// \deprecated See PathsToClosedPolylines that accepts ignition::math
+ /// objects
+ public: static void PathsToClosedPolylines(
+ const std::vector<common::SVGPath> &_paths,
+ double _tol,
+ std::vector< std::vector<math::Vector2d> > &_closedPolys,
+ std::vector< std::vector<math::Vector2d> > &_openPolys)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Reads in paths and outputs closed polylines and open polylines
+ /// \param[in] _paths The input paths
+ /// \param[in] _tol Tolerence when comparing distance between 2 points.
+ /// \param[out] _closedPolys A vector to collect new closed loops
+ /// \param[out] _openPolys A vector to collect non closed paths
+ public: static void PathsToClosedPolylines(
+ const std::vector<common::SVGPath> &_paths,
+ double _tol,
+ std::vector< std::vector<ignition::math::Vector2d> > &_closedPolys,
+ std::vector< std::vector<ignition::math::Vector2d> > &_openPolys);
+
+ /// \brief Outputs the content of the paths to file (or console)
+ /// \param[in] _paths The paths
+ /// \param[in] _out The output stream (can be a file or std::cout)
+ public: void DumpPaths(const std::vector<SVGPath> &_paths,
+ std::ostream &_out) const;
+
+ /// \brief Parses a list of strings into a path
+ /// \param[in] _tokens The tokenized path attribute from SVG file
+ /// \param[out] _path The path that receives the data
+ private: void GetPathCommands(const std::vector<std::string> &_tokens,
+ SVGPath &_path);
+
+ /// \brief Gets data from an XML path element
+ /// \param[in] _pElement The path Element
+ /// \param[out] _path The path that receives the data.
+ private: void GetPathAttribs(TiXmlElement *_pElement, SVGPath &_path);
+
+ /// \brief Reads the paths from the root XML element
+ /// \param[in] _pParent The parent XML node of the SVG file
+ /// \param[out] _paths The vector of paths that receives the data
+ private: void GetSvgPaths(TiXmlNode *_pParent,
+ std::vector<SVGPath> &_paths);
+
+ /// \brief Generates new commands for every repeat commands in subpaths
+ /// \param[in] _subpaths The subpaths (with repeats)
+ /// \param[out] _path The path that receives the data.
+ private: void ExpandCommands(
+ const std::vector< std::vector<SVGCommand> > &_subpaths,
+ SVGPath& _path);
+
+ /// \brief Splits a list of commands into subpaths
+ /// \param[in] _cmds The flat list of commands for all the subpaths
+ /// \param[out] _subpaths The vector of subpathts that receives the data
+ /// \throws SvgError
+ private: void SplitSubpaths(const std::vector<SVGCommand> &_cmds,
+ std::vector< std::vector<SVGCommand> > &_subpaths);
+
+ /// \brief Generates a list of polylines from a path
+ /// \param[in] _path The path
+ /// \param[in] _resolution The step size (between 0 and 1)
+ /// \param[out] _polys The vector that receives the polylines
+ private: void PathToPoints(const SVGPath &_path,
+ double _resolution,
+ std::vector< std::vector<ignition::math::Vector2d> > &_polys);
+
+ /// \brief Generates polylines for each SVG subpath
+ /// \param[in] _subpath The subpath commands
+ /// \param[in] _last The previous position (for relative path commands)
+ /// \param[out] _polyline The polyline that receives the data
+ /// \return The last point of the subpath
+ private: ignition::math::Vector2d SubpathToPolyline(
+ const std::vector<SVGCommand> &_subpath,
+ ignition::math::Vector2d _last,
+ std::vector<ignition::math::Vector2d> &_polyline);
+
+ /// \internal
+ /// \brief Pointer to private data
+ private: SVGLoaderPrivate *dataPtr;
+ };
+ }
+}
+
+#endif
diff --git a/gazebo/common/SVGLoaderPrivate.hh b/gazebo/common/SVGLoaderPrivate.hh
new file mode 100644
index 0000000..34d53ed
--- /dev/null
+++ b/gazebo/common/SVGLoaderPrivate.hh
@@ -0,0 +1,43 @@
+/*
+ * 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_SVGLOADER_PRIVATE_HH_
+#define _GAZEBO_SVGLOADER_PRIVATE_HH_
+
+#include <string>
+
+#include <gazebo/math/Vector2d.hh>
+#include <gazebo/math/Matrix3.hh>
+
+class TiXmlElement;
+class TiXmlNode;
+
+namespace gazebo
+{
+ namespace common
+ {
+ class SVGLoaderPrivate
+ {
+ /// \brief The step distance between 2 sampled points in a bezier curve
+ /// It is the inverse of the number of samples in the spline, and should
+ /// be between 0 and 1
+ public: double resolution;
+ };
+ }
+}
+
+#endif
diff --git a/gazebo/common/SVGLoader_TEST.cc b/gazebo/common/SVGLoader_TEST.cc
new file mode 100644
index 0000000..0bb5deb
--- /dev/null
+++ b/gazebo/common/SVGLoader_TEST.cc
@@ -0,0 +1,218 @@
+/*
+ * 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/SVGLoader.hh"
+
+#include "test/util.hh"
+
+using namespace gazebo;
+
+class SVGLoader : public gazebo::testing::AutoLogFixture { };
+
+
+// tolerance
+// minimum distance between 2 distinct points
+double tol = 0.05;
+
+/////////////////////////////////////////////////
+TEST_F(SVGLoader, LoadPaths)
+{
+ common::SVGLoader loader(10);
+ std::vector<common::SVGPath> paths;
+
+ // bad path
+ bool success;
+ std::string bad = "/not/a/file.svg";
+ success = loader.Parse(bad, paths);
+ EXPECT_FALSE(success);
+
+ std::string filePath = std::string(PROJECT_SOURCE_PATH);
+ filePath += "/test/data/svg/loader.svg";
+ loader.Parse(filePath, paths);
+
+ // useful to see the points on screen
+ // loader.DumpPaths(paths, std::cout);
+
+ // or in a file
+ std::string foutput = "";
+ if (!foutput.empty())
+ {
+ std::ofstream out(foutput.c_str() );
+ loader.DumpPaths(paths, out);
+ out.close();
+ }
+
+ // the test file has 3 paths inside
+ EXPECT_EQ(3u, paths.size());
+ common::SVGPath &a = paths[0];
+ EXPECT_EQ("letterA", a.id);
+
+ // the letter A has 2 subpaths:
+ EXPECT_EQ(2u, a.subpaths.size());
+
+ // The hole of A
+ // 4 commands
+ EXPECT_EQ(4u, a.subpaths[0].size());
+ // 4 points
+ EXPECT_EQ(4u, a.polylines[0].size());
+ // The A contour has 9
+ EXPECT_EQ(9u, a.polylines[1].size());
+
+ // see what's going on
+ loader.DumpPaths(paths, std::cout);
+
+ // the second path
+ common::SVGPath &p2 = paths[1];
+ EXPECT_EQ(1u, p2.subpaths.size());
+ EXPECT_EQ("path2984", p2.id);
+
+ // 8 commands
+ EXPECT_EQ(8u, p2.subpaths[0].size());
+ // since it has splines, there are more
+ // points than commands
+ EXPECT_EQ(68u, p2.polylines[0].size());
+}
+
+/////////////////////////////////////////////////
+TEST_F(SVGLoader, LoadArcs)
+{
+ // check for arc command support
+ // this test loads a file with 2 superimposed 180 degree arc:
+ // * drawn from right to left
+ // * with a round top (looks like an igloo, not a bowl)
+ // therefore, y DECREASES (y is down in svg)
+ //
+ // This test ensures that both path coincide (and not form a circle)
+
+ common::SVGLoader loader(3);
+ std::vector<common::SVGPath> paths;
+
+ std::string filePath = std::string(PROJECT_SOURCE_PATH);
+ filePath += "/test/data/svg/arc_test.svg";
+ bool success = loader.Parse(filePath, paths);
+ EXPECT_EQ(true, success);
+
+ // the test file has 2 paths inside
+ EXPECT_EQ(2u, paths.size());
+ // each path has the same number of points
+ EXPECT_EQ(1u, paths[0].polylines.size());
+ EXPECT_EQ(1u, paths[1].polylines.size());
+ auto &polyline1 = paths[0].polylines[0];
+ auto &polyline2 = paths[1].polylines[0];
+
+ for (auto i = 0u; i != polyline1.size(); ++i)
+ {
+ // extract points
+ auto p1 = polyline1[i];
+ auto p2 = polyline2[i];
+
+ EXPECT_NEAR(p1.X(), p2.X(), 0.25);
+ EXPECT_NEAR(p1.Y(), p2.Y(), 0.25);
+ }
+
+ std::ofstream out("arc_test.html");
+ loader.DumpPaths(paths, out);
+ out.close();
+}
+
+/////////////////////////////////////////////////
+TEST_F(SVGLoader, Capsule)
+{
+ // check for arc command support
+ // this test loads a file with 2 superimposed 180 degree arc:
+ // * drawn from right to left
+ // * with a round top (looks like an igloo, not a bowl)
+ // therefore, y DECREASES (y is down in svg)
+ //
+ // This test ensures that both path coincide (and not form a circle)
+
+ common::SVGLoader loader(3);
+ std::vector<common::SVGPath> paths;
+
+ std::string filePath = std::string(PROJECT_SOURCE_PATH);
+ filePath += "/test/data/svg/capsule.svg";
+
+ bool success = loader.Parse(filePath, paths);
+ EXPECT_EQ(true, success);
+
+ // the test file has 2 paths inside
+ std::ofstream out("capsule.html");
+ loader.DumpPaths(paths, out);
+ out.close();
+}
+
+/////////////////////////////////////////////////
+TEST_F(SVGLoader, ghost_edges)
+{
+ // check for invalid edges in svg
+ // this test loads a file with a circle made of 2 arcs:
+ //
+ // The resulting polyline should not have duplicate points
+
+ common::SVGLoader loader(3);
+ std::vector<common::SVGPath> paths;
+
+ std::string filePath = std::string(PROJECT_SOURCE_PATH);
+ filePath += "/test/data/svg/arc_circle.svg";
+ bool success = loader.Parse(filePath, paths);
+
+ // save for inspection
+ std::ofstream out("arc_circle.html");
+ loader.DumpPaths(paths, out);
+ out.close();
+
+ EXPECT_EQ(true, success);
+ // the test file has 2 paths inside
+ EXPECT_EQ(1u, paths.size());
+ EXPECT_EQ(1u, paths[0].polylines.size());
+}
+
+/////////////////////////////////////////////////
+TEST_F(SVGLoader, ClosedLoops)
+{
+ // this tests the PathsToClosedPolylines function
+ common::SVGLoader loader(3);
+ std::vector<common::SVGPath> paths;
+ std::string filePath = std::string(PROJECT_SOURCE_PATH);
+ filePath += "/test/data/svg/chassis.svg";
+ bool success = loader.Parse(filePath, paths);
+ EXPECT_EQ(true, success);
+
+ // save for inspection
+ std::ofstream out("chassis.html");
+ loader.DumpPaths(paths, out);
+ out.close();
+
+
+ std::vector< std::vector<ignition::math::Vector2d> > closedPolys;
+ std::vector< std::vector<ignition::math::Vector2d> > openPolys;
+
+ loader.PathsToClosedPolylines(paths, tol, closedPolys, openPolys);
+ EXPECT_EQ(0u, openPolys.size());
+ EXPECT_EQ(23u, closedPolys.size());
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/common/SingletonT.hh b/gazebo/common/SingletonT.hh
index 50098a9..7cded34 100644
--- a/gazebo/common/SingletonT.hh
+++ b/gazebo/common/SingletonT.hh
@@ -30,7 +30,7 @@
/// \class SingletonT SingletonT.hh common/common.hh
/// \brief Singleton template class
template <class T>
-class GAZEBO_VISIBLE SingletonT
+class GZ_COMMON_VISIBLE SingletonT
{
/// \brief Get an instance of the singleton
public: static T *Instance()
@@ -57,7 +57,6 @@ class GAZEBO_VISIBLE SingletonT
/// \brief Initialization of the singleton instance.
template <class T>
-GAZEBO_VISIBLE
T &SingletonT<T>::myself = SingletonT<T>::GetInstance();
/// \}
diff --git a/gazebo/common/Skeleton.cc b/gazebo/common/Skeleton.cc
index b4d6f2a..ef1efa9 100644
--- a/gazebo/common/Skeleton.cc
+++ b/gazebo/common/Skeleton.cc
@@ -109,9 +109,9 @@ void Skeleton::Scale(double _scale)
iter != this->nodes.end(); ++iter)
{
SkeletonNode *node = iter->second;
- math::Matrix4 trans = node->GetTransform();
- math::Vector3 pos = trans.GetTranslation();
- trans.SetTranslate(pos * _scale);
+ ignition::math::Matrix4d trans(node->Transform());
+ ignition::math::Vector3d pos(trans.Translation());
+ trans.Translate(pos * _scale);
node->SetTransform(trans, false);
}
@@ -119,7 +119,7 @@ void Skeleton::Scale(double _scale)
this->root->UpdateChildrenTransforms();
// scale the animation data
- for (unsigned int i = 0; i < this->anims.size(); i++)
+ for (unsigned int i = 0; i < this->anims.size(); ++i)
this->anims[i]->Scale(_scale);
}
@@ -148,12 +148,24 @@ void Skeleton::BuildNodeMap()
//////////////////////////////////////////////////
void Skeleton::SetBindShapeTransform(math::Matrix4 _trans)
{
+ this->SetBindShapeTransform(_trans.Ign());
+}
+
+//////////////////////////////////////////////////
+void Skeleton::SetBindShapeTransform(const ignition::math::Matrix4d &_trans)
+{
this->bindShapeTransform = _trans;
}
//////////////////////////////////////////////////
math::Matrix4 Skeleton::GetBindShapeTransform()
{
+ return this->BindShapeTransform();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Matrix4d Skeleton::BindShapeTransform()
+{
return this->bindShapeTransform;
}
@@ -166,7 +178,7 @@ void Skeleton::PrintTransforms()
SkeletonNode *node = iter->second;
std::cerr << "---------------\n" << node->GetName() << "\n";
- for (unsigned int i = 0; i < node->GetNumRawTrans(); i++)
+ for (unsigned int i = 0; i < node->GetNumRawTrans(); ++i)
{
NodeTransform nt = node->GetRawTransform(i);
std::cerr << "\t" << nt.GetSID();
@@ -183,10 +195,10 @@ void Skeleton::PrintTransforms()
std::cerr << nt() << "\n+++++++++++\n";
}
- std::cerr << node->GetModelTransform() << "\n";
+ std::cerr << node->ModelTransform() << "\n";
if (node->IsJoint())
- std::cerr << node->GetInverseBindTransform() << "\n";
+ std::cerr << node->InverseBindTransform() << "\n";
}
}
@@ -319,12 +331,19 @@ bool SkeletonNode::IsJoint()
//////////////////////////////////////////////////
void SkeletonNode::SetTransform(math::Matrix4 _trans, bool _updateChildren)
{
+ this->SetTransform(_trans.Ign(), _updateChildren);
+}
+
+//////////////////////////////////////////////////
+void SkeletonNode::SetTransform(const ignition::math::Matrix4d &_trans,
+ bool _updateChildren)
+{
this->transform = _trans;
if (this->parent == NULL)
this->modelTransform = _trans;
else
- this->modelTransform = this->parent->GetModelTransform() * _trans;
+ this->modelTransform = this->parent->ModelTransform() * _trans;
/// propagate the change to the children nodes
if (_updateChildren)
@@ -334,6 +353,12 @@ void SkeletonNode::SetTransform(math::Matrix4 _trans, bool _updateChildren)
//////////////////////////////////////////////////
void SkeletonNode::SetInitialTransform(math::Matrix4 _trans)
{
+ this->SetInitialTransform(_trans.Ign());
+}
+
+//////////////////////////////////////////////////
+void SkeletonNode::SetInitialTransform(const ignition::math::Matrix4d &_trans)
+{
this->initialTransform = _trans;
this->SetTransform(_trans);
}
@@ -344,7 +369,7 @@ void SkeletonNode::Reset(bool resetChildren)
this->SetTransform(this->initialTransform);
if (resetChildren)
- for (unsigned int i = 0; i < this->GetChildCount(); i++)
+ for (unsigned int i = 0; i < this->GetChildCount(); ++i)
this->GetChild(i)->Reset(true);
}
@@ -352,7 +377,7 @@ void SkeletonNode::Reset(bool resetChildren)
void SkeletonNode::UpdateChildrenTransforms()
{
std::list<SkeletonNode*> toVisit;
- for (unsigned int i = 0; i < this->children.size(); i++)
+ for (unsigned int i = 0; i < this->children.size(); ++i)
toVisit.push_back(this->children[i]);
while (!toVisit.empty())
@@ -370,19 +395,35 @@ void SkeletonNode::UpdateChildrenTransforms()
//////////////////////////////////////////////////
math::Matrix4 SkeletonNode::GetTransform()
{
+ return this->Transform();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Matrix4d SkeletonNode::Transform()
+{
return this->transform;
}
//////////////////////////////////////////////////
void SkeletonNode::SetModelTransform(math::Matrix4 _trans, bool _updateChildren)
{
+ this->SetModelTransform(_trans.Ign(), _updateChildren);
+}
+
+//////////////////////////////////////////////////
+void SkeletonNode::SetModelTransform(
+ const ignition::math::Matrix4d &_trans, const bool _updateChildren)
+{
this->modelTransform = _trans;
if (this->parent == NULL)
+ {
this->transform = _trans;
+ }
else
{
- math::Matrix4 invParentTrans = this->parent->GetModelTransform().Inverse();
+ ignition::math::Matrix4d invParentTrans =
+ this->parent->ModelTransform().Inverse();
this->transform = invParentTrans * this->modelTransform;
}
@@ -393,6 +434,12 @@ void SkeletonNode::SetModelTransform(math::Matrix4 _trans, bool _updateChildren)
//////////////////////////////////////////////////
math::Matrix4 SkeletonNode::GetModelTransform()
{
+ return this->ModelTransform();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Matrix4d SkeletonNode::ModelTransform() const
+{
return this->modelTransform;
}
@@ -438,7 +485,7 @@ SkeletonNode* SkeletonNode::GetChild(unsigned int _index)
//////////////////////////////////////////////////
SkeletonNode* SkeletonNode::GetChildByName(std::string _name)
{
- for (unsigned int i = 0; i < this->children.size(); i++)
+ for (unsigned int i = 0; i < this->children.size(); ++i)
if (this->children[i]->GetName() == _name)
return this->children[i];
@@ -448,7 +495,7 @@ SkeletonNode* SkeletonNode::GetChildByName(std::string _name)
//////////////////////////////////////////////////
SkeletonNode* SkeletonNode::GetChildById(std::string _id)
{
- for (unsigned int i = 0; i < this->children.size(); i++)
+ for (unsigned int i = 0; i < this->children.size(); ++i)
if (this->children[i]->GetId() == _id)
return this->children[i];
@@ -470,12 +517,25 @@ unsigned int SkeletonNode::GetHandle()
//////////////////////////////////////////////////
void SkeletonNode::SetInverseBindTransform(math::Matrix4 _invBM)
{
+ this->SetInverseBindTransform(_invBM.Ign());
+}
+
+//////////////////////////////////////////////////
+void SkeletonNode::SetInverseBindTransform(
+ const ignition::math::Matrix4d &_invBM)
+{
this->invBindTransform = _invBM;
}
//////////////////////////////////////////////////
math::Matrix4 SkeletonNode::GetInverseBindTransform()
{
+ return this->InverseBindTransform();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Matrix4d SkeletonNode::InverseBindTransform()
+{
return this->invBindTransform;
}
@@ -520,12 +580,22 @@ NodeTransform::NodeTransform(TransformType _type)
{
this->sid = "_default_";
this->type = _type;
- this->transform = math::Matrix4(math::Matrix4::IDENTITY);
+ this->transform =
+ ignition::math::Matrix4d(ignition::math::Matrix4d::Identity);
}
//////////////////////////////////////////////////
NodeTransform::NodeTransform(math::Matrix4 _mat, std::string _sid,
- TransformType _type)
+ TransformType _type)
+{
+ this->sid = _sid;
+ this->type = _type;
+ this->transform = _mat.Ign();
+}
+
+//////////////////////////////////////////////////
+NodeTransform::NodeTransform(const ignition::math::Matrix4d &_mat,
+ const std::string &_sid, TransformType _type)
{
this->sid = _sid;
this->type = _type;
@@ -540,6 +610,12 @@ NodeTransform::~NodeTransform()
//////////////////////////////////////////////////
void NodeTransform::Set(math::Matrix4 _mat)
{
+ this->Set(_mat.Ign());
+}
+
+//////////////////////////////////////////////////
+void NodeTransform::Set(const ignition::math::Matrix4d &_mat)
+{
this->transform = _mat;
}
@@ -558,6 +634,12 @@ void NodeTransform::SetSID(std::string _sid)
//////////////////////////////////////////////////
math::Matrix4 NodeTransform::Get()
{
+ return this->GetTransform();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Matrix4d NodeTransform::GetTransform() const
+{
return this->transform;
}
@@ -582,15 +664,19 @@ void NodeTransform::SetComponent(unsigned int _idx, double _value)
//////////////////////////////////////////////////
void NodeTransform::SetSourceValues(math::Matrix4 _mat)
{
- double *row;
+ this->SetSourceValues(_mat.Ign());
+}
+
+//////////////////////////////////////////////////
+void NodeTransform::SetSourceValues(const ignition::math::Matrix4d &_mat)
+{
this->source.resize(16);
unsigned int idx = 0;
- for (unsigned int i = 0; i < 4; i++)
+ for (unsigned int i = 0; i < 4; ++i)
{
- row = _mat[i];
- for (unsigned int j = 0; j < 4; j++)
+ for (unsigned int j = 0; j < 4; ++j)
{
- this->source[idx] = row[j];
+ this->source[idx] = _mat(i, j);
idx++;
}
}
@@ -599,19 +685,32 @@ void NodeTransform::SetSourceValues(math::Matrix4 _mat)
//////////////////////////////////////////////////
void NodeTransform::SetSourceValues(math::Vector3 _vec)
{
+ this->SetSourceValues(_vec.Ign());
+}
+
+//////////////////////////////////////////////////
+void NodeTransform::SetSourceValues(const ignition::math::Vector3d &_vec)
+{
this->source.resize(3);
- this->source[0] = _vec.x;
- this->source[1] = _vec.y;
- this->source[2] = _vec.z;
+ this->source[0] = _vec.X();
+ this->source[1] = _vec.Y();
+ this->source[2] = _vec.Z();
}
//////////////////////////////////////////////////
void NodeTransform::SetSourceValues(math::Vector3 _axis, double _angle)
{
+ this->SetSourceValues(_axis.Ign(), _angle);
+}
+
+//////////////////////////////////////////////////
+void NodeTransform::SetSourceValues(
+ const ignition::math::Vector3d &_axis, const double _angle)
+{
this->source.resize(4);
- this->source[0] = _axis.x;
- this->source[1] = _axis.y;
- this->source[2] = _axis.z;
+ this->source[0] = _axis.X();
+ this->source[1] = _axis.Y();
+ this->source[2] = _axis.Z();
this->source[3] = _angle;
}
@@ -630,34 +729,36 @@ void NodeTransform::RecalculateMatrix()
else
if (this->type == TRANSLATE)
{
- this->transform.SetTranslate(math::Vector3(this->source[0],
- this->source[1], this->source[2]));
+ this->transform.Translate(
+ ignition::math::Vector3d(this->source[0],
+ this->source[1], this->source[2]));
}
else
if (this->type == ROTATE)
{
- math::Matrix3 mat;
- mat.SetFromAxis(math::Vector3(this->source[0], this->source[1],
- this->source[2]), GZ_DTOR(this->source[3]));
+ ignition::math::Matrix3d mat;
+ mat.Axis(ignition::math::Vector3d(
+ this->source[0], this->source[1], this->source[2]),
+ IGN_DTOR(this->source[3]));
this->transform = mat;
}
else
{
- this->transform.SetScale(math::Vector3(this->source[0], this->source[1],
- this->source[2]));
+ this->transform.Scale(ignition::math::Vector3d(
+ this->source[0], this->source[1], this->source[2]));
}
}
//////////////////////////////////////////////////
-math::Matrix4 NodeTransform::operator() ()
+ignition::math::Matrix4d NodeTransform::operator()()
{
return this->transform;
}
//////////////////////////////////////////////////
-math::Matrix4 NodeTransform::operator* (NodeTransform _t)
+ignition::math::Matrix4d NodeTransform::operator*(NodeTransform _t)
{
- math::Matrix4 m;
+ ignition::math::Matrix4d m;
m = this->transform * _t();
@@ -665,10 +766,21 @@ math::Matrix4 NodeTransform::operator* (NodeTransform _t)
}
//////////////////////////////////////////////////
-math::Matrix4 NodeTransform::operator* (math::Matrix4 _m)
+math::Matrix4 NodeTransform::operator*(math::Matrix4 _m)
{
math::Matrix4 m;
+ m = gazebo::math::Matrix4(this->transform) * _m;
+
+ return m;
+}
+
+//////////////////////////////////////////////////
+ignition::math::Matrix4d NodeTransform::operator*(
+ const ignition::math::Matrix4d &_m)
+{
+ ignition::math::Matrix4d m;
+
m = this->transform * _m;
return m;
@@ -678,7 +790,7 @@ math::Matrix4 NodeTransform::operator* (math::Matrix4 _m)
void NodeTransform::PrintSource()
{
std::cerr << this->sid;
- for (unsigned int i = 0; i < this->source.size(); i++)
+ for (unsigned int i = 0; i < this->source.size(); ++i)
std::cerr << " " << this->source[i];
std::cerr << "\n";
}
diff --git a/gazebo/common/Skeleton.hh b/gazebo/common/Skeleton.hh
index d5992fa..d3f5ec7 100644
--- a/gazebo/common/Skeleton.hh
+++ b/gazebo/common/Skeleton.hh
@@ -14,15 +14,18 @@
* limitations under the License.
*
*/
-#ifndef _SKELETON_HH_
-#define _SKELETON_HH_
+#ifndef _GAZEBO_SKELETON_HH_
+#define _GAZEBO_SKELETON_HH_
#include <vector>
#include <string>
#include <map>
#include <utility>
+#include <ignition/math/Matrix4.hh>
+
#include "gazebo/math/Matrix4.hh"
+#include "gazebo/common/CommonTypes.hh"
#include "gazebo/util/system.hh"
namespace gazebo
@@ -47,7 +50,7 @@ namespace gazebo
/// \class Skeleton Skeleton.hh common/common.hh
/// \brief A skeleton
- class GAZEBO_VISIBLE Skeleton
+ class GZ_COMMON_VISIBLE Skeleton
{
/// \brief Constructor
public: Skeleton();
@@ -96,11 +99,25 @@ namespace gazebo
/// \brief Set the bind pose skeletal transform
/// \param[in] _trans the transform
- public: void SetBindShapeTransform(math::Matrix4 _trans);
+ /// \deprecated See SetBindShapeTransform that accepts
+ /// ignition::math::Matrix4d
+ public: void SetBindShapeTransform(math::Matrix4 _trans)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set the bind pose skeletal transform
+ /// \param[in] _trans the transform
+ public: void SetBindShapeTransform(
+ const ignition::math::Matrix4d &_trans);
+
+ /// \brief Return bind pose skeletal transform
+ /// \return a matrix
+ /// \deprecated See BindShapeTransform that return
+ /// ignition::math::Matrix4d.
+ public: math::Matrix4 GetBindShapeTransform() GAZEBO_DEPRECATED(6.0);
/// \brief Return bind pose skeletal transform
/// \return a matrix
- public: math::Matrix4 GetBindShapeTransform();
+ public: ignition::math::Matrix4d BindShapeTransform();
/// \brief Outputs the transforms to std::err stream
public: void PrintTransforms();
@@ -156,7 +173,7 @@ namespace gazebo
protected: NodeMap nodes;
/// \brief the bind pose skeletal transform
- protected: math::Matrix4 bindShapeTransform;
+ protected: ignition::math::Matrix4d bindShapeTransform;
/// \brief the node weight table
protected: RawNodeWeights rawNW;
@@ -167,7 +184,7 @@ namespace gazebo
/// \class SkeletonNode Skeleton.hh common/common.hh
/// \brief A skeleton node
- class GAZEBO_VISIBLE SkeletonNode
+ class GZ_COMMON_VISIBLE SkeletonNode
{
/// \brief enumeration of node types
public: enum SkeletonNodeType {NODE, JOINT};
@@ -215,22 +232,47 @@ namespace gazebo
/// \param[in] _trans the transformation
/// \param[in] _updateChildren when true the UpdateChildrenTransforms
/// operation is performed
+ /// \deprecated See SetTransform function that accepts
+ /// ignition::math::Matrix4d.
public: void SetTransform(math::Matrix4 _trans,
- bool _updateChildren = true);
+ bool _updateChildren = true) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set a transformation
+ /// \param[in] _trans the transformation
+ /// \param[in] _updateChildren when true the UpdateChildrenTransforms
+ /// operation is performed
+ public: void SetTransform(const ignition::math::Matrix4d &_trans,
+ bool _updateChildren = true);
/// \brief Set the model transformation
/// \param[in] _trans the transformation
/// \param[in] _updateChildren when true the UpdateChildrenTransforms
/// operation is performed
+ /// \deprecated See SetModelTransform that accepts
+ /// ignition::math::Matrix4d.
public: void SetModelTransform(math::Matrix4 _trans,
- bool _updateChildren = true);
+ bool _updateChildren = true) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set the model transformation
+ /// \param[in] _trans the transformation
+ /// \param[in] _updateChildren when true the UpdateChildrenTransforms
+ /// operation is performed
+ public: void SetModelTransform(const ignition::math::Matrix4d &_trans,
+ bool _updateChildren = true);
/// \brief Apply model transformations in order for each node in the tree
public: void UpdateChildrenTransforms();
/// \brief Sets the initial transformation
/// \param[in] _tras the transfromation matrix
- public: void SetInitialTransform(math::Matrix4 _tras);
+ /// \deprecated See SetInitialTransform that accepts
+ /// ignition::math::Matrix4d.
+ public: void SetInitialTransform(math::Matrix4 _tras)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Sets the initial transformation
+ /// \param[in] _tras the transfromation matrix
+ public: void SetInitialTransform(const ignition::math::Matrix4d &_tras);
/// \brief Reset the transformation to the initial transformation
/// \param[in] _resetChildren when true, performs the operation for every
@@ -238,7 +280,14 @@ namespace gazebo
public: void Reset(bool _resetChildren);
/// \brief Get transform relative to parent
- public: math::Matrix4 GetTransform();
+ /// \return Transform relative to parent
+ /// \deprecated See Transform function that return
+ /// ignition::math::Matrix4d.
+ public: math::Matrix4 GetTransform() GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get transform relative to parent
+ /// \return Transform relative to parent
+ public: ignition::math::Matrix4d Transform();
/// \brief Set the parent node
/// \param[in] _parent the new parent
@@ -285,15 +334,35 @@ namespace gazebo
/// \brief Assign the inverse of the bind pose skeletal transform
/// \param[in] _invBM the transform
- public: void SetInverseBindTransform(math::Matrix4 _invBM);
+ /// \deprecated See SetInverseBindTransform that accepts
+ /// ignition::math::Matrix4d.
+ public: void SetInverseBindTransform(math::Matrix4 _invBM)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Assign the inverse of the bind pose skeletal transform
+ /// \param[in] _invBM the transform
+ public: void SetInverseBindTransform(
+ const ignition::math::Matrix4d &_invBM);
+
+ /// \brief Retrieve the inverse of the bind pose skeletal transform
+ /// \return the transform
+ /// \deprecated See InverseBindTransform function that returns
+ /// ignition::math::Matrix4d.
+ public: math::Matrix4 GetInverseBindTransform() GAZEBO_DEPRECATED(6.0);
/// \brief Retrieve the inverse of the bind pose skeletal transform
/// \return the transform
- public: math::Matrix4 GetInverseBindTransform();
+ public: ignition::math::Matrix4d InverseBindTransform();
+
+ /// \brief Retrieve the model transform
+ /// \return the transform
+ /// \deprecated See ModelTransform function that returns
+ /// ignition::math::Matrix4d.
+ public: math::Matrix4 GetModelTransform() GAZEBO_DEPRECATED(6.0);
/// \brief Retrieve the model transform
/// \return the transform
- public: math::Matrix4 GetModelTransform();
+ public: ignition::math::Matrix4d ModelTransform() const;
/// \brief Retrieve the raw transformations
/// \return an array of transformations
@@ -326,16 +395,16 @@ namespace gazebo
protected: SkeletonNodeType type;
/// \brief the transform
- protected: math::Matrix4 transform;
+ protected: ignition::math::Matrix4d transform;
/// \brief the initial transformation
- protected: math::Matrix4 initialTransform;
+ protected: ignition::math::Matrix4d initialTransform;
/// \brief the model transformation
- protected: math::Matrix4 modelTransform;
+ protected: ignition::math::Matrix4d modelTransform;
/// \brief the inverse of the bind pose skeletal transform
- protected: math::Matrix4 invBindTransform;
+ protected: ignition::math::Matrix4d invBindTransform;
/// \brief the parent node
protected: SkeletonNode *parent;
@@ -352,7 +421,7 @@ namespace gazebo
/// \clas NodeTransform Skeleton.hh common/common.hh
/// \brief A transformation node
- class GAZEBO_VISIBLE NodeTransform
+ class GZ_COMMON_VISIBLE NodeTransform
{
/// \brief Enumeration of the transform types
public: enum TransformType {TRANSLATE, ROTATE, SCALE, MATRIX};
@@ -365,15 +434,31 @@ namespace gazebo
/// \param[in] _mat the matrix
/// \param[in] _sid identifier
/// \param[in] _type the type of transform
+ /// \deprecated See NodeTransform constructor that accepts
+ /// ignition::math::Matrix4d.
public: NodeTransform(math::Matrix4 _mat, std::string _sid = "_default_",
- TransformType _type = MATRIX);
+ TransformType _type = MATRIX) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Constructor
+ /// \param[in] _mat the matrix
+ /// \param[in] _sid identifier
+ /// \param[in] _type the type of transform
+ public: NodeTransform(const ignition::math::Matrix4d &_mat,
+ const std::string &_sid = "_default_",
+ TransformType _type = MATRIX);
/// \brief Destructor. It does nothing.
public: ~NodeTransform();
/// \brief Assign a transformation
/// \param[in] _mat the transform
- public: void Set(math::Matrix4 _mat);
+ /// \deprecated See Set function that accepts
+ /// ignition::math::Matrix4d.
+ public: void Set(math::Matrix4 _mat) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Assign a transformation
+ /// \param[in] _mat the transform
+ public: void Set(const ignition::math::Matrix4d &_mat);
/// \brief Set transform type
/// \param[in] _type the type
@@ -383,9 +468,15 @@ namespace gazebo
/// \param[in] _sid the sid
public: void SetSID(std::string _sid);
- /// Returns the transformation matrix
+ /// \brief Returns the transformation matrix
/// \return the matrix
- public: math::Matrix4 Get();
+ /// \deprecated See GetTransform function that returns
+ /// ignition::math::Matrix4d.
+ public: math::Matrix4 Get() GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Returns the transformation matrix
+ /// \return The transform matrix
+ public: ignition::math::Matrix4d GetTransform() const;
/// \brief Returns the transformation type
/// \return the type
@@ -400,17 +491,39 @@ namespace gazebo
/// \param[in] _value the value
public: void SetComponent(unsigned int _idx, double _value);
- /// \brief Set source data values _
+ /// \brief Set source data values
/// param[in] _mat the values
- public: void SetSourceValues(math::Matrix4 _mat);
+ /// \deprecated See SetSourceValues function that accepts
+ /// ignition::math::Matrix4d.
+ public: void SetSourceValues(math::Matrix4 _mat) GAZEBO_DEPRECATED(6.0);
/// \brief Set source data values
- public: void SetSourceValues(math::Vector3 _vec);
+ /// param[in] _mat the values
+ public: void SetSourceValues(const ignition::math::Matrix4d &_mat);
+
+ /// \brief Set source data values
+ /// \param[in] _vec Vector to set source data values from.
+ /// \deprecated See SetSourceValues function that accepts
+ /// ignition::math::Vector3d.
+ public: void SetSourceValues(math::Vector3 _vec) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set source data values
+ /// \param[in] _vec Vector to set source data values from.
+ public: void SetSourceValues(const ignition::math::Vector3d &_vec);
/// \brief Sets source matrix values from roation
/// \param[in] _axis of rotation
/// \param[in] _angle of rotation
- public: void SetSourceValues(math::Vector3 _axis, double _angle);
+ /// \deprecated See SetSourceValues function that accepts
+ /// ignition::math::Vector3d.
+ public: void SetSourceValues(math::Vector3 _axis, double _angle)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Sets source matrix values from roation
+ /// \param[in] _axis of rotation
+ /// \param[in] _angle of rotation
+ public: void SetSourceValues(const ignition::math::Vector3d &_axis,
+ const double _angle);
/// \brief Sets the transform matrix from the source according to the type
public: void RecalculateMatrix();
@@ -420,17 +533,24 @@ namespace gazebo
/// \brief Matrix cast operator
/// \return the transform
- public: math::Matrix4 operator() ();
+ public: ignition::math::Matrix4d operator()();
/// \brief Node transform multiplication operator
/// \param[in] _t a transform
/// \return transform matrix multiplied by _t's transform
- public: math::Matrix4 operator* (NodeTransform _t);
+ public: ignition::math::Matrix4d operator*(NodeTransform _t);
+
+ /// \brief Matrix multiplication operator
+ /// \param[in] _m a matrix
+ /// \return transform matrix multiplied by _m
+ /// \deprecated See operator* that accepts ignition::math::Matrix4d.
+ public: math::Matrix4 operator* (math::Matrix4 _m) GAZEBO_DEPRECATED(6.0);
/// \brief Matrix multiplication operator
/// \param[in] _m a matrix
/// \return transform matrix multiplied by _m
- public: math::Matrix4 operator* (math::Matrix4 _m);
+ public: ignition::math::Matrix4d operator*(
+ const ignition::math::Matrix4d &_m);
/// \brief the sid
protected: std::string sid;
@@ -439,7 +559,7 @@ namespace gazebo
protected: TransformType type;
/// \brief transform
- protected: math::Matrix4 transform;
+ protected: ignition::math::Matrix4d transform;
/// \brief source data values (can be a matrix, a position or rotation)
protected: std::vector<double> source;
diff --git a/gazebo/common/SkeletonAnimation.cc b/gazebo/common/SkeletonAnimation.cc
index d6d8c21..864eee5 100644
--- a/gazebo/common/SkeletonAnimation.cc
+++ b/gazebo/common/SkeletonAnimation.cc
@@ -50,6 +50,13 @@ std::string NodeAnimation::GetName() const
//////////////////////////////////////////////////
void NodeAnimation::AddKeyFrame(const double _time, const math::Matrix4 &_trans)
{
+ this->AddKeyFrame(_time, _trans.Ign());
+}
+
+//////////////////////////////////////////////////
+void NodeAnimation::AddKeyFrame(const double _time,
+ const ignition::math::Matrix4d &_trans)
+{
if (_time > this->length)
this->length = _time;
@@ -59,8 +66,15 @@ void NodeAnimation::AddKeyFrame(const double _time, const math::Matrix4 &_trans)
//////////////////////////////////////////////////
void NodeAnimation::AddKeyFrame(const double _time, const math::Pose &_pose)
{
- math::Matrix4 mat(_pose.rot.GetAsMatrix4());
- mat.SetTranslate(_pose.pos);
+ this->AddKeyFrame(_time, _pose.Ign());
+}
+
+//////////////////////////////////////////////////
+void NodeAnimation::AddKeyFrame(const double _time,
+ const ignition::math::Pose3d &_pose)
+{
+ ignition::math::Matrix4d mat(_pose.Rot());
+ mat.Translate(_pose.Pos());
this->AddKeyFrame(_time, mat);
}
@@ -72,8 +86,17 @@ unsigned int NodeAnimation::GetFrameCount() const
}
//////////////////////////////////////////////////
-void NodeAnimation::GetKeyFrame(const unsigned int _i, double& _time,
- math::Matrix4& _trans) const
+void NodeAnimation::GetKeyFrame(const unsigned int _i, double &_time,
+ math::Matrix4 &_trans) const
+{
+ ignition::math::Matrix4d trans;
+ this->GetKeyFrame(_i, _time, trans);
+ _trans = trans;
+}
+
+//////////////////////////////////////////////////
+void NodeAnimation::GetKeyFrame(const unsigned int _i, double &_time,
+ ignition::math::Matrix4d &_trans) const
{
if (_i >= this->keyFrames.size())
{
@@ -82,8 +105,8 @@ void NodeAnimation::GetKeyFrame(const unsigned int _i, double& _time,
}
else
{
- std::map<double, math::Matrix4>::const_iterator iter =
- this->keyFrames.begin();
+ std::map<double, ignition::math::Matrix4d>::const_iterator iter =
+ this->keyFrames.begin();
std::advance(iter, _i);
@@ -96,8 +119,15 @@ void NodeAnimation::GetKeyFrame(const unsigned int _i, double& _time,
std::pair<double, math::Matrix4> NodeAnimation::GetKeyFrame(
const unsigned int _i) const
{
+ return this->KeyFrame(_i);
+}
+
+//////////////////////////////////////////////////
+std::pair<double, ignition::math::Matrix4d> NodeAnimation::KeyFrame(
+ const unsigned int _i) const
+{
double t;
- math::Matrix4 mat(math::Matrix4::IDENTITY);
+ ignition::math::Matrix4d mat(ignition::math::Matrix4d::Identity);
this->GetKeyFrame(_i, t, mat);
return std::make_pair(t, mat);
@@ -112,6 +142,12 @@ double NodeAnimation::GetLength() const
//////////////////////////////////////////////////
math::Matrix4 NodeAnimation::GetFrameAt(double _time, bool _loop) const
{
+ return this->FrameAt(_time, _loop);
+}
+
+//////////////////////////////////////////////////
+ignition::math::Matrix4d NodeAnimation::FrameAt(double _time, bool _loop) const
+{
double time = _time;
if (time > this->length)
{
@@ -122,40 +158,42 @@ math::Matrix4 NodeAnimation::GetFrameAt(double _time, bool _loop) const
time = this->length;
}
- if (math::equal(time, this->length))
+ if (ignition::math::equal(time, this->length))
return this->keyFrames.rbegin()->second;
- std::map<double, math::Matrix4>::const_iterator it1 =
- this->keyFrames.upper_bound(time);
+ std::map<double, ignition::math::Matrix4d>::const_iterator it1 =
+ this->keyFrames.upper_bound(time);
- if (it1 == this->keyFrames.begin() || math::equal(it1->first, time))
+ if (it1 == this->keyFrames.begin() || ignition::math::equal(it1->first, time))
return it1->second;
- std::map<double, math::Matrix4>::const_iterator it2 = it1--;
+ std::map<double, ignition::math::Matrix4d>::const_iterator it2 = it1--;
- if (it1 == this->keyFrames.begin() || math::equal(it1->first, time))
+ if (it1 == this->keyFrames.begin() || ignition::math::equal(it1->first, time))
return it1->second;
double nextKey = it2->first;
- math::Matrix4 nextTrans = it2->second;
+ ignition::math::Matrix4d nextTrans = it2->second;
double prevKey = it1->first;
- math::Matrix4 prevTrans = it2->second;
+ ignition::math::Matrix4d prevTrans = it2->second;
double t = (time - prevKey) / (nextKey - prevKey);
GZ_ASSERT(t >= 0.0 && t <= 1.0, "t is not in the range 0.0..1.0");
- math::Vector3 nextPos = nextTrans.GetTranslation();
- math::Vector3 prevPos = prevTrans.GetTranslation();
- math::Vector3 pos = math::Vector3(prevPos.x + ((nextPos.x - prevPos.x) * t),
- prevPos.y + ((nextPos.y - prevPos.y) * t),
- prevPos.z + ((nextPos.z - prevPos.z) * t));
+ ignition::math::Vector3d nextPos = nextTrans.Translation();
+ ignition::math::Vector3d prevPos = prevTrans.Translation();
+ ignition::math::Vector3d pos = ignition::math::Vector3d(
+ prevPos.X() + ((nextPos.X() - prevPos.X()) * t),
+ prevPos.Y() + ((nextPos.Y() - prevPos.Y()) * t),
+ prevPos.Z() + ((nextPos.Z() - prevPos.Z()) * t));
- math::Quaternion nextRot = nextTrans.GetRotation();
- math::Quaternion prevRot = prevTrans.GetRotation();
- math::Quaternion rot = math::Quaternion::Slerp(t, prevRot, nextRot, true);
+ ignition::math::Quaterniond nextRot = nextTrans.Rotation();
+ ignition::math::Quaterniond prevRot = prevTrans.Rotation();
+ ignition::math::Quaterniond rot = ignition::math::Quaterniond::Slerp(t,
+ prevRot, nextRot, true);
- math::Matrix4 trans(rot.GetAsMatrix4());
- trans.SetTranslate(pos);
+ ignition::math::Matrix4d trans(rot);
+ trans.Translate(pos);
return trans;
}
@@ -163,29 +201,32 @@ math::Matrix4 NodeAnimation::GetFrameAt(double _time, bool _loop) const
//////////////////////////////////////////////////
void NodeAnimation::Scale(const double _scale)
{
- for (std::map<double, math::Matrix4>::iterator iter = this->keyFrames.begin();
- iter != this->keyFrames.end(); ++iter)
+ for (auto &frame : this->keyFrames)
{
- math::Matrix4 *mat = &iter->second;
- math::Vector3 pos = mat->GetTranslation();
- mat->SetTranslate(pos * _scale);
+ ignition::math::Matrix4d *mat = &frame.second;
+ ignition::math::Vector3d pos = mat->Translation();
+ mat->Translate(pos * _scale);
}
}
//////////////////////////////////////////////////
double NodeAnimation::GetTimeAtX(const double _x) const
{
- std::map<double, math::Matrix4>::const_iterator it1 = this->keyFrames.begin();
- while (it1->second.GetTranslation().x < _x)
+ std::map<double, ignition::math::Matrix4d>::const_iterator it1 =
+ this->keyFrames.begin();
+
+ while (it1->second.Translation().X() < _x)
++it1;
if (it1 == this->keyFrames.begin() ||
- math::equal(it1->second.GetTranslation().x, _x))
+ ignition::math::equal(it1->second.Translation().X(), _x))
+ {
return it1->first;
+ }
- std::map<double, math::Matrix4>::const_iterator it2 = it1--;
- double x1 = it1->second.GetTranslation().x;
- double x2 = it2->second.GetTranslation().x;
+ std::map<double, ignition::math::Matrix4d>::const_iterator it2 = it1--;
+ double x1 = it1->second.Translation().X();
+ double x2 = it2->second.Translation().X();
double t1 = it1->first;
double t2 = it2->first;
@@ -229,9 +270,16 @@ bool SkeletonAnimation::HasNode(const std::string& _node) const
}
//////////////////////////////////////////////////
-void SkeletonAnimation::AddKeyFrame(const std::string& _node,
+void SkeletonAnimation::AddKeyFrame(const std::string &_node,
const double _time, const math::Matrix4 &_mat)
{
+ this->AddKeyFrame(_node, _time, _mat.Ign());
+}
+
+//////////////////////////////////////////////////
+void SkeletonAnimation::AddKeyFrame(const std::string& _node,
+ const double _time, const ignition::math::Matrix4d &_mat)
+{
if (this->animations.find(_node) == this->animations.end())
this->animations[_node] = new NodeAnimation(_node);
@@ -242,9 +290,16 @@ void SkeletonAnimation::AddKeyFrame(const std::string& _node,
}
//////////////////////////////////////////////////
-void SkeletonAnimation::AddKeyFrame(const std::string& _node,
+void SkeletonAnimation::AddKeyFrame(const std::string &_node,
const double _time, const math::Pose &_pose)
{
+ this->AddKeyFrame(_node, _time, _pose.Ign());
+}
+
+//////////////////////////////////////////////////
+void SkeletonAnimation::AddKeyFrame(const std::string &_node,
+ const double _time, const ignition::math::Pose3d &_pose)
+{
if (this->animations.find(_node) == this->animations.end())
this->animations[_node] = new NodeAnimation(_node);
@@ -255,53 +310,84 @@ void SkeletonAnimation::AddKeyFrame(const std::string& _node,
}
//////////////////////////////////////////////////
-math::Matrix4 SkeletonAnimation::GetNodePoseAt(const std::string& _node,
- const double _time, const bool _loop)
+math::Matrix4 SkeletonAnimation::GetNodePoseAt(const std::string &_nod,
+ const double _time, const bool _loop)
+{
+ return this->NodePoseAt(_nod, _time, _loop);
+}
+
+//////////////////////////////////////////////////
+ignition::math::Matrix4d SkeletonAnimation::NodePoseAt(
+ const std::string &_node, const double _time, const bool _loop)
{
- math::Matrix4 mat;
+ ignition::math::Matrix4d mat;
if (this->animations[_node])
- mat = this->animations[_node]->GetFrameAt(_time, _loop);
+ mat = this->animations[_node]->FrameAt(_time, _loop);
return mat;
}
//////////////////////////////////////////////////
std::map<std::string, math::Matrix4> SkeletonAnimation::GetPoseAt(
- const double _time, const bool _loop) const
+ const double _time, const bool _loop) const
+{
+ std::map<std::string, ignition::math::Matrix4d> tmp;
+ std::map<std::string, math::Matrix4> result;
+ tmp = this->PoseAt(_time, _loop);
+
+ for (auto const &t : tmp)
+ result[t.first] = t.second;
+ return result;
+}
+
+//////////////////////////////////////////////////
+std::map<std::string, ignition::math::Matrix4d> SkeletonAnimation::PoseAt(
+ const double _time, const bool _loop) const
{
/// TODO need to make sure that all nodes have keyframes at the same
/// points in time and create the missing keyframes. if the animation
- /// comes from bvh this is guaranteed, but if it's comming from collada
+ /// comes from bvh this is guaranteed, but if it's coming from collada
/// it's not guaranteed. fixing this will help not having to find the
/// prev and next keyframe for each node at each time step, but rather
/// doing it only once per time step.
- std::map<std::string, math::Matrix4> pose;
- for (std::map<std::string, NodeAnimation*>::const_iterator iter =
- this->animations.begin(); iter != this->animations.end(); ++iter)
- {
- pose[iter->first] = iter->second->GetFrameAt(_time, _loop);
- }
+ std::map<std::string, ignition::math::Matrix4d> pose;
+ for (auto const &anim : this->animations)
+ pose[anim.first] = anim.second->FrameAt(_time, _loop);
return pose;
}
//////////////////////////////////////////////////
std::map<std::string, math::Matrix4> SkeletonAnimation::GetPoseAtX(
- const double _x, const std::string& _node, const bool _loop) const
+ const double _x, const std::string &_node, const bool _loop) const
+{
+ std::map<std::string, ignition::math::Matrix4d> tmp;
+ std::map<std::string, math::Matrix4> result;
+ tmp = this->PoseAtX(_x, _node, _loop);
+
+ for (auto const &t : tmp)
+ result[t.first] = t.second;
+ return result;
+}
+
+//////////////////////////////////////////////////
+std::map<std::string, ignition::math::Matrix4d> SkeletonAnimation::PoseAtX(
+ const double _x, const std::string &_node, const bool _loop) const
{
std::map<std::string, NodeAnimation*>::const_iterator nodeAnim =
this->animations.find(_node);
- math::Matrix4 lastPos = nodeAnim->second->GetKeyFrame(
+
+ ignition::math::Matrix4d lastPos = nodeAnim->second->KeyFrame(
nodeAnim->second->GetFrameCount() - 1).second;
- math::Matrix4 firstPos = nodeAnim->second->GetKeyFrame(0).second;
+ ignition::math::Matrix4d firstPos = nodeAnim->second->KeyFrame(0).second;
double x = _x;
- if (x < firstPos.GetTranslation().x)
- x = firstPos.GetTranslation().x;
+ if (x < firstPos.Translation().X())
+ x = firstPos.Translation().X();
- double lastX = lastPos.GetTranslation().x;
+ double lastX = lastPos.Translation().X();
if (x > lastX && !_loop)
x = lastX;
while (x > lastX)
@@ -309,7 +395,7 @@ std::map<std::string, math::Matrix4> SkeletonAnimation::GetPoseAtX(
double time = nodeAnim->second->GetTimeAtX(x);
- return this->GetPoseAt(time, _loop);
+ return this->PoseAt(time, _loop);
}
//////////////////////////////////////////////////
diff --git a/gazebo/common/SkeletonAnimation.hh b/gazebo/common/SkeletonAnimation.hh
index be2bc69..b4adb85 100644
--- a/gazebo/common/SkeletonAnimation.hh
+++ b/gazebo/common/SkeletonAnimation.hh
@@ -14,16 +14,20 @@
* limitations under the License.
*
*/
-#ifndef _SKELETONANIMATION_HH_
-#define _SKELETONANIMATION_HH_
+#ifndef _GAZEBO_SKELETONANIMATION_HH_
+#define _GAZEBO_SKELETONANIMATION_HH_
#include <map>
#include <utility>
#include <string>
+#include <ignition/math/Matrix4.hh>
+#include <ignition/math/Pose3.hh>
+
#include "gazebo/math/Matrix4.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/util/system.hh"
+#include "gazebo/common/CommonTypes.hh"
namespace gazebo
{
@@ -34,18 +38,18 @@ namespace gazebo
/// \class NodeAnimation SkeletonAnimation.hh common/common.hh
/// \brief Node animation
- class GAZEBO_VISIBLE NodeAnimation
+ class GZ_COMMON_VISIBLE NodeAnimation
{
/// \brief constructor
/// \param[in] _name the name of the node
- public: NodeAnimation(const std::string& _name);
+ public: NodeAnimation(const std::string &_name);
/// \brief Destructor. It empties the key frames list
public: ~NodeAnimation();
/// \brief Changes the name of the animation
/// \param[in] the new name
- public: void SetName(const std::string& _name);
+ public: void SetName(const std::string &_name);
/// \brief Returns the name
/// \return the name
@@ -54,12 +58,30 @@ namespace gazebo
/// \brief Adds a key frame at a specific time
/// \param[in] _time the time of the key frame
/// \param[in] _trans the transformation
- public: void AddKeyFrame(const double _time, const math::Matrix4 &_trans);
+ /// \deprecated See AddKeyFrame function accepts
+ /// ignition::math::Matrix4d.
+ public: void AddKeyFrame(const double _time,
+ const math::Matrix4 &_trans) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Adds a key frame at a specific time
+ /// \param[in] _time the time of the key frame
+ /// \param[in] _trans the transformation
+ public: void AddKeyFrame(const double _time,
+ const ignition::math::Matrix4d &_trans);
+
+ /// \brief Adds a key frame at a specific time
+ /// \param[in] _time the time of the key frame
+ /// \param[in] _pose the pose
+ /// \deprecated See AddKeyFrame function that accepts
+ /// ignition::math::Pose3d
+ public: void AddKeyFrame(const double _time,
+ const math::Pose &_pose) GAZEBO_DEPRECATED(6.0);
- /// \brief Adds a key fram at a specific time
- /// \param[in] _time the tiem of the key frame
+ /// \brief Adds a key frame at a specific time
+ /// \param[in] _time the time of the key frame
/// \param[in] _pose the pose
- public: void AddKeyFrame(const double _time, const math::Pose &_pose);
+ public: void AddKeyFrame(const double _time,
+ const ignition::math::Pose3d &_pose);
/// \brief Returns the number of key frames.
/// \return the count
@@ -71,15 +93,36 @@ namespace gazebo
/// \param[out] _time the time of the frame, or -1 if the index id is out
/// of bounds
/// \param[out] _trans the transformation for this key frame
- public: void GetKeyFrame(const unsigned int _i, double& _time,
- math::Matrix4& _trans) const;
+ /// \deprecated See GetKeyFrame function that accepts
+ /// ignition::math::Matrix4d.
+ public: void GetKeyFrame(const unsigned int _i, double &_time,
+ math::Matrix4 &_trans) const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Finds a key frame using the index. Note the index of a key
+ /// frame can change as frames are added.
+ /// \param[in] _i the index
+ /// \param[out] _time the time of the frame, or -1 if the index id is out
+ /// of bounds
+ /// \param[out] _trans the transformation for this key frame
+ public: void GetKeyFrame(const unsigned int _i, double &_time,
+ ignition::math::Matrix4d &_trans) const;
/// \brief Returns a key frame using the index. Note the index of a key
/// frame can change as frames are added.
/// \param[in] _i the index
/// \return a pair that contains the time and transformation. Time is -1
/// if the index is out of bounds
+ /// \deprecated See KeyFrame function that return
+ /// ignition::math::Matrix4d.
public: std::pair<double, math::Matrix4> GetKeyFrame(
+ const unsigned int _i) const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Returns a key frame using the index. Note the index of a key
+ /// frame can change as frames are added.
+ /// \param[in] _i the index
+ /// \return a pair that contains the time and transformation. Time is -1
+ /// if the index is out of bounds
+ public: std::pair<double, ignition::math::Matrix4d> KeyFrame(
const unsigned int _i) const;
/// \brief Returns the duration of the animations
@@ -92,7 +135,19 @@ namespace gazebo
/// \param[in] _time the time
/// \param[in] _loop when true, the time is divided by the duration
/// (see GetLength)
- public: math::Matrix4 GetFrameAt(double _time, bool _loop = true) const;
+ /// \deprecated See FrameAt function that return
+ /// ignition::math::Matrix4d.
+ public: math::Matrix4 GetFrameAt(double _time, bool _loop = true) const
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Returns a frame transformation at a specific time
+ /// if a node does not exist at that time (with tolerance of 1e-6 sec),
+ /// the transformation is interpolated.
+ /// \param[in] _time the time
+ /// \param[in] _loop when true, the time is divided by the duration
+ /// (see GetLength)
+ public: ignition::math::Matrix4d FrameAt(
+ double _time, bool _loop = true) const;
/// \brief Scales each transformation in the key frames. This only affects
/// the translational values.
@@ -111,18 +166,18 @@ namespace gazebo
protected: std::string name;
/// \brief the dictionary of key frames, indexed by time
- protected: std::map<double, math::Matrix4> keyFrames;
+ protected: std::map<double, ignition::math::Matrix4d> keyFrames;
/// \brief the duration of the animations (time of last key frame)
protected: double length;
};
/// \brief Skeleton animation
- class GAZEBO_VISIBLE SkeletonAnimation
+ class GZ_COMMON_VISIBLE SkeletonAnimation
{
/// \brief The Constructor
/// \param[in] _name the name of the animation
- public: SkeletonAnimation(const std::string& _name);
+ public: SkeletonAnimation(const std::string &_name);
/// \brief The destructor. Clears the list without destroying
/// the animations
@@ -130,7 +185,7 @@ namespace gazebo
/// \brief Changes the name
/// \param[in] _name the new name
- public: void SetName(const std::string& _name);
+ public: void SetName(const std::string &_name);
/// \brief Returns the name
/// \return the name
@@ -143,21 +198,40 @@ namespace gazebo
/// \brief Looks for a node with a specific name in the animations
/// \param[in] _node the name of the node
/// \return true if the node exits
- public: bool HasNode(const std::string& _node) const;
+ public: bool HasNode(const std::string &_node) const;
/// \brief Adds or replaces a named key frame at a specific time
/// \param[in] _node the name of the new or existing node
/// \param[in] _time the time
/// \param[in] _mat the key frame transformation
- public: void AddKeyFrame(const std::string& _node, const double _time,
- const math::Matrix4 &_mat);
+ /// \deprecated See AddKeyFrame function that accepts
+ /// ignition::math::Matrix4d.
+ public: void AddKeyFrame(const std::string &_node, const double _time,
+ const math::Matrix4 &_mat) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Adds or replaces a named key frame at a specific time
+ /// \param[in] _node the name of the new or existing node
+ /// \param[in] _time the time
+ /// \param[in] _mat the key frame transformation
+ public: void AddKeyFrame(const std::string &_node, const double _time,
+ const ignition::math::Matrix4d &_mat);
/// \brief Adds or replaces a named key frame at a specific time
/// \param[in] _node the name of the new or existing node
/// \param[in] _time the time
/// \param[in] _pose the key frame transformation as a math::Pose
- public: void AddKeyFrame(const std::string& _node, const double _time,
- const math::Pose &_pose);
+ /// \deprecated See AddKeyFrame function that accepts
+ /// ignition::math::Pose3d.
+ public: void AddKeyFrame(const std::string &_node, const double _time,
+ const math::Pose &_pose) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Adds or replaces a named key frame at a specific time
+ /// \param[in] _node the name of the new or existing node
+ /// \param[in] _time the time
+ /// \param[in] _pose the key frame transformation as a
+ /// ignition::math::Pose3d
+ public: void AddKeyFrame(const std::string &_node, const double _time,
+ const ignition::math::Pose3d &_pose);
/// \brief Returns the key frame transformation for a named animation at
/// a specific time
@@ -168,7 +242,22 @@ namespace gazebo
/// \param[in] _loop when true, the time is divided by the duration
/// (see GetLength)
/// \return the transformation
- public: math::Matrix4 GetNodePoseAt(const std::string& _node,
+ /// \deprecated See NodePoseAt function that return
+ /// ignition::math::Matrix4d.
+ public: math::Matrix4 GetNodePoseAt(const std::string &_node,
+ const double _time, const bool _loop = true)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Returns the key frame transformation for a named animation at
+ /// a specific time
+ /// if a node does not exist at that time (with tolerance of 1e-6 sec),
+ /// the transformation is interpolated.
+ /// \param[in] _node the name of the animation node
+ /// \param[in] _time the time
+ /// \param[in] _loop when true, the time is divided by the duration
+ /// (see GetLength)
+ /// \return the transformation
+ public: ignition::math::Matrix4d NodePoseAt(const std::string &_node,
const double _time, const bool _loop = true);
/// \brief Returns a dictionary of transformations indexed by name at
@@ -179,8 +268,22 @@ namespace gazebo
/// \param[in] _loop when true, the time is divided by the duration
/// (see GetLength)
/// \return the transformation for every node
- public: std::map<std::string, math::Matrix4> GetPoseAt(const double _time,
- const bool _loop = true) const;
+ /// \deprecated See PoseAt function that returns
+ /// ignition::math::Matrix4d.
+ public: std::map<std::string, math::Matrix4> GetPoseAt(
+ const double _time, const bool _loop = true) const
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Returns a dictionary of transformations indexed by name at
+ /// a specific time
+ /// if a node does not exist at that specific time
+ /// (with tolerance of 1e-6 sec), the transformation is interpolated.
+ /// \param[in] _time the time
+ /// \param[in] _loop when true, the time is divided by the duration
+ /// (see GetLength)
+ /// \return the transformation for every node
+ public: std::map<std::string, ignition::math::Matrix4d> PoseAt(
+ const double _time, const bool _loop = true) const;
/// \brief Returns a dictionary of transformations indexed by name where
/// a named node transformation's translational value along the X axis is
@@ -190,8 +293,24 @@ namespace gazebo
/// \param[in] _node the name of the animation node
/// \param[in] _loop when true, the time is divided by the duration
/// (see GetLength)
+ /// \deprecated See PoseAtX function that returns
+ /// ignition::math::Matrix4d.
public: std::map<std::string, math::Matrix4> GetPoseAtX(const double _x,
- const std::string& _node, const bool _loop = true) const;
+ const std::string &_node, const bool _loop = true) const
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Returns a dictionary of transformations indexed by name where
+ /// a named node transformation's translational value along the X axis is
+ /// equal to _x.
+ /// \param[in] _x the value along x. You must ensure that _x is within a
+ /// valid range.
+ /// \param[in] _node the name of the animation node
+ /// \param[in] _loop when true, the time is divided by the duration
+ /// (see GetLength)
+ public: std::map<std::string, ignition::math::Matrix4d> PoseAtX(
+ const double _x, const std::string &_node,
+ const bool _loop = true) const;
+
/// \brief Scales every animation in the animations list
/// \param[in] _scale the scaling factor
diff --git a/gazebo/common/SphericalCoordinates.cc b/gazebo/common/SphericalCoordinates.cc
index b3fede2..d1dd9ca 100644
--- a/gazebo/common/SphericalCoordinates.cc
+++ b/gazebo/common/SphericalCoordinates.cc
@@ -14,7 +14,6 @@
* limitations under the License.
*
*/
-
#include <string>
#include <math.h>
@@ -22,7 +21,6 @@
#include "gazebo/common/SphericalCoordinates.hh"
#include "gazebo/common/SphericalCoordinatesPrivate.hh"
-
using namespace gazebo;
using namespace common;
@@ -67,11 +65,26 @@ SphericalCoordinates::SphericalCoordinates(const SurfaceType _type)
//////////////////////////////////////////////////
SphericalCoordinates::SphericalCoordinates(const SurfaceType _type,
- const math::Angle &_latitude,
- const math::Angle &_longitude,
- double _elevation,
- const math::Angle &_heading)
- : dataPtr(new SphericalCoordinatesPrivate)
+ const math::Angle &_latitude,
+ const math::Angle &_longitude,
+ double _elevation,
+ const math::Angle &_heading)
+: dataPtr(new SphericalCoordinatesPrivate)
+{
+ 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()));
+}
+
+//////////////////////////////////////////////////
+SphericalCoordinates::SphericalCoordinates(const SurfaceType _type,
+ const ignition::math::Angle &_latitude,
+ const ignition::math::Angle &_longitude,
+ double _elevation,
+ const ignition::math::Angle &_heading)
+: dataPtr(new SphericalCoordinatesPrivate)
{
this->SetSurfaceType(_type);
this->SetLatitudeReference(_latitude);
@@ -96,12 +109,24 @@ SphericalCoordinates::SurfaceType SphericalCoordinates::GetSurfaceType() const
//////////////////////////////////////////////////
math::Angle SphericalCoordinates::GetLatitudeReference() const
{
+ return this->LatitudeReference();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle SphericalCoordinates::LatitudeReference() const
+{
return this->dataPtr->latitudeReference;
}
//////////////////////////////////////////////////
math::Angle SphericalCoordinates::GetLongitudeReference() const
{
+ return this->LongitudeReference();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle SphericalCoordinates::LongitudeReference() const
+{
return this->dataPtr->longitudeReference;
}
@@ -114,6 +139,12 @@ double SphericalCoordinates::GetElevationReference() const
//////////////////////////////////////////////////
math::Angle SphericalCoordinates::GetHeadingOffset() const
{
+ return this->HeadingOffset();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle SphericalCoordinates::HeadingOffset() const
+{
return this->dataPtr->headingOffset;
}
@@ -124,15 +155,31 @@ void SphericalCoordinates::SetSurfaceType(const SurfaceType &_type)
}
//////////////////////////////////////////////////
-void SphericalCoordinates::SetLatitudeReference(const math::Angle &_angle)
+void SphericalCoordinates::SetLatitudeReference(
+ const math::Angle &_angle)
+{
+ this->SetLatitudeReference(ignition::math::Angle(_angle.Radian()));
+}
+
+//////////////////////////////////////////////////
+void SphericalCoordinates::SetLatitudeReference(
+ const ignition::math::Angle &_angle)
+{
+ this->dataPtr->latitudeReference.Radian(_angle.Radian());
+}
+
+//////////////////////////////////////////////////
+void SphericalCoordinates::SetLongitudeReference(
+ const math::Angle &_angle)
{
- this->dataPtr->latitudeReference.SetFromRadian(_angle.Radian());
+ this->SetLongitudeReference(ignition::math::Angle(_angle.Radian()));
}
//////////////////////////////////////////////////
-void SphericalCoordinates::SetLongitudeReference(const math::Angle &_angle)
+void SphericalCoordinates::SetLongitudeReference(
+ const ignition::math::Angle &_angle)
{
- this->dataPtr->longitudeReference.SetFromRadian(_angle.Radian());
+ this->dataPtr->longitudeReference.Radian(_angle.Radian());
}
//////////////////////////////////////////////////
@@ -144,13 +191,26 @@ void SphericalCoordinates::SetElevationReference(double _elevation)
//////////////////////////////////////////////////
void SphericalCoordinates::SetHeadingOffset(const math::Angle &_angle)
{
- this->dataPtr->headingOffset.SetFromRadian(_angle.Radian());
+ this->SetHeadingOffset(ignition::math::Angle(_angle.Radian()));
+}
+
+//////////////////////////////////////////////////
+void SphericalCoordinates::SetHeadingOffset(const ignition::math::Angle &_angle)
+{
+ this->dataPtr->headingOffset.Radian(_angle.Radian());
}
//////////////////////////////////////////////////
math::Vector3 SphericalCoordinates::SphericalFromLocal(
const math::Vector3 &_xyz) const
{
+ return this->SphericalFromLocal(_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());
@@ -177,33 +237,40 @@ math::Vector3 SphericalCoordinates::SphericalFromLocal(
break;
}
- math::Vector3 spherical;
- double east = _xyz.x * headingCosine - _xyz.y * headingSine;
- double north = _xyz.x * headingSine + _xyz.y * headingCosine;
+ 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.
- math::Angle deltaLatitude(north / radiusMeridional);
- math::Angle deltaLongitude(east / radiusNormal);
+ ignition::math::Angle deltaLatitude(north / radiusMeridional);
+ ignition::math::Angle deltaLongitude(east / radiusNormal);
// geodetic latitude in degrees
- spherical.x = this->dataPtr->latitudeReference.Degree() +
- deltaLatitude.Degree();
+ spherical.X(this->dataPtr->latitudeReference.Degree() +
+ deltaLatitude.Degree());
// geodetic longitude in degrees
- spherical.y = this->dataPtr->longitudeReference.Degree() +
- deltaLongitude.Degree();
+ spherical.Y(this->dataPtr->longitudeReference.Degree() +
+ deltaLongitude.Degree());
// altitude relative to sea level
- spherical.z = this->dataPtr->elevationReference + _xyz.z;
+ spherical.Z(this->dataPtr->elevationReference + _xyz.Z());
return spherical;
}
//////////////////////////////////////////////////
-math::Vector3 SphericalCoordinates::GlobalFromLocal(const math::Vector3 &_xyz)
- const
+math::Vector3 SphericalCoordinates::GlobalFromLocal(
+ const math::Vector3 &_xyz) const
+{
+ return this->GlobalFromLocal(_xyz.Ign());
+}
+
+//////////////////////////////////////////////////
+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 math::Vector3(east, north, _xyz.z);
+ double east = _xyz.X() * headingCosine - _xyz.Y() * headingSine;
+ double north = _xyz.X() * headingSine + _xyz.Y() * headingCosine;
+ return ignition::math::Vector3d(east, north, _xyz.Z());
}
//////////////////////////////////////////////////
@@ -213,8 +280,21 @@ double SphericalCoordinates::Distance(const math::Angle &_latA,
const math::Angle &_latB,
const math::Angle &_lonB)
{
- math::Angle dLat = _latB - _latA;
- math::Angle dLon = _lonB - _lonA;
+ return Distance(ignition::math::Angle(_latA.Radian()),
+ ignition::math::Angle(_lonA.Radian()),
+ ignition::math::Angle(_latB.Radian()),
+ ignition::math::Angle(_lonB.Radian()));
+}
+
+//////////////////////////////////////////////////
+/// Based on Haversine formula (http://en.wikipedia.org/wiki/Haversine_formula).
+double SphericalCoordinates::Distance(const ignition::math::Angle &_latA,
+ const ignition::math::Angle &_lonA,
+ const ignition::math::Angle &_latB,
+ const ignition::math::Angle &_lonB)
+{
+ 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());
diff --git a/gazebo/common/SphericalCoordinates.hh b/gazebo/common/SphericalCoordinates.hh
index 4daab0c..24ea101 100644
--- a/gazebo/common/SphericalCoordinates.hh
+++ b/gazebo/common/SphericalCoordinates.hh
@@ -14,12 +14,15 @@
* limitations under the License.
*
*/
-
-#ifndef _SPHERICALCOORDINATES_HH_
-#define _SPHERICALCOORDINATES_HH_
+#ifndef _GAZEBO_SPHERICALCOORDINATES_HH_
+#define _GAZEBO_SPHERICALCOORDINATES_HH_
#include <string>
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Vector3.hh>
+
+#include "gazebo/common/CommonTypes.hh"
#include "gazebo/math/Angle.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/util/system.hh"
@@ -35,7 +38,7 @@ namespace gazebo
/// \class SphericalCoordinates SphericalCoordinates.hh commmon/common.hh
/// \brief Convert spherical coordinates for planetary surfaces.
- class GAZEBO_VISIBLE SphericalCoordinates
+ class GZ_COMMON_VISIBLE SphericalCoordinates
{
/// \enum SurfaceType
/// \brief Unique identifiers for planetary surface models.
@@ -59,11 +62,25 @@ namespace gazebo
/// \param[in] _longitude Reference longitude.
/// \param[in] _elevation Reference elevation.
/// \param[in] _heading Heading offset.
+ /// \deprecated See SphericalCoordinates constructor that accepts
+ /// ignition::math::Angle objects.
public: SphericalCoordinates(const SurfaceType _type,
- const math::Angle &_latitude,
- const math::Angle &_longitude,
+ const math::Angle &_latitude,
+ const math::Angle &_longitude,
+ double _elevation,
+ const math::Angle &_heading) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Constructor with surface type, angle, and elevation inputs.
+ /// \param[in] _type SurfaceType specification.
+ /// \param[in] _latitude Reference latitude.
+ /// \param[in] _longitude Reference longitude.
+ /// \param[in] _elevation Reference elevation.
+ /// \param[in] _heading Heading offset.
+ public: SphericalCoordinates(const SurfaceType _type,
+ const ignition::math::Angle &_latitude,
+ const ignition::math::Angle &_longitude,
double _elevation,
- const math::Angle &_heading);
+ const ignition::math::Angle &_heading);
/// \brief Destructor.
public: ~SphericalCoordinates();
@@ -72,13 +89,33 @@ namespace gazebo
/// \param[in] _xyz Cartesian position vector in gazebo's world frame.
/// \return Cooordinates: geodetic latitude (deg), longitude (deg),
/// altitude above sea level (m).
- public: math::Vector3 SphericalFromLocal(const math::Vector3 &_xyz) const;
+ /// \deprecated See SphericalFromLocal function that use
+ /// ignition::math::Vector3d objects.
+ public: math::Vector3 SphericalFromLocal(const math::Vector3 &_xyz) const
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Convert a Cartesian position vector to geodetic coordinates.
+ /// \param[in] _xyz Cartesian position vector in gazebo's world frame.
+ /// \return Cooordinates: geodetic latitude (deg), longitude (deg),
+ /// altitude above sea level (m).
+ public: ignition::math::Vector3d SphericalFromLocal(
+ const ignition::math::Vector3d &_xyz) const;
+
+ /// \brief Convert a Cartesian velocity vector in the local gazebo frame
+ /// to a global Cartesian frame with components East, North, Up.
+ /// \param[in] _xyz Cartesian vector in gazebo's world frame.
+ /// \return Rotated vector with components (x,y,z): (East, North, Up).
+ /// \deprecated See GlobalFromLocal function that use
+ /// ignition::math::Vector3d objects.
+ public: math::Vector3 GlobalFromLocal(const math::Vector3 &_xyz) const
+ GAZEBO_DEPRECATED(6.0);
/// \brief Convert a Cartesian velocity vector in the local gazebo frame
/// to a global Cartesian frame with components East, North, Up.
/// \param[in] _xyz Cartesian vector in gazebo's world frame.
/// \return Rotated vector with components (x,y,z): (East, North, Up).
- public: math::Vector3 GlobalFromLocal(const math::Vector3 &_xyz) const;
+ public: ignition::math::Vector3d GlobalFromLocal(
+ const ignition::math::Vector3d &_xyz) const;
/// \brief Convert a string to a SurfaceType.
/// \param[in] _str String to convert.
@@ -94,10 +131,27 @@ namespace gazebo
/// \param[in] _latB Latitude of point B.
/// \param[in] _longB Longitude of point B.
/// \return Distance in meters.
+ /// \deprecated See Distance() function that accepts
+ /// ignition::math::Angle objects.
public: static double Distance(const math::Angle &_latA,
- const math::Angle &_lonA,
- const math::Angle &_latB,
- const math::Angle &_lonB);
+ const math::Angle &_lonA,
+ const math::Angle &_latB,
+ const math::Angle &_lonB) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the distance between two points expressed in geographic
+ /// latitude and longitude. It assumes that both points are at sea level.
+ /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents
+ /// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W.
+ /// \param[in] _latA Latitude of point A.
+ /// \param[in] _longA Longitude of point A.
+ /// \param[in] _latB Latitude of point B.
+ /// \param[in] _longB Longitude of point B.
+ /// \return Distance in meters.
+ public: static double Distance(const ignition::math::Angle &_latA,
+ const ignition::math::Angle &_lonA,
+ const ignition::math::Angle &_latB,
+ const ignition::math::Angle &_lonB);
+
/// \brief Get SurfaceType currently in use.
/// \return Current SurfaceType value.
@@ -105,11 +159,23 @@ namespace gazebo
/// \brief Get reference geodetic latitude.
/// \return Reference geodetic latitude.
- public: math::Angle GetLatitudeReference() const;
+ /// \deprecated See LatitudeReference function that returns an
+ /// ignition::math::Angle object.
+ public: math::Angle GetLatitudeReference() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get reference geodetic latitude.
+ /// \return Reference geodetic latitude.
+ public: ignition::math::Angle LatitudeReference() const;
+
+ /// \brief Get reference longitude.
+ /// \return Reference longitude.
+ /// \deprecated See LongitudeReference function that returns an
+ /// ignition::math::Angle object.
+ public: math::Angle GetLongitudeReference() const GAZEBO_DEPRECATED(6.0);
/// \brief Get reference longitude.
/// \return Reference longitude.
- public: math::Angle GetLongitudeReference() const;
+ public: ignition::math::Angle LongitudeReference() const;
/// \brief Get reference elevation in meters.
/// \return Reference elevation.
@@ -119,7 +185,15 @@ namespace gazebo
/// angle from East to gazebo x-axis, or equivalently
/// from North to gazebo y-axis.
/// \return Heading offset of gazebo reference frame.
- public: math::Angle GetHeadingOffset() const;
+ /// \deprecated See HeadingOffset() function that returns an
+ /// ignition::math::Angle object.
+ public: math::Angle GetHeadingOffset() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get heading offset for gazebo reference frame, expressed as
+ /// angle from East to gazebo x-axis, or equivalently
+ /// from North to gazebo y-axis.
+ /// \return Heading offset of gazebo reference frame.
+ public: ignition::math::Angle HeadingOffset() const;
/// \brief Set SurfaceType for planetary surface model.
/// \param[in] _type SurfaceType value.
@@ -127,11 +201,25 @@ namespace gazebo
/// \brief Set reference geodetic latitude.
/// \param[in] _angle Reference geodetic latitude.
- public: void SetLatitudeReference(const math::Angle &_angle);
+ /// \deprecated See SetLatitudeReference function that accepts an
+ /// ignition::math::Angle object.
+ public: void SetLatitudeReference(const math::Angle &_angle)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set reference geodetic latitude.
+ /// \param[in] _angle Reference geodetic latitude.
+ public: void SetLatitudeReference(const ignition::math::Angle &_angle);
/// \brief Set reference longitude.
/// \param[in] _angle Reference longitude.
- public: void SetLongitudeReference(const math::Angle &_angle);
+ /// \deprecated See SetLongitudeReference function that accepts an
+ /// ignition::math::Angle object.
+ public: void SetLongitudeReference(const math::Angle &_angle)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set reference longitude.
+ /// \param[in] _angle Reference longitude.
+ public: void SetLongitudeReference(const ignition::math::Angle &_angle);
/// \brief Set reference elevation above sea level in meters.
/// \param[in] _elevation Reference elevation.
@@ -139,7 +227,14 @@ namespace gazebo
/// \brief Set heading angle offset for gazebo frame.
/// \param[in] _angle Heading offset for gazebo frame.
- public: void SetHeadingOffset(const math::Angle &_angle);
+ /// \deprecated See SetHeadingOffset function that accepts an
+ /// ignition::math::Angle object.
+ public: void SetHeadingOffset(const math::Angle &_angle)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set heading angle offset for gazebo frame.
+ /// \param[in] _angle Heading offset for gazebo frame.
+ public: void SetHeadingOffset(const ignition::math::Angle &_angle);
/// internal
/// \brief Pointer to the private data
diff --git a/gazebo/common/SphericalCoordinatesPrivate.hh b/gazebo/common/SphericalCoordinatesPrivate.hh
index 95c1078..994425e 100644
--- a/gazebo/common/SphericalCoordinatesPrivate.hh
+++ b/gazebo/common/SphericalCoordinatesPrivate.hh
@@ -15,14 +15,15 @@
*
*/
-#ifndef _SPHERICALCOORDINATES_PRIVATE_HH_
-#define _SPHERICALCOORDINATES_PRIVATE_HH_
+#ifndef _GAZEBO_SPHERICALCOORDINATES_PRIVATE_HH_
+#define _GAZEBO_SPHERICALCOORDINATES_PRIVATE_HH_
+
+#include <ignition/math/Angle.hh>
#include "gazebo/common/SphericalCoordinates.hh"
#include "gazebo/math/Angle.hh"
#include "gazebo/util/system.hh"
-
namespace gazebo
{
namespace common
@@ -35,23 +36,23 @@ namespace gazebo
/// \class SphericalCoordinatesPrivate SphericalCoordinatesPrivate.hh
/// commmon/common.hh
/// \brief Private data for the SphericalCoordinates class.
- class GAZEBO_VISIBLE SphericalCoordinatesPrivate
+ class GZ_COMMON_VISIBLE SphericalCoordinatesPrivate
{
/// \brief Type of surface being used.
public: SphericalCoordinates::SurfaceType surfaceType;
/// \brief Latitude of reference point.
- public: math::Angle latitudeReference;
+ public: ignition::math::Angle latitudeReference;
/// \brief Longitude of reference point.
- public: math::Angle longitudeReference;
+ public: ignition::math::Angle longitudeReference;
/// \brief Elevation of reference point relative to sea level in meters.
public: double elevationReference;
/// \brief Heading offset, expressed as angle from East to
/// gazebo x-axis, or equivalently from North to gazebo y-axis.
- public: math::Angle headingOffset;
+ public: ignition::math::Angle headingOffset;
};
/// \}
}
diff --git a/gazebo/common/SphericalCoordinates_TEST.cc b/gazebo/common/SphericalCoordinates_TEST.cc
index cb591c8..aa5ed38 100644
--- a/gazebo/common/SphericalCoordinates_TEST.cc
+++ b/gazebo/common/SphericalCoordinates_TEST.cc
@@ -37,9 +37,9 @@ TEST_F(SphericalCoordinatesTest, Constructor)
{
common::SphericalCoordinates sc;
EXPECT_EQ(sc.GetSurfaceType(), st);
- EXPECT_EQ(sc.GetLatitudeReference(), math::Angle());
- EXPECT_EQ(sc.GetLongitudeReference(), math::Angle());
- EXPECT_EQ(sc.GetHeadingOffset(), math::Angle());
+ EXPECT_EQ(sc.LatitudeReference(), ignition::math::Angle());
+ EXPECT_EQ(sc.LongitudeReference(), ignition::math::Angle());
+ EXPECT_EQ(sc.HeadingOffset(), ignition::math::Angle());
EXPECT_NEAR(sc.GetElevationReference(), 0.0, 1e-6);
}
@@ -47,21 +47,21 @@ TEST_F(SphericalCoordinatesTest, Constructor)
{
common::SphericalCoordinates sc(st);
EXPECT_EQ(sc.GetSurfaceType(), st);
- EXPECT_EQ(sc.GetLatitudeReference(), math::Angle());
- EXPECT_EQ(sc.GetLongitudeReference(), math::Angle());
- EXPECT_EQ(sc.GetHeadingOffset(), math::Angle());
+ EXPECT_EQ(sc.LatitudeReference(), ignition::math::Angle());
+ EXPECT_EQ(sc.LongitudeReference(), ignition::math::Angle());
+ EXPECT_EQ(sc.HeadingOffset(), ignition::math::Angle());
EXPECT_NEAR(sc.GetElevationReference(), 0.0, 1e-6);
}
// All arguments
{
- math::Angle lat(0.3), lon(-1.2), heading(0.5);
+ ignition::math::Angle lat(0.3), lon(-1.2), heading(0.5);
double elev = 354.1;
common::SphericalCoordinates sc(st, lat, lon, elev, heading);
EXPECT_EQ(sc.GetSurfaceType(), st);
- EXPECT_EQ(sc.GetLatitudeReference(), lat);
- EXPECT_EQ(sc.GetLongitudeReference(), lon);
- EXPECT_EQ(sc.GetHeadingOffset(), heading);
+ EXPECT_EQ(sc.LatitudeReference(), lat);
+ EXPECT_EQ(sc.LongitudeReference(), lon);
+ EXPECT_EQ(sc.HeadingOffset(), heading);
EXPECT_NEAR(sc.GetElevationReference(), elev, 1e-6);
}
}
@@ -88,13 +88,13 @@ TEST_F(SphericalCoordinatesTest, SetFunctions)
// Default parameters
common::SphericalCoordinates sc;
EXPECT_EQ(sc.GetSurfaceType(), st);
- EXPECT_EQ(sc.GetLatitudeReference(), math::Angle());
- EXPECT_EQ(sc.GetLongitudeReference(), math::Angle());
- EXPECT_EQ(sc.GetHeadingOffset(), math::Angle());
+ EXPECT_EQ(sc.LatitudeReference(), ignition::math::Angle());
+ EXPECT_EQ(sc.LongitudeReference(), ignition::math::Angle());
+ EXPECT_EQ(sc.HeadingOffset(), ignition::math::Angle());
EXPECT_NEAR(sc.GetElevationReference(), 0.0, 1e-6);
{
- math::Angle lat(0.3), lon(-1.2), heading(0.5);
+ ignition::math::Angle lat(0.3), lon(-1.2), heading(0.5);
double elev = 354.1;
sc.SetSurfaceType(st);
sc.SetLatitudeReference(lat);
@@ -103,9 +103,9 @@ TEST_F(SphericalCoordinatesTest, SetFunctions)
sc.SetElevationReference(elev);
EXPECT_EQ(sc.GetSurfaceType(), st);
- EXPECT_EQ(sc.GetLatitudeReference(), lat);
- EXPECT_EQ(sc.GetLongitudeReference(), lon);
- EXPECT_EQ(sc.GetHeadingOffset(), heading);
+ EXPECT_EQ(sc.LatitudeReference(), lat);
+ EXPECT_EQ(sc.LongitudeReference(), lon);
+ EXPECT_EQ(sc.HeadingOffset(), heading);
EXPECT_NEAR(sc.GetElevationReference(), elev, 1e-6);
}
}
@@ -120,64 +120,65 @@ TEST_F(SphericalCoordinatesTest, CoordinateTransforms)
{
// Parameters
- math::Angle lat(0.3), lon(-1.2), heading(math::Angle::HalfPi);
+ ignition::math::Angle lat(0.3), lon(-1.2),
+ heading(ignition::math::Angle::HalfPi);
double elev = 354.1;
common::SphericalCoordinates sc(st, lat, lon, elev, heading);
// Check GlobalFromLocal with heading offset of 90 degrees
{
// local frame
- math::Vector3 xyz;
+ ignition::math::Vector3d xyz;
// east, north, up
- math::Vector3 enu;
+ ignition::math::Vector3d enu;
xyz.Set(1, 0, 0);
enu = sc.GlobalFromLocal(xyz);
- EXPECT_NEAR(enu.y, xyz.x, 1e-6);
- EXPECT_NEAR(enu.x, -xyz.y, 1e-6);
+ EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6);
+ EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6);
xyz.Set(0, 1, 0);
enu = sc.GlobalFromLocal(xyz);
- EXPECT_NEAR(enu.y, xyz.x, 1e-6);
- EXPECT_NEAR(enu.x, -xyz.y, 1e-6);
+ EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6);
+ EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6);
xyz.Set(1, -1, 0);
enu = sc.GlobalFromLocal(xyz);
- EXPECT_NEAR(enu.y, xyz.x, 1e-6);
- EXPECT_NEAR(enu.x, -xyz.y, 1e-6);
+ EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6);
+ EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6);
xyz.Set(2243.52334, 556.35, 435.6553);
enu = sc.GlobalFromLocal(xyz);
- EXPECT_NEAR(enu.y, xyz.x, 1e-6);
- EXPECT_NEAR(enu.x, -xyz.y, 1e-6);
+ EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6);
+ EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6);
}
// Check SphericalFromLocal
{
// local frame
- math::Vector3 xyz;
+ ignition::math::Vector3d xyz;
// spherical coordinates
- math::Vector3 sph;
+ ignition::math::Vector3d sph;
// No offset
xyz.Set(0, 0, 0);
sph = sc.SphericalFromLocal(xyz);
// latitude
- EXPECT_NEAR(sph.x, lat.Degree(), 1e-6);
+ EXPECT_NEAR(sph.X(), lat.Degree(), 1e-6);
// longitude
- EXPECT_NEAR(sph.y, lon.Degree(), 1e-6);
+ EXPECT_NEAR(sph.Y(), lon.Degree(), 1e-6);
// elevation
- EXPECT_NEAR(sph.z, elev, 1e-6);
+ EXPECT_NEAR(sph.Z(), elev, 1e-6);
// 200 km offset in x (pi/2 heading offset means North)
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);
+ EXPECT_NEAR(sph.X(), lat.Degree() + 1.8, 0.008);
// no change in longitude
- EXPECT_NEAR(sph.y, lon.Degree(), 1e-6);
+ EXPECT_NEAR(sph.Y(), lon.Degree(), 1e-6);
// no change in elevation
- EXPECT_NEAR(sph.z, elev, 1e-6);
+ EXPECT_NEAR(sph.Z(), elev, 1e-6);
}
}
}
@@ -186,11 +187,11 @@ TEST_F(SphericalCoordinatesTest, CoordinateTransforms)
// Test distance
TEST_F(SphericalCoordinatesTest, Distance)
{
- math::Angle latA, longA, latB, longB;
- latA.SetFromDegree(46.250944);
- longA.SetFromDegree(-122.249972);
- latB.SetFromDegree(46.124953);
- longB.SetFromDegree(-122.251683);
+ ignition::math::Angle latA, longA, latB, longB;
+ latA.Degree(46.250944);
+ longA.Degree(-122.249972);
+ latB.Degree(46.124953);
+ longB.Degree(-122.251683);
double d = common::SphericalCoordinates::Distance(latA, longA, latB, longB);
EXPECT_NEAR(14002, d, 20);
diff --git a/gazebo/common/SystemPaths.cc b/gazebo/common/SystemPaths.cc
index 5911842..552c55d 100644
--- a/gazebo/common/SystemPaths.cc
+++ b/gazebo/common/SystemPaths.cc
@@ -15,17 +15,24 @@
*
*/
-#include <dirent.h>
-#include <sys/stat.h>
-#include <sys/types.h>
-
-#include <boost/filesystem.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
+#include <boost/filesystem.hpp>
#include <sdf/sdf.hh>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+// See below for Windows dirent include. cpplint complains about system
+// header order if "win_dirent.h" is in the wrong location.
+#ifndef _WIN32
+ #include <dirent.h>
+#else
+ #include "win_dirent.h"
+#endif
+
#include "gazebo/common/Console.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/ModelDatabase.hh"
@@ -85,7 +92,11 @@ SystemPaths::SystemPaths()
DIR *dir = opendir(fullPath.c_str());
if (!dir)
{
+#ifdef _WIN32
+ mkdir(fullPath.c_str());
+#else
mkdir(fullPath.c_str(), S_IRWXU | S_IRGRP | S_IROTH);
+#endif
}
else
closedir(dir);
diff --git a/gazebo/common/SystemPaths.hh b/gazebo/common/SystemPaths.hh
index ddc0d0e..e3b8d19 100644
--- a/gazebo/common/SystemPaths.hh
+++ b/gazebo/common/SystemPaths.hh
@@ -20,7 +20,7 @@
#include <stdio.h>
#define LINUX
-#ifdef WINDOWS
+#ifdef _WIN32
#include <direct.h>
#define GetCurrentDir _getcwd
#else
@@ -53,7 +53,7 @@ namespace gazebo
/// Should point to Ogre RenderSystem_GL.so et. al.
/// \li SystemPaths#pluginPaths - plugin library paths
/// for common::WorldPlugin
- class GAZEBO_VISIBLE SystemPaths : public SingletonT<SystemPaths>
+ class GZ_COMMON_VISIBLE SystemPaths : public SingletonT<SystemPaths>
{
/// Constructor for SystemPaths
private: SystemPaths();
diff --git a/gazebo/common/SystemPaths_TEST.cc b/gazebo/common/SystemPaths_TEST.cc
index f42a13b..7f47944 100644
--- a/gazebo/common/SystemPaths_TEST.cc
+++ b/gazebo/common/SystemPaths_TEST.cc
@@ -54,7 +54,7 @@ TEST_F(SystemPathsTest, SystemPaths)
std::string gzResourcePath = "GAZEBO_RESOURCE_PATH=" + paths->GetTmpPath() +
"/resource:/test/me/now";
putenv(const_cast<char*>(gzResourcePath.c_str()));
- const std::list<std::string> pathList1 = paths->GetGazeboPaths();
+ const std::list<std::string> &pathList1 = paths->GetGazeboPaths();
EXPECT_EQ(static_cast<unsigned int>(2), pathList1.size());
EXPECT_STREQ((paths->GetTmpPath() + "/resource").c_str(),
pathList1.front().c_str());
@@ -63,7 +63,7 @@ TEST_F(SystemPathsTest, SystemPaths)
std::string ogreResourcePath = "OGRE_RESOURCE_PATH=" + paths->GetTmpPath() +
"/ogre:/test/ogre/now";
putenv(const_cast<char*>(ogreResourcePath.c_str()));
- const std::list<std::string> pathList2 = paths->GetOgrePaths();
+ const std::list<std::string> &pathList2 = paths->GetOgrePaths();
EXPECT_EQ(static_cast<unsigned int>(2), pathList2.size());
EXPECT_STREQ((paths->GetTmpPath() + "/ogre").c_str(),
pathList2.front().c_str());
@@ -72,7 +72,7 @@ TEST_F(SystemPathsTest, SystemPaths)
std::string gzPluginPath = "GAZEBO_PLUGIN_PATH=" + paths->GetTmpPath() +
"/plugin:/test/plugin/now";
putenv(const_cast<char*>(gzPluginPath.c_str()));
- const std::list<std::string> pathList3 = paths->GetPluginPaths();
+ const std::list<std::string> &pathList3 = paths->GetPluginPaths();
EXPECT_EQ(static_cast<unsigned int>(2), pathList3.size());
EXPECT_STREQ((paths->GetTmpPath() + "/plugin").c_str(),
pathList3.front().c_str());
diff --git a/gazebo/common/Time.cc b/gazebo/common/Time.cc
index 97b41c7..dad57df 100644
--- a/gazebo/common/Time.cc
+++ b/gazebo/common/Time.cc
@@ -14,23 +14,30 @@
* limitations under the License.
*
*/
-/* Desc: Time class
- * Author: Nate Koenig
- * Date: 3 Apr 2007
- */
-#include <sys/time.h>
+#ifdef _WIN32
+ #include <Windows.h>
+ #include <Winsock2.h>
+ #include <cstdint>
+ struct timespec
+ {
+ int64_t tv_sec;
+ int64_t tv_nsec;
+ };
+#else
+ #include <unistd.h>
+ #include <sys/time.h>
+#endif
+
#include <time.h>
#include <math.h>
-#include <unistd.h>
#include <boost/date_time.hpp>
#ifdef __MACH__
-#include <mach/clock.h>
-#include <mach/mach.h>
+ #include <mach/clock.h>
+ #include <mach/mach.h>
#endif
-#include "gazebo/math/Helpers.hh"
#include "gazebo/common/Time.hh"
#include "gazebo/common/Console.hh"
@@ -41,8 +48,11 @@ Time Time::wallTime;
std::string Time::wallTimeISO;
struct timespec Time::clockResolution;
-
const Time Time::Zero = common::Time(0, 0);
+const Time Time::Second = common::Time(1, 0);
+const Time Time::Hour = common::Time(3600, 0);
+const int32_t Time::nsInSec = 1000000000L;
+const int32_t Time::nsInMs = 1000000;
/////////////////////////////////////////////////
Time::Time()
@@ -52,6 +62,13 @@ Time::Time()
#ifdef __MACH__
clockResolution.tv_sec = 1 / sysconf(_SC_CLK_TCK);
+#elif defined(_WIN32)
+ LARGE_INTEGER freq;
+ QueryPerformanceFrequency(&freq);
+ double period = 1.0/freq.QuadPart;
+ clockResolution.tv_sec = static_cast<int64_t>(floor(period));
+ clockResolution.tv_nsec =
+ static_cast<int64_t>((period - floor(period)) * nsInSec);
#else
// get clock resolution, skip sleep if resolution is larger then
// requested sleep time
@@ -110,6 +127,83 @@ const Time &Time::GetWallTime()
mach_port_deallocate(mach_task_self(), cclock);
tv.tv_sec = mts.tv_sec;
tv.tv_nsec = mts.tv_nsec;
+#elif defined(_WIN32)
+ // Borrowed from roscpp_core/rostime/src/time.cpp
+ // Win32 implementation
+ // unless I've missed something obvious, the only way to get high-precision
+ // time on Windows is via the QueryPerformanceCounter() call. However,
+ // this is somewhat problematic in Windows XP on some processors, especially
+ // AMD, because the Windows implementation can freak out when the CPU clocks
+ // down to save power. Time can jump or even go backwards. Microsoft has
+ // fixed this bug for most systems now, but it can still show up if you have
+ // not installed the latest CPU drivers (an oxymoron). They fixed all these
+ // problems in Windows Vista, and this API is by far the most accurate that
+ // I know of in Windows, so I'll use it here despite all these caveats
+ static LARGE_INTEGER cpuFreq, initCpuTime;
+ static uint32_t startSec = 0;
+ static uint32_t startNSec = 0;
+ if ((startSec == 0) && (startNSec == 0))
+ {
+ QueryPerformanceFrequency(&cpuFreq);
+ QueryPerformanceCounter(&initCpuTime);
+
+ // compute an offset from the Epoch using the lower-performance timer API
+ FILETIME ft;
+ GetSystemTimeAsFileTime(&ft);
+ LARGE_INTEGER startLi;
+ startLi.LowPart = ft.dwLowDateTime;
+ startLi.HighPart = ft.dwHighDateTime;
+
+ // why did they choose 1601 as the time zero, instead of 1970?
+ // there were no outstanding hard rock bands in 1601.
+#ifdef _MSC_VER
+ startLi.QuadPart -= 116444736000000000Ui64;
+#else
+ startLi.QuadPart -= 116444736000000000ULL;
+#endif
+
+ // 100-ns units. odd.
+ startSec = static_cast<uint32_t>(startLi.QuadPart / 10000000);
+ startNSec = (startLi.LowPart % 10000000) * 100;
+ }
+
+ LARGE_INTEGER curTime;
+ QueryPerformanceCounter(&curTime);
+ LARGE_INTEGER deltaCpuTime;
+ deltaCpuTime.QuadPart = curTime.QuadPart - initCpuTime.QuadPart;
+
+ // todo: how to handle cpu clock drift. not sure it's a big deal for us.
+ // also, think about clock wraparound. seems extremely unlikey, but possible
+ double dDeltaCpuTime = deltaCpuTime.QuadPart /
+ static_cast<double>(cpuFreq.QuadPart);
+ uint32_t deltaSec = static_cast<uint32_t>(floor(dDeltaCpuTime));
+ uint32_t deltaNSec = static_cast<uint32_t>(
+ std::round((dDeltaCpuTime-deltaSec) * nsInSec));
+
+ int64_t secSum = static_cast<int64_t>(startSec) +
+ static_cast<int64_t>(deltaSec);
+ int64_t nsecSum = static_cast<int64_t>(startNSec) +
+ static_cast<int64_t>(deltaNSec);
+
+ // Normalize
+ {
+ int64_t nsecPart = nsecSum % nsInSec;
+ int64_t secPart = secSum + nsecSum / nsInSec;
+ if (nsecPart < 0)
+ {
+ nsecPart += nsInSec;
+ --secPart;
+ }
+
+ if (secPart < 0 || secPart > UINT_MAX)
+ gzerr << "Time is out of dual 32-bit range\n";
+
+ secSum = secPart;
+ nsecSum = nsecPart;
+ }
+
+ tv.tv_sec = secSum;
+ tv.tv_nsec = nsecSum;
#else
clock_gettime(0, &tv);
#endif
@@ -145,7 +239,7 @@ void Time::Set(int32_t _sec, int32_t _nsec)
void Time::Set(double _seconds)
{
this->sec = (int32_t)(floor(_seconds));
- this->nsec = (int32_t)(round((_seconds - this->sec) * 1e9));
+ this->nsec = (int32_t)(round((_seconds - this->sec) * nsInSec));
this->Correct();
}
@@ -163,6 +257,111 @@ float Time::Float() const
}
/////////////////////////////////////////////////
+std::string Time::FormattedString(FormatOption _start, FormatOption _end) const
+{
+ if (_start > MILLISECONDS)
+ {
+ gzwarn << "Invalid start [" << _start << "], using millisecond [4]." <<
+ std::endl;
+ _start = MILLISECONDS;
+ }
+
+ if (_end < _start)
+ {
+ gzwarn << "Invalid end [" << _end << "], using start [" << _start << "]."
+ << std::endl;
+ _end = _start;
+ }
+
+ if (_end > MILLISECONDS)
+ {
+ gzwarn << "Invalid end [" << _end << "], using millisecond [4]." <<
+ std::endl;
+ _end = MILLISECONDS;
+ }
+
+ std::ostringstream stream;
+ unsigned int s, msec;
+
+ stream.str("");
+
+ // Get seconds
+ s = this->sec;
+
+ // Get milliseconds
+ msec = this->nsec / nsInMs;
+
+ // Get seconds from milliseconds
+ int seconds = msec / 1000;
+ msec -= seconds * 1000;
+ s += seconds;
+
+ // Days
+ if (_start <= 0)
+ {
+ unsigned int day = s / 86400;
+ s -= day * 86400;
+ stream << std::setw(2) << std::setfill('0') << day;
+ }
+
+ // Hours
+ if (_end >= 1)
+ {
+ if (_start < 1)
+ stream << " ";
+
+ if (_start <= 1)
+ {
+ unsigned int hour = s / 3600;
+ s -= hour * 3600;
+ stream << std::setw(2) << std::setfill('0') << hour;
+ }
+ }
+
+ // Minutes
+ if (_end >= 2)
+ {
+ if (_start < 2)
+ stream << ":";
+
+ if (_start <= 2)
+ {
+ unsigned int min = s / 60;
+ s -= min * 60;
+ stream << std::setw(2) << std::setfill('0') << min;
+ }
+ }
+
+ // Seconds
+ if (_end >= 3)
+ {
+ if (_start < 3)
+ stream << ":";
+
+ if (_start <= 3)
+ {
+ stream << std::setw(2) << std::setfill('0') << s;
+ }
+ }
+
+ // Milliseconds
+ if (_end >= 4)
+ {
+ if (_start < 4)
+ stream << ".";
+ else
+ msec = msec + s * 1000;
+
+ if (_start <= 4)
+ {
+ stream << std::setw(3) << std::setfill('0') << msec;
+ }
+ }
+
+ return stream.str();
+}
+
+/////////////////////////////////////////////////
Time Time::Sleep(const common::Time &_time)
{
Time result;
@@ -191,13 +390,46 @@ Time Time::Sleep(const common::Time &_time)
#ifdef __MACH__
if (nanosleep(&interval, &remainder) == -1)
+ {
+ result.sec = remainder.tv_sec;
+ result.nsec = remainder.tv_nsec;
+ }
+#elif defined(_WIN32)
+ // Borrowed from roscpp_core/rostime/src/time.cpp
+ HANDLE timer = NULL;
+ LARGE_INTEGER sleepTime;
+ sleepTime.QuadPart = -
+ static_cast<int64_t>(interval.tv_sec)*10000000LL -
+ static_cast<int64_t>(interval.tv_nsec) / 100LL;
+
+ timer = CreateWaitableTimer(NULL, TRUE, NULL);
+ if (timer == NULL)
+ {
+ gzerr << "Unable to create waitable timer. Sleep will be incorrect.\n";
+ return result;
+ }
+
+ if (!SetWaitableTimer (timer, &sleepTime, 0, NULL, NULL, 0))
+ {
+ gzerr << "Unable to use waitable timer. Sleep will be incorrect.\n";
+ return result;
+ }
+
+ if (WaitForSingleObject (timer, INFINITE) != WAIT_OBJECT_0)
+ {
+ gzerr << "Unable to wait for a single object. Sleep will be incorrect.\n";
+ return result;
+ }
+
+ result.sec = 0;
+ result.nsec = 0;
#else
if (clock_nanosleep(CLOCK_REALTIME, 0, &interval, &remainder) == -1)
-#endif
{
result.sec = remainder.tv_sec;
result.nsec = remainder.tv_nsec;
}
+#endif
}
else
{
diff --git a/gazebo/common/Time.hh b/gazebo/common/Time.hh
index 251e0c9..1fdc9d6 100644
--- a/gazebo/common/Time.hh
+++ b/gazebo/common/Time.hh
@@ -14,13 +14,9 @@
* limitations under the License.
*
*/
-/* Desc: External interfaces for Gazebo
- * Author: Nate Koenig
- * Date: 03 Apr 2007
- */
-#ifndef _TIME_HH_
-#define _TIME_HH_
+#ifndef _GAZEBO_COMMON_TIME_HH_
+#define _GAZEBO_COMMON_TIME_HH_
#include <string>
#include <stdlib.h>
@@ -40,11 +36,33 @@ namespace gazebo
/// \class Time Time.hh common/common.hh
/// \brief A Time class, can be used to hold wall- or sim-time.
/// stored as sec and nano-sec.
- class GAZEBO_VISIBLE Time
+ class GZ_COMMON_VISIBLE Time
{
/// \brief A static zero time variable set to common::Time(0, 0).
public: static const Time Zero;
+ /// \brief A static time variable set to a second: common::Time(1, 0).
+ public: static const Time Second;
+
+ /// \brief A static time variable set to an hour: common::Time(3600, 0).
+ public: static const Time Hour;
+
+ /// \enum Format options
+ /// \brief Options for formatting time as a string.
+ public: enum FormatOption
+ {
+ /// \brief Days
+ DAYS = 0,
+ /// \brief Hours
+ HOURS = 1,
+ /// \brief Minutes
+ MINUTES = 2,
+ /// \brief Seconds
+ SECONDS = 3,
+ /// \brief Milliseconds
+ MILLISECONDS = 4
+ };
+
/// \brief Constructors
public: Time();
@@ -100,6 +118,14 @@ namespace gazebo
/// \return Time as a float in seconds
public: float Float() const;
+ /// \brief Get the time as a string formatted as "DD hh:mm:ss.mmm", with
+ /// the option to choose the start/end.
+ /// \param[in] _start Start point.
+ /// \param[in] _end End point.
+ /// \return String representing time.
+ public: std::string FormattedString(FormatOption _start = DAYS,
+ FormatOption _end = MILLISECONDS) const;
+
/// \brief Sleep for the specified time
/// \param[in] _time Sleep time
/// \return Time actually slept
@@ -431,22 +457,30 @@ namespace gazebo
// In the case sec and nsec have different signs, normalize
if (this->sec > 0 && this->nsec < 0)
{
- int32_t n = abs(this->nsec / 1e9) + 1;
+ int32_t n = abs(this->nsec / this->nsInSec) + 1;
this->sec -= n;
- this->nsec += n * 1e9;
+ this->nsec += n * this->nsInSec;
}
if (this->sec < 0 && this->nsec > 0)
{
- int32_t n = abs(this->nsec / 1e9) + 1;
+ int32_t n = abs(this->nsec / this->nsInSec) + 1;
this->sec += n;
- this->nsec -= n * 1e9;
+ this->nsec -= n * this->nsInSec;
}
// Make any corrections
- this->sec += this->nsec / static_cast<int32_t>(1e9);
- this->nsec = this->nsec % static_cast<int32_t>(1e9);
+ this->sec += this->nsec / this->nsInSec;
+ this->nsec = this->nsec % this->nsInSec;
}
+
private: static struct timespec clockResolution;
+
+ /// \brief Constant multiplier to convert from nanoseconds to seconds.
+ private: static const int32_t nsInSec;
+
+ /// \brief Constant multiplier to convert from nanoseconds to
+ /// milliseconds.
+ private: static const int32_t nsInMs;
};
/// \}
}
diff --git a/gazebo/common/Time_TEST.cc b/gazebo/common/Time_TEST.cc
index e7ad62b..a681116 100644
--- a/gazebo/common/Time_TEST.cc
+++ b/gazebo/common/Time_TEST.cc
@@ -18,7 +18,6 @@
#include <sys/time.h>
#include <gtest/gtest.h>
-#include "gazebo/common/Timer.hh"
#include "gazebo/common/Time.hh"
#include "test/util.hh"
@@ -26,13 +25,9 @@ using namespace gazebo;
class TimeTest : public gazebo::testing::AutoLogFixture { };
+/////////////////////////////////////////////////
TEST_F(TimeTest, Time)
{
- common::Timer timer;
- timer.Start();
- common::Time::MSleep(100);
- EXPECT_TRUE(timer.GetElapsed() > common::Time(0, 100000000));
-
struct timeval tv;
gettimeofday(&tv, NULL);
common::Time time(tv);
@@ -136,6 +131,101 @@ TEST_F(TimeTest, Time)
EXPECT_DOUBLE_EQ(nsec, common::Time::MicToNano(usec));
}
+/////////////////////////////////////////////////
+TEST_F(TimeTest, String)
+{
+ common::Time time(0);
+
+ // Several combinations
+ EXPECT_EQ(time.FormattedString(), "00 00:00:00.000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::HOURS),
+ "00:00:00.000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::MINUTES),
+ "00:00.000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::SECONDS),
+ "00.000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::MILLISECONDS),
+ "000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::DAYS,
+ common::Time::FormatOption::MILLISECONDS),
+ "00 00:00:00.000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::DAYS,
+ common::Time::FormatOption::SECONDS),
+ "00 00:00:00");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::DAYS,
+ common::Time::FormatOption::MINUTES),
+ "00 00:00");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::DAYS,
+ common::Time::FormatOption::HOURS),
+ "00 00");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::DAYS,
+ common::Time::FormatOption::DAYS),
+ "00");
+
+ // start > end: start pushes end
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::MILLISECONDS,
+ common::Time::FormatOption::MINUTES),
+ "000");
+
+ // 1 second
+ time = common::Time(0, 1000000000);
+ EXPECT_EQ(time.FormattedString(), "00 00:00:01.000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::MILLISECONDS,
+ common::Time::FormatOption::MILLISECONDS),
+ "1000");
+
+ // 30.5 seconds
+ time = common::Time(30, 500000000);
+ EXPECT_EQ(time.FormattedString(), "00 00:00:30.500");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::SECONDS,
+ common::Time::FormatOption::SECONDS),
+ "30");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::MILLISECONDS,
+ common::Time::FormatOption::MILLISECONDS),
+ "30500");
+
+ // 1 min
+ time = common::Time(60);
+ EXPECT_EQ(time.FormattedString(), "00 00:01:00.000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::SECONDS,
+ common::Time::FormatOption::SECONDS),
+ "60");
+
+ // 2.5 hours
+ time = common::Time(9000);
+ EXPECT_EQ(time.FormattedString(), "00 02:30:00.000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::HOURS,
+ common::Time::FormatOption::MINUTES),
+ "02:30");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::SECONDS,
+ common::Time::FormatOption::SECONDS),
+ "9000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::MINUTES,
+ common::Time::FormatOption::MINUTES),
+ "150");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::HOURS,
+ common::Time::FormatOption::HOURS),
+ "02");
+
+ // 3 days
+ time = common::Time(259200);
+ EXPECT_EQ(time.FormattedString(), "03 00:00:00.000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::DAYS,
+ common::Time::FormatOption::DAYS),
+ "03");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::HOURS),
+ "72:00:00.000");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::HOURS,
+ common::Time::FormatOption::HOURS),
+ "72");
+ EXPECT_EQ(time.FormattedString(common::Time::FormatOption::MINUTES,
+ common::Time::FormatOption::MINUTES),
+ "4320");
+
+ // Large time
+ time = common::Time(1234567890, 123456789);
+ EXPECT_EQ(time.FormattedString(), "14288 23:31:30.123");
+}
/////////////////////////////////////////////////
int main(int argc, char **argv)
diff --git a/gazebo/common/Timer.cc b/gazebo/common/Timer.cc
index 8752c88..bc718d3 100644
--- a/gazebo/common/Timer.cc
+++ b/gazebo/common/Timer.cc
@@ -26,7 +26,13 @@ using namespace common;
//////////////////////////////////////////////////
Timer::Timer()
- : reset(true), running(false)
+ : reset(true), running(false), countdown(false)
+{
+}
+
+//////////////////////////////////////////////////
+Timer::Timer(const Time &_maxTime, const bool _countdown)
+ : reset(true), running(false), countdown(_countdown), maxTime(_maxTime)
{
}
@@ -43,6 +49,11 @@ void Timer::Start()
this->start = Time::GetWallTime();
this->reset = false;
}
+ else if (!this->running)
+ {
+ // Add the time that has elapsed since stopping to the start time.
+ this->start += (Time::GetWallTime() - this->stop);
+ }
this->running = true;
}
@@ -71,13 +82,26 @@ bool Timer::GetRunning() const
//////////////////////////////////////////////////
Time Timer::GetElapsed() const
{
+ Time elapsedTime;
if (this->running)
{
- Time currentTime;
- currentTime = Time::GetWallTime();
-
- return currentTime - this->start;
+ elapsedTime = Time::GetWallTime() - this->start;
}
else
- return this->stop - this->start;
+ {
+ elapsedTime = this->stop - this->start;
+ }
+
+ // If we're counting down, return the countdown time minus the total
+ // elapsed time.
+ if (this->countdown)
+ {
+ if (elapsedTime > this->maxTime)
+ {
+ // If elapsed time is past the countdown time, return 0 (out of time)
+ return Time::Zero;
+ }
+ return this->maxTime - elapsedTime;
+ }
+ return elapsedTime;
}
diff --git a/gazebo/common/Timer.hh b/gazebo/common/Timer.hh
index d738e40..b327d99 100644
--- a/gazebo/common/Timer.hh
+++ b/gazebo/common/Timer.hh
@@ -35,11 +35,17 @@ namespace gazebo
/// \class Timer Timer.hh common/common.hh
/// \brief A timer class, used to time things in real world walltime
- class GAZEBO_VISIBLE Timer
+ class GZ_COMMON_VISIBLE Timer
{
- /// \brief Constructor
+ /// \brief Default constructor
public: Timer();
+ /// \brief Countdown constructor
+ /// \param[in] _maxTime The time to count down from.
+ /// \param[in] _countdown. Whether or not to count down. True by default
+ /// if this constructor is used.
+ public: Timer(const Time &_maxTime, const bool _countdown = true);
+
/// \brief Destructor
public: virtual ~Timer();
@@ -74,11 +80,18 @@ namespace gazebo
/// \brief True if the timer is running.
private: bool running;
+ /// \brief True if the timer is counting down, false otherwise.
+ /// False by default.
+ private: bool countdown;
+
/// \brief The time of the last call to Start
private: Time start;
/// \brief The time when Stop was called.
private: Time stop;
+
+ /// \brief Maximum time, only used in countdown.
+ private: Time maxTime;
};
/// \}
}
diff --git a/gazebo/common/Timer_TEST.cc b/gazebo/common/Timer_TEST.cc
new file mode 100644
index 0000000..93159ca
--- /dev/null
+++ b/gazebo/common/Timer_TEST.cc
@@ -0,0 +1,108 @@
+/*
+ * 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/common/Timer.hh"
+#include "test/util.hh"
+
+using namespace gazebo;
+
+class TimerTest : public gazebo::testing::AutoLogFixture
+{
+};
+
+/////////////////////////////////////////////////
+TEST_F(TimerTest, Timer)
+{
+ common::Timer timer;
+ // Expect the timer to be paused on initialization and at 0 time
+ EXPECT_EQ(timer.GetElapsed().sec, 0);
+ EXPECT_EQ(timer.GetElapsed().nsec, 0);
+ EXPECT_FALSE(timer.GetRunning());
+
+ timer.Start();
+ common::Time::MSleep(200);
+ EXPECT_TRUE(timer.GetRunning());
+ common::Time elapsed = timer.GetElapsed();
+ EXPECT_GE(elapsed.nsec, 200000000);
+ EXPECT_LE(elapsed.nsec, 200200000);
+ EXPECT_EQ(elapsed.sec, 0);
+
+ // Check that the time after stopping and sleeping matches the time right
+ // before pausing
+ timer.Stop();
+ common::Time::MSleep(100);
+ EXPECT_GE(elapsed.nsec, timer.GetElapsed().nsec - 100000);
+ EXPECT_LE(elapsed.nsec, timer.GetElapsed().nsec + 100000);
+ EXPECT_EQ(elapsed.sec, timer.GetElapsed().sec);
+ EXPECT_FALSE(timer.GetRunning());
+
+ // Expect that we start from where we left off
+ timer.Start();
+ common::Time::MSleep(100);
+ EXPECT_GE(timer.GetElapsed().nsec, 300000000);
+ EXPECT_LE(timer.GetElapsed().nsec, 301000000);
+ EXPECT_EQ(timer.GetElapsed().sec, 0);
+
+ // Expect reset to reset the current time and stop the timer
+ timer.Reset();
+ EXPECT_EQ(timer.GetElapsed().sec, 0);
+ EXPECT_EQ(timer.GetElapsed().nsec, 0);
+ EXPECT_FALSE(timer.GetRunning());
+}
+
+/////////////////////////////////////////////////
+TEST_F(TimerTest, CountdownTimer)
+{
+ // Count down from 1 second
+ common::Time maxTime(1);
+ common::Timer timer(maxTime, true);
+ EXPECT_EQ(timer.GetElapsed().sec, 1);
+ EXPECT_EQ(timer.GetElapsed().nsec, 0);
+ EXPECT_FALSE(timer.GetRunning());
+
+ timer.Start();
+ common::Time::MSleep(200);
+ EXPECT_TRUE(timer.GetRunning());
+ common::Time elapsed = timer.GetElapsed();
+ EXPECT_GE(elapsed.nsec, 799800000);
+ EXPECT_LE(elapsed.nsec, 800100000);
+ EXPECT_EQ(elapsed.sec, 0);
+
+ // Check that the time after stopping and sleeping matches the time right
+ // before pausing
+ timer.Stop();
+ common::Time::MSleep(100);
+ EXPECT_GE(elapsed.nsec, timer.GetElapsed().nsec - 100000);
+ EXPECT_LE(elapsed.nsec, timer.GetElapsed().nsec + 100000);
+ EXPECT_EQ(elapsed.sec, timer.GetElapsed().sec);
+ EXPECT_FALSE(timer.GetRunning());
+
+ // Expect that we start from where we left off
+ timer.Start();
+ common::Time::MSleep(100);
+ EXPECT_GE(timer.GetElapsed().nsec, 699000000);
+ EXPECT_LE(timer.GetElapsed().nsec, 701000000);
+ EXPECT_EQ(timer.GetElapsed().sec, 0);
+
+ // Expect reset to reset the current time and stop the timer
+ timer.Reset();
+ EXPECT_EQ(timer.GetElapsed().sec, 1);
+ EXPECT_EQ(timer.GetElapsed().nsec, 0);
+ EXPECT_FALSE(timer.GetRunning());
+}
diff --git a/gazebo/common/UpdateInfo.hh b/gazebo/common/UpdateInfo.hh
index e119eb1..132eddb 100644
--- a/gazebo/common/UpdateInfo.hh
+++ b/gazebo/common/UpdateInfo.hh
@@ -27,7 +27,7 @@ namespace gazebo
{
/// \class UpdateInfo UpdateInfo.hh common/common.hh
/// \brief Information for use in an update event.
- class GAZEBO_VISIBLE UpdateInfo
+ class GZ_COMMON_VISIBLE UpdateInfo
{
/// \brief Name of the world.
public: std::string worldName;
diff --git a/gazebo/common/Video.cc b/gazebo/common/Video.cc
index 8a0bab1..6a62da6 100644
--- a/gazebo/common/Video.cc
+++ b/gazebo/common/Video.cc
@@ -33,7 +33,7 @@ using namespace common;
//
// f = fopen(filename, "w");
// fprintf(f, "P6\n%d %d\n%d\n", xsize, ysize, 255);
-// for(i = 0; i < ysize; i++)
+// for(i = 0; i < ysize; ++i)
// fwrite(buf + i * wrap, 1, xsize * 3, f);
// fclose(f);
// }
@@ -92,7 +92,7 @@ bool Video::Load(const std::string &_filename)
if (this->formatCtx || this->avFrame || this->codecCtx)
this->Cleanup();
- this->avFrame = avcodec_alloc_frame();
+ this->avFrame = common::AVFrameAlloc();
// Open video file
if (avformat_open_input(&this->formatCtx, _filename.c_str(), NULL, NULL) < 0)
diff --git a/gazebo/common/Video.hh b/gazebo/common/Video.hh
index 5784717..698351e 100644
--- a/gazebo/common/Video.hh
+++ b/gazebo/common/Video.hh
@@ -35,7 +35,7 @@ namespace gazebo
/// \class Video Video.hh common/common.hh
/// \brief Handle video encoding and decoding using libavcodec
- class GAZEBO_VISIBLE Video
+ class GZ_COMMON_VISIBLE Video
{
/// \brief Constructor
public: Video();
diff --git a/gazebo/common/ffmpeg_inc.cc b/gazebo/common/ffmpeg_inc.cc
new file mode 100644
index 0000000..4135320
--- /dev/null
+++ b/gazebo/common/ffmpeg_inc.cc
@@ -0,0 +1,44 @@
+/*
+ * 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/common/ffmpeg_inc.h"
+
+#ifdef HAVE_FFMPEG
+using namespace gazebo;
+
+//////////////////////////////////////////////////
+AVFrame *common::AVFrameAlloc(void)
+{
+#if LIBAVCODEC_VERSION_INT >= AV_VERSION_INT(55, 28, 1)
+ return av_frame_alloc();
+#else
+ return avcodec_alloc_frame();
+#endif
+}
+
+//////////////////////////////////////////////////
+void common::AVFrameUnref(AVFrame *_frame)
+{
+#if LIBAVCODEC_VERSION_INT >= AV_VERSION_INT(55, 28, 1)
+ av_frame_unref(_frame);
+#else
+ avcodec_get_frame_defaults(_frame);
+#endif
+}
+
+// ifdef HAVE_FFMPEG
+#endif
diff --git a/gazebo/common/ffmpeg_inc.h b/gazebo/common/ffmpeg_inc.h
index 8933ef0..ffe6f65 100644
--- a/gazebo/common/ffmpeg_inc.h
+++ b/gazebo/common/ffmpeg_inc.h
@@ -18,6 +18,8 @@
#ifndef _GAZEBO_FFMPEG_INC_HH_
#define _GAZEBO_FFMPEG_INC_HH_
+#include <gazebo/gazebo_config.h>
+
#pragma GCC system_header
#ifdef HAVE_FFMPEG
@@ -32,6 +34,24 @@ extern "C" {
#include <libavutil/opt.h>
#include <libswscale/swscale.h>
}
-#endif // ifdef HAVE_FFMPEG
-#endif // ifndef _GAZEBO_FFMPEG_INC_HH
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace common
+ {
+ /// \brief Helper function to avoid deprecation warnings.
+ GZ_COMMON_VISIBLE
+ AVFrame *AVFrameAlloc(void);
+
+ /// \brief Helper function to avoid deprecation warnings.
+ GZ_COMMON_VISIBLE
+ void AVFrameUnref(AVFrame *_frame);
+ }
+}
+// ifdef HAVE_FFMPEG
+#endif
+
+// ifndef _GAZEBO_FFMPEG_INC_HH
+#endif
diff --git a/gazebo/common/win_dirent.h b/gazebo/common/win_dirent.h
new file mode 100644
index 0000000..2857679
--- /dev/null
+++ b/gazebo/common/win_dirent.h
@@ -0,0 +1,783 @@
+/*
+ * dirent.h - dirent API for Microsoft Visual Studio
+ *
+ * Copyright (C) 2006-2012 Toni Ronkko
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * ``Software''), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED ``AS IS'', WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL TONI RONKKO BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _GAZEBO_WIN_DIRENT_H_
+#define _GAZEBO_WIN_DIRENT_H_
+
+// Define architecture flags so we don't need to include windows.h.
+// Avoiding windows.h makes it simpler to use windows sockets in conjunction
+// with dirent.h.
+#if !defined(_68K_) && !defined(_MPPC_) && !defined(_X86_) && \
+ !defined(_IA64_) && !defined(_AMD64_) && defined(_M_IX86)
+# define _X86_
+#endif
+#if !defined(_68K_) && !defined(_MPPC_) && !defined(_X86_) && \
+ !defined(_IA64_) && !defined(_AMD64_) && defined(_M_AMD64)
+#define _AMD64_
+#endif
+
+#include <cstdint>
+#include <stdio.h>
+#include <stdarg.h>
+#include <windef.h>
+#include <winbase.h>
+#include <wchar.h>
+#include <string.h>
+#include <stdlib.h>
+#include <malloc.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <errno.h>
+
+// Indicates that d_type field is available in dirent structure
+#define _DIRENT_HAVE_D_TYPE
+
+// Indicates that d_namlen field is available in dirent structure
+#define _DIRENT_HAVE_D_NAMLEN
+
+// Entries missing from MSVC 6.0
+#if !defined(FILE_ATTRIBUTE_DEVICE)
+# define FILE_ATTRIBUTE_DEVICE 0x40
+#endif
+
+// File type and permission flags for stat()
+#if !defined(S_IFMT)
+ // File type mask
+# define S_IFMT _S_IFMT
+#endif
+#if !defined(S_IFDIR)
+ // Directory
+# define S_IFDIR _S_IFDIR
+#endif
+#if !defined(S_IFCHR)
+ // Character device
+# define S_IFCHR _S_IFCHR
+#endif
+#if !defined(S_IFFIFO)
+ // Pipe
+# define S_IFFIFO _S_IFFIFO
+#endif
+#if !defined(S_IFREG)
+ // Regular file
+# define S_IFREG _S_IFREG
+#endif
+#if !defined(S_IREAD)
+ // Read permission
+# define S_IREAD _S_IREAD
+#endif
+#if !defined(S_IWRITE)
+ // Write permission
+# define S_IWRITE _S_IWRITE
+#endif
+#if !defined(S_IEXEC)
+ // Execute permission
+# define S_IEXEC _S_IEXEC
+#endif
+#if !defined(S_IFIFO)
+ // Pipe
+# define S_IFIFO _S_IFIFO
+#endif
+#if !defined(S_IFBLK)
+ // Block device
+# define S_IFBLK 0
+#endif
+#if !defined(S_IFLNK)
+ // Link
+# define S_IFLNK 0
+#endif
+#if !defined(S_IFSOCK)
+ // Socket
+# define S_IFSOCK 0
+#endif
+
+#if defined(_MSC_VER)
+ // Read user
+# define S_IRUSR S_IREAD
+ // Write user
+# define S_IWUSR S_IWRITE
+ // Execute user
+# define S_IXUSR 0
+ // Read group
+# define S_IRGRP 0
+ // Write group
+# define S_IWGRP 0
+ // Execute group
+# define S_IXGRP 0
+ // Read others
+# define S_IROTH 0
+ // Write others
+# define S_IWOTH 0
+ // Execute others
+# define S_IXOTH 0
+#endif
+
+// Maximum length of file name
+#if !defined(PATH_MAX)
+# define PATH_MAX MAX_PATH
+#endif
+#if !defined(FILENAME_MAX)
+# define FILENAME_MAX MAX_PATH
+#endif
+#if !defined(NAME_MAX)
+# define NAME_MAX FILENAME_MAX
+#endif
+
+// File type flags for d_type
+#define DT_UNKNOWN 0
+#define DT_REG S_IFREG
+#define DT_DIR S_IFDIR
+#define DT_FIFO S_IFIFO
+#define DT_SOCK S_IFSOCK
+#define DT_CHR S_IFCHR
+#define DT_BLK S_IFBLK
+#define DT_LNK S_IFLNK
+
+// Macros for converting between st_mode and d_type
+#define IFTODT(mode) ((mode) & S_IFMT)
+#define DTTOIF(type) (type)
+
+// File type macros. Note that block devices, sockets and links cannot be
+// distinguished on Windows and the macros S_ISBLK, S_ISSOCK and S_ISLNK are
+// only defined for compatibility. These macros should always return false
+// on Windows.
+#define S_ISFIFO(mode) (((mode) & S_IFMT) == S_IFIFO)
+#define S_ISDIR(mode) (((mode) & S_IFMT) == S_IFDIR)
+#define S_ISREG(mode) (((mode) & S_IFMT) == S_IFREG)
+#define S_ISLNK(mode) (((mode) & S_IFMT) == S_IFLNK)
+#define S_ISSOCK(mode) (((mode) & S_IFMT) == S_IFSOCK)
+#define S_ISCHR(mode) (((mode) & S_IFMT) == S_IFCHR)
+#define S_ISBLK(mode) (((mode) & S_IFMT) == S_IFBLK)
+
+// Return the exact length of d_namlen without zero terminator
+#define _D_EXACT_NAMLEN(p) ((p)->d_namlen)
+
+// Return number of bytes needed to store d_namlen
+#define _D_ALLOC_NAMLEN(p) (PATH_MAX)
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+ // Wide-character version
+ struct _wdirent
+ {
+ // Always zero
+ int64_t d_ino;
+
+ // Structure size
+ uint16_t d_reclen;
+
+ // Length of name without \0
+ size_t d_namlen;
+
+ // File type
+ int d_type;
+
+ // File name
+ wchar_t d_name[PATH_MAX];
+ };
+ typedef struct _wdirent _wdirent;
+
+ struct _WDIR
+ {
+ // Current directory entry
+ struct _wdirent ent;
+
+ // Private file data
+ WIN32_FIND_DATAW data;
+
+ // True if data is valid
+ int cached;
+
+ // Win32 search handle
+ HANDLE handle;
+
+ // Initial directory name
+ wchar_t *patt;
+ };
+ typedef struct _WDIR _WDIR;
+
+ static _WDIR *_wopendir(const wchar_t *dirname);
+ static struct _wdirent *_wreaddir(_WDIR *dirp);
+ static int _wclosedir(_WDIR *dirp);
+ static void _wrewinddir(_WDIR* dirp);
+
+ // For compatibility with Symbian
+# define wdirent _wdirent
+# define WDIR _WDIR
+# define wopendir _wopendir
+# define wreaddir _wreaddir
+# define wclosedir _wclosedir
+# define wrewinddir _wrewinddir
+
+
+ // Multi-byte character versions
+ struct dirent
+ {
+ // Always zero
+ int64_t d_ino;
+
+ // Structure size
+ uint16_t d_reclen;
+
+ // Length of name without \0
+ size_t d_namlen;
+
+ // File type
+ int d_type;
+
+ // File name
+ char d_name[PATH_MAX];
+ };
+ typedef struct dirent dirent;
+
+ struct DIR
+ {
+ struct dirent ent;
+ struct _WDIR *wdirp;
+ };
+ typedef struct DIR DIR;
+
+ static DIR *opendir(const char *dirname);
+ static int closedir(DIR *dirp);
+ static void rewinddir(DIR* dirp);
+
+
+ // Internal utility functions
+ static WIN32_FIND_DATAW *dirent_first(_WDIR *dirp);
+ static WIN32_FIND_DATAW *dirent_next(_WDIR *dirp);
+
+ static int dirent_mbstowcs_s(
+ size_t *pReturnValue,
+ wchar_t *wcstr,
+ size_t sizeInWords,
+ const char *mbstr,
+ size_t count);
+
+ static int dirent_wcstombs_s(
+ size_t *pReturnValue,
+ char *mbstr,
+ size_t sizeInBytes,
+ const wchar_t *wcstr,
+ size_t count);
+
+ static void dirent_set_errno(int error);
+
+ // Open directory stream DIRNAME for read and return a pointer to the
+ // internal working area that is used to retrieve individual directory
+ // entries.
+ static _WDIR* _wopendir(const wchar_t *dirname)
+ {
+ _WDIR *dirp = NULL;
+ int error;
+
+ // Must have directory name
+ if (dirname == NULL || dirname[0] == '\0')
+ {
+ dirent_set_errno(ENOENT);
+ return NULL;
+ }
+
+ // Allocate new _WDIR structure
+ dirp = static_cast<_WDIR*>(malloc(sizeof(struct _WDIR)));
+ if (dirp != NULL)
+ {
+ DWORD n;
+
+ // Reset _WDIR structure
+ dirp->handle = INVALID_HANDLE_VALUE;
+ dirp->patt = NULL;
+ dirp->cached = 0;
+
+ // Compute the length of full path plus zero terminator
+ n = GetFullPathNameW(dirname, 0, NULL, NULL);
+
+ // Allocate room for absolute directory name and search pattern
+ dirp->patt = static_cast<wchar_t*>(malloc(sizeof(wchar_t) * n + 16));
+ if (dirp->patt)
+ {
+ // Convert relative directory name to an absolute one. This
+ // allows rewinddir() to function correctly even when current
+ // working directory is changed between opendir() and rewinddir().
+ n = GetFullPathNameW(dirname, n, dirp->patt, NULL);
+
+ if (n > 0)
+ {
+ wchar_t *p;
+
+ // Append search pattern \* to the directory name
+ p = dirp->patt + n;
+ if (dirp->patt < p)
+ {
+ switch (p[-1])
+ {
+ case '\\':
+ case '/':
+ case ':':
+ // Directory ends in path separator, e.g. c:\temp\
+ // NOP
+ break;
+
+ default:
+ // Directory name doesn't end in path separator
+ *p++ = '\\';
+ }
+ }
+
+ *p++ = '*';
+ *p = '\0';
+
+ // Open directory stream and retrieve the first entry
+ if (dirent_first(dirp))
+ {
+ // Directory stream opened successfully
+ error = 0;
+ }
+ else
+ {
+ // Cannot retrieve first entry
+ error = 1;
+ dirent_set_errno(ENOENT);
+ }
+ }
+ else
+ {
+ // Cannot retrieve full path name
+ dirent_set_errno(ENOENT);
+ error = 1;
+ }
+ }
+ else
+ {
+ // Cannot allocate memory for search pattern
+ error = 1;
+ }
+ }
+ else
+ {
+ // Cannot allocate _WDIR structure
+ error = 1;
+ }
+
+ // Clean up in case of error
+ if (error && dirp)
+ {
+ _wclosedir(dirp);
+ dirp = NULL;
+ }
+
+ return dirp;
+ }
+
+ // Read next directory entry. The directory entry is returned in dirent
+ // structure in the d_name field. Individual directory entries returned by
+ // this function include regular files, sub-directories, pseudo-directories
+ // "." and ".." as well as volume labels, hidden files and system files.
+ static struct _wdirent* _wreaddir(_WDIR *dirp)
+ {
+ WIN32_FIND_DATAW *datap;
+ struct _wdirent *entp;
+
+ // Read next directory entry
+ datap = dirent_next(dirp);
+ if (datap)
+ {
+ size_t n;
+ DWORD attr;
+
+ // Pointer to directory entry to return
+ entp = &dirp->ent;
+
+ // Copy file name as wide-character string. If the file name is too
+ // long to fit in to the destination buffer, then truncate file name
+ // to PATH_MAX characters and zero-terminate the buffer.
+ n = 0;
+ while (n + 1 < PATH_MAX && datap->cFileName[n] != 0)
+ {
+ entp->d_name[n] = datap->cFileName[n];
+ n++;
+ }
+ dirp->ent.d_name[n] = 0;
+
+ // Length of file name excluding zero terminator
+ entp->d_namlen = n;
+
+ // File type
+ attr = datap->dwFileAttributes;
+ if ((attr & FILE_ATTRIBUTE_DEVICE) != 0)
+ {
+ entp->d_type = DT_CHR;
+ }
+ else if ((attr & FILE_ATTRIBUTE_DIRECTORY) != 0)
+ {
+ entp->d_type = DT_DIR;
+ }
+ else
+ {
+ entp->d_type = DT_REG;
+ }
+
+ // Reset dummy fields
+ entp->d_ino = 0;
+ entp->d_reclen = sizeof(struct _wdirent);
+ }
+ else
+ {
+ // Last directory entry read
+ entp = NULL;
+ }
+
+ return entp;
+ }
+
+ // Close directory stream opened by opendir() function. This invalidates the
+ // DIR structure as well as any directory entry read previously by
+ // _wreaddir().
+ static int _wclosedir(_WDIR *dirp)
+ {
+ int ok;
+ if (dirp)
+ {
+ // Release search handle
+ if (dirp->handle != INVALID_HANDLE_VALUE)
+ {
+ FindClose(dirp->handle);
+ dirp->handle = INVALID_HANDLE_VALUE;
+ }
+
+ // Release search pattern
+ if (dirp->patt)
+ {
+ free(dirp->patt);
+ dirp->patt = NULL;
+ }
+
+ // Release directory structure
+ free(dirp);
+
+ // success
+ ok = 0;
+ }
+ else
+ {
+ // Invalid directory stream
+ dirent_set_errno(EBADF);
+
+ // failure
+ ok = -1;
+ }
+ return ok;
+ }
+
+ // Rewind directory stream such that _wreaddir() returns the very first
+ // file name again.
+ static void _wrewinddir(_WDIR* dirp)
+ {
+ if (dirp)
+ {
+ // Release existing search handle
+ if (dirp->handle != INVALID_HANDLE_VALUE)
+ {
+ FindClose(dirp->handle);
+ }
+
+ // Open new search handle
+ dirent_first(dirp);
+ }
+ }
+
+ // Get first directory entry (internal)
+ static WIN32_FIND_DATAW* dirent_first(_WDIR *dirp)
+ {
+ WIN32_FIND_DATAW *datap;
+
+ // Open directory and retrieve the first entry
+ dirp->handle = FindFirstFileW(dirp->patt, &dirp->data);
+ if (dirp->handle != INVALID_HANDLE_VALUE)
+ {
+ // a directory entry is now waiting in memory
+ datap = &dirp->data;
+ dirp->cached = 1;
+ }
+ else
+ {
+ // Failed to re-open directory: no directory entry in memory
+ dirp->cached = 0;
+ datap = NULL;
+ }
+ return datap;
+ }
+
+ // Get next directory entry (internal)
+ static WIN32_FIND_DATAW* dirent_next(_WDIR *dirp)
+ {
+ WIN32_FIND_DATAW *p;
+
+ // Get next directory entry
+ if (dirp->cached != 0)
+ {
+ // A valid directory entry already in memory
+ p = &dirp->data;
+ dirp->cached = 0;
+ }
+ else if (dirp->handle != INVALID_HANDLE_VALUE)
+ {
+ // Get the next directory entry from stream
+ if (FindNextFileW (dirp->handle, &dirp->data) != FALSE)
+ {
+ // Got a file
+ p = &dirp->data;
+ }
+ else
+ {
+ // The very last entry has been processed or an error occured
+ FindClose(dirp->handle);
+ dirp->handle = INVALID_HANDLE_VALUE;
+ p = NULL;
+ }
+ }
+ else
+ {
+ // End of directory stream reached
+ p = NULL;
+ }
+ return p;
+ }
+
+ // Open directory stream using plain old C-string.
+ static DIR* opendir(const char *dirname)
+ {
+ struct DIR *dirp;
+ int error;
+
+ // Must have directory name
+ if (dirname == NULL || dirname[0] == '\0')
+ {
+ dirent_set_errno(ENOENT);
+ return NULL;
+ }
+
+ // Allocate memory for DIR structure
+ dirp = static_cast<DIR*>(malloc(sizeof(struct DIR)));
+ if (dirp)
+ {
+ wchar_t wname[PATH_MAX];
+ size_t n;
+
+ // Convert directory name to wide-character string
+ error = dirent_mbstowcs_s(&n, wname, PATH_MAX, dirname, PATH_MAX);
+ if (!error)
+ {
+ // Open directory stream using wide-character name
+ dirp->wdirp = _wopendir(wname);
+ if (dirp->wdirp)
+ {
+ // Directory stream opened
+ error = 0;
+ }
+ else
+ {
+ // Failed to open directory stream
+ error = 1;
+ }
+ }
+ else
+ {
+ // Cannot convert file name to wide-character string. This
+ // occurs if the string contains invalid multi-byte sequences or
+ // the output buffer is too small to contain the resulting
+ // string.
+ error = 1;
+ }
+ }
+ else
+ {
+ // Cannot allocate DIR structure
+ error = 1;
+ }
+
+ // Clean up in case of error
+ if (error && dirp)
+ {
+ free(dirp);
+ dirp = NULL;
+ }
+ return dirp;
+ }
+
+ // Close directory stream.
+ static int closedir(DIR *dirp)
+ {
+ int ok;
+ if (dirp)
+ {
+ // Close wide-character directory stream
+ ok = _wclosedir(dirp->wdirp);
+ dirp->wdirp = NULL;
+
+ // Release multi-byte character version
+ free(dirp);
+ }
+ else
+ {
+ // Invalid directory stream
+ dirent_set_errno(EBADF);
+
+ // failure
+ ok = -1;
+ }
+ return ok;
+ }
+
+ // Rewind directory stream to beginning.
+ static void rewinddir(DIR* dirp)
+ {
+ // Rewind wide-character string directory stream
+ _wrewinddir(dirp->wdirp);
+ }
+
+ // Convert multi-byte string to wide character string
+ static int dirent_mbstowcs_s(
+ size_t *pReturnValue,
+ wchar_t *wcstr,
+ size_t sizeInWords,
+ const char *mbstr,
+ size_t count)
+ {
+ int error;
+
+# if defined(_MSC_VER) && _MSC_VER >= 1400
+ // Microsoft Visual Studio 2005 or later
+ error = mbstowcs_s(pReturnValue, wcstr, sizeInWords, mbstr, count);
+
+# else
+
+ // Older Visual Studio or non-Microsoft compiler
+ size_t n;
+
+ // Convert to wide-character string (or count characters)
+ n = mbstowcs(wcstr, mbstr, sizeInWords);
+ if (!wcstr || n < count)
+ {
+ // Zero-terminate output buffer
+ if (wcstr && sizeInWords)
+ {
+ if (n >= sizeInWords)
+ {
+ n = sizeInWords - 1;
+ }
+ wcstr[n] = 0;
+ }
+
+ // Length of resuting multi-byte string WITH zero terminator
+ if (pReturnValue)
+ {
+ *pReturnValue = n + 1;
+ }
+
+ // Success
+ error = 0;
+ }
+ else
+ {
+ // Could not convert string
+ error = 1;
+ }
+#endif
+ return error;
+ }
+
+ // Convert wide-character string to multi-byte string
+ static int dirent_wcstombs_s(
+ size_t *pReturnValue,
+ char *mbstr,
+ // max size of mbstr
+ size_t sizeInBytes,
+ const wchar_t *wcstr,
+ size_t count)
+ {
+ int error;
+
+# if defined(_MSC_VER) && _MSC_VER >= 1400
+
+ // Microsoft Visual Studio 2005 or later
+ error = wcstombs_s(pReturnValue, mbstr, sizeInBytes, wcstr, count);
+
+#else
+
+ // Older Visual Studio or non-Microsoft compiler
+ size_t n;
+
+ // Convert to multi-byte string (or count the number of bytes needed)
+ n = wcstombs(mbstr, wcstr, sizeInBytes);
+ if (!mbstr || n < count)
+ {
+ // Zero-terminate output buffer
+ if (mbstr && sizeInBytes)
+ {
+ if (n >= sizeInBytes)
+ {
+ n = sizeInBytes - 1;
+ }
+ mbstr[n] = '\0';
+ }
+
+ // Lenght of resulting multi-bytes string WITH zero-terminator
+ if (pReturnValue)
+ {
+ *pReturnValue = n + 1;
+ }
+
+ // Success
+ error = 0;
+ }
+ else
+ {
+ // Cannot convert string
+ error = 1;
+ }
+#endif
+
+ return error;
+ }
+
+ // Set errno variable
+ static void dirent_set_errno(int error)
+ {
+# if defined(_MSC_VER) && _MSC_VER >= 1400
+ // Microsoft Visual Studio 2005 and later
+ _set_errno(error);
+#else
+ // Non-Microsoft compiler or older Microsoft compiler
+ errno = error;
+#endif
+ }
+
+#ifdef __cplusplus
+}
+#endif
+
+// _GAZEBO_WIN_DIRENT_H_
+#endif
diff --git a/gazebo/gazebo.1.ronn b/gazebo/gazebo.1.ronn
index 7bf3b35..3bdfe6f 100644
--- a/gazebo/gazebo.1.ronn
+++ b/gazebo/gazebo.1.ronn
@@ -39,6 +39,8 @@ Gazebo server runs simulation and handles commandline options, starts a Master,
Load a GUI plugin.
* -s, --server-plugin arg :
Load a server plugin.
+* -o, --profile arg :
+ Physics preset profile name from the options in the world file.
## AUTHOR
diff --git a/gazebo/gazebo.cc b/gazebo/gazebo.cc
index c6269a0..f93ee6e 100644
--- a/gazebo/gazebo.cc
+++ b/gazebo/gazebo.cc
@@ -14,6 +14,13 @@
* 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 <vector>
#include <boost/thread/mutex.hpp>
#include <sdf/sdf.hh>
@@ -26,6 +33,7 @@
#include "gazebo/util/LogRecord.hh"
#include "gazebo/math/gzmath.hh"
#include "gazebo/gazebo_config.h"
+#include "gazebo/gazebo_shared.hh"
#include "gazebo/gazebo.hh"
boost::mutex fini_mutex;
@@ -43,72 +51,15 @@ struct g_vectorStringDup
};
/////////////////////////////////////////////////
-// This function is used by both setupClient and setupServer
-bool setup(const std::string &_prefix, int _argc, char **_argv)
-{
- gazebo::common::load();
-
- // The SDF find file callback.
- sdf::setFindCallback(boost::bind(&gazebo::common::find_file, _1));
-
- // Initialize the informational logger. This will log warnings, and
- // errors.
- gzLogInit(_prefix, "default.log");
-
- // Load all the system plugins
- for (std::vector<gazebo::SystemPluginPtr>::iterator iter =
- g_plugins.begin(); iter != g_plugins.end(); ++iter)
- {
- (*iter)->Load(_argc, _argv);
- }
-
- if (!gazebo::transport::init())
- {
- gzerr << "Unable to initialize transport.\n";
- return false;
- }
-
- // Make sure the model database has started.
- gazebo::common::ModelDatabase::Instance()->Start();
-
- // Run transport loop. Starts a thread
- gazebo::transport::run();
-
- // Init all system plugins
- for (std::vector<gazebo::SystemPluginPtr>::iterator iter = g_plugins.begin();
- iter != g_plugins.end(); ++iter)
- {
- (*iter)->Init();
- }
-
- return true;
-}
-
-/////////////////////////////////////////////////
void gazebo::printVersion()
{
- fprintf(stderr, "%s", GAZEBO_VERSION_HEADER);
+ gazebo_shared::printVersion();
}
/////////////////////////////////////////////////
void gazebo::addPlugin(const std::string &_filename)
{
- if (_filename.empty())
- return;
- gazebo::SystemPluginPtr plugin =
- gazebo::SystemPlugin::Create(_filename, _filename);
-
- if (plugin)
- {
- if (plugin->GetType() != SYSTEM_PLUGIN)
- {
- gzerr << "System is attempting to load "
- << "a plugin, but detected an incorrect plugin type. "
- << "Plugin filename[" << _filename << "].\n";
- return;
- }
- g_plugins.push_back(plugin);
- }
+ gazebo_shared::addPlugin(_filename, g_plugins);
}
/////////////////////////////////////////////////
@@ -123,7 +74,7 @@ bool gazebo::setupServer(int _argc, char **_argv)
g_master->Init(port);
g_master->RunThread();
- if (!setup("server-", _argc, _argv))
+ if (!gazebo_shared::setup("server-", _argc, _argv, g_plugins))
{
gzerr << "Unable to setup Gazebo\n";
return false;
@@ -169,29 +120,11 @@ bool gazebo::setupServer(const std::vector<std::string> &_args)
/////////////////////////////////////////////////
bool gazebo::setupClient(int _argc, char **_argv)
{
- if (!setup("client-", _argc, _argv))
+ if (!gazebo_shared::setup("client-", _argc, _argv, g_plugins))
{
gzerr << "Unable to setup Gazebo\n";
return false;
}
-
- common::Time waitTime(1, 0);
- int waitCount = 0;
- int maxWaitCount = 10;
-
- // Wait for namespaces.
- while (!gazebo::transport::waitForNamespaces(waitTime) &&
- (waitCount++) < maxWaitCount)
- {
- gzwarn << "Waited " << waitTime.Double() << "seconds for namespaces.\n";
- }
-
- if (waitCount >= maxWaitCount)
- {
- gzerr << "Waited " << (waitTime * waitCount).Double()
- << " seconds for namespaces. Giving up.\n";
- }
-
return true;
}
@@ -202,7 +135,15 @@ bool gazebo::setupClient(const std::vector<std::string> &_args)
std::transform(_args.begin(), _args.end(), pointers.begin(),
g_vectorStringDup());
pointers.push_back(0);
+
+#ifndef _WIN32
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
bool result = gazebo::setupClient(_args.size(), &pointers[0]);
+#ifndef _WIN32
+#pragma GCC diagnostic pop
+#endif
// Deallocate memory for the command line arguments alloocated with strdup.
for (size_t i = 0; i < pointers.size(); ++i)
@@ -272,7 +213,7 @@ gazebo::physics::WorldPtr gazebo::loadWorld(const std::string &_worldFile)
}
world = gazebo::physics::create_world();
- gazebo::physics::load_world(world, sdf->root->GetElement("world"));
+ gazebo::physics::load_world(world, sdf->Root()->GetElement("world"));
gazebo::physics::init_world(world);
diff --git a/gazebo/gazebo.hh b/gazebo/gazebo.hh
index f22c1ab..8c7d861 100644
--- a/gazebo/gazebo.hh
+++ b/gazebo/gazebo.hh
@@ -57,8 +57,9 @@ namespace gazebo
/// \param[in] _argc Number of commandline arguments.
/// \param[in] _argv The commandline arguments.
/// \return True on success.
+ /// \sa gazebo_client.hh
GAZEBO_VISIBLE
- bool setupClient(int _argc = 0, char **_argv = 0);
+ bool setupClient(int _argc = 0, char **_argv = 0) GAZEBO_DEPRECATED(6.0);
/// \brief Start a gazebo client. This starts transportation, and makes it
/// possible to connect to a running simulation.
@@ -68,8 +69,10 @@ namespace gazebo
/// arguments are only parsed by the system plugins.
/// \sa gazebo::SystemPlugin::Load()
/// \return True on success.
+ /// \sa gazebo_client.hh
GAZEBO_VISIBLE
- bool setupClient(const std::vector<std::string> &_args);
+ bool setupClient(const std::vector<std::string> &_args)
+ GAZEBO_DEPRECATED(6.0);
/// \brief Create and load a new world from an SDF world file.
/// \param[in] _worldFile The world file to load from.
diff --git a/gazebo/gazebo_client.cc b/gazebo/gazebo_client.cc
new file mode 100644
index 0000000..b567fde
--- /dev/null
+++ b/gazebo/gazebo_client.cc
@@ -0,0 +1,103 @@
+/*
+ * 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.
+ *
+ */
+#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/thread/mutex.hpp>
+#include <sdf/sdf.hh>
+
+#include "gazebo/transport/transport.hh"
+#include "gazebo/util/LogRecord.hh"
+#include "gazebo/util/system.hh"
+#include "gazebo/gazebo_config.h"
+#include "gazebo/gazebo_shared.hh"
+#include "gazebo/gazebo_client.hh"
+
+boost::mutex fini_mutex;
+std::vector<gazebo::SystemPluginPtr> g_plugins;
+
+/////////////////////////////////////////////////
+struct g_vectorStringDup
+{
+ char *operator()(const std::string &_s)
+ {
+ return strdup(_s.c_str());
+ }
+};
+
+/////////////////////////////////////////////////
+void gazebo::client::printVersion()
+{
+ gazebo_shared::printVersion();
+}
+
+/////////////////////////////////////////////////
+void gazebo::client::addPlugin(const std::string &_filename)
+{
+ gazebo_shared::addPlugin(_filename, g_plugins);
+}
+
+/////////////////////////////////////////////////
+bool gazebo::client::setup(int _argc, char **_argv)
+{
+ if (!gazebo_shared::setup("client-", _argc, _argv, g_plugins))
+ {
+ gzerr << "Unable to setup Gazebo\n";
+ return false;
+ }
+ return true;
+}
+
+/////////////////////////////////////////////////
+bool gazebo::client::setup(const std::vector<std::string> &_args)
+{
+ std::vector<char *> pointers(_args.size());
+ std::transform(_args.begin(), _args.end(), pointers.begin(),
+ g_vectorStringDup());
+ pointers.push_back(0);
+ bool result = gazebo::client::setup(_args.size(), &pointers[0]);
+
+ // Deallocate memory for the command line arguments alloocated with strdup.
+ for (size_t i = 0; i < pointers.size(); ++i)
+ free(pointers.at(i));
+
+ return result;
+}
+
+/////////////////////////////////////////////////
+bool gazebo::client::shutdown()
+{
+ // Stop log recording
+ util::LogRecord::Instance()->Stop();
+
+ // Stop transport
+ gazebo::transport::stop();
+
+ // Make sure to shut everything down.
+ boost::mutex::scoped_lock lock(fini_mutex);
+ util::LogRecord::Instance()->Fini();
+ g_plugins.clear();
+ gazebo::transport::fini();
+
+ // Cleanup model database.
+ common::ModelDatabase::Instance()->Fini();
+
+ return true;
+}
diff --git a/gazebo/gazebo_client.hh b/gazebo/gazebo_client.hh
new file mode 100644
index 0000000..10e454c
--- /dev/null
+++ b/gazebo/gazebo_client.hh
@@ -0,0 +1,64 @@
+/*
+ * 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_CLIENT_INTERFACE_HH_
+#define _GAZEBO_CLIENT_INTERFACE_HH_
+
+#include <string>
+#include <vector>
+
+#include <gazebo/common/common.hh>
+#include <gazebo/util/system.hh>
+
+namespace gazebo
+{
+ namespace client
+ {
+ /// \brief Output version information to the terminal.
+ GAZEBO_VISIBLE
+ void printVersion();
+
+ /// \brief Add a system plugin.
+ /// \param[in] _filename Path to the plugin.
+ GAZEBO_VISIBLE
+ void addPlugin(const std::string &_filename);
+
+ /// \brief Start a gazebo client. This starts transportation, and makes it
+ /// possible to connect to a running simulation.
+ /// \param[in] _argc Number of commandline arguments.
+ /// \param[in] _argv The commandline arguments.
+ /// \return True on success.
+ GAZEBO_VISIBLE
+ bool setup(int _argc = 0, char **_argv = 0);
+
+ /// \brief Start a gazebo client. This starts transportation, and makes it
+ /// possible to connect to a running simulation.
+ /// \param[in] _args Vector of arguments only parsed by the system plugins.
+ /// Note that when you run gazebo/gzserver, all the options (--version,
+ /// --server-plugin, etc.) are parsed but when using Gazebo as a library,
+ /// the arguments are only parsed by the system plugins.
+ /// \sa gazebo::SystemPlugin::Load()
+ /// \return True on success.
+ GAZEBO_VISIBLE
+ bool setup(const std::vector<std::string> &_args);
+
+ /// \brief Stop and cleanup simulation.
+ /// \return True if the simulation is shutdown; false otherwise.
+ GAZEBO_VISIBLE
+ bool shutdown();
+ }
+}
+#endif
diff --git a/gazebo/gazebo_main.cc b/gazebo/gazebo_main.cc
index d19185c..c6b46ad 100644
--- a/gazebo/gazebo_main.cc
+++ b/gazebo/gazebo_main.cc
@@ -59,7 +59,11 @@ void help()
<< " --minimal_comms Reduce the TCP/IP traffic output by "
<< "gazebo.\n"
<< " -g [ --gui-plugin ] arg Load a GUI plugin.\n"
- << " -s [ --server-plugin ] arg Load a server plugin.\n\n";
+ << " -s [ --server-plugin ] arg Load a server plugin.\n"
+ << " -o [ --profile ] arg Physics preset profile name from the "
+ << "options in\n"
+ << " the world file.\n"
+ << "\n";
}
/////////////////////////////////////////////////
diff --git a/gazebo/gazebo_shared.cc b/gazebo/gazebo_shared.cc
new file mode 100644
index 0000000..eaeab4d
--- /dev/null
+++ b/gazebo/gazebo_shared.cc
@@ -0,0 +1,101 @@
+/*
+ * 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.
+ *
+ */
+#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 <sdf/sdf.hh>
+
+#include "gazebo/transport/TransportIface.hh"
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/common/CommonIface.hh"
+#include "gazebo/common/ModelDatabase.hh"
+#include "gazebo/gazebo_config.h"
+#include "gazebo_shared.hh"
+
+/////////////////////////////////////////////////
+void gazebo_shared::printVersion()
+{
+ fprintf(stderr, "%s", GAZEBO_VERSION_HEADER);
+}
+
+/////////////////////////////////////////////////
+void gazebo_shared::addPlugin(const std::string &_filename,
+ std::vector<gazebo::SystemPluginPtr> &_plugins)
+{
+ if (_filename.empty())
+ return;
+
+ gazebo::SystemPluginPtr plugin =
+ gazebo::SystemPlugin::Create(_filename, _filename);
+
+ if (plugin)
+ {
+ if (plugin->GetType() != gazebo::SYSTEM_PLUGIN)
+ {
+ gzerr << "System is attempting to load "
+ << "a plugin, but detected an incorrect plugin type. "
+ << "Plugin filename[" << _filename << "].\n";
+ return;
+ }
+ _plugins.push_back(plugin);
+ }
+}
+
+/////////////////////////////////////////////////
+bool gazebo_shared::setup(const std::string &_prefix, int _argc, char **_argv,
+ std::vector<gazebo::SystemPluginPtr> &_plugins)
+{
+ gazebo::common::load();
+
+ // The SDF find file callback.
+ sdf::setFindCallback(boost::bind(&gazebo::common::find_file, _1));
+
+ // Initialize the informational logger. This will log warnings, and
+ // errors.
+ gzLogInit(_prefix, "default.log");
+
+ // Load all the system plugins
+ for (std::vector<gazebo::SystemPluginPtr>::iterator iter =
+ _plugins.begin(); iter != _plugins.end(); ++iter)
+ {
+ (*iter)->Load(_argc, _argv);
+ }
+
+ if (!gazebo::transport::init())
+ {
+ gzerr << "Unable to initialize transport.\n";
+ return false;
+ }
+
+ // Make sure the model database has started.
+ gazebo::common::ModelDatabase::Instance()->Start();
+
+ // Run transport loop. Starts a thread
+ gazebo::transport::run();
+
+ // Init all system plugins
+ for (std::vector<gazebo::SystemPluginPtr>::iterator iter = _plugins.begin();
+ iter != _plugins.end(); ++iter)
+ {
+ (*iter)->Init();
+ }
+
+ return true;
+}
diff --git a/gazebo/gazebo_shared.hh b/gazebo/gazebo_shared.hh
new file mode 100644
index 0000000..26f7092
--- /dev/null
+++ b/gazebo/gazebo_shared.hh
@@ -0,0 +1,49 @@
+/*
+ * 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_SHARED_INTERFACE_HH_
+#define _GAZEBO_SHARED_INTERFACE_HH_
+
+/// \internal
+#include <string>
+#include <vector>
+#include "gazebo/common/CommonTypes.hh"
+
+namespace gazebo_shared
+{
+ /// \internal
+ /// \brief Output version information to the terminal.
+ void printVersion();
+
+ /// \internal
+ /// \brief Add a system plugin.
+ /// \param[in] _filename Path to the plugin.
+ /// \param[in] _plugins Vector of plugins into which the new plugin should
+ /// be added
+ void addPlugin(const std::string &_filename,
+ std::vector<gazebo::SystemPluginPtr> &_plugins);
+
+ /// \internal
+ /// \brief Setup the based gazebo system.
+ /// \param[in] _prefix Prefix name, usually "client" or "server"
+ /// \param[in] _argc Argument count
+ /// \param[in] _argv Argument array
+ /// \param[in] _plugins Vector of plugins to process
+ /// \return True on success.
+ bool setup(const std::string &_prefix, int _argc, char **_argv,
+ std::vector<gazebo::SystemPluginPtr> &_plugins);
+}
+#endif
diff --git a/gazebo/gui/Actions.cc b/gazebo/gui/Actions.cc
index 1342358..812e51c 100644
--- a/gazebo/gui/Actions.cc
+++ b/gazebo/gui/Actions.cc
@@ -18,116 +18,136 @@
#include "gazebo/gui/Actions.hh"
#include "gazebo/util/system.hh"
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_arrowAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_translateAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_rotateAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_scaleAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_newAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_openAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_importAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_saveAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_saveAsAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_saveCfgAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_cloneAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_aboutAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_quitAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_dataLoggerAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_resetModelsAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_resetWorldAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_editBuildingAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_editTerrainAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_editModelAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_playAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_pauseAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_stepAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_boxCreateAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_sphereCreateAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_cylinderCreateAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_meshCreateAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_pointLghtCreateAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_spotLghtCreateAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_dirLghtCreateAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_screenshotAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_showCollisionsAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_showGridAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
+QAction *gazebo::gui::g_showOriginAct = 0;
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_showContactsAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_showJointsAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_showCOMAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
+QAction *gazebo::gui::g_showInertiaAct = 0;
+GZ_GUI_VISIBLE
+QAction *gazebo::gui::g_showLinkFrameAct = 0;
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_transparentAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_viewWireframeAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_viewOculusAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
+QAction *gazebo::gui::g_cameraOrthoAct = 0;
+GZ_GUI_VISIBLE
+QAction *gazebo::gui::g_cameraPerspectiveAct = 0;
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_resetAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
+QAction *gazebo::gui::g_showToolbarsAct = 0;
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_fullScreenAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_fpsAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_orbitAct = 0;
-
GAZEBO_VISIBLE
+QAction *gazebo::gui::g_overlayAct = 0;
+
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_topicVisAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_diagnosticsAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
gazebo::gui::DeleteAction *gazebo::gui::g_deleteAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_copyAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_pasteAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_snapAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QWidgetAction *gazebo::gui::g_alignAct = 0;
-GAZEBO_VISIBLE
+GZ_GUI_VISIBLE
QAction *gazebo::gui::g_alignButtonAct = 0;
+
+GZ_GUI_VISIBLE
+QWidgetAction *gazebo::gui::g_viewAngleAct = 0;
+
+GZ_GUI_VISIBLE
+QAction *gazebo::gui::g_viewAngleButtonAct = 0;
diff --git a/gazebo/gui/Actions.hh b/gazebo/gui/Actions.hh
index 69fc76d..de84e71 100644
--- a/gazebo/gui/Actions.hh
+++ b/gazebo/gui/Actions.hh
@@ -26,69 +26,102 @@ namespace gazebo
{
namespace gui
{
- extern QAction *g_newAct;
- extern QAction *g_openAct;
- extern QAction *g_importAct;
- extern QAction *g_saveAct;
- extern QAction *g_saveAsAct;
- extern QAction *g_saveCfgAct;
- extern QAction *g_cloneAct;
- extern QAction *g_aboutAct;
- extern QAction *g_quitAct;
-
- extern QAction *g_dataLoggerAct;
-
- extern QAction *g_resetModelsAct;
- extern QAction *g_resetWorldAct;
- extern QAction *g_editBuildingAct;
- extern QAction *g_editTerrainAct;
- extern QAction *g_editModelAct;
-
- extern QAction *g_playAct;
- extern QAction *g_pauseAct;
- extern QAction *g_stepAct;
-
- extern QAction *g_boxCreateAct;
- extern QAction *g_sphereCreateAct;
- extern QAction *g_cylinderCreateAct;
- extern QAction *g_meshCreateAct;
- extern QAction *g_pointLghtCreateAct;
- extern QAction *g_spotLghtCreateAct;
- extern QAction *g_dirLghtCreateAct;
-
- extern QAction *g_screenshotAct;
-
- extern QAction *g_showCollisionsAct;
- extern QAction *g_showGridAct;
- extern QAction *g_showContactsAct;
- extern QAction *g_showJointsAct;
- extern QAction *g_showCOMAct;
- extern QAction *g_transparentAct;
-
- extern QAction *g_resetAct;
- extern QAction *g_fullScreenAct;
- extern QAction *g_fpsAct;
- extern QAction *g_orbitAct;
-
- extern QAction *g_arrowAct;
- extern QAction *g_translateAct;
- extern QAction *g_rotateAct;
- extern QAction *g_scaleAct;
-
- extern QAction *g_topicVisAct;
-
- extern QAction *g_diagnosticsAct;
-
- extern QAction *g_viewWireframeAct;
-
- extern QAction *g_viewOculusAct;
-
- extern QAction *g_copyAct;
- extern QAction *g_pasteAct;
-
- extern QWidgetAction *g_alignAct;
- extern QAction *g_alignButtonAct;
- extern QAction *g_snapAct;
+ extern GZ_GUI_VISIBLE QAction *g_newAct;
+ extern GZ_GUI_VISIBLE QAction *g_openAct;
+ extern GZ_GUI_VISIBLE QAction *g_importAct;
+ extern GZ_GUI_VISIBLE QAction *g_saveAct;
+ extern GZ_GUI_VISIBLE QAction *g_saveAsAct;
+ extern GZ_GUI_VISIBLE QAction *g_saveCfgAct;
+ extern GZ_GUI_VISIBLE QAction *g_cloneAct;
+ extern GZ_GUI_VISIBLE QAction *g_aboutAct;
+ extern GZ_GUI_VISIBLE QAction *g_quitAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_dataLoggerAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_resetModelsAct;
+ extern GZ_GUI_VISIBLE QAction *g_resetWorldAct;
+ extern GZ_GUI_VISIBLE QAction *g_editBuildingAct;
+ extern GZ_GUI_VISIBLE QAction *g_editTerrainAct;
+ extern GZ_GUI_VISIBLE QAction *g_editModelAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_playAct;
+ extern GZ_GUI_VISIBLE QAction *g_pauseAct;
+ extern GZ_GUI_VISIBLE QAction *g_stepAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_boxCreateAct;
+ extern GZ_GUI_VISIBLE QAction *g_sphereCreateAct;
+ extern GZ_GUI_VISIBLE QAction *g_cylinderCreateAct;
+ extern GZ_GUI_VISIBLE QAction *g_meshCreateAct;
+ extern GZ_GUI_VISIBLE QAction *g_pointLghtCreateAct;
+ extern GZ_GUI_VISIBLE QAction *g_spotLghtCreateAct;
+ extern GZ_GUI_VISIBLE QAction *g_dirLghtCreateAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_screenshotAct;
+
+ /// \brief Action to show/hide collision visuals for all models.
+ extern GZ_GUI_VISIBLE QAction *g_showCollisionsAct;
+
+ /// \brief Action to show/hide the grid over the ground plane.
+ extern GZ_GUI_VISIBLE QAction *g_showGridAct;
+
+ /// \brief Action to show/hide the world origin indicator.
+ extern GZ_GUI_VISIBLE QAction *g_showOriginAct;
+
+ /// \brief Action to show/hide contact visuals for all models.
+ extern GZ_GUI_VISIBLE QAction *g_showContactsAct;
+
+ /// \brief Action to show/hide joint visuals for all models.
+ extern GZ_GUI_VISIBLE QAction *g_showJointsAct;
+
+ /// \brief Action to show/hide CoM visuals for all models.
+ extern GZ_GUI_VISIBLE QAction *g_showCOMAct;
+
+ /// \brief Action to show/hide inertia visuals for all models.
+ extern GZ_GUI_VISIBLE QAction *g_showInertiaAct;
+
+ /// \brief Action to show/hide link frame visuals for all models.
+ extern GZ_GUI_VISIBLE QAction *g_showLinkFrameAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_transparentAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_resetAct;
+ extern GZ_GUI_VISIBLE QAction *g_showToolbarsAct;
+ extern GZ_GUI_VISIBLE QAction *g_fullScreenAct;
+ extern GZ_GUI_VISIBLE QAction *g_fpsAct;
+ extern GZ_GUI_VISIBLE QAction *g_orbitAct;
+ extern GZ_GUI_VISIBLE QAction *g_overlayAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_arrowAct;
+ extern GZ_GUI_VISIBLE QAction *g_translateAct;
+ extern GZ_GUI_VISIBLE QAction *g_rotateAct;
+ extern GZ_GUI_VISIBLE QAction *g_scaleAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_topicVisAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_diagnosticsAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_viewWireframeAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_viewOculusAct;
+
+ /// \brief Enable orthographic projection action
+ extern GZ_GUI_VISIBLE QAction *g_cameraOrthoAct;
+
+ /// \brief Enable perspective projection action
+ extern GZ_GUI_VISIBLE QAction *g_cameraPerspectiveAct;
+
+ extern GZ_GUI_VISIBLE QAction *g_copyAct;
+ extern GZ_GUI_VISIBLE QAction *g_pasteAct;
+
+ extern GZ_GUI_VISIBLE QWidgetAction *g_alignAct;
+ extern GZ_GUI_VISIBLE QAction *g_alignButtonAct;
+ extern GZ_GUI_VISIBLE QAction *g_snapAct;
+
+ /// \brief Action to open the view angle widget.
+ extern GZ_GUI_VISIBLE QWidgetAction *g_viewAngleAct;
+
+ /// \brief Action attached to the view angle button.
+ extern GZ_GUI_VISIBLE QAction *g_viewAngleButtonAct;
/// \class DeleteAction Actions.hh gui/gui.hh
/// \brief Custom delete action.
@@ -112,7 +145,7 @@ namespace gazebo
};
/// \brief Action used to delete a model.
- extern DeleteAction *g_deleteAct;
+ extern GZ_GUI_VISIBLE DeleteAction *g_deleteAct;
}
}
#endif
diff --git a/gazebo/gui/ApplyWrenchDialog.cc b/gazebo/gui/ApplyWrenchDialog.cc
new file mode 100644
index 0000000..0d1186e
--- /dev/null
+++ b/gazebo/gui/ApplyWrenchDialog.cc
@@ -0,0 +1,1285 @@
+/*
+ * Copyright 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#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 "gazebo/transport/Node.hh"
+#include "gazebo/transport/Publisher.hh"
+
+#include "gazebo/rendering/UserCamera.hh"
+#include "gazebo/rendering/Scene.hh"
+#include "gazebo/rendering/COMVisual.hh"
+#include "gazebo/rendering/Visual.hh"
+#include "gazebo/rendering/SelectionObj.hh"
+#include "gazebo/rendering/ApplyWrenchVisual.hh"
+
+#include "gazebo/gui/Actions.hh"
+#include "gazebo/gui/MainWindow.hh"
+#include "gazebo/gui/MouseEventHandler.hh"
+#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/ApplyWrenchDialogPrivate.hh"
+#include "gazebo/gui/ApplyWrenchDialog.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+ApplyWrenchDialog::ApplyWrenchDialog(QWidget *_parent)
+ : QDialog(_parent), dataPtr(new ApplyWrenchDialogPrivate)
+{
+ this->setObjectName("ApplyWrenchDialog");
+ this->dataPtr->mainWindow = gui::get_main_window();
+
+ this->setWindowTitle(tr("Apply Force and Torque"));
+ this->setWindowFlags(Qt::WindowStaysOnTopHint);
+ this->setWindowModality(Qt::NonModal);
+ this->setStyleSheet(
+ "QPushButton {\
+ border-radius: 5px;\
+ border-radius: 5px;\
+ }");
+
+ this->dataPtr->modelLabel = new QLabel();
+
+ // Links list
+ QHBoxLayout *linkLayout = new QHBoxLayout();
+ QLabel *linkLabel = new QLabel(tr("<b>Apply to link:<b> "));
+ this->dataPtr->linksComboBox = new QComboBox();
+ this->dataPtr->linksComboBox->installEventFilter(this);
+ this->dataPtr->linksComboBox->setMinimumWidth(200);
+ connect(this->dataPtr->linksComboBox, SIGNAL(currentIndexChanged(QString)),
+ this, SLOT(SetLink(QString)));
+
+ linkLayout->addWidget(linkLabel);
+ linkLayout->addWidget(this->dataPtr->linksComboBox);
+
+ // Force
+ QLabel *forceLabel = new QLabel(tr(
+ "<font size=4>Force</font>"));
+ forceLabel->setObjectName("forceLabel");
+ forceLabel->setStyleSheet(
+ "QLabel#forceLabel {\
+ background-color: #444;\
+ border-radius: 5px;\
+ padding-left: 10px;\
+ min-height: 40px;\
+ }");
+
+ // Force vector layout
+ QGridLayout *forceVectorLayout = new QGridLayout();
+
+ // Force Vector
+ this->dataPtr->forceXSpin = new QDoubleSpinBox();
+ this->dataPtr->forceYSpin = new QDoubleSpinBox();
+ this->dataPtr->forceZSpin = new QDoubleSpinBox();
+
+ std::vector<QDoubleSpinBox *> forceSpins;
+ forceSpins.push_back(this->dataPtr->forceXSpin);
+ forceSpins.push_back(this->dataPtr->forceYSpin);
+ forceSpins.push_back(this->dataPtr->forceZSpin);
+
+ for (unsigned int i = 0; i < forceSpins.size(); ++i)
+ {
+ QLabel *forceElementLabel = new QLabel();
+ if (i == 0)
+ forceElementLabel->setText(tr("X:"));
+ else if (i == 1)
+ forceElementLabel->setText(tr("Y:"));
+ else if (i == 2)
+ forceElementLabel->setText(tr("Z:"));
+ QLabel *forceUnitLabel = new QLabel(tr("N"));
+
+ forceSpins[i]->setRange(-GZ_DBL_MAX, GZ_DBL_MAX);
+ forceSpins[i]->setSingleStep(100);
+ forceSpins[i]->setDecimals(3);
+ forceSpins[i]->setValue(0);
+ forceSpins[i]->setMaximumWidth(100);
+ forceSpins[i]->installEventFilter(this);
+ connect(forceSpins[i], SIGNAL(valueChanged(double)), this,
+ SLOT(OnForceChanged(double)));
+
+ forceVectorLayout->addWidget(forceElementLabel, i, 0, Qt::AlignRight);
+ forceVectorLayout->addWidget(forceSpins[i], i, 1);
+ forceVectorLayout->addWidget(forceUnitLabel, i, 2);
+ }
+
+ // Force total
+ QLabel *forceMagLabel = new QLabel(tr("Mag:"));
+ QLabel *forceMagUnitLabel = new QLabel(tr("N"));
+
+ this->dataPtr->forceMagSpin = new QDoubleSpinBox();
+ this->dataPtr->forceMagSpin->setRange(0, GZ_DBL_MAX);
+ this->dataPtr->forceMagSpin->setSingleStep(100);
+ this->dataPtr->forceMagSpin->setDecimals(3);
+ this->dataPtr->forceMagSpin->setValue(0);
+ this->dataPtr->forceMagSpin->setMaximumWidth(100);
+ this->dataPtr->forceMagSpin->installEventFilter(this);
+ connect(this->dataPtr->forceMagSpin, SIGNAL(valueChanged(double)), this,
+ SLOT(OnForceMagChanged(double)));
+
+ forceVectorLayout->addWidget(forceMagLabel, 3, 0, Qt::AlignRight);
+ forceVectorLayout->addWidget(this->dataPtr->forceMagSpin, 3, 1);
+ forceVectorLayout->addWidget(forceMagUnitLabel, 3, 2);
+
+ // Clear force
+ QPushButton *forceClearButton = new QPushButton(tr("Clear"));
+ connect(forceClearButton, SIGNAL(clicked()), this, SLOT(OnForceClear()));
+ forceVectorLayout->addWidget(forceClearButton, 4, 0, 1, 3, Qt::AlignLeft);
+
+ // Vertical separator
+ QFrame *separator = new QFrame();
+ separator->setFrameShape(QFrame::VLine);
+ separator->setLineWidth(10);
+
+ // Force Position
+ QLabel *forcePosLabel = new QLabel(tr("Application Point:"));
+ forcePosLabel->setObjectName("forcePosLabel");
+ forcePosLabel->setStyleSheet(
+ "QLabel#forcePosLabel {\
+ max-height: 15px;\
+ }");
+
+ // CoM
+ QLabel *comLabel = new QLabel(tr("Center of mass"));
+
+ QLabel *comPixLabel = new QLabel();
+ QPixmap comPixmap(":images/com.png");
+ comPixmap = comPixmap.scaled(QSize(20, 20));
+ comPixLabel->setPixmap(comPixmap);
+ comPixLabel->setMask(comPixmap.mask());
+
+ QHBoxLayout *comLabelLayout = new QHBoxLayout();
+ comLabelLayout->addWidget(comLabel);
+ comLabelLayout->addWidget(comPixLabel);
+
+ this->dataPtr->comRadio = new QRadioButton();
+ this->dataPtr->forcePosRadio = new QRadioButton();
+ this->dataPtr->comRadio->setChecked(true);
+ connect(this->dataPtr->comRadio, SIGNAL(toggled(bool)), this,
+ SLOT(ToggleComRadio(bool)));
+
+ // Force Position layout
+ QGridLayout *forcePosLayout = new QGridLayout();
+ forcePosLayout->setContentsMargins(0, 0, 0, 0);
+ forcePosLayout->addWidget(forcePosLabel, 0, 0, 1, 4, Qt::AlignLeft);
+ forcePosLayout->addWidget(this->dataPtr->comRadio, 1, 0);
+ forcePosLayout->addLayout(comLabelLayout, 1, 1, 1, 3, Qt::AlignLeft);
+ forcePosLayout->addWidget(this->dataPtr->forcePosRadio, 2, 0);
+
+ // Force Position Vector
+ this->dataPtr->forcePosXSpin = new QDoubleSpinBox();
+ this->dataPtr->forcePosYSpin = new QDoubleSpinBox();
+ this->dataPtr->forcePosZSpin = new QDoubleSpinBox();
+
+ std::vector<QDoubleSpinBox *> forcePosSpins;
+ forcePosSpins.push_back(this->dataPtr->forcePosXSpin);
+ forcePosSpins.push_back(this->dataPtr->forcePosYSpin);
+ forcePosSpins.push_back(this->dataPtr->forcePosZSpin);
+
+ for (unsigned int i = 0; i < forcePosSpins.size(); ++i)
+ {
+ QLabel *forcePosElementLabel = new QLabel();
+ if (i == 0)
+ forcePosElementLabel->setText(tr("X:"));
+ else if (i == 1)
+ forcePosElementLabel->setText(tr("Y:"));
+ else if (i == 2)
+ forcePosElementLabel->setText(tr("Z:"));
+ QLabel *forcePosUnitLabel = new QLabel(tr("m"));
+
+ forcePosSpins[i]->setRange(-GZ_DBL_MAX, GZ_DBL_MAX);
+ forcePosSpins[i]->setSingleStep(0.1);
+ forcePosSpins[i]->setDecimals(3);
+ forcePosSpins[i]->setValue(0);
+ forcePosSpins[i]->setMaximumWidth(100);
+ forcePosSpins[i]->installEventFilter(this);
+ connect(forcePosSpins[i], SIGNAL(valueChanged(double)), this,
+ SLOT(OnForcePosChanged(double)));
+
+ forcePosLayout->addWidget(forcePosElementLabel, i+2, 1, Qt::AlignRight);
+ forcePosLayout->addWidget(forcePosSpins[i], i+2, 2);
+ forcePosLayout->addWidget(forcePosUnitLabel, i+2, 3);
+ }
+
+ // Apply force
+ QPushButton *applyForceButton = new QPushButton("Apply Force");
+ connect(applyForceButton, SIGNAL(clicked()), this, SLOT(OnApplyForce()));
+
+ // Force layout
+ QGridLayout *forceLayout = new QGridLayout();
+ forceLayout->setContentsMargins(0, 0, 0, 0);
+ forceLayout->addWidget(forceLabel, 0, 0, 1, 5);
+ forceLayout->addItem(new QSpacerItem(10, 10), 1, 0, 1, 5);
+ forceLayout->addLayout(forceVectorLayout, 2, 1);
+ forceLayout->addWidget(separator, 2, 2);
+ forceLayout->addLayout(forcePosLayout, 2, 3);
+ forceLayout->addItem(new QSpacerItem(10, 10), 3, 0, 1, 5);
+ forceLayout->addWidget(applyForceButton, 4, 1, 1, 3, Qt::AlignRight);
+ forceLayout->addItem(new QSpacerItem(5, 10), 5, 0);
+ forceLayout->addItem(new QSpacerItem(7, 10), 5, 4);
+
+ QFrame *forceFrame = new QFrame();
+ forceFrame->setLayout(forceLayout);
+ forceFrame->setObjectName("forceLayout");
+ forceFrame->setFrameShape(QFrame::StyledPanel);
+
+ forceFrame->setStyleSheet(
+ "QFrame#forceLayout {\
+ background-color: #666;\
+ border-radius: 10px;\
+ }");
+
+ QGraphicsDropShadowEffect *forceEffect = new QGraphicsDropShadowEffect;
+ forceEffect->setBlurRadius(5);
+ forceEffect->setXOffset(5);
+ forceEffect->setYOffset(5);
+ forceEffect->setColor(Qt::black);
+ forceFrame->setGraphicsEffect(forceEffect);
+
+ // Torque
+ QLabel *torqueLabel = new QLabel(tr("<font size=4>Torque</font>"));
+ torqueLabel->setObjectName("torqueLabel");
+ torqueLabel->setStyleSheet(
+ "QLabel#torqueLabel {\
+ background-color: #444;\
+ border-radius: 5px;\
+ padding-left: 10px;\
+ min-height: 40px;\
+ }");
+
+ // Torque vector layout
+ QGridLayout *torqueVectorLayout = new QGridLayout();
+
+ // Torque Vector
+ this->dataPtr->torqueXSpin = new QDoubleSpinBox();
+ this->dataPtr->torqueYSpin = new QDoubleSpinBox();
+ this->dataPtr->torqueZSpin = new QDoubleSpinBox();
+
+ std::vector<QDoubleSpinBox *> torqueSpins;
+ torqueSpins.push_back(this->dataPtr->torqueXSpin);
+ torqueSpins.push_back(this->dataPtr->torqueYSpin);
+ torqueSpins.push_back(this->dataPtr->torqueZSpin);
+
+ for (unsigned int i = 0; i < torqueSpins.size(); ++i)
+ {
+ QLabel *torqueElementLabel = new QLabel();
+ if (i == 0)
+ torqueElementLabel->setText(tr("X:"));
+ else if (i == 1)
+ torqueElementLabel->setText(tr("Y:"));
+ else if (i == 2)
+ torqueElementLabel->setText(tr("Z:"));
+ QLabel *torqueUnitLabel = new QLabel(tr("Nm"));
+
+ torqueSpins[i]->setRange(-GZ_DBL_MAX, GZ_DBL_MAX);
+ torqueSpins[i]->setSingleStep(100);
+ torqueSpins[i]->setDecimals(3);
+ torqueSpins[i]->setValue(0);
+ torqueSpins[i]->setMaximumWidth(100);
+ torqueSpins[i]->installEventFilter(this);
+ connect(torqueSpins[i], SIGNAL(valueChanged(double)), this,
+ SLOT(OnTorqueChanged(double)));
+
+ torqueVectorLayout->addWidget(torqueElementLabel, i, 0, Qt::AlignRight);
+ torqueVectorLayout->addWidget(torqueSpins[i], i, 1);
+ torqueVectorLayout->addWidget(torqueUnitLabel, i, 2);
+ }
+
+ // Torque magnitude
+ QLabel *torqueMagLabel = new QLabel(tr("Mag:"));
+ QLabel *torqueMagUnitLabel = new QLabel(tr("Nm"));
+
+ this->dataPtr->torqueMagSpin = new QDoubleSpinBox();
+ this->dataPtr->torqueMagSpin->setRange(0, GZ_DBL_MAX);
+ this->dataPtr->torqueMagSpin->setSingleStep(100);
+ this->dataPtr->torqueMagSpin->setDecimals(3);
+ this->dataPtr->torqueMagSpin->setValue(0);
+ this->dataPtr->torqueMagSpin->setMaximumWidth(100);
+ this->dataPtr->torqueMagSpin->installEventFilter(this);
+ connect(this->dataPtr->torqueMagSpin, SIGNAL(valueChanged(double)), this,
+ SLOT(OnTorqueMagChanged(double)));
+
+ torqueVectorLayout->addWidget(torqueMagLabel, 3, 0, Qt::AlignRight);
+ torqueVectorLayout->addWidget(this->dataPtr->torqueMagSpin, 3, 1);
+ torqueVectorLayout->addWidget(torqueMagUnitLabel, 3, 2);
+
+ // Clear torque
+ QPushButton *torqueClearButton = new QPushButton(tr("Clear"));
+ connect(torqueClearButton, SIGNAL(clicked()), this, SLOT(OnTorqueClear()));
+ torqueVectorLayout->addWidget(torqueClearButton, 4, 0, 1, 3, Qt::AlignLeft);
+
+ // Apply torque
+ QPushButton *applyTorqueButton = new QPushButton("Apply Torque");
+ connect(applyTorqueButton, SIGNAL(clicked()), this, SLOT(OnApplyTorque()));
+
+ // Torque layout
+ QGridLayout *torqueLayout = new QGridLayout();
+ torqueLayout->setContentsMargins(0, 0, 0, 0);
+ torqueLayout->addWidget(torqueLabel, 0, 0, 1, 3);
+ torqueLayout->addItem(new QSpacerItem(10, 10), 1, 0, 1, 3);
+ torqueLayout->addLayout(torqueVectorLayout, 2, 1);
+ torqueLayout->addItem(new QSpacerItem(10, 10), 3, 0, 1, 3);
+ torqueLayout->addWidget(applyTorqueButton, 4, 1, 1, 1, Qt::AlignRight);
+ torqueLayout->addItem(new QSpacerItem(5, 10), 5, 0);
+ torqueLayout->addItem(new QSpacerItem(5, 10), 5, 2);
+
+ QFrame *torqueFrame = new QFrame();
+ torqueFrame->setLayout(torqueLayout);
+ torqueFrame->setObjectName("torqueLayout");
+ torqueFrame->setFrameShape(QFrame::StyledPanel);
+
+ torqueFrame->setStyleSheet(
+ "QFrame#torqueLayout {\
+ background-color: #666;\
+ border-radius: 10px;\
+ }");
+
+ QGraphicsDropShadowEffect *torqueEffect = new QGraphicsDropShadowEffect;
+ torqueEffect->setBlurRadius(5);
+ torqueEffect->setXOffset(5);
+ torqueEffect->setYOffset(5);
+ torqueEffect->setColor(Qt::black);
+ torqueFrame->setGraphicsEffect(torqueEffect);
+
+ // Buttons
+ QPushButton *cancelButton = new QPushButton(tr("Cancel"));
+ connect(cancelButton, SIGNAL(clicked()), this, SLOT(OnCancel()));
+
+ QPushButton *applyAllButton = new QPushButton("Apply All");
+ applyAllButton->setDefault(true);
+ connect(applyAllButton, SIGNAL(clicked()), this, SLOT(OnApplyAll()));
+
+ QHBoxLayout *buttonsLayout = new QHBoxLayout;
+ buttonsLayout->addWidget(cancelButton);
+ buttonsLayout->addWidget(applyAllButton);
+
+ // Main layout
+ QGridLayout *mainLayout = new QGridLayout();
+ mainLayout->setSizeConstraint(QLayout::SetFixedSize);
+ mainLayout->addWidget(this->dataPtr->modelLabel, 0, 0, 1, 2, Qt::AlignLeft);
+ mainLayout->addLayout(linkLayout, 1, 0, 1, 2, Qt::AlignLeft);
+ mainLayout->addWidget(forceFrame, 2, 0);
+ mainLayout->addWidget(torqueFrame, 2, 1);
+ mainLayout->addLayout(buttonsLayout, 3, 0, 1, 2, Qt::AlignRight);
+
+ this->setLayout(mainLayout);
+
+ // Transport
+ this->dataPtr->node = transport::NodePtr(new transport::Node());
+ this->dataPtr->node->Init();
+
+ this->dataPtr->comVector = math::Vector3::Zero;
+ this->dataPtr->forceVector = math::Vector3::Zero;
+ this->dataPtr->torqueVector = math::Vector3::Zero;
+}
+
+/////////////////////////////////////////////////
+ApplyWrenchDialog::~ApplyWrenchDialog()
+{
+ this->Fini();
+
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::Init(const std::string &_modelName,
+ const std::string &_linkName)
+{
+ if (!this->SetModel(_modelName))
+ {
+ this->Fini();
+ return;
+ }
+
+ if (!this->SetLink(_linkName))
+ {
+ this->Fini();
+ return;
+ }
+
+ connect(this, SIGNAL(rejected()), this, SLOT(OnCancel()));
+
+ if (g_rotateAct)
+ connect(g_rotateAct, SIGNAL(triggered()), this, SLOT(OnManipulation()));
+ if (g_translateAct)
+ connect(g_translateAct, SIGNAL(triggered()), this, SLOT(OnManipulation()));
+ if (g_scaleAct)
+ connect(g_scaleAct, SIGNAL(triggered()), this, SLOT(OnManipulation()));
+
+ this->move(QCursor::pos());
+ this->show();
+ this->ActivateWindow();
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::Fini()
+{
+ if (this->dataPtr->mainWindow)
+ this->dataPtr->mainWindow->removeEventFilter(this);
+
+ this->dataPtr->wrenchPub.reset();
+ this->dataPtr->node->Fini();
+ this->dataPtr->connections.clear();
+
+ if (this->dataPtr->applyWrenchVisual)
+ {
+ MouseEventHandler::Instance()->RemoveReleaseFilter(
+ "dialog_"+this->dataPtr->applyWrenchVisual->GetName());
+ MouseEventHandler::Instance()->RemovePressFilter(
+ "dialog_"+this->dataPtr->applyWrenchVisual->GetName());
+ MouseEventHandler::Instance()->RemoveMoveFilter(
+ "dialog_"+this->dataPtr->applyWrenchVisual->GetName());
+
+ this->dataPtr->applyWrenchVisual->Fini();
+ }
+ this->dataPtr->applyWrenchVisual.reset();
+
+ this->deleteLater();
+}
+
+/////////////////////////////////////////////////
+bool ApplyWrenchDialog::SetModel(const std::string &_modelName)
+{
+ if (!gui::get_active_camera() || !gui::get_active_camera()->GetScene())
+ return false;
+
+ rendering::VisualPtr vis = gui::get_active_camera()->GetScene()->
+ GetVisual(_modelName);
+
+ if (!vis)
+ {
+ gzerr << "Model [" << _modelName << "] could not be found." << std::endl;
+ return false;
+ }
+
+ this->dataPtr->modelName = _modelName;
+
+ // Check if model/link hasn't been deleted on PreRender
+ this->dataPtr->connections.push_back(
+ event::Events::ConnectPreRender(
+ boost::bind(&ApplyWrenchDialog::OnPreRender, this)));
+
+ this->dataPtr->modelLabel->setText(("<b>Model:</b> " + _modelName).c_str());
+
+ // Don't fire signals while inserting items
+ this->dataPtr->linksComboBox->blockSignals(true);
+ this->dataPtr->linksComboBox->clear();
+
+ for (unsigned int i = 0; i < vis->GetChildCount(); ++i)
+ {
+ rendering::VisualPtr childVis = vis->GetChild(i);
+ std::string linkName = childVis->GetName();
+
+ // Issue #1553: This is failing to get real links sometimes:
+ // uint32_t flags = childVis->GetVisibilityFlags();
+ // if (!((flags != GZ_VISIBILITY_ALL) && (flags & GZ_VISIBILITY_GUI)))
+ if (linkName.find("_GL_MANIP_") == std::string::npos)
+ {
+ std::string unscopedLinkName = linkName.substr(linkName.find("::") + 2);
+ this->dataPtr->linksComboBox->addItem(
+ QString::fromStdString(unscopedLinkName));
+
+ // Get CoM from link's COMVisual
+ for (unsigned int j = 0; j < childVis->GetChildCount(); ++j)
+ {
+ rendering::COMVisualPtr comVis =
+ boost::dynamic_pointer_cast<rendering::COMVisual>(
+ childVis->GetChild(j));
+
+ if (comVis)
+ {
+ this->dataPtr->linkToCOMMap[linkName] = comVis->GetInertiaPose().pos;
+ break;
+ }
+ }
+ }
+ }
+
+ // Sort alphabetically
+ QSortFilterProxyModel *proxy = new QSortFilterProxyModel(
+ this->dataPtr->linksComboBox);
+ proxy->setSourceModel(this->dataPtr->linksComboBox->model());
+ this->dataPtr->linksComboBox->model()->setParent(proxy);
+ this->dataPtr->linksComboBox->setModel(proxy);
+ this->dataPtr->linksComboBox->model()->sort(0);
+
+ this->dataPtr->linksComboBox->blockSignals(false);
+
+ if (this->dataPtr->linksComboBox->count() > 0)
+ return true;
+
+ gzerr << "Couldn't find links in model ' [" << _modelName << "]."
+ << std::endl;
+
+ return false;
+}
+
+/////////////////////////////////////////////////
+bool ApplyWrenchDialog::SetLink(const std::string &_linkName)
+{
+ if (!gui::get_active_camera() || !gui::get_active_camera()->GetScene())
+ return false;
+
+ // Select on combo box
+ std::string unscopedLinkName = _linkName.substr(_linkName.find("::") + 2);
+ int index = -1;
+ for (int i = 0; i < this->dataPtr->linksComboBox->count(); ++i)
+ {
+ if ((this->dataPtr->linksComboBox->itemText(i)).toStdString() ==
+ unscopedLinkName)
+ {
+ index = i;
+ break;
+ }
+ }
+ if (index == -1)
+ {
+ gzerr << "Link [" << _linkName << "] could not be found in the combo box."
+ << std::endl;
+ return false;
+ }
+ this->dataPtr->linksComboBox->setCurrentIndex(index);
+
+ // Visual
+ this->dataPtr->linkName = _linkName;
+ rendering::VisualPtr vis = gui::get_active_camera()->GetScene()->
+ GetVisual(this->dataPtr->linkName);
+
+ if (!vis)
+ {
+ gzerr << "A visual named [" << this->dataPtr->linkName
+ << "] could not be found." << std::endl;
+ return false;
+ }
+ this->dataPtr->linkVisual = vis;
+ this->AttachVisuals();
+
+ // Set publisher
+ std::string topicName = "~/";
+ topicName += this->dataPtr->linkName + "/wrench";
+ boost::replace_all(topicName, "::", "/");
+
+ this->dataPtr->wrenchPub.reset();
+ this->dataPtr->wrenchPub =
+ this->dataPtr->node->Advertise<msgs::Wrench>(topicName);
+
+ // Filter main window activate events
+ if (this->dataPtr->mainWindow)
+ this->dataPtr->mainWindow->installEventFilter(this);
+
+ // MouseRelease filter to gain focus
+ if (this->dataPtr->applyWrenchVisual)
+ {
+ MouseEventHandler::Instance()->AddReleaseFilter(
+ "dialog_"+this->dataPtr->applyWrenchVisual->GetName(),
+ boost::bind(&ApplyWrenchDialog::OnMouseRelease, this, _1));
+ }
+
+ return true;
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::SetLink(const QString _linkName)
+{
+ // Remove previous link's filter
+ if (this->dataPtr->applyWrenchVisual)
+ {
+ MouseEventHandler::Instance()->RemoveReleaseFilter(
+ "dialog_"+this->dataPtr->applyWrenchVisual->GetName());
+ }
+
+ if (!this->SetLink(this->dataPtr->modelName + "::" + _linkName.toStdString()))
+ this->Fini();
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnApplyAll()
+{
+ msgs::Wrench msg;
+ msgs::Set(msg.mutable_force(), this->dataPtr->forceVector.Ign());
+ msgs::Set(msg.mutable_torque(), this->dataPtr->torqueVector.Ign());
+ msgs::Set(msg.mutable_force_offset(), this->dataPtr->forcePosVector.Ign());
+
+ this->dataPtr->wrenchPub->Publish(msg);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnApplyForce()
+{
+ msgs::Wrench msg;
+ msgs::Set(msg.mutable_force(), this->dataPtr->forceVector.Ign());
+ msgs::Set(msg.mutable_torque(), ignition::math::Vector3d::Zero);
+ msgs::Set(msg.mutable_force_offset(), this->dataPtr->forcePosVector.Ign());
+
+ this->dataPtr->wrenchPub->Publish(msg);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnApplyTorque()
+{
+ msgs::Wrench msg;
+ msgs::Set(msg.mutable_force(), ignition::math::Vector3d::Zero);
+ msgs::Set(msg.mutable_torque(), this->dataPtr->torqueVector.Ign());
+
+ this->dataPtr->wrenchPub->Publish(msg);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnCancel()
+{
+ this->Fini();
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnForcePosChanged(double /*_value*/)
+{
+ // Update forcePos vector with values from XYZ spins
+ this->SetForcePos(
+ math::Vector3(this->dataPtr->forcePosXSpin->value(),
+ this->dataPtr->forcePosYSpin->value(),
+ this->dataPtr->forcePosZSpin->value()));
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnForceMagChanged(double /*_magnitude*/)
+{
+ // Update force vector proportionally
+ // Normalize current vector
+ math::Vector3 v = this->dataPtr->forceVector;
+ if (v == math::Vector3::Zero)
+ v = math::Vector3::UnitX;
+ else
+ v.Normalize();
+
+ // Multiply by new magnitude
+ this->SetForce(v * this->dataPtr->forceMagSpin->value());
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnForceChanged(double /*_value*/)
+{
+ // Update force vector with values from XYZ spins
+ this->SetForce(math::Vector3(this->dataPtr->forceXSpin->value(),
+ this->dataPtr->forceYSpin->value(),
+ this->dataPtr->forceZSpin->value()));
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnForceClear()
+{
+ this->SetForce(math::Vector3::Zero);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnTorqueMagChanged(double /*_magnitude*/)
+{
+ // Update torque vector proportionally
+ // Normalize current vector
+ math::Vector3 v = this->dataPtr->torqueVector;
+ if (v == math::Vector3::Zero)
+ v = math::Vector3::UnitX;
+ else
+ v.Normalize();
+
+ // Multiply by new magnitude
+ this->SetTorque(v * this->dataPtr->torqueMagSpin->value());
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnTorqueChanged(double /*_value*/)
+{
+ // Update torque vector with values from XYZ spins
+ this->SetTorque(math::Vector3(this->dataPtr->torqueXSpin->value(),
+ this->dataPtr->torqueYSpin->value(),
+ this->dataPtr->torqueZSpin->value()));
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnTorqueClear()
+{
+ this->SetTorque(math::Vector3::Zero);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::ToggleComRadio(bool _checked)
+{
+ if (_checked)
+ {
+ this->SetForcePos(this->dataPtr->comVector);
+ }
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::SetSpinValue(QDoubleSpinBox *_spin, double _value)
+{
+ _spin->blockSignals(true);
+ _spin->setValue(_value);
+ _spin->blockSignals(false);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::SetForcePos(const math::Vector3 &_forcePos)
+{
+ this->dataPtr->forcePosVector = _forcePos;
+
+ // Spins
+ this->SetSpinValue(this->dataPtr->forcePosXSpin, _forcePos.x);
+ this->SetSpinValue(this->dataPtr->forcePosYSpin, _forcePos.y);
+ this->SetSpinValue(this->dataPtr->forcePosZSpin, _forcePos.z);
+
+ // Check COM box
+ if (_forcePos == this->dataPtr->comVector)
+ {
+ this->dataPtr->comRadio->setChecked(true);
+ }
+ else
+ {
+ this->dataPtr->forcePosRadio->setChecked(true);
+ }
+
+ // Visuals
+ if (!this->dataPtr->applyWrenchVisual)
+ {
+ gzwarn << "No wrench visual found, so it won't be updated" << std::endl;
+ return;
+ }
+
+ this->dataPtr->applyWrenchVisual->SetForcePos(this->dataPtr->forcePosVector);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::SetForce(const math::Vector3 &_force,
+ const bool _rotatedByMouse)
+{
+ // This can be called from the dialog or the mouse
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+
+ this->dataPtr->forceVector = _force;
+
+ // Spins
+ this->SetSpinValue(this->dataPtr->forceXSpin, _force.x);
+ this->SetSpinValue(this->dataPtr->forceYSpin, _force.y);
+ this->SetSpinValue(this->dataPtr->forceZSpin, _force.z);
+ this->SetSpinValue(this->dataPtr->forceMagSpin, _force.GetLength());
+
+ // Mode
+ if (_force == math::Vector3::Zero)
+ {
+ if (this->dataPtr->torqueVector == math::Vector3::Zero)
+ this->SetMode(Mode::NONE);
+ else
+ this->SetMode(Mode::TORQUE);
+ }
+ else
+ {
+ this->SetMode(Mode::FORCE);
+ }
+
+ // Visuals
+ if (!this->dataPtr->applyWrenchVisual)
+ {
+ gzwarn << "No wrench visual found, so it won't be updated" << std::endl;
+ return;
+ }
+
+ this->dataPtr->applyWrenchVisual->SetForce(_force, _rotatedByMouse);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::SetTorque(const math::Vector3 &_torque,
+ const bool _rotatedByMouse)
+{
+ // This can be called from the dialog or the mouse
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+
+ this->dataPtr->torqueVector = _torque;
+
+ // Spins
+ this->SetSpinValue(this->dataPtr->torqueXSpin, _torque.x);
+ this->SetSpinValue(this->dataPtr->torqueYSpin, _torque.y);
+ this->SetSpinValue(this->dataPtr->torqueZSpin, _torque.z);
+ this->SetSpinValue(this->dataPtr->torqueMagSpin, _torque.GetLength());
+
+ // Mode
+ if (_torque == math::Vector3::Zero)
+ {
+ if (this->dataPtr->forceVector == math::Vector3::Zero)
+ this->SetMode(Mode::NONE);
+ else
+ this->SetMode(Mode::FORCE);
+ }
+ else
+ {
+ this->SetMode(Mode::TORQUE);
+ }
+
+ // Visuals
+ if (!this->dataPtr->applyWrenchVisual)
+ {
+ gzwarn << "No wrench visual found, so it won't be updated" << std::endl;
+ return;
+ }
+
+ this->dataPtr->applyWrenchVisual->SetTorque(_torque, _rotatedByMouse);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::SetCoM(const math::Vector3 &_com)
+{
+ this->dataPtr->comVector = _com;
+
+ // Visuals
+ if (!this->dataPtr->applyWrenchVisual)
+ {
+ gzwarn << "No wrench visual found, so it won't be updated" << std::endl;
+ return;
+ }
+
+ this->dataPtr->applyWrenchVisual->SetCoM(this->dataPtr->comVector);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnPreRender()
+{
+ if (!gui::get_active_camera() || !gui::get_active_camera()->GetScene())
+ return;
+
+ rendering::VisualPtr vis = gui::get_active_camera()->GetScene()->
+ GetVisual(this->dataPtr->linkName);
+
+ // Close dialog in case visual has been deleted
+ if (!vis)
+ this->Fini();
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::AttachVisuals()
+{
+ if (!gui::get_active_camera() || !gui::get_active_camera()->GetScene())
+ {
+ gzerr << "Camera or scene missing" << std::endl;
+ return;
+ }
+ if (!this->dataPtr->linkVisual)
+ {
+ gzerr << "No link visual specified." << std::endl;
+ return;
+ }
+
+ // Attaching for the first time
+ if (!this->dataPtr->applyWrenchVisual)
+ {
+ // Generate unique name
+ std::string visNameBase = this->dataPtr->modelName + "__APPLY_WRENCH__";
+ rendering::VisualPtr vis = gui::get_active_camera()->GetScene()->
+ GetVisual(visNameBase);
+
+ std::string visName(visNameBase);
+ int count = 0;
+ while (vis)
+ {
+ visName = visNameBase + std::to_string(count);
+ vis = gui::get_active_camera()->GetScene()->GetVisual(visName);
+ ++count;
+ }
+
+ this->dataPtr->applyWrenchVisual.reset(new rendering::ApplyWrenchVisual(
+ visName, this->dataPtr->linkVisual));
+
+ this->dataPtr->applyWrenchVisual->Load();
+ }
+ // Different link
+ else if (!this->dataPtr->applyWrenchVisual->GetParent() ||
+ this->dataPtr->applyWrenchVisual->GetParent() !=
+ this->dataPtr->linkVisual)
+ {
+ this->dataPtr->linkVisual->AttachVisual(this->dataPtr->applyWrenchVisual);
+ this->dataPtr->applyWrenchVisual->Resize();
+ }
+
+ if (!this->dataPtr->applyWrenchVisual)
+ {
+ gzwarn << "Failed to attach wrench visual. " <<
+ "Dialog will work without it." << std::endl;
+ }
+
+ // Set COM
+ this->SetCoM(this->dataPtr->linkToCOMMap[this->dataPtr->linkName]);
+ // Apply force at com by default
+ this->SetForcePos(this->dataPtr->comVector);
+ this->SetTorque(this->dataPtr->torqueVector);
+ this->SetForce(this->dataPtr->forceVector);
+}
+
+/////////////////////////////////////////////////
+bool ApplyWrenchDialog::OnMousePress(const common::MouseEvent &_event)
+{
+ rendering::UserCameraPtr userCamera = gui::get_active_camera();
+ if (!userCamera || !this->dataPtr->applyWrenchVisual)
+ return false;
+
+ this->dataPtr->draggingTool = false;
+
+ rendering::VisualPtr vis = userCamera->GetVisual(_event.Pos(),
+ this->dataPtr->manipState);
+
+ if (vis)
+ return false;
+
+ // If on top of a circle handle
+ if (this->dataPtr->manipState == "rot_z" ||
+ this->dataPtr->manipState == "rot_y")
+ {
+ this->dataPtr->draggingTool = true;
+
+ // Highlight dragged circle
+ this->dataPtr->applyWrenchVisual->GetRotTool()->SetState(
+ this->dataPtr->manipState);
+
+ // Register rotTool pose at drag start
+ this->dataPtr->dragStartPose =
+ this->dataPtr->applyWrenchVisual->GetRotTool()->GetWorldPose();
+ }
+ return false;
+}
+
+/////////////////////////////////////////////////
+bool ApplyWrenchDialog::OnMouseRelease(const common::MouseEvent &_event)
+{
+ rendering::UserCameraPtr userCamera = gui::get_active_camera();
+ if (!userCamera || !this->dataPtr->applyWrenchVisual)
+ return false;
+
+ rendering::VisualPtr vis = userCamera->GetVisual(_event.Pos(),
+ this->dataPtr->manipState);
+
+ if (!vis || _event.Dragging())
+ return false;
+
+ // Force/torque clicked: activate dialog and prevent event propagation
+ if (vis == this->dataPtr->applyWrenchVisual->GetForceVisual())
+ {
+ this->ActivateWindow();
+
+ // Activate visual, can't attach rot tool with zero vector, UnitX by default
+ if (this->dataPtr->forceVector == math::Vector3::Zero)
+ this->SetForce(math::Vector3::UnitX);
+ else
+ this->SetForce(this->dataPtr->forceVector);
+
+ return true;
+ }
+ else if (vis == this->dataPtr->applyWrenchVisual->GetTorqueVisual())
+ {
+ this->ActivateWindow();
+
+ // Activate visual, can't attach rot tool with zero vector, UnitX by default
+ if (this->dataPtr->torqueVector == math::Vector3::Zero)
+ this->SetTorque(math::Vector3::UnitX);
+ else
+ this->SetTorque(this->dataPtr->torqueVector);
+
+ return true;
+ }
+ return false;
+}
+
+/////////////////////////////////////////////////
+bool ApplyWrenchDialog::OnMouseMove(const common::MouseEvent &_event)
+{
+ rendering::UserCameraPtr userCamera = gui::get_active_camera();
+ if (!userCamera || !this->dataPtr->applyWrenchVisual)
+ return false;
+
+ // Must make a Qt check as well because Gazebo event is not working with test
+ bool isDragging = _event.Dragging() ||
+ QApplication::mouseButtons() != Qt::NoButton;
+ bool isLeftButton = _event.Button() == common::MouseEvent::LEFT ||
+ QApplication::mouseButtons() == Qt::LeftButton;
+
+ // Dragging tool, adapted from ModelManipulator::RotateEntity
+ if (isDragging && isLeftButton && this->dataPtr->draggingTool)
+ {
+ math::Vector3 normal;
+ math::Vector3 axis;
+ if (this->dataPtr->manipState == "rot_z")
+ {
+ normal = this->dataPtr->dragStartPose.rot.GetZAxis();
+ axis = math::Vector3::UnitZ;
+ }
+ else if (this->dataPtr->manipState == "rot_y")
+ {
+ normal = this->dataPtr->dragStartPose.rot.GetYAxis();
+ axis = math::Vector3::UnitY;
+ }
+ else
+ {
+ gzerr << "Dragging tool on wrong manip state, this shouldn't happen" <<
+ std::endl;
+ return false;
+ }
+
+ double offset = this->dataPtr->dragStartPose.pos.Dot(normal);
+
+ math::Vector3 pressPoint;
+ userCamera->GetWorldPointOnPlane(_event.PressPos().X(),
+ _event.PressPos().Y(),
+ math::Plane(normal, offset), pressPoint);
+
+ math::Vector3 newPoint;
+ userCamera->GetWorldPointOnPlane(_event.Pos().X(), _event.Pos().Y(),
+ math::Plane(normal, offset), newPoint);
+
+ math::Vector3 v1 = pressPoint - this->dataPtr->dragStartPose.pos;
+ math::Vector3 v2 = newPoint - this->dataPtr->dragStartPose.pos;
+ v1 = v1.Normalize();
+ v2 = v2.Normalize();
+ double signTest = v1.Cross(v2).Dot(normal);
+ double angle = atan2((v1.Cross(v2)).GetLength(), v1.Dot(v2));
+
+ if (signTest < 0)
+ angle *= -1;
+
+ if (QApplication::keyboardModifiers() & Qt::ControlModifier)
+ angle = rint(angle / (M_PI * 0.25)) * (M_PI * 0.25);
+
+ math::Quaternion rot(axis, angle);
+ rot = this->dataPtr->dragStartPose.rot * rot;
+
+ // Must rotate the tool here to make sure we have proper roll,
+ // once the rotation gets transformed into a vector we lose a DOF
+ this->dataPtr->applyWrenchVisual->GetRotTool()->SetWorldRotation(rot);
+
+ // Get direction from tool orientation
+ math::Vector3 vec;
+ math::Vector3 rotEuler;
+ rotEuler = rot.GetAsEuler();
+ vec.x = cos(rotEuler.z)*cos(rotEuler.y);
+ vec.y = sin(rotEuler.z)*cos(rotEuler.y);
+ vec.z = -sin(rotEuler.y);
+
+ // To local frame
+ vec = this->dataPtr->linkVisual->GetWorldPose().rot.RotateVectorReverse(
+ vec);
+
+ if (this->GetMode() == Mode::FORCE)
+ {
+ this->NewForceDirection(vec);
+ }
+ else if (this->GetMode() == Mode::TORQUE)
+ {
+ this->NewTorqueDirection(vec);
+ }
+ return true;
+ }
+ // Highlight hovered tools
+ else
+ {
+ userCamera->GetVisual(_event.Pos(), this->dataPtr->manipState);
+
+ if (this->dataPtr->manipState == "rot_z" ||
+ this->dataPtr->manipState == "rot_y")
+ {
+ this->dataPtr->applyWrenchVisual->GetRotTool()->SetState(
+ this->dataPtr->manipState);
+ }
+ else
+ {
+ this->dataPtr->applyWrenchVisual->GetRotTool()->SetState("");
+ }
+ }
+
+ return false;
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::SetMode(Mode _mode)
+{
+ this->dataPtr->mode = _mode;
+}
+
+/////////////////////////////////////////////////
+ApplyWrenchDialog::Mode ApplyWrenchDialog::GetMode() const
+{
+ return this->dataPtr->mode;
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::NewForceDirection(const math::Vector3 &_dir)
+{
+ // Normalize direction
+ math::Vector3 v = _dir;
+ if (v == math::Vector3::Zero)
+ v = math::Vector3::UnitX;
+ else
+ v.Normalize();
+
+ // Multiply by magnitude
+ this->SetForce(v * this->dataPtr->forceMagSpin->value(), true);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::NewTorqueDirection(const math::Vector3 &_dir)
+{
+ // Normalize direction
+ math::Vector3 v = _dir;
+ if (v == math::Vector3::Zero)
+ v = math::Vector3::UnitX;
+ else
+ v.Normalize();
+
+ // Multiply by magnitude
+ this->SetTorque(v * this->dataPtr->torqueMagSpin->value(), true);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::SetActive(bool _active)
+{
+ if (!this->dataPtr->applyWrenchVisual)
+ {
+ gzerr << "No apply wrench visual." << std::endl;
+ this->Fini();
+ return;
+ }
+ if (_active)
+ {
+ // Set visible
+ this->dataPtr->applyWrenchVisual->SetMode(
+ static_cast<rendering::ApplyWrenchVisual::Mode>(this->GetMode()));
+
+ // Set selected
+ event::Events::setSelectedEntity(this->dataPtr->linkName, "normal");
+
+ // Set arrow mode
+ if (g_arrowAct)
+ g_arrowAct->trigger();
+
+ MouseEventHandler::Instance()->AddPressFilter(
+ "dialog_"+this->dataPtr->applyWrenchVisual->GetName(),
+ boost::bind(&ApplyWrenchDialog::OnMousePress, this, _1));
+
+ MouseEventHandler::Instance()->AddMoveFilter(
+ "dialog_"+this->dataPtr->applyWrenchVisual->GetName(),
+ boost::bind(&ApplyWrenchDialog::OnMouseMove, this, _1));
+ }
+ else
+ {
+ this->dataPtr->applyWrenchVisual->SetMode(
+ rendering::ApplyWrenchVisual::Mode::NONE);
+
+ MouseEventHandler::Instance()->RemovePressFilter(
+ "dialog_"+this->dataPtr->applyWrenchVisual->GetName());
+ MouseEventHandler::Instance()->RemoveMoveFilter(
+ "dialog_"+this->dataPtr->applyWrenchVisual->GetName());
+ }
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::OnManipulation()
+{
+ this->SetActive(false);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::ActivateWindow()
+{
+ if (!this->isActiveWindow())
+ {
+ // Clear focus before activating not to trigger FucusIn
+ QWidget *focusedWidget = this->focusWidget();
+ if (focusedWidget)
+ focusedWidget->clearFocus();
+
+ this->activateWindow();
+ }
+}
+
+/////////////////////////////////////////////////
+bool ApplyWrenchDialog::eventFilter(QObject *_object, QEvent *_event)
+{
+ // Attach rotation tool to focused mode
+ if (_event->type() == QEvent::FocusIn)
+ {
+ if (_object == this->dataPtr->forceMagSpin ||
+ _object == this->dataPtr->forceXSpin ||
+ _object == this->dataPtr->forceYSpin ||
+ _object == this->dataPtr->forceZSpin ||
+ _object == this->dataPtr->forcePosXSpin ||
+ _object == this->dataPtr->forcePosYSpin ||
+ _object == this->dataPtr->forcePosZSpin ||
+ (_object == this->dataPtr->linksComboBox &&
+ this->dataPtr->mode != Mode::TORQUE))
+ {
+ this->SetForce(this->dataPtr->forceVector);
+ }
+ else if (_object == this->dataPtr->torqueMagSpin ||
+ _object == this->dataPtr->torqueXSpin ||
+ _object == this->dataPtr->torqueYSpin ||
+ _object == this->dataPtr->torqueZSpin ||
+ (_object == this->dataPtr->linksComboBox &&
+ this->dataPtr->mode == Mode::TORQUE))
+ {
+ this->SetTorque(this->dataPtr->torqueVector);
+ }
+ }
+ // Deactivate this when another dialog is focused
+ else if (_event->type() == QEvent::ActivationChange)
+ {
+ if (!this->dataPtr->mainWindow)
+ return false;
+
+ if (_object == this->dataPtr->mainWindow)
+ {
+ if (!this->isActiveWindow() &&
+ !this->dataPtr->mainWindow->isActiveWindow())
+ {
+ this->SetActive(false);
+ }
+ }
+ }
+ // Activate when changing spinboxes with mousewheel
+ else if (_event->type() == QEvent::Wheel)
+ {
+ this->ActivateWindow();
+ }
+
+ return false;
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog::changeEvent(QEvent *_event)
+{
+ // Focus in this dialog
+ if (_event->type() == QEvent::ActivationChange)
+ {
+ // During tests it seems not to find main window, so this is true by default
+ bool mainWindowActive = true;
+
+ if (!this->dataPtr->mainWindow || (this->dataPtr->mainWindow &&
+ !this->dataPtr->mainWindow->isActiveWindow()))
+ {
+ mainWindowActive = false;
+ }
+
+ this->SetActive(this->isActiveWindow() || mainWindowActive);
+ }
+}
+
diff --git a/gazebo/gui/ApplyWrenchDialog.hh b/gazebo/gui/ApplyWrenchDialog.hh
new file mode 100644
index 0000000..5db83d6
--- /dev/null
+++ b/gazebo/gui/ApplyWrenchDialog.hh
@@ -0,0 +1,224 @@
+/*
+ * Copyright 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef _GAZEBO_APPLY_WRENCH_DIALOG_HH_
+#define _GAZEBO_APPLY_WRENCH_DIALOG_HH_
+
+#include <string>
+
+#include "gazebo/common/MouseEvent.hh"
+#include "gazebo/gui/qt.h"
+#include "gazebo/math/Vector3.hh"
+#include "gazebo/transport/TransportTypes.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class ApplyWrenchDialogPrivate;
+
+ /// \addtogroup gazebo_gui
+ /// \{
+
+ /// \class ApplyWrenchDialog ApplyWrenchDialog.hh gui/gui.hh
+ /// \brief Dialog for applying force and torque to a model.
+ class GAZEBO_VISIBLE ApplyWrenchDialog : public QDialog
+ {
+ Q_OBJECT
+
+ /// \enum Modes
+ /// \brief Dialog is in one of these modes at each time.
+ public: enum Mode
+ {
+ /// \brief None
+ NONE = 0,
+ /// \brief Force
+ FORCE = 1,
+ /// \brief Torque
+ TORQUE = 2
+ };
+
+ /// \brief Constructor.
+ /// \param[in] _parent Parent QWidget.
+ public: ApplyWrenchDialog(QWidget *_parent = 0);
+
+ /// \brief Destructor.
+ public: ~ApplyWrenchDialog();
+
+ /// \brief Initiate the dialog.
+ /// \param[in] _modelName Scoped name of the model.
+ /// \param[in] _linkName Scoped name of a link within the model to which
+ /// a wrench will be applied. The link might be changed later, but not
+ /// the model.
+ public: void Init(const std::string &_modelName,
+ const std::string &_linkName);
+
+ /// \brief Finish the dialog.
+ public: void Fini();
+
+ /// \brief Get the mode, either force, torque or none.
+ /// \return Current mode.
+ public: Mode GetMode() const;
+
+ /// \brief Set model to which wrench will be applied.
+ /// \param[in] _modelName Scoped model name.
+ /// \return True if model was properly set.
+ private: bool SetModel(const std::string &_modelName);
+
+ /// \brief Set link to which wrench will be applied.
+ /// \param[in] _linkName Scoped link name.
+ /// \return True if link was properly set.
+ private: bool SetLink(const std::string &_linkName);
+
+ /// \brief Set link from combo box.
+ /// \param[in] _linkName Link leaf name.
+ private slots: void SetLink(const QString _linkName);
+
+ /// \brief Qt callback when the Apply All button is pressed.
+ private slots: void OnApplyAll();
+
+ /// \brief Qt callback when the Apply Force button is pressed.
+ private slots: void OnApplyForce();
+
+ /// \brief Qt callback when the Apply Torque button is pressed.
+ private slots: void OnApplyTorque();
+
+ /// \brief Qt callback when the Cancel button is pressed.
+ private slots: void OnCancel();
+
+ /// \brief Qt callback to set position to CoM.
+ /// \param[in] _checked Whether it is checked or not.
+ private slots: void ToggleComRadio(bool _checked);
+
+ /// \brief Qt callback when some force position component was changed.
+ /// \param[in] _value New value, not used.
+ private slots: void OnForcePosChanged(double _value);
+
+ /// \brief Qt callback when the force magnitude was changed.
+ /// \param[in] _magnitude Force magnitude.
+ private slots: void OnForceMagChanged(double _magnitude);
+
+ /// \brief Qt callback when some force component was changed.
+ /// \param[in] _value New value, not used.
+ private slots: void OnForceChanged(double _value);
+
+ /// \brief Qt callback when the the force clear button is clicked.
+ private slots: void OnForceClear();
+
+ /// \brief Qt callback when the torque magnitude was changed.
+ /// \param[in] _magnitude Torque magnitude.
+ private slots: void OnTorqueMagChanged(double _magnitude);
+
+ /// \brief Qt callback when some torque component was changed.
+ /// \param[in] _value New value, not used.
+ private slots: void OnTorqueChanged(double _value);
+
+ /// \brief Qt callback when the the torque clear button is clicked.
+ private slots: void OnTorqueClear();
+
+ /// \brief Qt callback when entering a manipulation mode.
+ private slots: void OnManipulation();
+
+ /// \brief Filter events from other Qt objects.
+ /// param[in] _object Qt object watched by the event filter
+ /// param[in] _event Qt event to be filtered.
+ /// \return True to stop event propagation.
+ private slots: bool eventFilter(QObject *_object, QEvent *_event);
+
+ /// \brief Handle change events related to this dialog.
+ /// param[in] _event Qt event.
+ private slots: void changeEvent(QEvent *_event);
+
+ /// \brief Set the value of a specific spin without triggering signals to
+ /// avoid recursion loops.
+ /// \param[in] _spin Spin whose value will be changed.
+ /// \param[in] _value New value.
+ private: void SetSpinValue(QDoubleSpinBox *_spin, double _value);
+
+ /// \brief Set force position vector, send it to visuals and update spins.
+ /// \param[in] _forcePos New force position.
+ private: void SetForcePos(const math::Vector3 &_forcePos);
+
+ /// \brief Set force vector, send it to visuals and update spins.
+ /// \param[in] _force New force.
+ /// \param[in] _rotatedByMouse Rot tool has been rotated by the mouse.
+ private: void SetForce(const math::Vector3 &_force,
+ const bool _rotatedByMouse = false);
+
+ /// \brief Set torque vector, send it to visuals and update spins.
+ /// \param[in] _torque New torque.
+ /// \param[in] _rotatedByMouse Rot tool has been rotated by the mouse.
+ private: void SetTorque(const math::Vector3 &_torque,
+ const bool _rotatedByMouse = false);
+
+ /// \brief Callback on prerender to check if target link hasn't been
+ /// deleted.
+ private: void OnPreRender();
+
+ /// \brief Attach apply wrench visual to target link.
+ private: void AttachVisuals();
+
+ /// \brief Set CoM vector and send it to visuals.
+ /// \param[in] _com CoM position in link frame.
+ private: void SetCoM(const math::Vector3 &_com);
+
+ /// \brief Callback for a mouse press event.
+ /// \param[in] _event The mouse press event
+ /// \return True if handled by this function.
+ private: bool OnMousePress(const common::MouseEvent &_event);
+
+ /// \brief Callback for a mouse release event.
+ /// \param[in] _event The mouse release event
+ /// \return True if handled by this function.
+ private: bool OnMouseRelease(const common::MouseEvent &_event);
+
+ /// \brief Callback for a mouse move event.
+ /// \param[in] _event The mouse move event
+ /// \return True if handled by this function.
+ private: bool OnMouseMove(const common::MouseEvent &_event);
+
+ /// \brief Set the mode to either force, torque or none.
+ /// \param[in] _mode Current mode.
+ private: void SetMode(Mode _mode);
+
+ /// \brief Update force vector with direction given by mouse, magnitude
+ /// from spin.
+ /// \param[in] _dir New direction, doesn't need to be normalized.
+ private: void NewForceDirection(const math::Vector3 &_dir);
+
+ /// \brief Update torque vector with direction given by mouse, magnitude
+ /// from spin.
+ /// \param[in] _dir New direction, doesn't need to be normalized.
+ private: void NewTorqueDirection(const math::Vector3 &_dir);
+
+ /// \brief Set this dialog to be active, visuals visible and mouse
+ /// filters on.
+ /// \param[in] _active True to make it active.
+ private: void SetActive(bool _active);
+
+ /// \brief Set this dialog window to be active.
+ private: void ActivateWindow();
+
+ /// \internal
+ /// \brief Pointer to private data.
+ private: ApplyWrenchDialogPrivate *dataPtr;
+ };
+ /// \}
+ }
+}
+
+#endif
diff --git a/gazebo/gui/ApplyWrenchDialogPrivate.hh b/gazebo/gui/ApplyWrenchDialogPrivate.hh
new file mode 100644
index 0000000..0275852
--- /dev/null
+++ b/gazebo/gui/ApplyWrenchDialogPrivate.hh
@@ -0,0 +1,140 @@
+/*
+ * 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_APPLY_WRENCH_DIALOG_PRIVATE_HH_
+#define _GAZEBO_APPLY_WRENCH_DIALOG_PRIVATE_HH_
+
+#include <string>
+#include <vector>
+#include <map>
+#include <mutex>
+
+#include "gazebo/gui/qt.h"
+#include "gazebo/gui/ApplyWrenchDialog.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \class ApplyWrenchDialogPrivate ApplyWrenchDialogPrivate.hh
+ /// \brief Private data for the ApplyWrenchDialog class
+ class ApplyWrenchDialogPrivate
+ {
+ /// \brief Node for communication.
+ public: transport::NodePtr node;
+
+ /// \brief Name of the model this is connected to.
+ public: std::string modelName;
+
+ /// \brief Name of the link currently targeted.
+ public: std::string linkName;
+
+ /// \brief Label holding the model name.
+ public: QLabel *modelLabel;
+
+ /// \brief Dropdown holding all link names.
+ public: QComboBox *linksComboBox;
+
+ /// \brief Radio button for CoM.
+ public: QRadioButton *comRadio;
+
+ /// \brief Radio button for force position.
+ public: QRadioButton *forcePosRadio;
+
+ /// \brief Spin for force position X.
+ public: QDoubleSpinBox *forcePosXSpin;
+
+ /// \brief Spin for force position Y.
+ public: QDoubleSpinBox *forcePosYSpin;
+
+ /// \brief Spin for force position Z.
+ public: QDoubleSpinBox *forcePosZSpin;
+
+ /// \brief Spin for force magnitude.
+ public: QDoubleSpinBox *forceMagSpin;
+
+ /// \brief Spin for force X.
+ public: QDoubleSpinBox *forceXSpin;
+
+ /// \brief Spin for force Y.
+ public: QDoubleSpinBox *forceYSpin;
+
+ /// \brief Spin for force Z.
+ public: QDoubleSpinBox *forceZSpin;
+
+ /// \brief Spin for torque magnitude.
+ public: QDoubleSpinBox *torqueMagSpin;
+
+ /// \brief Spin for torque X.
+ public: QDoubleSpinBox *torqueXSpin;
+
+ /// \brief Spin for torque Y.
+ public: QDoubleSpinBox *torqueYSpin;
+
+ /// \brief Spin for torque Z.
+ public: QDoubleSpinBox *torqueZSpin;
+
+ /// \brief CoM coordinates in link frame.
+ public: math::Vector3 comVector;
+
+ /// \brief Force position coordinates in link frame.
+ public: math::Vector3 forcePosVector;
+
+ /// \brief Force vector.
+ public: math::Vector3 forceVector;
+
+ /// \brief Torque vector.
+ public: math::Vector3 torqueVector;
+
+ /// \brief Publishes the wrench message.
+ public: transport::PublisherPtr wrenchPub;
+
+ /// \brief Visual of the targeted link.
+ public: rendering::VisualPtr linkVisual;
+
+ /// \brief A list of events connected to this.
+ public: std::vector<event::ConnectionPtr> connections;
+
+ /// \brief Map link name to link CoM vector.
+ public: std::map<std::string, math::Vector3> linkToCOMMap;
+
+ /// \brief Interactive visual which represents the wrench to be applied.
+ public: rendering::ApplyWrenchVisualPtr applyWrenchVisual;
+
+ /// \brief Indicate whether mouse is dragging on top the
+ /// rotation tool or not.
+ public: bool draggingTool;
+
+ /// \brief World pose of the rotation tool the moment dragging
+ /// started.
+ public: math::Pose dragStartPose;
+
+ /// \brief State of the manipulation tool, here only using "rot_y"
+ /// and "rot_z".
+ public: std::string manipState;
+
+ /// \brief Current mode, either force, torque or none.
+ public: ApplyWrenchDialog::Mode mode;
+
+ /// \brief Mutex to protect variables.
+ public: std::mutex mutex;
+
+ /// \brief Pointer to the main window.
+ public: MainWindow *mainWindow;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/ApplyWrenchDialog_TEST.cc b/gazebo/gui/ApplyWrenchDialog_TEST.cc
new file mode 100644
index 0000000..58318d0
--- /dev/null
+++ b/gazebo/gui/ApplyWrenchDialog_TEST.cc
@@ -0,0 +1,656 @@
+/*
+ * 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/gui/Actions.hh"
+#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/MainWindow.hh"
+#include "gazebo/gui/GLWidget.hh"
+
+#include "gazebo/gui/ApplyWrenchDialog.hh"
+#include "gazebo/gui/ApplyWrenchDialog_TEST.hh"
+
+#include "test_config.h"
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog_TEST::ApplyForceTorqueFromDialog()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ // World with one model which has 2 links, no ground plane and gravity is off
+ this->Load("worlds/multilink_shape.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();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+ gazebo::rendering::ScenePtr scene = cam->GetScene();
+ QVERIFY(scene != NULL);
+
+ // Get the model
+ gazebo::rendering::VisualPtr modelVis = scene->GetVisual("multilink");
+ QVERIFY(modelVis != NULL);
+
+ // Get the box link
+ gazebo::rendering::VisualPtr boxLinkVis =
+ scene->GetVisual("multilink::box_link");
+ QVERIFY(boxLinkVis != NULL);
+ gazebo::math::Pose boxLinkPose = boxLinkVis->GetWorldPose();
+ QVERIFY(boxLinkPose == boxLinkVis->GetWorldPose());
+
+ // Get the sphere link
+ gazebo::rendering::VisualPtr sphereLinkVis =
+ scene->GetVisual("multilink::sphere_link");
+ QVERIFY(sphereLinkVis != NULL);
+ gazebo::math::Pose sphereLinkPose = sphereLinkVis->GetWorldPose();
+ QVERIFY(sphereLinkPose == sphereLinkVis->GetWorldPose());
+
+ // Check that an inexistent model doesn't break anything
+ gazebo::gui::ApplyWrenchDialog *applyWrenchDialogFakeModel =
+ new gazebo::gui::ApplyWrenchDialog();
+ applyWrenchDialogFakeModel->Init("fake_model", "fake_link");
+
+ // Check that an inexistent link doesn't break anything
+ gazebo::gui::ApplyWrenchDialog *applyWrenchDialogFakeLink =
+ new gazebo::gui::ApplyWrenchDialog();
+ applyWrenchDialogFakeLink->Init("multilink", "fake_link");
+
+ // Initialize dialog with the box link
+ gazebo::gui::ApplyWrenchDialog *applyWrenchDialog =
+ new gazebo::gui::ApplyWrenchDialog();
+ applyWrenchDialog->Init("multilink", "multilink::box_link");
+
+ // Get combo box
+ QList<QComboBox *> comboBoxes =
+ applyWrenchDialog->findChildren<QComboBox *>();
+ QVERIFY(comboBoxes.size() == 1u);
+
+ // Check the combo box's items
+ QVERIFY(comboBoxes[0]->count() == 2u);
+ QVERIFY(comboBoxes[0]->itemText(0) == "box_link");
+ QVERIFY(comboBoxes[0]->itemText(1) == "sphere_link");
+ QVERIFY(comboBoxes[0]->currentIndex() == 0u);
+
+ // Get radio buttons
+ QList<QRadioButton *> radioButtons =
+ applyWrenchDialog->findChildren<QRadioButton *>();
+ QVERIFY(radioButtons.size() == 2u);
+
+ // Get spins
+ QList<QDoubleSpinBox *> spins =
+ applyWrenchDialog->findChildren<QDoubleSpinBox *>();
+ QVERIFY(spins.size() == 11u);
+
+ // Get buttons
+ QList<QPushButton *> buttons =
+ applyWrenchDialog->findChildren<QPushButton *>();
+ QVERIFY(buttons.size() == 6u);
+
+ QPushButton *applyForceButton = NULL;
+ QPushButton *applyTorqueButton = NULL;
+ QPushButton *applyAllButton = NULL;
+ QPushButton *clearForceButton = NULL;
+ QPushButton *clearTorqueButton = NULL;
+ QPushButton *cancelButton = NULL;
+ for (auto it : buttons)
+ {
+ QVERIFY(it);
+ if (it->text().toLower().toStdString() == "apply force")
+ applyForceButton = it;
+ else if (it->text().toLower().toStdString() == "apply torque")
+ applyTorqueButton = it;
+ else if (it->text().toLower().toStdString() == "apply all")
+ applyAllButton = it;
+ else if (it->text().toLower().toStdString() == "clear" && !clearForceButton)
+ clearForceButton = it;
+ else if (it->text().toLower().toStdString() == "clear")
+ clearTorqueButton = it;
+ else if (it->text().toLower().toStdString() == "cancel")
+ cancelButton = it;
+ }
+ QVERIFY(applyForceButton);
+ QVERIFY(applyTorqueButton);
+ QVERIFY(applyAllButton);
+ QVERIFY(clearForceButton);
+ QVERIFY(clearTorqueButton);
+ QVERIFY(cancelButton);
+
+ // Set and apply force on X axis, magnitude 1000
+ spins[0]->setValue(1.0);
+ spins[3]->setValue(1000.0);
+ applyForceButton->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that force spin was updated according to magnitude
+ QCOMPARE(spins[0]->value(), 1000.0);
+
+ // Check that link moved on X axis
+ QVERIFY(boxLinkPose.pos.x < boxLinkVis->GetWorldPose().pos.x);
+ QVERIFY(boxLinkPose.pos.y - boxLinkVis->GetWorldPose().pos.y < 1e-6);
+ QVERIFY(boxLinkPose.pos.z - boxLinkVis->GetWorldPose().pos.z < 1e-6);
+ QCOMPARE(boxLinkPose.rot, boxLinkVis->GetWorldPose().rot);
+
+ // Save current pose
+ boxLinkPose = boxLinkVis->GetWorldPose();
+ QVERIFY(boxLinkPose == boxLinkVis->GetWorldPose());
+
+ // Set and apply torque about -Z axis, magnitude 1000
+ spins[9]->setValue(-1.0);
+ spins[10]->setValue(1000.0);
+ applyTorqueButton->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that torque spin was updated according to magnitude
+ QCOMPARE(spins[9]->value(), -1000.0);
+
+ // Check that link rotated
+ QVERIFY(boxLinkPose.pos.x < boxLinkVis->GetWorldPose().pos.x);
+ QVERIFY(boxLinkPose.pos.y - boxLinkVis->GetWorldPose().pos.y < 1e-6);
+ QVERIFY(boxLinkPose.pos.z - boxLinkVis->GetWorldPose().pos.z < 1e-6);
+ QVERIFY(boxLinkPose.rot != boxLinkVis->GetWorldPose().rot);
+
+ // Save current pose
+ boxLinkPose = boxLinkVis->GetWorldPose();
+ QVERIFY(boxLinkPose == boxLinkVis->GetWorldPose());
+
+ // Apply force and torque
+ applyAllButton->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that link translated and rotated
+ QVERIFY(boxLinkPose.pos != boxLinkVis->GetWorldPose().pos);
+ QVERIFY(boxLinkPose.rot != boxLinkVis->GetWorldPose().rot);
+
+ // Change link
+ comboBoxes[0]->setCurrentIndex(1);
+ QVERIFY(comboBoxes[0]->currentText() == "sphere_link");
+
+ // Clear force
+ clearForceButton->click();
+ QCOMPARE(spins[0]->value(), 0.0);
+ QCOMPARE(spins[1]->value(), 0.0);
+ QCOMPARE(spins[2]->value(), 0.0);
+ QCOMPARE(spins[3]->value(), 0.0);
+
+ // Clear torque
+ clearTorqueButton->click();
+ QCOMPARE(spins[7]->value(), 0.0);
+ QCOMPARE(spins[8]->value(), 0.0);
+ QCOMPARE(spins[9]->value(), 0.0);
+ QCOMPARE(spins[10]->value(), 0.0);
+
+ // Apply zero force and torque
+ applyAllButton->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that link didn't move
+ QVERIFY(sphereLinkPose.pos == sphereLinkVis->GetWorldPose().pos);
+ QVERIFY(sphereLinkPose.rot == sphereLinkVis->GetWorldPose().rot);
+
+ // Set and apply force on Y axis with an offset on X
+ spins[1]->setValue(1000.0);
+ spins[4]->setValue(1.0);
+ applyForceButton->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that link translated and rotated
+ QVERIFY(sphereLinkPose.pos != sphereLinkVis->GetWorldPose().pos);
+ QVERIFY(sphereLinkPose.rot != sphereLinkVis->GetWorldPose().rot);
+
+ // Select CoM as application point
+ radioButtons[0]->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that force offset spins were updated
+ QCOMPARE(spins[4]->value(), 0.0);
+ QCOMPARE(spins[5]->value(), 0.0);
+ QCOMPARE(spins[6]->value(), 0.0);
+
+ // Close dialog
+ cancelButton->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ delete applyWrenchDialog;
+
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchDialog_TEST::MouseInteractions()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ // World with one model which has 2 links, no ground plane and gravity is off
+ this->Load("worlds/multilink_shape.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();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+ gazebo::rendering::ScenePtr scene = cam->GetScene();
+ QVERIFY(scene != NULL);
+
+ // Get GLWidget
+ gazebo::gui::GLWidget *glWidget =
+ mainWindow->findChild<gazebo::gui::GLWidget *>("GLWidget");
+ QVERIFY(glWidget != NULL);
+
+ // Get the model
+ gazebo::rendering::VisualPtr modelVis = scene->GetVisual("multilink");
+ QVERIFY(modelVis != NULL);
+
+ // Get the box link
+ gazebo::rendering::VisualPtr boxLinkVis =
+ scene->GetVisual("multilink::box_link");
+ QVERIFY(boxLinkVis != NULL);
+ gazebo::math::Pose boxLinkPose = boxLinkVis->GetWorldPose();
+ QVERIFY(boxLinkPose == boxLinkVis->GetWorldPose());
+
+ // Get the sphere link
+ gazebo::rendering::VisualPtr sphereLinkVis =
+ scene->GetVisual("multilink::sphere_link");
+ QVERIFY(sphereLinkVis != NULL);
+ gazebo::math::Pose sphereLinkPose = sphereLinkVis->GetWorldPose();
+ QVERIFY(sphereLinkPose == sphereLinkVis->GetWorldPose());
+
+ // Move the mouse to the corner of the screen so the dialogs open there
+ QTest::mouseMove(glWidget, QPoint(-glWidget->width()*0.5,
+ -glWidget->height()*0.5));
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Initialize dialog for the box link
+ gazebo::gui::ApplyWrenchDialog *applyWrenchDialogBox =
+ new gazebo::gui::ApplyWrenchDialog();
+ applyWrenchDialogBox->Init("multilink", "multilink::box_link");
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that box dialog has focus
+ QVERIFY(applyWrenchDialogBox->isActiveWindow());
+
+ // Check mode
+ QVERIFY(applyWrenchDialogBox->GetMode() ==
+ gazebo::gui::ApplyWrenchDialog::Mode::NONE);
+
+ // Get the ApplyWrenchVisual for the box
+ gazebo::rendering::ApplyWrenchVisualPtr boxApplyWrenchVis =
+ boost::dynamic_pointer_cast<gazebo::rendering::ApplyWrenchVisual>(
+ scene->GetVisual("multilink__APPLY_WRENCH__"));
+ QVERIFY(boxApplyWrenchVis != NULL);
+ QVERIFY(boxApplyWrenchVis->GetParent() == boxLinkVis);
+
+ // Check that the force visual is visible and inactive
+ QVERIFY(boxApplyWrenchVis->GetForceVisual() != NULL);
+ QVERIFY(boxApplyWrenchVis->GetForceVisual()->GetMaterialName().find(
+ "Gazebo/DarkOrangeTransparentOverlay") != std::string::npos);
+
+ // Check that the torque visual is visible and inactive
+ QVERIFY(boxApplyWrenchVis->GetTorqueVisual() != NULL);
+ QVERIFY(boxApplyWrenchVis->GetTorqueVisual()->GetMaterialName().find(
+ "Gazebo/DarkOrangeTransparentOverlay") != std::string::npos);
+
+ // Check that rot tool is not visible, not selected and in rotate mode
+ QVERIFY(!boxApplyWrenchVis->GetRotTool()->GetHandleVisible(
+ gazebo::rendering::SelectionObj::SelectionMode::ROT_Y));
+ QVERIFY(!boxApplyWrenchVis->GetRotTool()->GetHandleVisible(
+ gazebo::rendering::SelectionObj::SelectionMode::ROT_Z));
+ QVERIFY(boxApplyWrenchVis->GetRotTool()->GetState() ==
+ gazebo::rendering::SelectionObj::SelectionMode::SELECTION_NONE);
+ QVERIFY(boxApplyWrenchVis->GetRotTool()->GetMode() ==
+ gazebo::rendering::SelectionObj::SelectionMode::ROT);
+
+ // Initialize dialog for the sphere link
+ gazebo::gui::ApplyWrenchDialog *applyWrenchDialogSphere =
+ new gazebo::gui::ApplyWrenchDialog();
+ applyWrenchDialogSphere->Init("multilink", "multilink::sphere_link");
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that sphere dialog has focus
+ QVERIFY(!applyWrenchDialogBox->isActiveWindow());
+ QVERIFY(applyWrenchDialogSphere->isActiveWindow());
+
+ // Check mode
+ QVERIFY(applyWrenchDialogSphere->GetMode() ==
+ gazebo::gui::ApplyWrenchDialog::Mode::NONE);
+
+ // Get the ApplyWrenchVisual for the sphere
+ gazebo::rendering::ApplyWrenchVisualPtr sphereApplyWrenchVis =
+ boost::dynamic_pointer_cast<gazebo::rendering::ApplyWrenchVisual>(
+ scene->GetVisual("multilink__APPLY_WRENCH__0"));
+ QVERIFY(sphereApplyWrenchVis != NULL);
+ QVERIFY(sphereApplyWrenchVis->GetParent() == sphereLinkVis);
+
+ // Check that the force visual is visible and inactive
+ QVERIFY(sphereApplyWrenchVis->GetForceVisual() != NULL);
+ QVERIFY(sphereApplyWrenchVis->GetForceVisual()->GetMaterialName().find(
+ "Gazebo/DarkOrangeTransparentOverlay") != std::string::npos);
+
+ // Check that the torque visual is visible and inactive
+ QVERIFY(sphereApplyWrenchVis->GetTorqueVisual() != NULL);
+ QVERIFY(sphereApplyWrenchVis->GetTorqueVisual()->GetMaterialName().find(
+ "Gazebo/DarkOrangeTransparentOverlay") != std::string::npos);
+
+ // Check that rot tool is not visible, not selected and in rotate mode
+ QVERIFY(!sphereApplyWrenchVis->GetRotTool()->GetHandleVisible(
+ gazebo::rendering::SelectionObj::SelectionMode::ROT_Y));
+ QVERIFY(!sphereApplyWrenchVis->GetRotTool()->GetHandleVisible(
+ gazebo::rendering::SelectionObj::SelectionMode::ROT_Z));
+ QVERIFY(sphereApplyWrenchVis->GetRotTool()->GetState() ==
+ gazebo::rendering::SelectionObj::SelectionMode::SELECTION_NONE);
+ QVERIFY(sphereApplyWrenchVis->GetRotTool()->GetMode() ==
+ gazebo::rendering::SelectionObj::SelectionMode::ROT);
+
+ // Give focus to main window
+ mainWindow->show();
+ mainWindow->raise();
+ mainWindow->activateWindow();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that main window has focus
+ QVERIFY(!applyWrenchDialogSphere->isActiveWindow());
+ QVERIFY(mainWindow->isActiveWindow());
+ QVERIFY(!applyWrenchDialogBox->isActiveWindow());
+
+ // Get spins
+ QList<QDoubleSpinBox *> spins =
+ applyWrenchDialogBox->findChildren<QDoubleSpinBox *>();
+ QVERIFY(spins.size() == 11u);
+
+ // Check spins values
+ for (auto i = 0; i < spins.size(); ++i)
+ {
+ QCOMPARE(spins[i]->value(), 0.0);
+ }
+
+ // Find the box's torque visual
+ bool found = false;
+ gazebo::math::Vector2i mousePoint(glWidget->width()/2, glWidget->height()/2);
+ while (mousePoint.x < glWidget->width())
+ {
+ gazebo::rendering::VisualPtr vis = cam->GetVisual(mousePoint);
+ if (vis && vis == boxApplyWrenchVis->GetTorqueVisual())
+ {
+ found = true;
+ break;
+ }
+ mousePoint.x += 5;
+ }
+
+ if (!found)
+ {
+ std::cout << "Couldn't find torque visual, interrupting test." << std::endl;
+ return;
+ }
+
+ // Click on the box's torque visual
+ QPoint clickPoint(mousePoint.x, mousePoint.y);
+ QTest::mouseClick(glWidget, Qt::LeftButton, Qt::NoModifier, clickPoint,
+ 100);
+ QCoreApplication::processEvents();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that box dialog has focus
+ QVERIFY(applyWrenchDialogBox->isActiveWindow());
+ QVERIFY(!applyWrenchDialogSphere->isActiveWindow());
+
+ // Check mode
+ QVERIFY(applyWrenchDialogBox->GetMode() ==
+ gazebo::gui::ApplyWrenchDialog::Mode::TORQUE);
+
+ // Check that torque spins changed to UnitX
+ QCOMPARE(spins[7]->value(), 1.0);
+ QCOMPARE(spins[8]->value(), 0.0);
+ QCOMPARE(spins[9]->value(), 0.0);
+ QCOMPARE(spins[10]->value(), 1.0);
+
+ // Check that torque visual is the only one highlighted
+ QVERIFY(boxApplyWrenchVis->GetForceVisual()->GetMaterialName().find(
+ "Gazebo/DarkOrangeTransparentOverlay") != std::string::npos);
+ QVERIFY(boxApplyWrenchVis->GetTorqueVisual()->GetMaterialName().find(
+ "Gazebo/OrangeTransparentOverlay") != std::string::npos);
+
+ // Check that rot tool is visible and not highlighted
+ QVERIFY(boxApplyWrenchVis->GetRotTool()->GetHandleVisible(
+ gazebo::rendering::SelectionObj::SelectionMode::ROT_Y));
+ QVERIFY(boxApplyWrenchVis->GetRotTool()->GetHandleVisible(
+ gazebo::rendering::SelectionObj::SelectionMode::ROT_Z));
+ QVERIFY(boxApplyWrenchVis->GetRotTool()->GetState() ==
+ gazebo::rendering::SelectionObj::SelectionMode::SELECTION_NONE);
+
+ // Find the rot tool
+ std::string manipState = "";
+ while (mousePoint.y < glWidget->height())
+ {
+ gazebo::rendering::VisualPtr vis = cam->GetVisual(mousePoint, manipState);
+ if (!vis && manipState == "rot_y")
+ {
+ mousePoint.y += 20;
+ break;
+ }
+ mousePoint.y += 5;
+ }
+
+ if (manipState.empty())
+ {
+ return;
+ }
+
+ // Move mouse to the rot tool
+ QPoint pressPoint(mousePoint.x, mousePoint.y);
+ QTest::mouseMove(glWidget, pressPoint);
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that rot tool is highlighted
+ QVERIFY(boxApplyWrenchVis->GetRotTool()->GetState() ==
+ gazebo::rendering::SelectionObj::SelectionMode::ROT_Y);
+
+ // Get/check the initial visual poses
+ QVERIFY(boxApplyWrenchVis->GetRotTool()->GetPose() ==
+ gazebo::math::Pose::Zero);
+
+ gazebo::math::Pose boxTorquePose0 =
+ boxApplyWrenchVis->GetTorqueVisual()->GetPose();
+ gazebo::math::Pose boxForcePose0 =
+ boxApplyWrenchVis->GetForceVisual()->GetPose();
+
+ // Drag the tool
+ QTestEventList events;
+ events.addMousePress(Qt::LeftButton, Qt::NoModifier, pressPoint, 100);
+ for (size_t i = 0; i < 10; ++i)
+ {
+ events.addMouseMove(QPoint(pressPoint.x(), pressPoint.y()-10*i), 100);
+ }
+ events.simulate(glWidget);
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check mode
+ QVERIFY(applyWrenchDialogBox->GetMode() ==
+ gazebo::gui::ApplyWrenchDialog::Mode::TORQUE);
+
+ // Check that only rot tool and torque were rotated
+ QVERIFY(boxApplyWrenchVis->GetRotTool()->GetPose().pos ==
+ gazebo::math::Vector3::Zero);
+ QVERIFY(boxApplyWrenchVis->GetRotTool()->GetPose().rot !=
+ gazebo::math::Vector3::Zero);
+ QVERIFY(boxApplyWrenchVis->GetTorqueVisual()->GetPose() !=
+ boxTorquePose0);
+ QVERIFY(boxApplyWrenchVis->GetForceVisual()->GetPose() == boxForcePose0);
+
+ // Check that only torque spins X and Z changed
+ QVERIFY(spins[7]->value() < 1.0);
+ QCOMPARE(spins[8]->value(), 0.0);
+ QVERIFY(spins[9]->value() > 0.0);
+ QCOMPARE(spins[10]->value(), 1.0);
+
+ // Trigger a manipulation mode
+ gazebo::gui::g_translateAct->trigger();
+
+ // Check that the force visual is visible and inactive
+ QVERIFY(boxApplyWrenchVis->GetForceVisual() != NULL);
+ QVERIFY(boxApplyWrenchVis->GetForceVisual()->GetMaterialName().find(
+ "Gazebo/DarkOrangeTransparentOverlay") != std::string::npos);
+
+ // Check that the torque visual is visible and inactive
+ QVERIFY(boxApplyWrenchVis->GetTorqueVisual() != NULL);
+ QVERIFY(boxApplyWrenchVis->GetTorqueVisual()->GetMaterialName().find(
+ "Gazebo/DarkOrangeTransparentOverlay") != std::string::npos);
+
+ // Check that rot tool is not visible
+ QVERIFY(!boxApplyWrenchVis->GetRotTool()->GetHandleVisible(
+ gazebo::rendering::SelectionObj::SelectionMode::ROT_Y));
+ QVERIFY(!boxApplyWrenchVis->GetRotTool()->GetHandleVisible(
+ gazebo::rendering::SelectionObj::SelectionMode::ROT_Z));
+
+ // Clean up
+ delete applyWrenchDialogBox;
+ delete applyWrenchDialogSphere;
+
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(ApplyWrenchDialog_TEST)
diff --git a/gazebo/gui/ApplyWrenchDialog_TEST.hh b/gazebo/gui/ApplyWrenchDialog_TEST.hh
new file mode 100644
index 0000000..95de31b
--- /dev/null
+++ b/gazebo/gui/ApplyWrenchDialog_TEST.hh
@@ -0,0 +1,37 @@
+/*
+ * 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_APPLY_WRENCH_DIALOG_TEST_HH_
+#define _GAZEBO_APPLY_WRENCH_DIALOG_TEST_HH_
+
+#include <string>
+#include "gazebo/gui/QTestFixture.hh"
+
+
+/// \brief A test class for the ApplyWrenchDialog.
+class ApplyWrenchDialog_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Test applying force and torque through the dialog.
+ private slots: void ApplyForceTorqueFromDialog();
+
+ /// \brief Test mouse interactions.
+ private slots: void MouseInteractions();
+};
+
+#endif
diff --git a/gazebo/gui/BoxMaker.cc b/gazebo/gui/BoxMaker.cc
index 76c2519..66c1458 100644
--- a/gazebo/gui/BoxMaker.cc
+++ b/gazebo/gui/BoxMaker.cc
@@ -14,6 +14,13 @@
* 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 <sstream>
#include "gazebo/msgs/msgs.hh"
@@ -46,7 +53,7 @@ BoxMaker::BoxMaker()
this->visualMsg->mutable_material()->mutable_script()->set_name(
"Gazebo/TurquoiseGlowOutline");
msgs::Set(this->visualMsg->mutable_pose()->mutable_orientation(),
- math::Quaternion());
+ ignition::math::Quaterniond());
}
BoxMaker::~BoxMaker()
@@ -88,7 +95,7 @@ void BoxMaker::OnMousePush(const common::MouseEvent &_event)
if (this->state == 0)
return;
- this->mousePushPos = _event.pressPos;
+ this->mousePushPos = _event.PressPos();
}
/////////////////////////////////////////////////
@@ -99,7 +106,7 @@ void BoxMaker::OnMouseRelease(const common::MouseEvent &_event)
this->state++;
- this->mouseReleasePos = _event.pos;
+ this->mouseReleasePos = _event.Pos();
if (this->state == 3)
{
@@ -114,13 +121,15 @@ void BoxMaker::OnMouseMove(const common::MouseEvent &_event)
if (this->state != 2)
return;
- math::Vector3 p = msgs::Convert(this->visualMsg->pose().position());
- math::Vector3 scale = msgs::Convert(this->visualMsg->geometry().box().size());
+ ignition::math::Vector3d p =
+ msgs::ConvertIgn(this->visualMsg->pose().position());
+ ignition::math::Vector3d scale =
+ msgs::ConvertIgn(this->visualMsg->geometry().box().size());
- scale.z = (this->mouseReleasePos.y - _event.pos.y)*0.01;
- if (!_event.shift)
- scale.z = rint(scale.z);
- p.z = scale.z/2.0;
+ scale.Z((this->mouseReleasePos.y - _event.Pos().Y())*0.01);
+ if (!_event.Shift())
+ scale.Z(rint(scale.Z()));
+ p.Z(scale.Z()/2.0);
msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p);
msgs::Set(this->visualMsg->mutable_geometry()->mutable_box()->mutable_size(),
@@ -149,7 +158,7 @@ void BoxMaker::OnMouseDrag(const common::MouseEvent &_event)
p1 = this->GetSnappedPoint(p1);
if (!this->camera->GetWorldPointOnPlane(
- _event.pos.x, _event.pos.y , math::Plane(norm), p2))
+ _event.Pos().X(), _event.Pos().Y() , math::Plane(norm), p2))
{
gzerr << "Invalid mouse point\n";
return;
@@ -157,19 +166,19 @@ void BoxMaker::OnMouseDrag(const common::MouseEvent &_event)
p2 = this->GetSnappedPoint(p2);
- msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p1);
+ msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p1.Ign());
math::Vector3 scale = p1-p2;
- math::Vector3 p = msgs::Convert(this->visualMsg->pose().position());
+ math::Vector3 p = msgs::ConvertIgn(this->visualMsg->pose().position());
scale.z = 0.01;
p.x = p1.x - scale.x/2.0;
p.y = p1.y - scale.y/2.0;
- msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p);
+ msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p.Ign());
msgs::Set(this->visualMsg->mutable_geometry()->mutable_box()->mutable_size(),
- scale.GetAbs());
+ scale.GetAbs().Ign());
this->visPub->Publish(*this->visualMsg);
}
@@ -177,46 +186,26 @@ void BoxMaker::OnMouseDrag(const common::MouseEvent &_event)
/////////////////////////////////////////////////
std::string BoxMaker::GetSDFString()
{
- std::ostringstream newModelStr;
- newModelStr << "<sdf version ='" << SDF_VERSION << "'>"
- << "<model name='unit_box_" << counter << "'>"
- << "<pose>0 0 0.5 0 0 0</pose>"
- << "<link name ='link'>"
- << "<inertial><mass>1.0</mass></inertial>"
- << "<collision name ='collision'>"
- << "<geometry>"
- << "<box>"
- << "<size>1.0 1.0 1.0</size>"
- << "</box>"
- << "</geometry>"
- << "</collision>"
- << "<visual name ='visual'>"
- << "<geometry>"
- << "<box>"
- << "<size>1.0 1.0 1.0</size>"
- << "</box>"
- << "</geometry>"
- << "<material>"
- << "<script>"
- << "<uri>file://media/materials/scripts/gazebo.material</uri>"
- << "<name>Gazebo/Grey</name>"
- << "</script>"
- << "</material>"
- << "</visual>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- return newModelStr.str();
+ msgs::Model model;
+ {
+ std::ostringstream modelName;
+ modelName << "unit_box_" << counter;
+ model.set_name(modelName.str());
+ }
+ msgs::Set(model.mutable_pose(), ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0));
+ msgs::AddBoxLink(model, 1.0, ignition::math::Vector3d::One);
+ model.mutable_link(0)->set_name("link");
+
+ return "<sdf version='" + std::string(SDF_VERSION) + "'>"
+ + msgs::ModelToSDF(model)->ToString("")
+ + "</sdf>";
}
+/////////////////////////////////////////////////
void BoxMaker::CreateTheEntity()
{
msgs::Factory msg;
- math::Vector3 p = msgs::Convert(this->visualMsg->pose().position());
- math::Vector3 size = msgs::Convert(this->visualMsg->geometry().box().size());
-
msg.set_sdf(this->GetSDFString());
msgs::Request *requestMsg = msgs::CreateRequest("entity_delete",
diff --git a/gazebo/gui/CMakeLists.txt b/gazebo/gui/CMakeLists.txt
index 5e9a6fd..ddf50ab 100644
--- a/gazebo/gui/CMakeLists.txt
+++ b/gazebo/gui/CMakeLists.txt
@@ -4,12 +4,15 @@ include ( ${QT_USE_FILE} )
link_directories(
${Boost_LIBRARY_DIRS}
+ ${tinyxml_LIBRARY_DIRS}
)
include_directories(
${CMAKE_SOURCE_DIR}/deps
)
+include_directories(${tinyxml_INCLUDE_DIRS})
+
add_definitions(${QT_DEFINITIONS})
add_subdirectory(qtpropertybrowser)
@@ -21,6 +24,7 @@ add_subdirectory(viewers)
set (sources
Actions.cc
AlignWidget.cc
+ ApplyWrenchDialog.cc
BoxMaker.cc
CloneWindow.cc
ConfigWidget.cc
@@ -35,7 +39,9 @@ set (sources
InsertModelWidget.cc
JointControlWidget.cc
KeyEventHandler.cc
+ LayersWidget.cc
LightMaker.cc
+ LogPlayWidget.cc
MainWindow.cc
ModelAlign.cc
ModelManipulator.cc
@@ -49,14 +55,27 @@ set (sources
SaveDialog.cc
SpaceNav.cc
SphereMaker.cc
+ SplashScreen.cc
TimePanel.cc
+ TimeWidget.cc
ToolsWidget.cc
TopicSelector.cc
+ ViewAngleWidget.cc
)
+if (NOT USE_EXTERNAL_TINYXML)
+ include_directories (${CMAKE_SOURCE_DIR}/deps/win/tinyxml)
+ set (sources ${sources}
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinystr.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxml.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxmlerror.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxmlparser.cpp)
+endif()
+
set (qt_headers
Actions.hh
AlignWidget.hh
+ ApplyWrenchDialog.hh
CloneWindow.hh
ConfigWidget.hh
DataLogger.hh
@@ -64,15 +83,20 @@ set (qt_headers
GLWidget.hh
InsertModelWidget.hh
JointControlWidget.hh
+ LayersWidget.hh
+ LogPlayWidget.hh
MainWindow.hh
ModelListWidget.hh
ModelEditWidget.hh
ModelRightMenu.hh
RenderWidget.hh
SaveDialog.hh
+ SplashScreen.hh
TimePanel.hh
+ TimeWidget.hh
ToolsWidget.hh
TopicSelector.hh
+ ViewAngleWidget.hh
)
set (headers
@@ -120,20 +144,29 @@ set (qt_tests
DataLogger_TEST.cc
GuiIface_TEST.cc
ModelAlign_TEST.cc
+ RenderWidget_TEST.cc
+ SaveDialog_TEST.cc
+ SplashScreen_TEST.cc
TimePanel_TEST.cc
+ TimeWidget_TEST.cc
+ ViewAngleWidget_TEST.cc
)
# MainWindow is using screen rendering
if (ENABLE_SCREEN_TESTS)
set (qt_tests
${qt_tests}
+ ApplyWrenchDialog_TEST.cc
BuildingEditor_TEST.cc
ConfigWidget_TEST.cc
GLWidget_TEST.cc
GLWidget_TEST2.cc
+ LaserVisualization_TEST.cc
+ LayersWidget_TEST.cc
MainWindow_TEST.cc
ModelListWidget_TEST.cc
- ModelSnap_TEST.cc)
+ ModelSnap_TEST.cc
+ Projection_TEST.cc)
endif()
# Generate executables for each of the QT unit tests
@@ -144,54 +177,46 @@ set (resources resources.qrc)
QT4_WRAP_CPP(headers_MOC ${qt_headers})
QT4_ADD_RESOURCES(resources_RCC ${resources})
-gz_add_executable(gzclient ${sources} main.cc ${headers_MOC} ${headers} ${resources_RCC})
gz_add_library(gazebo_gui ${sources} ${headers_MOC} ${headers} ${resources_RCC})
+gz_add_executable(gzclient main.cc)
add_dependencies(gazebo_gui gazebo_msgs)
-add_dependencies(gzclient gazebo_msgs)
+add_dependencies(gzclient gazebo_gui)
+
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_GUI")
target_link_libraries(gazebo_gui
+ libgazebo_client
gazebo_common
gazebo_transport
gazebo_rendering
- gazebo_sensors
gazebo_msgs
gazebo_gui_building
gazebo_gui_model
gazebo_gui_terrain
gazebo_gui_viewers
gzqtpropertybrowser
- libgazebo
${QT_LIBRARIES}
${ogre_libraries}
${Boost_LIBRARIES}
- pthread
+ ${tinyxml_LIBRARIES}
)
-target_link_libraries(gzclient
- gazebo_common
- gazebo_transport
- gazebo_rendering
- gazebo_sensors
- gazebo_msgs
- gazebo_gui_building
- gazebo_gui_model
- gazebo_gui_terrain
- gazebo_gui_viewers
- gzqtpropertybrowser
- libgazebo
- ${QT_LIBRARIES}
- ${ogre_libraries}
- ${Boost_LIBRARIES}
- pthread
-)
+if (UNIX)
+ target_link_libraries(gazebo_gui pthread)
+endif()
+
+target_link_libraries(gzclient gazebo_gui)
+
+if (UNIX)
+ target_link_libraries(gzclient pthread)
+endif()
if (SPNAV_LIBRARY)
target_link_libraries(gazebo_gui ${SPNAV_LIBRARY})
target_link_libraries(gzclient ${SPNAV_LIBRARY})
endif()
-
add_definitions(${QT_DEFINITIONS})
set (gui_headers "" CACHE INTERNAL "gui headers" FORCE)
@@ -204,4 +229,7 @@ configure_file (${CMAKE_CURRENT_SOURCE_DIR}/gui.hh.in ${CMAKE_CURRENT_BINARY_DIR
gz_install_library(gazebo_gui)
gz_install_includes("gui" ${headers} ${qt_headers} ${CMAKE_CURRENT_BINARY_DIR}/gui.hh)
gz_install_executable(gzclient)
-manpage(gzclient 1)
+
+if (UNIX)
+ manpage(gzclient 1)
+endif()
diff --git a/gazebo/gui/ConfigWidget.cc b/gazebo/gui/ConfigWidget.cc
index 9e23a2d..7617bc4 100644
--- a/gazebo/gui/ConfigWidget.cc
+++ b/gazebo/gui/ConfigWidget.cc
@@ -24,6 +24,18 @@
using namespace gazebo;
using namespace gui;
+const QString ConfigWidget::level0BgColor = "#999999";
+const QString ConfigWidget::level1BgColor = "#777777";
+const QString ConfigWidget::level2BgColor = "#555555";
+const QString ConfigWidget::level3BgColor = "#333333";
+const QString ConfigWidget::level0WidgetColor = "#eeeeee";
+const QString ConfigWidget::level1WidgetColor = "#cccccc";
+const QString ConfigWidget::level2WidgetColor = "#aaaaaa";
+const QString ConfigWidget::level3WidgetColor = "#888888";
+const QString ConfigWidget::redColor = "#d42b2b";
+const QString ConfigWidget::greenColor = "#3bc43b";
+const QString ConfigWidget::blueColor = "#0d0df2";
+
/////////////////////////////////////////////////
ConfigWidget::ConfigWidget()
{
@@ -43,18 +55,36 @@ void ConfigWidget::Load(const google::protobuf::Message *_msg)
this->configMsg = _msg->New();
this->configMsg->CopyFrom(*_msg);
- QWidget *widget = this->Parse(this->configMsg);
+ QWidget *widget = this->Parse(this->configMsg, 0);
QVBoxLayout *mainLayout = new QVBoxLayout;
mainLayout->setAlignment(Qt::AlignTop);
mainLayout->addWidget(widget);
+
this->setLayout(mainLayout);
+
+ // set up event filter for scrollable widgets to make sure they don't steal
+ // focus when embedded in a QScrollArea.
+ QList<QAbstractSpinBox *> spinBoxes =
+ this->findChildren<QAbstractSpinBox *>();
+ for (int i = 0; i < spinBoxes.size(); ++i)
+ {
+ spinBoxes[i]->installEventFilter(this);
+ spinBoxes[i]->setFocusPolicy(Qt::StrongFocus);
+ }
+ QList<QComboBox *> comboBoxes =
+ this->findChildren<QComboBox *>();
+ for (int i = 0; i < comboBoxes.size(); ++i)
+ {
+ comboBoxes[i]->installEventFilter(this);
+ comboBoxes[i]->setFocusPolicy(Qt::StrongFocus);
+ }
}
/////////////////////////////////////////////////
void ConfigWidget::UpdateFromMsg(const google::protobuf::Message *_msg)
{
this->configMsg->CopyFrom(*_msg);
- this->Parse(this->configMsg);
+ this->Parse(this->configMsg, true);
}
/////////////////////////////////////////////////
@@ -65,6 +95,114 @@ google::protobuf::Message *ConfigWidget::GetMsg()
}
/////////////////////////////////////////////////
+std::string ConfigWidget::GetHumanReadableKey(const std::string &_key)
+{
+ std::string humanKey = _key;
+ humanKey[0] = std::toupper(humanKey[0]);
+ std::replace(humanKey.begin(), humanKey.end(), '_', ' ');
+ return humanKey;
+}
+
+/////////////////////////////////////////////////
+std::string ConfigWidget::GetUnitFromKey(const std::string &_key,
+ const std::string &_jointType)
+{
+ if (_key == "pos" || _key == "length" || _key == "min_depth")
+ {
+ return "m";
+ }
+
+ if (_key == "rot")
+ return "rad";
+
+ if (_key == "kp" || _key == "kd")
+ return "N/m";
+
+ if (_key == "max_vel")
+ return "m/s";
+
+ if (_key == "mass")
+ return "kg";
+
+ if (_key == "ixx" || _key == "ixy" || _key == "ixz" ||
+ _key == "iyy" || _key == "iyz" || _key == "izz")
+ {
+ return "kg·m<sup>2</sup>";
+ }
+
+ if (_key == "limit_lower" || _key == "limit_upper")
+ {
+ if (_jointType == "PRISMATIC")
+ return "m";
+ else if (_jointType != "")
+ return "rad";
+ }
+
+ if (_key == "limit_effort")
+ {
+ if (_jointType == "PRISMATIC")
+ return "N";
+ else if (_jointType != "")
+ return "Nm";
+ }
+
+ if (_key == "limit_velocity" || _key == "velocity")
+ {
+ if (_jointType == "PRISMATIC")
+ return "m/s";
+ else if (_jointType != "")
+ return "rad/s";
+ }
+
+ if (_key == "damping")
+ {
+ if (_jointType == "PRISMATIC")
+ return "Ns/m";
+ else if (_jointType != "")
+ return "Ns";
+ }
+
+ if (_key == "friction")
+ {
+ if (_jointType == "PRISMATIC")
+ return "N";
+ else if (_jointType != "")
+ return "Nm";
+ }
+
+ return "";
+}
+
+/////////////////////////////////////////////////
+void ConfigWidget::GetRangeFromKey(const std::string &_key, double &_min,
+ double &_max)
+{
+ // Maximum range by default
+ _min = -GZ_DBL_MAX;
+ _max = GZ_DBL_MAX;
+
+ if (_key == "mass" || _key == "ixx" || _key == "ixy" || _key == "ixz" ||
+ _key == "iyy" || _key == "iyz" || _key == "izz" || _key == "length" ||
+ _key == "min_depth")
+ {
+ _min = 0;
+ }
+ else if (_key == "bounce" || _key == "transparency" ||
+ _key == "laser_retro" || _key == "ambient" || _key == "diffuse" ||
+ _key == "specular" || _key == "emissive" ||
+ _key == "restitution_coefficient")
+ {
+ _min = 0;
+ _max = 1;
+ }
+ else if (_key == "fdir1" || _key == "xyz")
+ {
+ _min = -1;
+ _max = +1;
+ }
+}
+
+/////////////////////////////////////////////////
bool ConfigWidget::GetWidgetVisible(const std::string &_name) const
{
std::map <std::string, ConfigChildWidget *>::const_iterator iter =
@@ -153,101 +291,123 @@ void ConfigWidget::SetWidgetReadOnly(const std::string &_name, bool _readOnly)
}
/////////////////////////////////////////////////
-void ConfigWidget::SetIntWidgetValue(const std::string &_name, int _value)
+bool ConfigWidget::SetIntWidgetValue(const std::string &_name, int _value)
{
- std::map <std::string, ConfigChildWidget *>::iterator iter =
- this->configWidgets.find(_name);
+ auto iter = this->configWidgets.find(_name);
if (iter != this->configWidgets.end())
- this->UpdateIntWidget(iter->second, _value);
+ return this->UpdateIntWidget(iter->second, _value);
+
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::SetUIntWidgetValue(const std::string &_name,
+bool ConfigWidget::SetUIntWidgetValue(const std::string &_name,
unsigned int _value)
{
- std::map <std::string, ConfigChildWidget *>::iterator iter =
- this->configWidgets.find(_name);
+ auto iter = this->configWidgets.find(_name);
if (iter != this->configWidgets.end())
- this->UpdateUIntWidget(iter->second, _value);
+ return this->UpdateUIntWidget(iter->second, _value);
+
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::SetDoubleWidgetValue(const std::string &_name,
+bool ConfigWidget::SetDoubleWidgetValue(const std::string &_name,
double _value)
{
- std::map <std::string, ConfigChildWidget *>::iterator iter =
- this->configWidgets.find(_name);
+ auto iter = this->configWidgets.find(_name);
if (iter != this->configWidgets.end())
- this->UpdateDoubleWidget(iter->second, _value);
+ return this->UpdateDoubleWidget(iter->second, _value);
+
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::SetBoolWidgetValue(const std::string &_name,
+bool ConfigWidget::SetBoolWidgetValue(const std::string &_name,
bool _value)
{
- std::map <std::string, ConfigChildWidget *>::iterator iter =
- this->configWidgets.find(_name);
+ auto iter = this->configWidgets.find(_name);
if (iter != this->configWidgets.end())
- this->UpdateBoolWidget(iter->second, _value);
+ return this->UpdateBoolWidget(iter->second, _value);
+
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::SetStringWidgetValue(const std::string &_name,
+bool ConfigWidget::SetStringWidgetValue(const std::string &_name,
const std::string &_value)
{
- std::map <std::string, ConfigChildWidget *>::iterator iter =
- this->configWidgets.find(_name);
+ auto iter = this->configWidgets.find(_name);
if (iter != this->configWidgets.end())
- this->UpdateStringWidget(iter->second, _value);
+ return this->UpdateStringWidget(iter->second, _value);
+
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::SetVector3WidgetValue(const std::string &_name,
+bool ConfigWidget::SetVector3WidgetValue(const std::string &_name,
const math::Vector3 &_value)
{
- std::map <std::string, ConfigChildWidget *>::iterator iter =
- this->configWidgets.find(_name);
+ auto iter = this->configWidgets.find(_name);
if (iter != this->configWidgets.end())
- this->UpdateVector3Widget(iter->second, _value);
+ return this->UpdateVector3Widget(iter->second, _value);
+
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::SetColorWidgetValue(const std::string &_name,
+bool ConfigWidget::SetColorWidgetValue(const std::string &_name,
const common::Color &_value)
{
- std::map <std::string, ConfigChildWidget *>::iterator iter =
- this->configWidgets.find(_name);
+ auto iter = this->configWidgets.find(_name);
if (iter != this->configWidgets.end())
- this->UpdateColorWidget(iter->second, _value);
+ return this->UpdateColorWidget(iter->second, _value);
+
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::SetPoseWidgetValue(const std::string &_name,
+bool ConfigWidget::SetPoseWidgetValue(const std::string &_name,
const math::Pose &_value)
{
- std::map <std::string, ConfigChildWidget *>::iterator iter =
- this->configWidgets.find(_name);
+ auto iter = this->configWidgets.find(_name);
if (iter != this->configWidgets.end())
- this->UpdatePoseWidget(iter->second, _value);
+ return this->UpdatePoseWidget(iter->second, _value);
+
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::SetGeometryWidgetValue(const std::string &_name,
- const std::string &_value, const math::Vector3 &_dimensions)
+bool ConfigWidget::SetGeometryWidgetValue(const std::string &_name,
+ const std::string &_value, const math::Vector3 &_dimensions,
+ const std::string &_uri)
{
- std::map <std::string, ConfigChildWidget *>::iterator iter =
- this->configWidgets.find(_name);
+ auto iter = this->configWidgets.find(_name);
+
+ if (iter != this->configWidgets.end())
+ return this->UpdateGeometryWidget(iter->second, _value, _dimensions, _uri);
+
+ return false;
+}
+
+/////////////////////////////////////////////////
+bool ConfigWidget::SetEnumWidgetValue(const std::string &_name,
+ const std::string &_value)
+{
+ auto iter = this->configWidgets.find(_name);
if (iter != this->configWidgets.end())
- this->UpdateGeometryWidget(iter->second, _value, _dimensions);
+ return this->UpdateEnumWidget(iter->second, _value);
+
+ return false;
}
/////////////////////////////////////////////////
@@ -349,20 +509,31 @@ math::Pose ConfigWidget::GetPoseWidgetValue(const std::string &_name) const
/////////////////////////////////////////////////
std::string ConfigWidget::GetGeometryWidgetValue(const std::string &_name,
- math::Vector3 &_dimensions) const
+ math::Vector3 &_dimensions, std::string &_uri) const
{
std::string type;
std::map <std::string, ConfigChildWidget *>::const_iterator iter =
this->configWidgets.find(_name);
if (iter != this->configWidgets.end())
- type = this->GetGeometryWidgetValue(iter->second, _dimensions);
+ type = this->GetGeometryWidgetValue(iter->second, _dimensions, _uri);
return type;
}
/////////////////////////////////////////////////
-QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
- const std::string &_name)
+std::string ConfigWidget::GetEnumWidgetValue(const std::string &_name) const
+{
+ std::string value;
+ auto iter = this->configWidgets.find(_name);
+
+ if (iter != this->configWidgets.end())
+ value = this->GetEnumWidgetValue(iter->second);
+ return value;
+}
+
+/////////////////////////////////////////////////
+QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg, bool _update,
+ const std::string &_name, const int _level)
{
std::vector<QWidget *> newWidgets;
@@ -386,18 +557,22 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
std::string name = field->name();
// Parse each field in the message
- // TODO parse repeated fields and enum fields.
+ // TODO parse repeated fields
if (!field->is_repeated())
{
+ if (_update && !ref->HasField(*_msg, field))
+ continue;
+
QWidget *newFieldWidget = NULL;
ConfigChildWidget *configChildWidget = NULL;
bool newWidget = true;
std::string scopedName = _name.empty() ? name : _name + "::" + name;
if (this->configWidgets.find(scopedName) != this->configWidgets.end())
+ {
newWidget = false;
- else
configChildWidget = this->configWidgets[scopedName];
+ }
switch (field->cpp_type())
{
@@ -408,7 +583,7 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
value = 0;
if (newWidget)
{
- configChildWidget = CreateDoubleWidget(name);
+ configChildWidget = CreateDoubleWidget(name, _level);
newFieldWidget = configChildWidget;
}
this->UpdateDoubleWidget(configChildWidget, value);
@@ -421,7 +596,7 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
value = 0;
if (newWidget)
{
- configChildWidget = CreateDoubleWidget(name);
+ configChildWidget = CreateDoubleWidget(name, _level);
newFieldWidget = configChildWidget;
}
this->UpdateDoubleWidget(configChildWidget, value);
@@ -432,7 +607,7 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
int64_t value = ref->GetInt64(*_msg, field);
if (newWidget)
{
- configChildWidget = CreateIntWidget(name);
+ configChildWidget = CreateIntWidget(name, _level);
newFieldWidget = configChildWidget;
}
this->UpdateIntWidget(configChildWidget, value);
@@ -443,7 +618,7 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
uint64_t value = ref->GetUInt64(*_msg, field);
if (newWidget)
{
- configChildWidget = CreateUIntWidget(name);
+ configChildWidget = CreateUIntWidget(name, _level);
newFieldWidget = configChildWidget;
}
this->UpdateUIntWidget(configChildWidget, value);
@@ -454,7 +629,7 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
int32_t value = ref->GetInt32(*_msg, field);
if (newWidget)
{
- configChildWidget = CreateIntWidget(name);
+ configChildWidget = CreateIntWidget(name, _level);
newFieldWidget = configChildWidget;
}
this->UpdateIntWidget(configChildWidget, value);
@@ -465,7 +640,7 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
uint32_t value = ref->GetUInt32(*_msg, field);
if (newWidget)
{
- configChildWidget = CreateUIntWidget(name);
+ configChildWidget = CreateUIntWidget(name, _level);
newFieldWidget = configChildWidget;
}
this->UpdateUIntWidget(configChildWidget, value);
@@ -476,7 +651,7 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
bool value = ref->GetBool(*_msg, field);
if (newWidget)
{
- configChildWidget = CreateBoolWidget(name);
+ configChildWidget = CreateBoolWidget(name, _level);
newFieldWidget = configChildWidget;
}
this->UpdateBoolWidget(configChildWidget, value);
@@ -487,7 +662,7 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
std::string value = ref->GetString(*_msg, field);
if (newWidget)
{
- configChildWidget = CreateStringWidget(name);
+ configChildWidget = CreateStringWidget(name, _level);
newFieldWidget = configChildWidget;
}
this->UpdateStringWidget(configChildWidget, value);
@@ -503,7 +678,7 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
{
if (newWidget)
{
- configChildWidget = this->CreateGeometryWidget(name);
+ configChildWidget = this->CreateGeometryWidget(name, _level);
newFieldWidget = configChildWidget;
}
@@ -512,82 +687,92 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
valueMsg->GetDescriptor();
const google::protobuf::FieldDescriptor *typeField =
valueDescriptor->FindFieldByName("type");
- const google::protobuf::EnumValueDescriptor *typeValueDescriptor =
- valueMsg->GetReflection()->GetEnum(*valueMsg, typeField);
- std::string geometryTypeStr;
- if (typeValueDescriptor)
+ if (valueMsg->GetReflection()->HasField(*valueMsg, typeField))
{
- geometryTypeStr =
- QString(typeValueDescriptor->name().c_str()).toLower().
- toStdString();
- }
+ const google::protobuf::EnumValueDescriptor *typeValueDescriptor =
+ valueMsg->GetReflection()->GetEnum(*valueMsg, typeField);
- math::Vector3 dimensions;
- // dimensions
- for (int k = 0; k < valueDescriptor->field_count() ; ++k)
- {
- const google::protobuf::FieldDescriptor *geomField =
- valueDescriptor->field(k);
+ std::string geometryTypeStr;
+ if (typeValueDescriptor)
+ {
+ geometryTypeStr =
+ QString(typeValueDescriptor->name().c_str()).toLower().
+ toStdString();
+ }
- if (geomField->is_repeated())
- continue;
+ math::Vector3 dimensions;
+ // dimensions
+ for (int k = 0; k < valueDescriptor->field_count() ; ++k)
+ {
+ const google::protobuf::FieldDescriptor *geomField =
+ valueDescriptor->field(k);
- if (geomField->cpp_type() !=
- google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE ||
- !valueMsg->GetReflection()->HasField(*valueMsg, geomField))
- continue;
+ if (geomField->is_repeated())
+ continue;
- google::protobuf::Message *geomValueMsg =
- valueMsg->GetReflection()->MutableMessage(valueMsg, geomField);
- const google::protobuf::Descriptor *geomValueDescriptor =
- geomValueMsg->GetDescriptor();
+ if (geomField->cpp_type() !=
+ google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE ||
+ !valueMsg->GetReflection()->HasField(*valueMsg, geomField))
+ continue;
- std::string geomMsgName = geomField->message_type()->name();
- if (geomMsgName == "BoxGeom")
- {
- google::protobuf::Message *geomDimMsg =
- geomValueMsg->GetReflection()->MutableMessage(geomValueMsg,
- geomValueDescriptor->field(0));
- dimensions = this->ParseVector3(geomDimMsg);
- break;
- }
- else if (geomMsgName == "CylinderGeom")
- {
- const google::protobuf::FieldDescriptor *geomRadiusField =
- geomValueDescriptor->FindFieldByName("radius");
- double radius = geomValueMsg->GetReflection()->GetDouble(
- *geomValueMsg, geomRadiusField);
- const google::protobuf::FieldDescriptor *geomLengthField =
- geomValueDescriptor->FindFieldByName("length");
- double length = geomValueMsg->GetReflection()->GetDouble(
- *geomValueMsg, geomLengthField);
- dimensions.x = radius * 2.0;
- dimensions.y = radius * 2.0;
- dimensions.z = length;
- break;
- }
- else if (geomMsgName == "SphereGeom")
- {
- const google::protobuf::FieldDescriptor *geomRadiusField =
- geomValueDescriptor->FindFieldByName("radius");
- double radius = geomValueMsg->GetReflection()->GetDouble(
- *geomValueMsg, geomRadiusField);
- dimensions.x = radius;
- dimensions.y = radius;
- dimensions.z = radius;
- break;
+ google::protobuf::Message *geomValueMsg =
+ valueMsg->GetReflection()->MutableMessage(
+ valueMsg, geomField);
+ const google::protobuf::Descriptor *geomValueDescriptor =
+ geomValueMsg->GetDescriptor();
+
+ std::string geomMsgName = geomField->message_type()->name();
+ if (geomMsgName == "BoxGeom" || geomMsgName == "MeshGeom")
+ {
+ int fieldIdx = (geomMsgName == "BoxGeom") ? 0 : 1;
+ google::protobuf::Message *geomDimMsg =
+ geomValueMsg->GetReflection()->MutableMessage(
+ geomValueMsg, geomValueDescriptor->field(fieldIdx));
+ dimensions = this->ParseVector3(geomDimMsg);
+ break;
+ }
+ else if (geomMsgName == "CylinderGeom")
+ {
+ const google::protobuf::FieldDescriptor *geomRadiusField =
+ geomValueDescriptor->FindFieldByName("radius");
+ double radius = geomValueMsg->GetReflection()->GetDouble(
+ *geomValueMsg, geomRadiusField);
+ const google::protobuf::FieldDescriptor *geomLengthField =
+ geomValueDescriptor->FindFieldByName("length");
+ double length = geomValueMsg->GetReflection()->GetDouble(
+ *geomValueMsg, geomLengthField);
+ dimensions.x = radius * 2.0;
+ dimensions.y = dimensions.x;
+ dimensions.z = length;
+ break;
+ }
+ else if (geomMsgName == "SphereGeom")
+ {
+ const google::protobuf::FieldDescriptor *geomRadiusField =
+ geomValueDescriptor->FindFieldByName("radius");
+ double radius = geomValueMsg->GetReflection()->GetDouble(
+ *geomValueMsg, geomRadiusField);
+ dimensions.x = radius * 2.0;
+ dimensions.y = dimensions.x;
+ dimensions.z = dimensions.x;
+ break;
+ }
+ else if (geomMsgName == "PolylineGeom")
+ {
+ continue;
+ }
}
+ this->UpdateGeometryWidget(configChildWidget,
+ geometryTypeStr, dimensions);
}
- this->UpdateGeometryWidget(configChildWidget,
- geometryTypeStr, dimensions);
}
// parse and create custom pose widgets
else if (field->message_type()->name() == "Pose")
{
if (newWidget)
{
- configChildWidget = this->CreatePoseWidget(name);
+ configChildWidget = this->CreatePoseWidget(name, _level);
newFieldWidget = configChildWidget;
}
@@ -641,7 +826,7 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
{
if (newWidget)
{
- configChildWidget = this->CreateVector3dWidget(name);
+ configChildWidget = this->CreateVector3dWidget(name, _level);
newFieldWidget = configChildWidget;
}
@@ -653,7 +838,7 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
{
if (newWidget)
{
- configChildWidget = this->CreateColorWidget(name);
+ configChildWidget = this->CreateColorWidget(name, _level);
newFieldWidget = configChildWidget;
}
@@ -665,8 +850,13 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
{
const google::protobuf::FieldDescriptor *valueField =
valueDescriptor->field(j);
- values.push_back(
- valueMsg->GetReflection()->GetFloat(*valueMsg, valueField));
+ if (valueMsg->GetReflection()->HasField(*valueMsg, valueField))
+ {
+ values.push_back(valueMsg->GetReflection()->GetFloat(
+ *valueMsg, valueField));
+ }
+ else
+ values.push_back(0);
}
color.r = values[0];
color.g = values[1];
@@ -677,37 +867,125 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
else
{
// parse the message fields recursively
- QWidget *groupBoxWidget = Parse(valueMsg, scopedName);
- newFieldWidget = new ConfigChildWidget();
- QVBoxLayout *groupBoxLayout = new QVBoxLayout;
- groupBoxLayout->addWidget(groupBoxWidget);
- newFieldWidget->setLayout(groupBoxLayout);
- qobject_cast<ConfigChildWidget *>(newFieldWidget)->
- widgets.push_back(groupBoxWidget);
+ QWidget *groupBoxWidget =
+ this->Parse(valueMsg, _update, scopedName, _level+1);
+ if (groupBoxWidget)
+ {
+ newFieldWidget = new ConfigChildWidget();
+ QVBoxLayout *groupBoxLayout = new QVBoxLayout;
+ groupBoxLayout->setContentsMargins(0, 0, 0, 0);
+ groupBoxLayout->addWidget(groupBoxWidget);
+ newFieldWidget->setLayout(groupBoxLayout);
+ qobject_cast<ConfigChildWidget *>(newFieldWidget)->
+ widgets.push_back(groupBoxWidget);
+ }
}
if (newWidget)
{
- // create a group widget to collapse or expand child widgets
- // (contained in a group box).
+ // Button label
+ QLabel *buttonLabel = new QLabel(
+ tr(this->GetHumanReadableKey(name).c_str()));
+ buttonLabel->setToolTip(tr(name.c_str()));
+
+ // Button icon
+ QCheckBox *buttonIcon = new QCheckBox();
+ buttonIcon->setChecked(true);
+ buttonIcon->setStyleSheet(
+ "QCheckBox::indicator::unchecked {\
+ image: url(:/images/right_arrow.png);\
+ }\
+ QCheckBox::indicator::checked {\
+ image: url(:/images/down_arrow.png);\
+ }");
+
+ // Button layout
+ QHBoxLayout *buttonLayout = new QHBoxLayout();
+ buttonLayout->addItem(new QSpacerItem(20*_level, 1,
+ QSizePolicy::Fixed, QSizePolicy::Fixed));
+ buttonLayout->addWidget(buttonLabel);
+ buttonLayout->addWidget(buttonIcon);
+ buttonLayout->setAlignment(buttonIcon, Qt::AlignRight);
+
+ // Button frame
+ QFrame *buttonFrame = new QFrame();
+ buttonFrame->setFrameStyle(QFrame::Box);
+ buttonFrame->setLayout(buttonLayout);
+
+ // Set color for top level button
+ if (_level == 0)
+ {
+ buttonFrame->setStyleSheet(
+ "QWidget\
+ {\
+ background-color: " + this->level0BgColor +
+ "}");
+ }
+
+ // Child widgets are contained in a group box which can be collapsed
GroupWidget *groupWidget = new GroupWidget;
- newFieldWidget->setStyleSheet(
- "QGroupBox {border : 0px; padding-left : 20px}");
+ groupWidget->setStyleSheet(
+ "QGroupBox {\
+ border : 0;\
+ margin : 0;\
+ padding : 0;\
+ }");
- QVBoxLayout *configGroupLayout = new QVBoxLayout;
- configGroupLayout->setContentsMargins(0, 0, 0, 0);
- QPushButton *groupButton = new QPushButton(tr(name.c_str()));
+ connect(buttonIcon, SIGNAL(toggled(bool)), groupWidget,
+ SLOT(Toggle(bool)));
- // set the child widget so it can be toggled
+ // Set the child widget
groupWidget->childWidget = newFieldWidget;
qobject_cast<ConfigChildWidget *>(newFieldWidget)->groupWidget
= groupWidget;
- newFieldWidget->setContentsMargins(20, 0, 0, 0);
- configGroupLayout->addWidget(groupButton);
- configGroupLayout->addWidget(newFieldWidget);
+ newFieldWidget->setContentsMargins(0, 0, 0, 0);
+
+ // Set color for children
+ if (_level == 0)
+ {
+ newFieldWidget->setStyleSheet(
+ "QWidget\
+ {\
+ background-color: " + this->level1BgColor +
+ "}\
+ QDoubleSpinBox, QSpinBox, QLineEdit, QComboBox\
+ {\
+ background-color: " + this->level1WidgetColor +
+ "}");
+ }
+ else if (_level == 1)
+ {
+ newFieldWidget->setStyleSheet(
+ "QWidget\
+ {\
+ background-color: " + this->level2BgColor +
+ "}\
+ QDoubleSpinBox, QSpinBox, QLineEdit, QComboBox\
+ {\
+ background-color: " + this->level2WidgetColor +
+ "}");
+ }
+ else if (_level == 2)
+ {
+ newFieldWidget->setStyleSheet(
+ "QWidget\
+ {\
+ background-color: " + this->level2BgColor +
+ "}\
+ QDoubleSpinBox, QSpinBox, QLineEdit, QComboBox\
+ {\
+ background-color: " + this->level2WidgetColor +
+ "}");
+ }
+
+ // Group Layout
+ QGridLayout *configGroupLayout = new QGridLayout;
+ configGroupLayout->setContentsMargins(0, 0, 0, 0);
+ configGroupLayout->setSpacing(0);
+ configGroupLayout->addWidget(buttonFrame, 0, 0);
+ configGroupLayout->addWidget(newFieldWidget, 1, 0);
groupWidget->setLayout(configGroupLayout);
- connect(groupButton, SIGNAL(clicked()), groupWidget,
- SLOT(Toggle()));
+
// reset new field widget pointer in order for it to be added
// to the parent widget
newFieldWidget = groupWidget;
@@ -716,18 +994,72 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
break;
}
case google::protobuf::FieldDescriptor::CPPTYPE_ENUM:
- // const google::protobuf::EnumValueDescriptor *value =
- // ref->GetEnum(*_msg, field);
- // TODO Implement enum parsing
+ {
+ const google::protobuf::EnumValueDescriptor *value =
+ ref->GetEnum(*_msg, field);
+
+ if (!value)
+ {
+ gzerr << "Error retrieving enum value for '" << name << "'"
+ << std::endl;
+ break;
+ }
+
+ if (newWidget)
+ {
+ std::vector<std::string> enumValues;
+ const google::protobuf::EnumDescriptor *descriptor = value->type();
+ if (!descriptor)
+ break;
+
+ for (int j = 0; j < descriptor->value_count(); ++j)
+ {
+ const google::protobuf::EnumValueDescriptor *valueDescriptor =
+ descriptor->value(j);
+ if (valueDescriptor)
+ enumValues.push_back(valueDescriptor->name());
+ }
+ configChildWidget =
+ this->CreateEnumWidget(name, enumValues, _level);
+
+ if (!configChildWidget)
+ {
+ gzerr << "Error creating an enum widget for '" << name << "'"
+ << std::endl;
+ break;
+ }
+
+ // connect enum config widget event so that we can fire an other
+ // event from ConfigWidget that has the name of this field
+ connect(qobject_cast<EnumConfigWidget *>(configChildWidget),
+ SIGNAL(EnumValueChanged(const QString &)), this,
+ SLOT(OnEnumValueChanged(const QString &)));
+ newFieldWidget = configChildWidget;
+ }
+ this->UpdateEnumWidget(configChildWidget, value->name());
break;
+ }
default:
break;
}
- if (newWidget && newFieldWidget)
+
+ // Style widgets without parent (level 0)
+ if (newFieldWidget && _level == 0 &&
+ !qobject_cast<GroupWidget *>(newFieldWidget))
{
- newFieldWidget->setSizePolicy(QSizePolicy::Preferred,
- QSizePolicy::Fixed);
+ newFieldWidget->setStyleSheet(
+ "QWidget\
+ {\
+ background-color: " + this->level0BgColor +
+ "}\
+ QDoubleSpinBox, QSpinBox, QLineEdit, QComboBox\
+ {\
+ background-color: " + this->level0WidgetColor +
+ "}");
+ }
+ if (newWidget && newFieldWidget)
+ {
newWidgets.push_back(newFieldWidget);
// store the newly created widget in a map with a unique scoped name.
@@ -760,9 +1092,9 @@ QWidget *ConfigWidget::Parse(google::protobuf::Message *_msg,
}
widgetLayout->setContentsMargins(0, 0, 0, 0);
+ widgetLayout->setSpacing(0);
widgetLayout->setAlignment(Qt::AlignTop);
widget->setLayout(widgetLayout);
- widget->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Fixed);
return widget;
}
return NULL;
@@ -788,80 +1120,162 @@ math::Vector3 ConfigWidget::ParseVector3(const google::protobuf::Message *_msg)
}
/////////////////////////////////////////////////
-ConfigChildWidget *ConfigWidget::CreateUIntWidget(const std::string &_key)
+ConfigChildWidget *ConfigWidget::CreateUIntWidget(const std::string &_key,
+ const int _level)
{
- QHBoxLayout *widgetLayout = new QHBoxLayout;
- QLabel *keyLabel = new QLabel(tr(_key.c_str()));
- widgetLayout->addWidget(keyLabel);
+ // Label
+ QLabel *keyLabel = new QLabel(tr(this->GetHumanReadableKey(_key).c_str()));
+ keyLabel->setToolTip(tr(_key.c_str()));
+
+ // SpinBox
QSpinBox *valueSpinBox = new QSpinBox;
- valueSpinBox->setRange(0, 1e6);
+ valueSpinBox->setRange(0, 1e8);
valueSpinBox->setAlignment(Qt::AlignRight);
+
+ // Layout
+ QHBoxLayout *widgetLayout = new QHBoxLayout;
+ if (_level != 0)
+ {
+ widgetLayout->addItem(new QSpacerItem(20*_level, 1,
+ QSizePolicy::Fixed, QSizePolicy::Fixed));
+ }
+ widgetLayout->addWidget(keyLabel);
widgetLayout->addWidget(valueSpinBox);
+
+ // ChildWidget
ConfigChildWidget *widget = new ConfigChildWidget();
- widgetLayout->setContentsMargins(0, 0, 0, 0);
widget->setLayout(widgetLayout);
+ widget->setFrameStyle(QFrame::Box);
+
widget->widgets.push_back(valueSpinBox);
+
return widget;
}
/////////////////////////////////////////////////
-ConfigChildWidget *ConfigWidget::CreateIntWidget(const std::string &_key)
+ConfigChildWidget *ConfigWidget::CreateIntWidget(const std::string &_key,
+ const int _level)
{
- QHBoxLayout *widgetLayout = new QHBoxLayout;
- QLabel *keyLabel = new QLabel(tr(_key.c_str()));
- widgetLayout->addWidget(keyLabel);
+ // Label
+ QLabel *keyLabel = new QLabel(tr(this->GetHumanReadableKey(_key).c_str()));
+ keyLabel->setToolTip(tr(_key.c_str()));
+
+ // SpinBox
QSpinBox *valueSpinBox = new QSpinBox;
- valueSpinBox->setRange(-1e6, 1e6);
+ valueSpinBox->setRange(-1e8, 1e8);
valueSpinBox->setAlignment(Qt::AlignRight);
+
+ // Layout
+ QHBoxLayout *widgetLayout = new QHBoxLayout;
+ if (_level != 0)
+ {
+ widgetLayout->addItem(new QSpacerItem(20*_level, 1,
+ QSizePolicy::Fixed, QSizePolicy::Fixed));
+ }
+ widgetLayout->addWidget(keyLabel);
widgetLayout->addWidget(valueSpinBox);
+
+ // ChildWidget
ConfigChildWidget *widget = new ConfigChildWidget();
- widgetLayout->setContentsMargins(0, 0, 0, 0);
widget->setLayout(widgetLayout);
+ widget->setFrameStyle(QFrame::Box);
+
widget->widgets.push_back(valueSpinBox);
+
return widget;
}
/////////////////////////////////////////////////
-ConfigChildWidget *ConfigWidget::CreateDoubleWidget(const std::string &_key)
+ConfigChildWidget *ConfigWidget::CreateDoubleWidget(const std::string &_key,
+ const int _level)
{
- QHBoxLayout *widgetLayout = new QHBoxLayout;
- QLabel *keyLabel = new QLabel(tr(_key.c_str()));
- widgetLayout->addWidget(keyLabel);
+ // Label
+ QLabel *keyLabel = new QLabel(tr(this->GetHumanReadableKey(_key).c_str()));
+ keyLabel->setToolTip(tr(_key.c_str()));
+
+ // SpinBox
+ double min = 0;
+ double max = 0;
+ this->GetRangeFromKey(_key, min, max);
+
QDoubleSpinBox *valueSpinBox = new QDoubleSpinBox;
- valueSpinBox->setRange(-1e6, 1e6);
+ valueSpinBox->setRange(min, max);
+ valueSpinBox->setSingleStep(0.01);
+ valueSpinBox->setDecimals(6);
valueSpinBox->setAlignment(Qt::AlignRight);
+
+ // Unit
+ std::string jointType = this->GetEnumWidgetValue("type");
+ std::string unit = this->GetUnitFromKey(_key, jointType);
+
+ QLabel *unitLabel = new QLabel();
+ unitLabel->setMaximumWidth(40);
+ unitLabel->setText(QString::fromStdString(unit));
+
+ // Layout
+ QHBoxLayout *widgetLayout = new QHBoxLayout;
+ if (_level != 0)
+ {
+ widgetLayout->addItem(new QSpacerItem(20*_level, 1,
+ QSizePolicy::Fixed, QSizePolicy::Fixed));
+ }
+ widgetLayout->addWidget(keyLabel);
widgetLayout->addWidget(valueSpinBox);
+ if (unitLabel->text() != "")
+ widgetLayout->addWidget(unitLabel);
+
+ // ChildWidget
ConfigChildWidget *widget = new ConfigChildWidget();
- widgetLayout->setContentsMargins(0, 0, 0, 0);
+ widget->key = _key;
widget->setLayout(widgetLayout);
+ widget->setFrameStyle(QFrame::Box);
+
widget->widgets.push_back(valueSpinBox);
+ widget->mapWidgetToUnit[valueSpinBox] = unitLabel;
+
return widget;
}
/////////////////////////////////////////////////
-ConfigChildWidget *ConfigWidget::CreateStringWidget(const std::string &_key)
+ConfigChildWidget *ConfigWidget::CreateStringWidget(const std::string &_key,
+ const int _level)
{
+ // Label
+ QLabel *keyLabel = new QLabel(tr(this->GetHumanReadableKey(_key).c_str()));
+ keyLabel->setToolTip(tr(_key.c_str()));
+
+ // LineEdit
+ QLineEdit *valueLineEdit = new QLineEdit;
+
+ // Layout
QHBoxLayout *widgetLayout = new QHBoxLayout;
- QLabel *keyLabel = new QLabel(tr(_key.c_str()));
+ if (_level != 0)
+ {
+ widgetLayout->addItem(new QSpacerItem(20*_level, 1,
+ QSizePolicy::Fixed, QSizePolicy::Fixed));
+ }
widgetLayout->addWidget(keyLabel);
- QLineEdit *valueLineEdit = new QLineEdit;
- valueLineEdit->setSizePolicy(QSizePolicy::Minimum,
- QSizePolicy::Fixed);
widgetLayout->addWidget(valueLineEdit);
+
+ // ChildWidget
ConfigChildWidget *widget = new ConfigChildWidget();
- widgetLayout->setContentsMargins(0, 0, 0, 0);
widget->setLayout(widgetLayout);
+ widget->setFrameStyle(QFrame::Box);
+
widget->widgets.push_back(valueLineEdit);
+
return widget;
}
/////////////////////////////////////////////////
-ConfigChildWidget *ConfigWidget::CreateBoolWidget(const std::string &_key)
+ConfigChildWidget *ConfigWidget::CreateBoolWidget(const std::string &_key,
+ const int _level)
{
- QHBoxLayout *widgetLayout = new QHBoxLayout;
- QLabel *keyLabel = new QLabel(tr(_key.c_str()));
- widgetLayout->addWidget(keyLabel);
+ // Label
+ QLabel *keyLabel = new QLabel(tr(this->GetHumanReadableKey(_key).c_str()));
+ keyLabel->setToolTip(tr(_key.c_str()));
+ // Buttons
QHBoxLayout *buttonLayout = new QHBoxLayout;
QRadioButton *valueTrueRadioButton = new QRadioButton;
valueTrueRadioButton->setText(tr("True"));
@@ -873,45 +1287,73 @@ ConfigChildWidget *ConfigWidget::CreateBoolWidget(const std::string &_key)
boolButtonGroup->setExclusive(true);
buttonLayout->addWidget(valueTrueRadioButton);
buttonLayout->addWidget(valueFalseRadioButton);
+
+ // Layout
+ QHBoxLayout *widgetLayout = new QHBoxLayout;
+ if (_level != 0)
+ {
+ widgetLayout->addItem(new QSpacerItem(20*_level, 1,
+ QSizePolicy::Fixed, QSizePolicy::Fixed));
+ }
+ widgetLayout->addWidget(keyLabel);
widgetLayout->addLayout(buttonLayout);
+
+ // ChildWidget
ConfigChildWidget *widget = new ConfigChildWidget();
- widgetLayout->setContentsMargins(0, 0, 0, 0);
widget->setLayout(widgetLayout);
+ widget->setFrameStyle(QFrame::Box);
+
widget->widgets.push_back(valueTrueRadioButton);
widget->widgets.push_back(valueFalseRadioButton);
+
return widget;
}
/////////////////////////////////////////////////
ConfigChildWidget *ConfigWidget::CreateVector3dWidget(
- const std::string &/*_key*/)
+ const std::string &_key, const int _level)
{
- QLabel *vecXLabel = new QLabel(tr("x"));
- QLabel *vecYLabel = new QLabel(tr("y"));
- QLabel *vecZLabel = new QLabel(tr("z"));
+ // Labels
+ QLabel *vecXLabel = new QLabel(tr("X"));
+ QLabel *vecYLabel = new QLabel(tr("Y"));
+ QLabel *vecZLabel = new QLabel(tr("Z"));
+ vecXLabel->setToolTip(tr("x"));
+ vecYLabel->setToolTip(tr("y"));
+ vecZLabel->setToolTip(tr("z"));
+
+ // SpinBoxes
+ double min = 0;
+ double max = 0;
+ this->GetRangeFromKey(_key, min, max);
QDoubleSpinBox *vecXSpinBox = new QDoubleSpinBox;
- vecXSpinBox->setRange(-1e6, 1e6);
+ vecXSpinBox->setRange(min, max);
vecXSpinBox->setSingleStep(0.01);
vecXSpinBox->setDecimals(6);
vecXSpinBox->setAlignment(Qt::AlignRight);
- vecXSpinBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ vecXSpinBox->setMaximumWidth(100);
QDoubleSpinBox *vecYSpinBox = new QDoubleSpinBox;
- vecYSpinBox->setRange(-1e6, 1e6);
+ vecYSpinBox->setRange(min, max);
vecYSpinBox->setSingleStep(0.01);
vecYSpinBox->setDecimals(6);
vecYSpinBox->setAlignment(Qt::AlignRight);
- vecYSpinBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ vecYSpinBox->setMaximumWidth(100);
QDoubleSpinBox *vecZSpinBox = new QDoubleSpinBox;
- vecZSpinBox->setRange(-1e6, 1e6);
+ vecZSpinBox->setRange(min, max);
vecZSpinBox->setSingleStep(0.01);
vecZSpinBox->setDecimals(6);
vecZSpinBox->setAlignment(Qt::AlignRight);
- vecZSpinBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ vecZSpinBox->setMaximumWidth(100);
+
+ // This is inside a group
+ int level = _level + 1;
+ // Layout
QHBoxLayout *widgetLayout = new QHBoxLayout;
+ widgetLayout->addItem(new QSpacerItem(20*level, 1,
+ QSizePolicy::Fixed, QSizePolicy::Fixed));
widgetLayout->addWidget(vecXLabel);
widgetLayout->addWidget(vecXSpinBox);
widgetLayout->addWidget(vecYLabel);
@@ -919,53 +1361,76 @@ ConfigChildWidget *ConfigWidget::CreateVector3dWidget(
widgetLayout->addWidget(vecZLabel);
widgetLayout->addWidget(vecZSpinBox);
+ widgetLayout->setAlignment(vecXLabel, Qt::AlignRight);
+ widgetLayout->setAlignment(vecYLabel, Qt::AlignRight);
+ widgetLayout->setAlignment(vecZLabel, Qt::AlignRight);
+
+ // ChildWidget
ConfigChildWidget *widget = new ConfigChildWidget();
widget->setLayout(widgetLayout);
- widgetLayout->setContentsMargins(0, 0, 0, 0);
+ widget->setFrameStyle(QFrame::Box);
widget->widgets.push_back(vecXSpinBox);
widget->widgets.push_back(vecYSpinBox);
widget->widgets.push_back(vecZSpinBox);
+
return widget;
}
/////////////////////////////////////////////////
-ConfigChildWidget *ConfigWidget::CreateColorWidget(const std::string &/*_key*/)
+ConfigChildWidget *ConfigWidget::CreateColorWidget(const std::string &_key,
+ const int _level)
{
- QLabel *colorRLabel = new QLabel(tr("r"));
- QLabel *colorGLabel = new QLabel(tr("g"));
- QLabel *colorBLabel = new QLabel(tr("b"));
- QLabel *colorALabel = new QLabel(tr("a"));
+ // Labels
+ QLabel *colorRLabel = new QLabel(tr("R"));
+ QLabel *colorGLabel = new QLabel(tr("G"));
+ QLabel *colorBLabel = new QLabel(tr("B"));
+ QLabel *colorALabel = new QLabel(tr("A"));
+ colorRLabel->setToolTip(tr("r"));
+ colorGLabel->setToolTip(tr("g"));
+ colorBLabel->setToolTip(tr("b"));
+ colorALabel->setToolTip(tr("a"));
+
+ // SpinBoxes
+ double min = 0;
+ double max = 0;
+ this->GetRangeFromKey(_key, min, max);
QDoubleSpinBox *colorRSpinBox = new QDoubleSpinBox;
colorRSpinBox->setRange(0, 1.0);
- colorRSpinBox->setSingleStep(0.01);
+ colorRSpinBox->setSingleStep(0.1);
colorRSpinBox->setDecimals(3);
colorRSpinBox->setAlignment(Qt::AlignRight);
- colorRSpinBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ colorRSpinBox->setMaximumWidth(10);
QDoubleSpinBox *colorGSpinBox = new QDoubleSpinBox;
colorGSpinBox->setRange(0, 1.0);
- colorGSpinBox->setSingleStep(0.01);
+ colorGSpinBox->setSingleStep(0.1);
colorGSpinBox->setDecimals(3);
colorGSpinBox->setAlignment(Qt::AlignRight);
- colorGSpinBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ colorGSpinBox->setMaximumWidth(10);
QDoubleSpinBox *colorBSpinBox = new QDoubleSpinBox;
colorBSpinBox->setRange(0, 1.0);
- colorBSpinBox->setSingleStep(0.01);
+ colorBSpinBox->setSingleStep(0.1);
colorBSpinBox->setDecimals(3);
colorBSpinBox->setAlignment(Qt::AlignRight);
- colorBSpinBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ colorBSpinBox->setMaximumWidth(10);
QDoubleSpinBox *colorASpinBox = new QDoubleSpinBox;
colorASpinBox->setRange(0, 1.0);
- colorASpinBox->setSingleStep(0.01);
+ colorASpinBox->setSingleStep(0.1);
colorASpinBox->setDecimals(3);
colorASpinBox->setAlignment(Qt::AlignRight);
- colorASpinBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ colorASpinBox->setMaximumWidth(10);
+
+ // This is inside a group
+ int level = _level + 1;
+ // Layout
QHBoxLayout *widgetLayout = new QHBoxLayout;
+ widgetLayout->addItem(new QSpacerItem(20*level, 1,
+ QSizePolicy::Fixed, QSizePolicy::Fixed));
widgetLayout->addWidget(colorRLabel);
widgetLayout->addWidget(colorRSpinBox);
widgetLayout->addWidget(colorGLabel);
@@ -975,9 +1440,15 @@ ConfigChildWidget *ConfigWidget::CreateColorWidget(const std::string &/*_key*/)
widgetLayout->addWidget(colorALabel);
widgetLayout->addWidget(colorASpinBox);
+ widgetLayout->setAlignment(colorRLabel, Qt::AlignRight);
+ widgetLayout->setAlignment(colorGLabel, Qt::AlignRight);
+ widgetLayout->setAlignment(colorBLabel, Qt::AlignRight);
+ widgetLayout->setAlignment(colorALabel, Qt::AlignRight);
+
+ // ChildWidget
ConfigChildWidget *widget = new ConfigChildWidget();
widget->setLayout(widgetLayout);
- widgetLayout->setContentsMargins(0, 0, 0, 0);
+ widget->setFrameStyle(QFrame::Box);
widget->widgets.push_back(colorRSpinBox);
widget->widgets.push_back(colorGSpinBox);
@@ -988,185 +1459,291 @@ ConfigChildWidget *ConfigWidget::CreateColorWidget(const std::string &/*_key*/)
}
/////////////////////////////////////////////////
-ConfigChildWidget *ConfigWidget::CreatePoseWidget(const std::string &/*_key*/)
-{
- QLabel *posXLabel = new QLabel(tr("x"));
- QLabel *posYLabel = new QLabel(tr("y"));
- QLabel *posZLabel = new QLabel(tr("z"));
- QLabel *rotRLabel = new QLabel(tr("roll"));
- QLabel *rotPLabel = new QLabel(tr("pitch"));
- QLabel *rotYLabel = new QLabel(tr("yaw"));
-
- QDoubleSpinBox *posXSpinBox = new QDoubleSpinBox;
- posXSpinBox->setRange(-1e6, 1e6);
- posXSpinBox->setSingleStep(0.01);
- posXSpinBox->setDecimals(6);
- posXSpinBox->setAlignment(Qt::AlignRight);
-
- QDoubleSpinBox *posYSpinBox = new QDoubleSpinBox;
- posYSpinBox->setRange(-1e6, 1e6);
- posYSpinBox->setSingleStep(0.01);
- posYSpinBox->setDecimals(6);
- posYSpinBox->setAlignment(Qt::AlignRight);
-
- QDoubleSpinBox *posZSpinBox = new QDoubleSpinBox;
- posZSpinBox->setRange(-1e6, 1e6);
- posZSpinBox->setSingleStep(0.01);
- posZSpinBox->setDecimals(6);
- posZSpinBox->setAlignment(Qt::AlignRight);
-
- QDoubleSpinBox *rotRSpinBox = new QDoubleSpinBox;
- rotRSpinBox->setRange(-1e6, 1e6);
- rotRSpinBox->setSingleStep(0.01);
- rotRSpinBox->setDecimals(6);
- rotRSpinBox->setAlignment(Qt::AlignRight);
-
- QDoubleSpinBox *rotPSpinBox = new QDoubleSpinBox;
- rotPSpinBox->setRange(-1e6, 1e6);
- rotPSpinBox->setSingleStep(0.01);
- rotPSpinBox->setDecimals(6);
- rotPSpinBox->setAlignment(Qt::AlignRight);
-
- QDoubleSpinBox *rotYSpinBox = new QDoubleSpinBox;
- rotYSpinBox->setRange(-1e6, 1e6);
- rotYSpinBox->setSingleStep(0.01);
- rotYSpinBox->setDecimals(6);
- rotYSpinBox->setAlignment(Qt::AlignRight);
-
- QGridLayout *poseGroupLayout = new QGridLayout;
- poseGroupLayout->addWidget(posXLabel, 0, 0);
- poseGroupLayout->addWidget(posXSpinBox, 0, 1);
- poseGroupLayout->addWidget(posYLabel, 0, 2);
- poseGroupLayout->addWidget(posYSpinBox, 0, 3);
- poseGroupLayout->addWidget(posZLabel, 0, 4);
- poseGroupLayout->addWidget(posZSpinBox, 0, 5);
- poseGroupLayout->addWidget(rotRLabel, 1, 0);
- poseGroupLayout->addWidget(rotRSpinBox, 1, 1);
- poseGroupLayout->addWidget(rotPLabel, 1, 2);
- poseGroupLayout->addWidget(rotPSpinBox, 1, 3);
- poseGroupLayout->addWidget(rotYLabel, 1, 4);
- poseGroupLayout->addWidget(rotYSpinBox, 1, 5);
-
- poseGroupLayout->setColumnStretch(1, 1);
- poseGroupLayout->setAlignment(posXSpinBox, Qt::AlignLeft);
- poseGroupLayout->setAlignment(posYSpinBox, Qt::AlignLeft);
- poseGroupLayout->setAlignment(posZSpinBox, Qt::AlignLeft);
- poseGroupLayout->setAlignment(rotRSpinBox, Qt::AlignLeft);
- poseGroupLayout->setAlignment(rotPSpinBox, Qt::AlignLeft);
- poseGroupLayout->setAlignment(rotYSpinBox, Qt::AlignLeft);
+ConfigChildWidget *ConfigWidget::CreatePoseWidget(const std::string &/*_key*/,
+ const int _level)
+{
+ // Labels
+ std::vector<std::string> elements;
+ elements.push_back("x");
+ elements.push_back("y");
+ elements.push_back("z");
+ elements.push_back("roll");
+ elements.push_back("pitch");
+ elements.push_back("yaw");
+
+ // This is inside a group
+ int level = _level+1;
+
+ // Layout
+ QGridLayout *widgetLayout = new QGridLayout;
+ widgetLayout->setColumnStretch(3, 1);
+ widgetLayout->addItem(new QSpacerItem(20*level, 1, QSizePolicy::Fixed,
+ QSizePolicy::Fixed), 0, 0);
+
+ // ChildWidget
+ double min = 0;
+ double max = 0;
+ this->GetRangeFromKey("", min, max);
ConfigChildWidget *widget = new ConfigChildWidget();
- poseGroupLayout->setContentsMargins(0, 0, 0, 0);
- widget->setLayout(poseGroupLayout);
+ widget->setLayout(widgetLayout);
+ widget->setFrameStyle(QFrame::Box);
- widget->widgets.push_back(posXSpinBox);
- widget->widgets.push_back(posYSpinBox);
- widget->widgets.push_back(posZSpinBox);
- widget->widgets.push_back(rotRSpinBox);
- widget->widgets.push_back(rotPSpinBox);
- widget->widgets.push_back(rotYSpinBox);
+ for (unsigned int i = 0; i < elements.size(); ++i)
+ {
+ QDoubleSpinBox *spin = new QDoubleSpinBox();
+ widget->widgets.push_back(spin);
+
+ spin->setRange(min, max);
+ spin->setSingleStep(0.01);
+ spin->setDecimals(6);
+ spin->setAlignment(Qt::AlignRight);
+ spin->setMaximumWidth(100);
+
+ QLabel *label = new QLabel(this->GetHumanReadableKey(elements[i]).c_str());
+ label->setToolTip(tr(elements[i].c_str()));
+ if (i == 0)
+ label->setStyleSheet("QLabel{color: " + this->redColor + ";}");
+ else if (i == 1)
+ label->setStyleSheet("QLabel{color: " + this->greenColor + ";}");
+ else if (i == 2)
+ label->setStyleSheet("QLabel{color:" + this->blueColor + ";}");
+
+ QLabel *unitLabel = new QLabel();
+ unitLabel->setMaximumWidth(40);
+ unitLabel->setMinimumWidth(40);
+ if (i < 3)
+ unitLabel->setText(QString::fromStdString(this->GetUnitFromKey("pos")));
+ else
+ unitLabel->setText(QString::fromStdString(this->GetUnitFromKey("rot")));
+
+ widgetLayout->addWidget(label, i%3, std::floor(i/3)*3+1);
+ widgetLayout->addWidget(spin, i%3, std::floor(i/3)*3+2);
+ widgetLayout->addWidget(unitLabel, i%3, std::floor(i/3)*3+3);
+
+ widgetLayout->setAlignment(label, Qt::AlignLeft);
+ widgetLayout->setAlignment(spin, Qt::AlignLeft);
+ widgetLayout->setAlignment(unitLabel, Qt::AlignLeft);
+ }
return widget;
}
/////////////////////////////////////////////////
ConfigChildWidget *ConfigWidget::CreateGeometryWidget(
- const std::string &/*_key*/)
+ const std::string &/*_key*/, const int _level)
{
- QLabel *geometryLabel = new QLabel(tr("geometry"));
+ // Geometry ComboBox
+ QLabel *geometryLabel = new QLabel(tr("Geometry"));
+ geometryLabel->setToolTip(tr("geometry"));
QComboBox *geometryComboBox = new QComboBox;
geometryComboBox->addItem(tr("box"));
geometryComboBox->addItem(tr("cylinder"));
geometryComboBox->addItem(tr("sphere"));
- // geometryComboBox->addItem(tr("custom"));
+ geometryComboBox->addItem(tr("mesh"));
+ geometryComboBox->addItem(tr("polyline"));
+
+ // Size XYZ
+ double min = 0;
+ double max = 0;
+ this->GetRangeFromKey("length", min, max);
QDoubleSpinBox *geomSizeXSpinBox = new QDoubleSpinBox;
- geomSizeXSpinBox->setRange(-1000, 1000);
+ geomSizeXSpinBox->setRange(min, max);
geomSizeXSpinBox->setSingleStep(0.01);
geomSizeXSpinBox->setDecimals(6);
geomSizeXSpinBox->setValue(1.000);
+ geomSizeXSpinBox->setAlignment(Qt::AlignRight);
+ geomSizeXSpinBox->setMaximumWidth(100);
QDoubleSpinBox *geomSizeYSpinBox = new QDoubleSpinBox;
- geomSizeYSpinBox->setRange(-1000, 1000);
+ geomSizeYSpinBox->setRange(min, max);
geomSizeYSpinBox->setSingleStep(0.01);
geomSizeYSpinBox->setDecimals(6);
geomSizeYSpinBox->setValue(1.000);
+ geomSizeYSpinBox->setAlignment(Qt::AlignRight);
+ geomSizeYSpinBox->setMaximumWidth(100);
QDoubleSpinBox *geomSizeZSpinBox = new QDoubleSpinBox;
- geomSizeZSpinBox->setRange(-1000, 1000);
+ geomSizeZSpinBox->setRange(min, max);
geomSizeZSpinBox->setSingleStep(0.01);
geomSizeZSpinBox->setDecimals(6);
geomSizeZSpinBox->setValue(1.000);
-
- QLabel *geomSizeXLabel = new QLabel(tr("x"));
- QLabel *geomSizeYLabel = new QLabel(tr("y"));
- QLabel *geomSizeZLabel = new QLabel(tr("z"));
+ geomSizeZSpinBox->setAlignment(Qt::AlignRight);
+ geomSizeZSpinBox->setMaximumWidth(100);
+
+ QLabel *geomSizeXLabel = new QLabel(tr("X"));
+ QLabel *geomSizeYLabel = new QLabel(tr("Y"));
+ QLabel *geomSizeZLabel = new QLabel(tr("Z"));
+ geomSizeXLabel->setStyleSheet("QLabel{color: " + this->redColor + ";}");
+ geomSizeYLabel->setStyleSheet("QLabel{color: " + this->greenColor + ";}");
+ geomSizeZLabel->setStyleSheet("QLabel{color: " + this->blueColor + ";}");
+ geomSizeXLabel->setToolTip(tr("x"));
+ geomSizeYLabel->setToolTip(tr("y"));
+ geomSizeZLabel->setToolTip(tr("z"));
+
+ std::string unit = this->GetUnitFromKey("length");
+ QLabel *geomSizeXUnitLabel = new QLabel(QString::fromStdString(unit));
+ QLabel *geomSizeYUnitLabel = new QLabel(QString::fromStdString(unit));
+ QLabel *geomSizeZUnitLabel = new QLabel(QString::fromStdString(unit));
QHBoxLayout *geomSizeLayout = new QHBoxLayout;
geomSizeLayout->addWidget(geomSizeXLabel);
geomSizeLayout->addWidget(geomSizeXSpinBox);
+ geomSizeLayout->addWidget(geomSizeXUnitLabel);
geomSizeLayout->addWidget(geomSizeYLabel);
geomSizeLayout->addWidget(geomSizeYSpinBox);
+ geomSizeLayout->addWidget(geomSizeYUnitLabel);
geomSizeLayout->addWidget(geomSizeZLabel);
geomSizeLayout->addWidget(geomSizeZSpinBox);
+ geomSizeLayout->addWidget(geomSizeZUnitLabel);
+
+ geomSizeLayout->setAlignment(geomSizeXLabel, Qt::AlignRight);
+ geomSizeLayout->setAlignment(geomSizeYLabel, Qt::AlignRight);
+ geomSizeLayout->setAlignment(geomSizeZLabel, Qt::AlignRight);
- QLabel *geomRadiusLabel = new QLabel(tr("radius"));
- QLabel *geomLengthLabel = new QLabel(tr("length"));
+ // Uri
+ QLabel *geomFilenameLabel = new QLabel(tr("Uri"));
+ geomFilenameLabel->setToolTip(tr("uri"));
+ QLineEdit *geomFilenameLineEdit = new QLineEdit;
+ QPushButton *geomFilenameButton = new QPushButton(tr("..."));
+ geomFilenameButton->setMaximumWidth(30);
+
+ QHBoxLayout *geomFilenameLayout = new QHBoxLayout;
+ geomFilenameLayout->addWidget(geomFilenameLabel);
+ geomFilenameLayout->addWidget(geomFilenameLineEdit);
+ geomFilenameLayout->addWidget(geomFilenameButton);
+
+ QVBoxLayout *geomSizeFilenameLayout = new QVBoxLayout;
+ geomSizeFilenameLayout->addLayout(geomSizeLayout);
+ geomSizeFilenameLayout->addLayout(geomFilenameLayout);
+
+ QWidget *geomSizeWidget = new QWidget;
+ geomSizeWidget->setLayout(geomSizeFilenameLayout);
+
+ // Radius / Length
+ QLabel *geomRadiusLabel = new QLabel(tr("Radius"));
+ QLabel *geomLengthLabel = new QLabel(tr("Length"));
+ QLabel *geomRadiusUnitLabel = new QLabel(QString::fromStdString(unit));
+ QLabel *geomLengthUnitLabel = new QLabel(QString::fromStdString(unit));
+ geomRadiusLabel->setToolTip(tr("radius"));
+ geomLengthLabel->setToolTip(tr("length"));
QDoubleSpinBox *geomRadiusSpinBox = new QDoubleSpinBox;
- geomRadiusSpinBox->setRange(-1000, 1000);
+ geomRadiusSpinBox->setRange(min, max);
geomRadiusSpinBox->setSingleStep(0.01);
geomRadiusSpinBox->setDecimals(6);
geomRadiusSpinBox->setValue(0.500);
+ geomRadiusSpinBox->setAlignment(Qt::AlignRight);
+ geomRadiusSpinBox->setMaximumWidth(100);
QDoubleSpinBox *geomLengthSpinBox = new QDoubleSpinBox;
- geomLengthSpinBox->setRange(-1000, 1000);
+ geomLengthSpinBox->setRange(min, max);
geomLengthSpinBox->setSingleStep(0.01);
geomLengthSpinBox->setDecimals(6);
geomLengthSpinBox->setValue(1.000);
+ geomLengthSpinBox->setAlignment(Qt::AlignRight);
+ geomLengthSpinBox->setMaximumWidth(100);
QHBoxLayout *geomRLLayout = new QHBoxLayout;
geomRLLayout->addWidget(geomRadiusLabel);
geomRLLayout->addWidget(geomRadiusSpinBox);
+ geomRLLayout->addWidget(geomRadiusUnitLabel);
geomRLLayout->addWidget(geomLengthLabel);
geomRLLayout->addWidget(geomLengthSpinBox);
+ geomRLLayout->addWidget(geomLengthUnitLabel);
- QStackedWidget *geomDimensionWidget = new QStackedWidget;
-
- QWidget *geomSizeWidget = new QWidget;
- geomSizeWidget->setLayout(geomSizeLayout);
- geomDimensionWidget->insertWidget(0, geomSizeWidget);
+ geomRLLayout->setAlignment(geomRadiusLabel, Qt::AlignRight);
+ geomRLLayout->setAlignment(geomLengthLabel, Qt::AlignRight);
QWidget *geomRLWidget = new QWidget;
geomRLWidget->setLayout(geomRLLayout);
+
+ // Dimensions
+ QStackedWidget *geomDimensionWidget = new QStackedWidget;
+ geomDimensionWidget->insertWidget(0, geomSizeWidget);
+
geomDimensionWidget->insertWidget(1, geomRLWidget);
geomDimensionWidget->setCurrentIndex(0);
geomDimensionWidget->setSizePolicy(
QSizePolicy::Minimum, QSizePolicy::Minimum);
+ // This is inside a group
+ int level = _level + 1;
+
+ // Layout
QGridLayout *widgetLayout = new QGridLayout;
- widgetLayout->addWidget(geometryLabel, 1, 0);
- widgetLayout->addWidget(geometryComboBox, 1, 1);
- widgetLayout->addWidget(geomDimensionWidget, 2, 1);
+ widgetLayout->addItem(new QSpacerItem(20*level, 1,
+ QSizePolicy::Fixed, QSizePolicy::Fixed), 0, 0);
+ widgetLayout->addWidget(geometryLabel, 0, 1);
+ widgetLayout->addWidget(geometryComboBox, 0, 2, 1, 2);
+ widgetLayout->addWidget(geomDimensionWidget, 2, 1, 1, 3);
+ // ChildWidget
GeometryConfigWidget *widget = new GeometryConfigWidget;
+ widget->setFrameStyle(QFrame::Box);
widget->geomDimensionWidget = geomDimensionWidget;
widget->geomLengthSpinBox = geomLengthSpinBox;
widget->geomLengthLabel = geomLengthLabel;
+ widget->geomLengthUnitLabel = geomLengthUnitLabel;
+ widget->geomFilenameLabel = geomFilenameLabel;
+ widget->geomFilenameLineEdit = geomFilenameLineEdit;
+ widget->geomFilenameButton = geomFilenameButton;
- connect(geometryComboBox,
- SIGNAL(currentIndexChanged(const QString)),
- widget, SLOT(GeometryChanged(const QString)));
+ geomFilenameLabel->setVisible(false);
+ geomFilenameLineEdit->setVisible(false);
+ geomFilenameButton->setVisible(false);
- widgetLayout->setContentsMargins(0, 0, 0, 0);
- widget->setLayout(widgetLayout);
+ connect(geometryComboBox, SIGNAL(currentIndexChanged(const QString)),
+ widget, SLOT(GeometryChanged(const QString)));
+ connect(geomFilenameButton, SIGNAL(clicked()), widget, SLOT(OnSelectFile()));
+ widget->setLayout(widgetLayout);
widget->widgets.push_back(geometryComboBox);
widget->widgets.push_back(geomSizeXSpinBox);
widget->widgets.push_back(geomSizeYSpinBox);
widget->widgets.push_back(geomSizeZSpinBox);
widget->widgets.push_back(geomRadiusSpinBox);
widget->widgets.push_back(geomLengthSpinBox);
+ widget->widgets.push_back(geomFilenameLineEdit);
+ widget->widgets.push_back(geomFilenameButton);
+
+ return widget;
+}
+
+/////////////////////////////////////////////////
+ConfigChildWidget *ConfigWidget::CreateEnumWidget(
+ const std::string &_key, const std::vector<std::string> &_values,
+ const int _level)
+{
+ // Label
+ QLabel *enumLabel = new QLabel(this->GetHumanReadableKey(_key).c_str());
+ enumLabel->setToolTip(tr(_key.c_str()));
+
+ // ComboBox
+ QComboBox *enumComboBox = new QComboBox;
+
+ for (unsigned int i = 0; i < _values.size(); ++i)
+ enumComboBox->addItem(tr(_values[i].c_str()));
+
+ // Layout
+ QHBoxLayout *widgetLayout = new QHBoxLayout;
+ if (_level != 0)
+ {
+ widgetLayout->addItem(new QSpacerItem(20*_level, 1,
+ QSizePolicy::Fixed, QSizePolicy::Fixed));
+ }
+ widgetLayout->addWidget(enumLabel);
+ widgetLayout->addWidget(enumComboBox);
+
+ // ChildWidget
+ EnumConfigWidget *widget = new EnumConfigWidget();
+ widget->setLayout(widgetLayout);
+ widget->setFrameStyle(QFrame::Box);
+ connect(enumComboBox, SIGNAL(currentIndexChanged(const QString &)),
+ widget, SLOT(EnumChanged(const QString &)));
+
+ widget->widgets.push_back(enumComboBox);
return widget;
}
@@ -1195,13 +1772,17 @@ void ConfigWidget::UpdateMsg(google::protobuf::Message *_msg,
std::string name = field->name();
// Update each field in the message
- // TODO update repeated fields and enum fields
- if (!field->is_repeated() && ref->HasField(*_msg, field))
+ // TODO update repeated fields
+ if (!field->is_repeated() /*&& ref->HasField(*_msg, field)*/)
{
std::string scopedName = _name.empty() ? name : _name + "::" + name;
if (this->configWidgets.find(scopedName) == this->configWidgets.end())
continue;
+ // don't update msgs field that are associated with read-only widgets
+ if (this->GetWidgetReadOnly(scopedName))
+ continue;
+
ConfigChildWidget *childWidget = this->configWidgets[scopedName];
switch (field->cpp_type())
@@ -1285,7 +1866,7 @@ void ConfigWidget::UpdateMsg(google::protobuf::Message *_msg,
const google::protobuf::EnumDescriptor *typeEnumDescriptor =
typeField->enum_type();
- if (geomType == "box")
+ if (geomType == "box" || geomType == "mesh")
{
double sizeX = qobject_cast<QDoubleSpinBox *>(
childWidget->widgets[1])->value();
@@ -1293,11 +1874,13 @@ void ConfigWidget::UpdateMsg(google::protobuf::Message *_msg,
childWidget->widgets[2])->value();
double sizeZ = qobject_cast<QDoubleSpinBox *>(
childWidget->widgets[3])->value();
- math::Vector3 boxSize(sizeX, sizeY, sizeZ);
+ math::Vector3 geomSize(sizeX, sizeY, sizeZ);
// set type
+ std::string typeStr =
+ QString(tr(geomType.c_str())).toUpper().toStdString();
const google::protobuf::EnumValueDescriptor *geometryType =
- typeEnumDescriptor->FindValueByName("BOX");
+ typeEnumDescriptor->FindValueByName(typeStr);
geomReflection->SetEnum(valueMsg, typeField, geometryType);
// set dimensions
@@ -1305,10 +1888,22 @@ void ConfigWidget::UpdateMsg(google::protobuf::Message *_msg,
valueDescriptor->FindFieldByName(geomType);
google::protobuf::Message *geomValueMsg =
geomReflection->MutableMessage(valueMsg, geomFieldDescriptor);
+
+ int fieldIdx = (geomType == "box") ? 0 : 1;
google::protobuf::Message *geomDimensionMsg =
geomValueMsg->GetReflection()->MutableMessage(geomValueMsg,
- geomValueMsg->GetDescriptor()->field(0));
- this->UpdateVector3Msg(geomDimensionMsg, boxSize);
+ geomValueMsg->GetDescriptor()->field(fieldIdx));
+ this->UpdateVector3Msg(geomDimensionMsg, geomSize);
+
+ if (geomType == "mesh")
+ {
+ std::string uri = qobject_cast<QLineEdit *>(
+ childWidget->widgets[6])->text().toStdString();
+ const google::protobuf::FieldDescriptor *uriFieldDescriptor =
+ geomValueMsg->GetDescriptor()->field(0);
+ geomValueMsg->GetReflection()->SetString(geomValueMsg,
+ uriFieldDescriptor, uri);
+ }
}
else if (geomType == "cylinder")
{
@@ -1358,6 +1953,12 @@ void ConfigWidget::UpdateMsg(google::protobuf::Message *_msg,
geomValueMsg->GetReflection()->SetDouble(geomValueMsg,
geomRadiusField, radius);
}
+ else if (geomType == "polyline")
+ {
+ const google::protobuf::EnumValueDescriptor *geometryType =
+ typeEnumDescriptor->FindValueByName("POLYLINE");
+ geomReflection->SetEnum(valueMsg, typeField, geometryType);
+ }
}
// update pose msg field
else if (field->message_type()->name() == "Pose")
@@ -1461,8 +2062,27 @@ void ConfigWidget::UpdateMsg(google::protobuf::Message *_msg,
break;
}
case google::protobuf::FieldDescriptor::CPPTYPE_ENUM:
- // todo update enum fields.
+ {
+ QComboBox *valueComboBox =
+ qobject_cast<QComboBox *>(childWidget->widgets[0]);
+ if (valueComboBox)
+ {
+ std::string valueStr = valueComboBox->currentText().toStdString();
+ const google::protobuf::EnumDescriptor *enumDescriptor =
+ field->enum_type();
+ if (enumDescriptor)
+ {
+ const google::protobuf::EnumValueDescriptor *enumValue =
+ enumDescriptor->FindValueByName(valueStr);
+ if (enumValue)
+ ref->SetEnum(_msg, field, enumValue);
+ else
+ gzerr << "Unable to find enum value: '" << valueStr << "'"
+ << std::endl;
+ }
+ }
break;
+ }
default:
break;
}
@@ -1491,75 +2111,95 @@ void ConfigWidget::UpdateVector3Msg(google::protobuf::Message *_msg,
}
/////////////////////////////////////////////////
-void ConfigWidget::UpdateIntWidget(ConfigChildWidget *_widget, int _value)
+bool ConfigWidget::UpdateIntWidget(ConfigChildWidget *_widget, int _value)
{
if (_widget->widgets.size() == 1u)
{
qobject_cast<QSpinBox *>(_widget->widgets[0])->setValue(_value);
+ return true;
}
else
{
gzerr << "Error updating Int Config widget" << std::endl;
}
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::UpdateUIntWidget(ConfigChildWidget *_widget,
+bool ConfigWidget::UpdateUIntWidget(ConfigChildWidget *_widget,
unsigned int _value)
{
if (_widget->widgets.size() == 1u)
{
qobject_cast<QSpinBox *>(_widget->widgets[0])->setValue(_value);
+ return true;
}
else
{
gzerr << "Error updating UInt Config widget" << std::endl;
}
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::UpdateDoubleWidget(ConfigChildWidget *_widget, double _value)
+bool ConfigWidget::UpdateDoubleWidget(ConfigChildWidget *_widget, double _value)
{
if (_widget->widgets.size() == 1u)
{
- qobject_cast<QDoubleSpinBox *>(_widget->widgets[0])->setValue(_value);
+ // Spin value
+ QDoubleSpinBox *spin =
+ qobject_cast<QDoubleSpinBox *>(_widget->widgets[0]);
+ spin->setValue(_value);
+
+ // Unit label
+ std::string jointType = this->GetEnumWidgetValue("type");
+ std::string unit = this->GetUnitFromKey(_widget->key, jointType);
+ qobject_cast<QLabel *>(
+ _widget->mapWidgetToUnit[spin])->setText(QString::fromStdString(unit));
+
+ return true;
}
else
{
gzerr << "Error updating Double Config widget" << std::endl;
}
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::UpdateStringWidget(ConfigChildWidget *_widget,
+bool ConfigWidget::UpdateStringWidget(ConfigChildWidget *_widget,
const std::string &_value)
{
if (_widget->widgets.size() == 1u)
{
qobject_cast<QLineEdit *>(_widget->widgets[0])->setText(tr(_value.c_str()));
+ return true;
}
else
{
gzerr << "Error updating String Config Widget" << std::endl;
}
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::UpdateBoolWidget(ConfigChildWidget *_widget, bool _value)
+bool ConfigWidget::UpdateBoolWidget(ConfigChildWidget *_widget, bool _value)
{
if (_widget->widgets.size() == 2u)
{
qobject_cast<QRadioButton *>(_widget->widgets[0])->setChecked(_value);
qobject_cast<QRadioButton *>(_widget->widgets[1])->setChecked(!_value);
+ return true;
}
else
{
gzerr << "Error updating Bool Config widget" << std::endl;
}
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::UpdateVector3Widget(ConfigChildWidget *_widget,
+bool ConfigWidget::UpdateVector3Widget(ConfigChildWidget *_widget,
const math::Vector3 &_vec)
{
if (_widget->widgets.size() == 3u)
@@ -1567,15 +2207,17 @@ void ConfigWidget::UpdateVector3Widget(ConfigChildWidget *_widget,
qobject_cast<QDoubleSpinBox *>(_widget->widgets[0])->setValue(_vec.x);
qobject_cast<QDoubleSpinBox *>(_widget->widgets[1])->setValue(_vec.y);
qobject_cast<QDoubleSpinBox *>(_widget->widgets[2])->setValue(_vec.z);
+ return true;
}
else
{
gzerr << "Error updating Vector3 Config widget" << std::endl;
}
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::UpdateColorWidget(ConfigChildWidget *_widget,
+bool ConfigWidget::UpdateColorWidget(ConfigChildWidget *_widget,
const common::Color &_color)
{
if (_widget->widgets.size() == 4u)
@@ -1584,15 +2226,17 @@ void ConfigWidget::UpdateColorWidget(ConfigChildWidget *_widget,
qobject_cast<QDoubleSpinBox *>(_widget->widgets[1])->setValue(_color.g);
qobject_cast<QDoubleSpinBox *>(_widget->widgets[2])->setValue(_color.b);
qobject_cast<QDoubleSpinBox *>(_widget->widgets[3])->setValue(_color.a);
+ return true;
}
else
{
gzerr << "Error updating Color Config widget" << std::endl;
}
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::UpdatePoseWidget(ConfigChildWidget *_widget,
+bool ConfigWidget::UpdatePoseWidget(ConfigChildWidget *_widget,
const math::Pose &_pose)
{
if (_widget->widgets.size() == 6u)
@@ -1605,36 +2249,40 @@ void ConfigWidget::UpdatePoseWidget(ConfigChildWidget *_widget,
qobject_cast<QDoubleSpinBox *>(_widget->widgets[3])->setValue(rot.x);
qobject_cast<QDoubleSpinBox *>(_widget->widgets[4])->setValue(rot.y);
qobject_cast<QDoubleSpinBox *>(_widget->widgets[5])->setValue(rot.z);
+ return true;
}
else
{
gzerr << "Error updating Pose Config widget" << std::endl;
}
+ return false;
}
/////////////////////////////////////////////////
-void ConfigWidget::UpdateGeometryWidget(ConfigChildWidget *_widget,
- const std::string &_value, const math::Vector3 &_dimensions)
+bool ConfigWidget::UpdateGeometryWidget(ConfigChildWidget *_widget,
+ const std::string &_value, const math::Vector3 &_dimensions,
+ const std::string &_uri)
{
- if (_widget->widgets.size() != 6u)
+ if (_widget->widgets.size() != 8u)
{
gzerr << "Error updating Geometry Config widget " << std::endl;
- return;
+ return false;
}
- QComboBox * valueComboBox = qobject_cast<QComboBox *>(_widget->widgets[0]);
+ QComboBox *valueComboBox = qobject_cast<QComboBox *>(_widget->widgets[0]);
int index = valueComboBox->findText(tr(_value.c_str()));
if (index < 0)
{
gzerr << "Error updating Geometry Config widget: '" << _value <<
"' not found" << std::endl;
- return;
+ return false;
}
qobject_cast<QComboBox *>(_widget->widgets[0])->setCurrentIndex(index);
- if (_value == "box")
+ bool isMesh = _value == "mesh";
+ if (_value == "box" || isMesh)
{
qobject_cast<QDoubleSpinBox *>(_widget->widgets[1])->setValue(
_dimensions.x);
@@ -1646,15 +2294,56 @@ void ConfigWidget::UpdateGeometryWidget(ConfigChildWidget *_widget,
else if (_value == "cylinder")
{
qobject_cast<QDoubleSpinBox *>(_widget->widgets[4])->setValue(
- _dimensions.x/2.0);
+ _dimensions.x*0.5);
qobject_cast<QDoubleSpinBox *>(_widget->widgets[5])->setValue(
_dimensions.z);
}
else if (_value == "sphere")
{
qobject_cast<QDoubleSpinBox *>(_widget->widgets[4])->setValue(
- _dimensions.x/2.0);
+ _dimensions.x*0.5);
}
+ else if (_value == "polyline")
+ {
+ // do nothing
+ }
+
+ if (isMesh)
+ qobject_cast<QLineEdit *>(_widget->widgets[6])->setText(tr(_uri.c_str()));
+
+ return true;
+}
+
+
+/////////////////////////////////////////////////
+bool ConfigWidget::UpdateEnumWidget(ConfigChildWidget *_widget,
+ const std::string &_value)
+{
+ if (_widget->widgets.size() != 1u)
+ {
+ gzerr << "Error updating Enum Config widget" << std::endl;
+ return false;
+ }
+
+ QComboBox *valueComboBox = qobject_cast<QComboBox *>(_widget->widgets[0]);
+ if (!valueComboBox)
+ {
+ gzerr << "Error updating Enum Config widget" << std::endl;
+ return false;
+ }
+
+ int index = valueComboBox->findText(tr(_value.c_str()));
+
+ if (index < 0)
+ {
+ gzerr << "Error updating Enum Config widget: '" << _value <<
+ "' not found" << std::endl;
+ return false;
+ }
+
+ qobject_cast<QComboBox *>(_widget->widgets[0])->setCurrentIndex(index);
+
+ return true;
}
/////////////////////////////////////////////////
@@ -1795,10 +2484,10 @@ math::Pose ConfigWidget::GetPoseWidgetValue(ConfigChildWidget *_widget) const
/////////////////////////////////////////////////
std::string ConfigWidget::GetGeometryWidgetValue(ConfigChildWidget *_widget,
- math::Vector3 &_dimensions) const
+ math::Vector3 &_dimensions, std::string &_uri) const
{
std::string value;
- if (_widget->widgets.size() != 6u)
+ if (_widget->widgets.size() != 8u)
{
gzerr << "Error getting value from Geometry Config widget " << std::endl;
return value;
@@ -1807,7 +2496,8 @@ std::string ConfigWidget::GetGeometryWidgetValue(ConfigChildWidget *_widget,
QComboBox *valueComboBox = qobject_cast<QComboBox *>(_widget->widgets[0]);
value = valueComboBox->currentText().toStdString();
- if (value == "box")
+ bool isMesh = value == "mesh";
+ if (value == "box" || isMesh)
{
_dimensions.x =
qobject_cast<QDoubleSpinBox *>(_widget->widgets[1])->value();
@@ -1831,11 +2521,35 @@ std::string ConfigWidget::GetGeometryWidgetValue(ConfigChildWidget *_widget,
_dimensions.y = _dimensions.x;
_dimensions.z = _dimensions.x;
}
+ else if (value == "polyline")
+ {
+ // do nothing
+ }
else
{
gzerr << "Error getting geometry dimensions for type: '" << value << "'"
<< std::endl;
}
+
+ if (isMesh)
+ _uri = qobject_cast<QLineEdit *>(_widget->widgets[6])->text().toStdString();
+
+ return value;
+}
+
+/////////////////////////////////////////////////
+std::string ConfigWidget::GetEnumWidgetValue(ConfigChildWidget *_widget) const
+{
+ std::string value;
+ if (_widget->widgets.size() != 1u)
+ {
+ gzerr << "Error getting value from Enum Config widget " << std::endl;
+ return value;
+ }
+
+ QComboBox *valueComboBox = qobject_cast<QComboBox *>(_widget->widgets[0]);
+ value = valueComboBox->currentText().toStdString();
+
return value;
}
@@ -1848,12 +2562,62 @@ void ConfigWidget::OnItemSelection(QTreeWidgetItem *_item,
}
/////////////////////////////////////////////////
-void GroupWidget::Toggle()
+void ConfigWidget::OnEnumValueChanged(const QString &_value)
+{
+ ConfigChildWidget *widget =
+ qobject_cast<ConfigChildWidget *>(QObject::sender());
+
+ for (auto iter : this->configWidgets)
+ {
+ if (iter.second == widget)
+ {
+ std::string scopedName = iter.first;
+ emit EnumValueChanged(tr(scopedName.c_str()), _value);
+ return;
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+bool ConfigWidget::eventFilter(QObject *_obj, QEvent *_event)
+{
+ QAbstractSpinBox *spinBox = qobject_cast<QAbstractSpinBox *>(_obj);
+ QComboBox *comboBox = qobject_cast<QComboBox *>(_obj);
+ if (spinBox || comboBox)
+ {
+ QWidget *widget = qobject_cast<QWidget *>(_obj);
+ if (_event->type() == QEvent::Wheel)
+ {
+ if (widget->focusPolicy() == Qt::WheelFocus)
+ {
+ _event->accept();
+ return false;
+ }
+ else
+ {
+ _event->ignore();
+ return true;
+ }
+ }
+ else if (_event->type() == QEvent::FocusIn)
+ {
+ widget->setFocusPolicy(Qt::WheelFocus);
+ }
+ else if (_event->type() == QEvent::FocusOut)
+ {
+ widget->setFocusPolicy(Qt::StrongFocus);
+ }
+ }
+ return QObject::eventFilter(_obj, _event);
+}
+
+/////////////////////////////////////////////////
+void GroupWidget::Toggle(bool _checked)
{
if (!this->childWidget)
return;
- this->childWidget->setVisible(!this->childWidget->isVisible());
+ this->childWidget->setVisible(_checked);
}
/////////////////////////////////////////////////
@@ -1864,21 +2628,66 @@ void GeometryConfigWidget::GeometryChanged(const QString _text)
if (widget)
{
std::string textStr = _text.toStdString();
- if (textStr == "box")
+ bool isMesh = (textStr == "mesh");
+ if (textStr == "box" || isMesh)
{
+ this->geomDimensionWidget->show();
this->geomDimensionWidget->setCurrentIndex(0);
}
else if (textStr == "cylinder")
{
+ this->geomDimensionWidget->show();
this->geomDimensionWidget->setCurrentIndex(1);
this->geomLengthSpinBox->show();
this->geomLengthLabel->show();
+ this->geomLengthUnitLabel->show();
}
else if (textStr == "sphere")
{
+ this->geomDimensionWidget->show();
this->geomDimensionWidget->setCurrentIndex(1);
this->geomLengthSpinBox->hide();
this->geomLengthLabel->hide();
+ this->geomLengthUnitLabel->hide();
+ }
+ else if (textStr == "polyline")
+ {
+ this->geomDimensionWidget->hide();
+ }
+
+ this->geomFilenameLabel->setVisible(isMesh);
+ this->geomFilenameLineEdit->setVisible(isMesh);
+ this->geomFilenameButton->setVisible(isMesh);
+ }
+}
+
+/////////////////////////////////////////////////
+void GeometryConfigWidget::OnSelectFile()
+{
+ QWidget *widget= qobject_cast<QWidget *>(QObject::sender());
+
+ if (widget)
+ {
+ QFileDialog fd(this, tr("Select mesh file"), QDir::homePath(),
+ tr("Mesh files (*.dae *.stl)"));
+ fd.setFilter(QDir::AllDirs | QDir::Hidden);
+ fd.setFileMode(QFileDialog::ExistingFile);
+ if (fd.exec())
+ {
+ if (!fd.selectedFiles().isEmpty())
+ {
+ QString file = fd.selectedFiles().at(0);
+ if (!file.isEmpty())
+ {
+ dynamic_cast<QLineEdit *>(this->geomFilenameLineEdit)->setText(file);
+ }
+ }
}
}
}
+
+/////////////////////////////////////////////////
+void EnumConfigWidget::EnumChanged(const QString &_value)
+{
+ emit EnumValueChanged(_value);
+}
diff --git a/gazebo/gui/ConfigWidget.hh b/gazebo/gui/ConfigWidget.hh
index f0e6680..3b304aa 100644
--- a/gazebo/gui/ConfigWidget.hh
+++ b/gazebo/gui/ConfigWidget.hh
@@ -48,16 +48,22 @@ namespace gazebo
/// \class ConfigChildWidget ConfigWidget.hh
/// \brief A convenience widget that also holds pointers to a list of its
/// child widgets
- class GAZEBO_VISIBLE ConfigChildWidget : public QWidget
+ class GAZEBO_VISIBLE ConfigChildWidget : public QFrame
{
Q_OBJECT
/// \brief Constructor;
public: ConfigChildWidget() : groupWidget(NULL) {}
- /// \brief List of child widgets.
+ /// \brief Widget's key value.
+ public: std::string key;
+
+ /// \brief List of widgets holding values, such as Spins and LineEdits.
public: std::vector<QWidget *> widgets;
+ /// \brief Map a widget to the label holding its unit value.
+ public: std::map<QWidget *, QLabel *> mapWidgetToUnit;
+
/// \brief Pointer to group widget.
/// NULL if this widget is not contained inside a group widget.
public: GroupWidget *groupWidget;
@@ -79,9 +85,39 @@ namespace gazebo
/// \brief A label for the length widget.
public: QWidget *geomLengthLabel;
+ /// \brief A label for the unit of the length widget.
+ public: QWidget *geomLengthUnitLabel;
+
+ /// \brief A line edit for editing the mesh filename.
+ public: QWidget *geomFilenameLineEdit;
+
+ /// \brief A label for the mesh filename widget.
+ public: QWidget *geomFilenameLabel;
+
+ /// \brief A button for selecting the mesh filename.
+ public: QWidget *geomFilenameButton;
+
/// brief Callback when the geometry type is changed.
/// \param[in] _text New geometry type in string.
private slots: void GeometryChanged(const QString _text);
+
+ /// brief Callback when the file button is clicked.
+ private slots: void OnSelectFile();
+ };
+
+ /// \class EnumConfigWidget ConfigWidget.hh
+ /// \brief A widget for configuring enum values.
+ class GAZEBO_VISIBLE EnumConfigWidget : public ConfigChildWidget
+ {
+ Q_OBJECT
+
+ /// brief Signal an enum value change event.
+ /// \param[in] _value New enum value in string.
+ Q_SIGNALS: void EnumValueChanged(const QString &_value);
+
+ /// brief Callback when the enum value is changed.
+ /// \param[in] _value New enum value in string.
+ private slots: void EnumChanged(const QString &_value);
};
/// \class GroupWidget ConfigWidget.hh
@@ -94,7 +130,8 @@ namespace gazebo
public: QWidget *childWidget;
/// \brief Callback that collapses or expands the child widget.
- private slots: void Toggle();
+ /// _param[in] _checked Whether it is checked or not.
+ private slots: void Toggle(bool _checked);
};
/// \class ConfigWidget ConfigWidget.hh
@@ -117,6 +154,29 @@ namespace gazebo
/// \return Updated message.
public: google::protobuf::Message *GetMsg();
+ /// \brief Create a human readable key, capitalizing the first letter
+ /// and removing characters like "_".
+ /// \param[in] _key Non-human-readable key.
+ /// \return Human-redadable key.
+ public: std::string GetHumanReadableKey(const std::string &_key);
+
+ /// \brief Returns the unit for a given key. For example, the key "mass"
+ /// returns "kg".
+ /// \param[in] _key The key.
+ /// \param[in] _jointType In case the field belongs to a joint, the
+ /// joint's type.
+ /// \return The unit.
+ public: std::string GetUnitFromKey(const std::string &_key,
+ const std::string &_jointType = "");
+
+ /// \brief Returns the range for a given key. For example, the key
+ // "transparency" returns min == 0, max == 1.
+ /// \param[in] _key The key.
+ /// \param[out] _min The minimum value.
+ /// \param[out] _max The maximum value.
+ public: void GetRangeFromKey(const std::string &_key,
+ double &_min, double &_max);
+
/// \brief Set whether a child widget should be visible.
/// \param[in] _name Name of the child widget.
/// \param[in] _visible True to set the widget to be visible.
@@ -144,55 +204,71 @@ namespace gazebo
/// \brief Set an integer value to a child widget.
/// \param[in] _name Name of the child widget.
/// \param[in] _value Value to set to.
- public: void SetIntWidgetValue(const std::string &_name, int _value);
+ /// \return True if the value is set successfully.
+ public: bool SetIntWidgetValue(const std::string &_name, int _value);
/// \brief Set an unsigned integer value to a child widget.
/// \param[in] _name Name of the child widget.
/// \param[in] _value Value to set to.
- public: void SetUIntWidgetValue(const std::string &_name, unsigned int
+ /// \return True if the value is set successfully.
+ public: bool SetUIntWidgetValue(const std::string &_name, unsigned int
_value);
/// \brief Set a double value to a child widget.
/// \param[in] _name Name of the child widget.
/// \param[in] _value Value to set to.
- public: void SetDoubleWidgetValue(const std::string &_name,
+ /// \return True if the value is set successfully.
+ public: bool SetDoubleWidgetValue(const std::string &_name,
double _value);
/// \brief Set a bool value to a child widget.
/// \param[in] _name Name of the child widget.
/// \param[in] _value Value to set to.
- public: void SetBoolWidgetValue(const std::string &_name, bool _value);
+ /// \return True if the value is set successfully.
+ public: bool SetBoolWidgetValue(const std::string &_name, bool _value);
/// \brief Set a string value to a child widget.
/// \param[in] _name Name of the child widget.
/// \param[in] _value Value to set to.
- public: void SetStringWidgetValue(const std::string &_name,
+ public: bool SetStringWidgetValue(const std::string &_name,
const std::string &_value);
/// \brief Set a vector3 value to a child widget.
/// \param[in] _name Name of the child widget.
/// \param[in] _value Value to set to.
- public: void SetVector3WidgetValue(const std::string &_name,
+ /// \return True if the value is set successfully.
+ public: bool SetVector3WidgetValue(const std::string &_name,
const math::Vector3 &_value);
/// \brief Set a color value to a child widget.
/// \param[in] _name Name of the child widget.
/// \param[in] _value Value to set to.
- public: void SetColorWidgetValue(const std::string &_name,
+ /// \return True if the value is set successfully.
+ public: bool SetColorWidgetValue(const std::string &_name,
const common::Color &_value);
/// \brief Set a pose value to a child widget.
/// \param[in] _name Name of the child widget.
/// \param[in] _value Value to set to.
- public: void SetPoseWidgetValue(const std::string &_name,
+ /// \return True if the value is set successfully.
+ public: bool SetPoseWidgetValue(const std::string &_name,
const math::Pose &_value);
/// \brief Set a geometry value to a child widget.
/// \param[in] _name Name of the child widget.
/// \param[in] _value Type of geometry.
/// \param[in] _dimensions Dimensions of geometry.
- public: void SetGeometryWidgetValue(const std::string &_name,
- const std::string &_value, const math::Vector3 &_dimensions);
+ /// \return True if the value is set successfully.
+ public: bool SetGeometryWidgetValue(const std::string &_name,
+ const std::string &_value, const math::Vector3 &_dimensions,
+ const std::string &_uri = "");
+
+ /// \brief Set an enum value to a child widget.
+ /// \param[in] _name Name of the child widget.
+ /// \param[in] _value Value to set to.
+ /// \return True if the value is set successfully.
+ public: bool SetEnumWidgetValue(const std::string &_name,
+ const std::string &_value);
/// \brief Get an integer value from a child widget.
/// \param[in] _name Name of the child widget.
@@ -240,16 +316,25 @@ namespace gazebo
/// \param[out] _dimensions Dimensions of geometry.
/// \return Type of geometry.
public: std::string GetGeometryWidgetValue(const std::string &_name,
- math::Vector3 &_dimensions) const;
+ math::Vector3 &_dimensions, std::string &_uri) const;
+
+ /// \brief Get an enum value from a child widget.
+ /// \param[in] _name Name of the child widget.
+ /// \return Enum value.
+ public: std::string GetEnumWidgetValue(const std::string &_name) const;
/// \brief Parse the input message and either create widgets for
/// configuring fields of the message, or update the widgets with values
/// from the message.
/// \param[in] _msg Message.
+ /// \param[in] _update True to parse only fields that are specified in
+ /// the message rather than all the available fields in the message
/// \param[in] _name Name used when creating new widgets.
+ /// \param[in] _level Level of the widget in the tree.
/// return Updated widget.
private: QWidget *Parse(google::protobuf::Message *_msg,
- const std::string &_name = "");
+ bool _update = false, const std::string &_name = "",
+ const int _level = 0);
/// \brief Parse a vector3 message.
/// param[in] _msg Input vector3d message.
@@ -271,101 +356,145 @@ namespace gazebo
/// \brief Create a widget for configuring an unsigned integer value.
/// \param[in] _key A key that is used as a label for the widget.
+ /// \param[in] _level Level of the widget in the tree.
/// \return The newly created widget.
- private: ConfigChildWidget *CreateUIntWidget(const std::string &_key);
+ private: ConfigChildWidget *CreateUIntWidget(const std::string &_key,
+ const int _level = 0);
/// \brief Create a widget for configuring an integer value.
/// \param[in] _key A key that is used as a label for the widget.
+ /// \param[in] _level Level of the widget in the tree.
/// \return The newly created widget.
- private: ConfigChildWidget *CreateIntWidget(const std::string &_key);
+ private: ConfigChildWidget *CreateIntWidget(const std::string &_key,
+ const int _level = 0);
/// \brief Create a widget for configuring a double value.
/// \param[in] _key A key that is used as a label for the widget.
+ /// \param[in] _level Level of the widget in the tree.
/// \return The newly created widget.
- private: ConfigChildWidget *CreateDoubleWidget(const std::string &_key);
+ private: ConfigChildWidget *CreateDoubleWidget(const std::string &_key,
+ const int _level = 0);
/// \brief Create a widget for configuring a string value.
/// \param[in] _key A key that is used as a label for the widget.
+ /// \param[in] _level Level of the widget in the tree.
/// \return The newly created widget.
- private: ConfigChildWidget *CreateStringWidget(const std::string &_key);
+ private: ConfigChildWidget *CreateStringWidget(const std::string &_key,
+ const int _level = 0);
/// \brief Create a widget for configuring a bool value.
/// \param[in] _key A key that is used as a label for the widget.
+ /// \param[in] _level Level of the widget in the tree.
/// \return The newly created widget.
- private: ConfigChildWidget *CreateBoolWidget(const std::string &_key);
+ private: ConfigChildWidget *CreateBoolWidget(const std::string &_key,
+ const int _level = 0);
/// \brief Create a widget for configuring a vector3 value.
/// \param[in] _key A key that is used as a label for the widget.
+ /// \param[in] _level Level of the widget in the tree.
/// \return The newly created widget.
- private: ConfigChildWidget *CreateVector3dWidget(const std::string &_key);
+ private: ConfigChildWidget *CreateVector3dWidget(const std::string &_key,
+ const int _level = 0);
/// \brief Create a widget for configuring a color value.
/// \param[in] _key A key that is used as a label for the widget.
+ /// \param[in] _level Level of the widget in the tree.
/// \return The newly created widget.
- private: ConfigChildWidget *CreateColorWidget(const std::string &_key);
+ private: ConfigChildWidget *CreateColorWidget(const std::string &_key,
+ const int _level = 0);
/// \brief Create a widget for configuring a pose value.
/// \param[in] _key A key that is used as a label for the widget.
+ /// \param[in] _level Level of the widget in the tree.
/// \return The newly created widget.
- private: ConfigChildWidget *CreatePoseWidget(const std::string &_key);
+ private: ConfigChildWidget *CreatePoseWidget(const std::string &_key,
+ const int _level = 0);
/// \brief Create a widget for configuring a geometry value.
/// \param[in] _key A key that is used as a label for the widget.
+ /// \param[in] _level Level of the widget in the tree.
/// \return The newly created widget.
- private: ConfigChildWidget *CreateGeometryWidget(const std::string &_key);
+ private: ConfigChildWidget *CreateGeometryWidget(const std::string &_key,
+ const int _level = 0);
+
+ /// \brief Create a widget for configuring an enum value.
+ /// \param[in] _key A key that is used as a label for the widget.
+ /// \param[in] _values A list of enum values in string.
+ /// \param[in] _level Level of the widget in the tree.
+ /// \return The newly created widget.
+ private: ConfigChildWidget *CreateEnumWidget(const std::string &_key,
+ const std::vector<std::string> &_values, const int _level = 0);
/// \brief Update a child widget with an unsigned integer value.
/// \param[in] _widget Pointer to the child widget.
/// \param[in] _value Value to set to.
- private: void UpdateUIntWidget(ConfigChildWidget *_widget,
+ /// \return True if the update completed successfully.
+ private: bool UpdateUIntWidget(ConfigChildWidget *_widget,
unsigned int _value);
/// \brief Update a child widget with an integer value.
/// \param[in] _widget Pointer to the child widget.
/// \param[in] _value Value to set to.
- private: void UpdateIntWidget(ConfigChildWidget *_widget, int _value);
+ /// \return True if the update completed successfully.
+ private: bool UpdateIntWidget(ConfigChildWidget *_widget, int _value);
/// \brief Update a child widget with a double value.
/// \param[in] _widget Pointer to the child widget.
/// \param[in] _value Value to set to.
- private: void UpdateDoubleWidget(ConfigChildWidget *_widget,
+ /// \return True if the update completed successfully.
+ private: bool UpdateDoubleWidget(ConfigChildWidget *_widget,
double _value);
/// \brief Update a child widget with a string value.
/// \param[in] _widget Pointer to the child widget.
/// \param[in] _value Value to set to.
- private: void UpdateStringWidget(ConfigChildWidget *_widget,
+ /// \return True if the update completed successfully.
+ private: bool UpdateStringWidget(ConfigChildWidget *_widget,
const std::string &_value);
/// \brief Update a child widget with a bool value.
/// \param[in] _widget Pointer to the child widget.
/// \param[in] _value Value to set to.
- private: void UpdateBoolWidget(ConfigChildWidget *_widget, bool _value);
+ /// \return True if the update completed successfully.
+ private: bool UpdateBoolWidget(ConfigChildWidget *_widget, bool _value);
/// \brief Update a child widget with a vector3 value.
/// \param[in] _widget Pointer to the child widget.
/// \param[in] _value Value to set to.
- private: void UpdateVector3Widget(ConfigChildWidget *_widget,
+ /// \return True if the update completed successfully.
+ private: bool UpdateVector3Widget(ConfigChildWidget *_widget,
const math::Vector3 &_value);
/// \brief Update a child widget with a color value.
/// \param[in] _widget Pointer to the child widget.
/// \param[in] _value Value to set to.
- private: void UpdateColorWidget(ConfigChildWidget *_widget,
+ /// \return True if the update completed successfully.
+ private: bool UpdateColorWidget(ConfigChildWidget *_widget,
const common::Color &_value);
/// \brief Update a child widget with a pose value.
/// \param[in] _widget Pointer to the child widget.
/// \param[in] _value Value to set to.
- private: void UpdatePoseWidget(ConfigChildWidget *_widget,
+ /// \return True if the update completed successfully.
+ private: bool UpdatePoseWidget(ConfigChildWidget *_widget,
const math::Pose &_value);
/// \brief Update a child widget with a geometry type and dimensions.
/// \param[in] _widget Pointer to the child widget.
/// \param[in] _value Type of geometry.
/// \param[in] _dimensions Dimensions of the geometry.
- private: void UpdateGeometryWidget(ConfigChildWidget *_widget,
- const std::string &_value, const math::Vector3 &_dimensions);
+ /// \param[in] _uri URI of the geometry mesh, if any.
+ /// \return True if the update completed successfully.
+ private: bool UpdateGeometryWidget(ConfigChildWidget *_widget,
+ const std::string &_value, const math::Vector3 &_dimensions,
+ const std::string &_uri = "");
+
+ /// \brief Update a child widget with an enum value.
+ /// \param[in] _widget Pointer to the child widget.
+ /// \param[in] _value Value to set to.
+ /// \return True if the update completed successfully.
+ private: bool UpdateEnumWidget(ConfigChildWidget *_widget,
+ const std::string &_value);
/// \brief Get an integer value from a child widget.
/// \param[in] _widget Pointer to the child widget.
@@ -414,20 +543,75 @@ namespace gazebo
/// \brief Get a geometry value from a child widget.
/// \param[in] _widget Pointer to the child widget.
/// \param[out] _dimensions Dimensions of geometry.
+ /// \param[out] _uri URI of the geometry mesh, if any.
/// \return Type of geometry.
private: std::string GetGeometryWidgetValue(ConfigChildWidget *_widget,
- math::Vector3 &_dimensions) const;
+ math::Vector3 &_dimensions, std::string &_uri) const;
+
+ /// \brief Get an enum value from a child widget.
+ /// \param[in] _widget Pointer to the child widget.
+ /// \return Value of the widget.
+ private: std::string GetEnumWidgetValue(ConfigChildWidget *_widget) const;
/// \brief Received item selection user input.
/// \param[in] _item Item selected.
/// \param[in] _column Column index.
private slots: void OnItemSelection(QTreeWidgetItem *_item, int _column);
+ /// \brief Callback when an enum config widget's enum value has changed.
+ /// \param[in] _value New enum value in string.
+ private slots: void OnEnumValueChanged(const QString &_value);
+
+ /// \brief Signal that an enum config widget's enum value has changed.
+ /// \param[in] _name Scoped name of widget.
+ /// \param[in] _name New enum value string.
+ Q_SIGNALS: void EnumValueChanged(const QString &_name,
+ const QString &_value);
+
+ /// \brief Qt event filter currently used to filter mouse wheel events.
+ /// \param[in] _obj Object that is watched by the event filter.
+ /// \param[in] _event Qt event.
+ /// \return True if the event is handled.
+ private: bool eventFilter(QObject *_obj, QEvent *_event);
+
/// \brief A map of unique scoped names to correpsonding widgets.
private: std::map <std::string, ConfigChildWidget *> configWidgets;
/// \brief A copy of the message with fields to be configured by widgets.
private: google::protobuf::Message *configMsg;
+
+ /// \brief Color used for the background of level 0 widgets.
+ private: static const QString level0BgColor;
+
+ /// \brief Color used for the background of level 0 widgets.
+ private: static const QString level1BgColor;
+
+ /// \brief Color used for the background of level 0 widgets.
+ private: static const QString level2BgColor;
+
+ /// \brief Color used for the background of level 0 widgets.
+ private: static const QString level3BgColor;
+
+ /// \brief Color used for the widget areas of of level 0 widgets.
+ private: static const QString level0WidgetColor;
+
+ /// \brief Color used for the widget areas of of level 0 widgets.
+ private: static const QString level1WidgetColor;
+
+ /// \brief Color used for the widget areas of of level 0 widgets.
+ private: static const QString level2WidgetColor;
+
+ /// \brief Color used for the widget areas of of level 0 widgets.
+ private: static const QString level3WidgetColor;
+
+ /// \brief Red color used for "red" or "x" fields.
+ private: static const QString redColor;
+
+ /// \brief Red color used for "green" or "y" fields.
+ private: static const QString greenColor;
+
+ /// \brief Red color used for "blue" or "z" fields.
+ private: static const QString blueColor;
};
}
}
diff --git a/gazebo/gui/ConfigWidget_TEST.cc b/gazebo/gui/ConfigWidget_TEST.cc
index 15a13eb..1359a82 100644
--- a/gazebo/gui/ConfigWidget_TEST.cc
+++ b/gazebo/gui/ConfigWidget_TEST.cc
@@ -50,6 +50,408 @@ void ConfigWidget_TEST::EmptyMsgWidget()
}
/////////////////////////////////////////////////
+void ConfigWidget_TEST::JointMsgWidget()
+{
+ gazebo::gui::ConfigWidget *jointConfigWidget =
+ new gazebo::gui::ConfigWidget;
+ gazebo::msgs::Joint jointMsg;
+
+ {
+ // joint
+ jointMsg.set_name("test_joint");
+ jointMsg.set_id(1122u);
+ jointMsg.set_parent("test_joint_parent");
+ jointMsg.set_parent_id(212121u);
+ jointMsg.set_child("test_joint_child");
+ jointMsg.set_child_id(454545u);
+
+ // type
+ jointMsg.set_type(gazebo::msgs::ConvertJointType("revolute"));
+
+ // pose
+ ignition::math::Vector3d pos(4.0, -1.0, 3.5);
+ ignition::math::Quaterniond quat(0.0, 1.57, 0.0);
+ gazebo::msgs::Set(jointMsg.mutable_pose(),
+ ignition::math::Pose3d(pos, quat));
+
+ // axis1
+ gazebo::msgs::Axis *axisMsg = jointMsg.mutable_axis1();
+ gazebo::msgs::Set(axisMsg->mutable_xyz(), ignition::math::Vector3d::UnitX);
+ axisMsg->set_use_parent_model_frame(false);
+ axisMsg->set_limit_lower(-999.0);
+ axisMsg->set_limit_upper(999.0);
+ axisMsg->set_limit_effort(-1.0);
+ axisMsg->set_limit_velocity(-1.0);
+ axisMsg->set_damping(0.0);
+
+ // other joint physics properties
+ jointMsg.set_cfm(0.2);
+ jointMsg.set_bounce(0.3);
+ jointMsg.set_velocity(0.4);
+ jointMsg.set_fudge_factor(0.5);
+ jointMsg.set_limit_cfm(0.6);
+ jointMsg.set_limit_erp(0.7);
+ jointMsg.set_suspension_cfm(0.8);
+ jointMsg.set_suspension_erp(0.9);
+ }
+ jointConfigWidget->Load(&jointMsg);
+
+ // retrieve the message from the config widget and
+ // verify that all values have not been changed.
+ {
+ gazebo::msgs::Joint *retJointMsg =
+ dynamic_cast<gazebo::msgs::Joint *>(jointConfigWidget->GetMsg());
+ QVERIFY(retJointMsg != NULL);
+
+ // joint
+ QVERIFY(retJointMsg->name() == "test_joint");
+ QCOMPARE(retJointMsg->id(), 1122u);
+ QVERIFY(retJointMsg->parent() == "test_joint_parent");
+ QCOMPARE(retJointMsg->parent_id(), 212121u);
+ QVERIFY(retJointMsg->child() == "test_joint_child");
+ QCOMPARE(retJointMsg->child_id(), 454545u);
+
+ // type
+ QCOMPARE(retJointMsg->type(), gazebo::msgs::ConvertJointType("revolute"));
+
+ // pose
+ const gazebo::msgs::Pose poseMsg = retJointMsg->pose();
+ const gazebo::msgs::Vector3d posMsg = poseMsg.position();
+ QCOMPARE(posMsg.x(), 4.0);
+ QCOMPARE(posMsg.y(), -1.0);
+ QCOMPARE(posMsg.z(), 3.5);
+ const gazebo::msgs::Quaternion quatMsg = poseMsg.orientation();
+ gazebo::math::Quaternion quat(quatMsg.w(), quatMsg.x(), quatMsg.y(),
+ quatMsg.z());
+ QCOMPARE(quat.GetAsEuler().x, 0.0);
+ QCOMPARE(quat.GetAsEuler().y, 1.57);
+ QCOMPARE(quat.GetAsEuler().z, 0.0);
+
+ // axis1
+ gazebo::msgs::Axis *axisMsg = jointMsg.mutable_axis1();
+ QCOMPARE(axisMsg->xyz().x(), 1.0);
+ QCOMPARE(axisMsg->xyz().y(), 0.0);
+ QCOMPARE(axisMsg->xyz().z(), 0.0);
+ QCOMPARE(axisMsg->use_parent_model_frame(), false);
+ QCOMPARE(axisMsg->limit_lower(), -999.0);
+ QCOMPARE(axisMsg->limit_upper(), 999.0);
+ QCOMPARE(axisMsg->limit_effort(), -1.0);
+ QCOMPARE(axisMsg->limit_velocity(), -1.0);
+ QCOMPARE(axisMsg->damping(), 0.0);
+
+ // other joint physics properties
+ QCOMPARE(retJointMsg->cfm(), 0.2);
+ QCOMPARE(retJointMsg->bounce(), 0.3);
+ QCOMPARE(retJointMsg->velocity(), 0.4);
+ QCOMPARE(retJointMsg->fudge_factor(), 0.5);
+ QCOMPARE(retJointMsg->limit_cfm(), 0.6);
+ QCOMPARE(retJointMsg->limit_erp(), 0.7);
+ QCOMPARE(retJointMsg->suspension_cfm(), 0.8);
+ QCOMPARE(retJointMsg->suspension_erp(), 0.9);
+ }
+
+ // update fields in the config widget and
+ // verify that the new message contains the updated values.
+ // Joint type revolute -> universal
+ {
+ // joint
+ jointConfigWidget->SetStringWidgetValue("name", "test_joint_updated");
+ jointConfigWidget->SetUIntWidgetValue("id", 9999999u);
+ jointConfigWidget->SetStringWidgetValue("parent",
+ "test_joint_parent_updated");
+ jointConfigWidget->SetUIntWidgetValue("parent_id", 1u);
+ jointConfigWidget->SetStringWidgetValue("child",
+ "test_joint_child_updated");
+ jointConfigWidget->SetUIntWidgetValue("child_id", 2u);
+
+ // type
+ jointConfigWidget->SetEnumWidgetValue("type",
+ gazebo::msgs::Joint_Type_Name(
+ gazebo::msgs::Joint_Type_UNIVERSAL));
+
+ // pose
+ gazebo::math::Vector3 pos(2.0, 9.0, -4.0);
+ gazebo::math::Quaternion quat(0.0, 0.0, 1.57);
+ jointConfigWidget->SetPoseWidgetValue("pose",
+ gazebo::math::Pose(pos, quat));
+
+ // axis1
+ jointConfigWidget->SetVector3WidgetValue("axis1::xyz",
+ gazebo::math::Vector3::UnitY);
+ jointConfigWidget->SetBoolWidgetValue("axis1::use_parent_model_frame",
+ true);
+ jointConfigWidget->SetDoubleWidgetValue("axis1::limit_lower", -1.2);
+ jointConfigWidget->SetDoubleWidgetValue("axis1::limit_upper", -1.0);
+ jointConfigWidget->SetDoubleWidgetValue("axis1::limit_effort", 1.0);
+ jointConfigWidget->SetDoubleWidgetValue("axis1::limit_velocity", 100.0);
+ jointConfigWidget->SetDoubleWidgetValue("axis1::damping", 0.9);
+
+ // axis2
+ jointConfigWidget->SetVector3WidgetValue("axis2::xyz",
+ gazebo::math::Vector3::UnitZ);
+ jointConfigWidget->SetBoolWidgetValue("axis2::use_parent_model_frame",
+ true);
+ jointConfigWidget->SetDoubleWidgetValue("axis2::limit_lower", -3.2);
+ jointConfigWidget->SetDoubleWidgetValue("axis2::limit_upper", -3.0);
+ jointConfigWidget->SetDoubleWidgetValue("axis2::limit_effort", 3.0);
+ jointConfigWidget->SetDoubleWidgetValue("axis2::limit_velocity", 300.0);
+ jointConfigWidget->SetDoubleWidgetValue("axis2::damping", 3.9);
+
+ // other joint physics properties
+ jointConfigWidget->SetDoubleWidgetValue("cfm", 0.9);
+ jointConfigWidget->SetDoubleWidgetValue("bounce", 0.8);
+ jointConfigWidget->SetDoubleWidgetValue("velocity", 0.7);
+ jointConfigWidget->SetDoubleWidgetValue("fudge_factor", 0.6);
+ jointConfigWidget->SetDoubleWidgetValue("limit_cfm", 0.5);
+ jointConfigWidget->SetDoubleWidgetValue("limit_erp", 0.4);
+ jointConfigWidget->SetDoubleWidgetValue("suspension_cfm", 0.3);
+ jointConfigWidget->SetDoubleWidgetValue("suspension_erp", 0.2);
+ }
+
+ // verify widget values
+ {
+ // joint
+ QVERIFY(jointConfigWidget->GetStringWidgetValue("name") ==
+ "test_joint_updated");
+ QCOMPARE(jointConfigWidget->GetUIntWidgetValue("id"), 9999999u);
+ QVERIFY(jointConfigWidget->GetStringWidgetValue("parent") ==
+ "test_joint_parent_updated");
+ QCOMPARE(jointConfigWidget->GetUIntWidgetValue("parent_id"), 1u);
+ QVERIFY(jointConfigWidget->GetStringWidgetValue("child") ==
+ "test_joint_child_updated");
+ QCOMPARE(jointConfigWidget->GetUIntWidgetValue("child_id"), 2u);
+
+ // type
+ QCOMPARE(jointConfigWidget->GetEnumWidgetValue("type"),
+ gazebo::msgs::Joint_Type_Name(
+ gazebo::msgs::Joint_Type_UNIVERSAL));
+
+ // pose
+ gazebo::math::Vector3 pos(2.0, 9.0, -4.0);
+ gazebo::math::Quaternion quat(0.0, 0.0, 1.57);
+ QCOMPARE(jointConfigWidget->GetPoseWidgetValue("pose"),
+ gazebo::math::Pose(pos, quat));
+
+ // axis1
+ QCOMPARE(jointConfigWidget->GetVector3WidgetValue("axis1::xyz"),
+ gazebo::math::Vector3::UnitY);
+ QCOMPARE(jointConfigWidget->GetBoolWidgetValue(
+ "axis1::use_parent_model_frame"), true);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("axis1::limit_lower"),
+ -1.2);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("axis1::limit_upper"),
+ -1.0);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("axis1::limit_effort"),
+ 1.0);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("axis1::limit_velocity"),
+ 100.0);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("axis1::damping"), 0.9);
+
+ // axis2
+ QCOMPARE(jointConfigWidget->GetVector3WidgetValue("axis2::xyz"),
+ gazebo::math::Vector3::UnitZ);
+ QCOMPARE(jointConfigWidget->GetBoolWidgetValue(
+ "axis1::use_parent_model_frame"), true);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("axis2::limit_lower"),
+ -3.2);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("axis2::limit_upper"),
+ -3.0);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("axis2::limit_effort"),
+ 3.0);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("axis2::limit_velocity"),
+ 300.0);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("axis2::damping"), 3.9);
+
+ // other joint physics properties
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("cfm"), 0.9);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("bounce"), 0.8);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("velocity"), 0.7);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("fudge_factor"), 0.6);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("limit_cfm"), 0.5);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("limit_erp"), 0.4);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("suspension_cfm"), 0.3);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("suspension_erp"), 0.2);
+ }
+
+ // verify updates in new msg
+ {
+ gazebo::msgs::Joint *retJointMsg =
+ dynamic_cast<gazebo::msgs::Joint *>(jointConfigWidget->GetMsg());
+ QVERIFY(retJointMsg != NULL);
+
+ // joint
+ QVERIFY(retJointMsg->name() == "test_joint_updated");
+ QCOMPARE(retJointMsg->id(), 9999999u);
+ QVERIFY(retJointMsg->parent() == "test_joint_parent_updated");
+ QCOMPARE(retJointMsg->parent_id(), 1u);
+ QVERIFY(retJointMsg->child() == "test_joint_child_updated");
+ QCOMPARE(retJointMsg->child_id(), 2u);
+
+ // type
+ QCOMPARE(retJointMsg->type(), gazebo::msgs::ConvertJointType("universal"));
+
+ // pose
+ const gazebo::msgs::Pose poseMsg = retJointMsg->pose();
+ const gazebo::msgs::Vector3d posMsg = poseMsg.position();
+ QCOMPARE(posMsg.x(), 2.0);
+ QCOMPARE(posMsg.y(), 9.0);
+ QCOMPARE(posMsg.z(), -4.0);
+ const gazebo::msgs::Quaternion quatMsg = poseMsg.orientation();
+ gazebo::math::Quaternion quat(quatMsg.w(), quatMsg.x(), quatMsg.y(),
+ quatMsg.z());
+ QCOMPARE(quat.GetAsEuler().x, 0.0);
+ QCOMPARE(quat.GetAsEuler().y, 0.0);
+ QCOMPARE(quat.GetAsEuler().z, 1.57);
+
+ // axis1
+ gazebo::msgs::Axis *axisMsg = retJointMsg->mutable_axis1();
+ QCOMPARE(axisMsg->xyz().x(), 0.0);
+ QCOMPARE(axisMsg->xyz().y(), 1.0);
+ QCOMPARE(axisMsg->xyz().z(), 0.0);
+ QCOMPARE(axisMsg->use_parent_model_frame(), true);
+ QCOMPARE(axisMsg->limit_lower(), -1.2);
+ QCOMPARE(axisMsg->limit_upper(), -1.0);
+ QCOMPARE(axisMsg->limit_effort(), 1.0);
+ QCOMPARE(axisMsg->limit_velocity(), 100.0);
+ QCOMPARE(axisMsg->damping(), 0.9);
+
+ // axis2
+ gazebo::msgs::Axis *axis2Msg = retJointMsg->mutable_axis2();
+ QCOMPARE(axis2Msg->xyz().x(), 0.0);
+ QCOMPARE(axis2Msg->xyz().y(), 0.0);
+ QCOMPARE(axis2Msg->xyz().z(), 1.0);
+ QCOMPARE(axis2Msg->use_parent_model_frame(), true);
+ QCOMPARE(axis2Msg->limit_lower(), -3.2);
+ QCOMPARE(axis2Msg->limit_upper(), -3.0);
+ QCOMPARE(axis2Msg->limit_effort(), 3.0);
+ QCOMPARE(axis2Msg->limit_velocity(), 300.0);
+ QCOMPARE(axis2Msg->damping(), 3.9);
+
+ // other joint physics properties
+ QCOMPARE(retJointMsg->cfm(), 0.9);
+ QCOMPARE(retJointMsg->bounce(), 0.8);
+ QCOMPARE(retJointMsg->velocity(), 0.7);
+ QCOMPARE(retJointMsg->fudge_factor(), 0.6);
+ QCOMPARE(retJointMsg->limit_cfm(), 0.5);
+ QCOMPARE(retJointMsg->limit_erp(), 0.4);
+ QCOMPARE(retJointMsg->suspension_cfm(), 0.3);
+ QCOMPARE(retJointMsg->suspension_erp(), 0.2);
+ }
+
+ // update fields in the config widget and
+ // verify that the new message contains the updated values.
+ // Joint type universal -> ball
+ {
+ // joint
+ jointConfigWidget->SetStringWidgetValue("name", "test_joint_updated2");
+ jointConfigWidget->SetUIntWidgetValue("id", 2222222u);
+ jointConfigWidget->SetStringWidgetValue("parent",
+ "test_joint_parent_updated2");
+ jointConfigWidget->SetUIntWidgetValue("parent_id", 10u);
+ jointConfigWidget->SetStringWidgetValue("child",
+ "test_joint_child_updated2");
+ jointConfigWidget->SetUIntWidgetValue("child_id", 20u);
+
+ // type
+ jointConfigWidget->SetEnumWidgetValue("type",
+ gazebo::msgs::Joint_Type_Name(
+ gazebo::msgs::Joint_Type_BALL));
+
+ // pose
+ gazebo::math::Vector3 pos(-2.0, 1.0, 2.0);
+ gazebo::math::Quaternion quat(0.0, 0.0, 0.0);
+ jointConfigWidget->SetPoseWidgetValue("pose",
+ gazebo::math::Pose(pos, quat));
+
+ // other joint physics properties
+ jointConfigWidget->SetDoubleWidgetValue("cfm", 0.19);
+ jointConfigWidget->SetDoubleWidgetValue("bounce", 0.18);
+ jointConfigWidget->SetDoubleWidgetValue("velocity", 2.7);
+ jointConfigWidget->SetDoubleWidgetValue("fudge_factor", 0.26);
+ jointConfigWidget->SetDoubleWidgetValue("limit_cfm", 0.15);
+ jointConfigWidget->SetDoubleWidgetValue("limit_erp", 0.24);
+ jointConfigWidget->SetDoubleWidgetValue("suspension_cfm", 0.13);
+ jointConfigWidget->SetDoubleWidgetValue("suspension_erp", 0.12);
+ }
+
+ // verify widget values
+ {
+ // joint
+ QVERIFY(jointConfigWidget->GetStringWidgetValue("name") ==
+ "test_joint_updated2");
+ QCOMPARE(jointConfigWidget->GetUIntWidgetValue("id"), 2222222u);
+ QVERIFY(jointConfigWidget->GetStringWidgetValue("parent") ==
+ "test_joint_parent_updated2");
+ QCOMPARE(jointConfigWidget->GetUIntWidgetValue("parent_id"), 10u);
+ QVERIFY(jointConfigWidget->GetStringWidgetValue("child") ==
+ "test_joint_child_updated2");
+ QCOMPARE(jointConfigWidget->GetUIntWidgetValue("child_id"), 20u);
+
+ // type
+ QCOMPARE(jointConfigWidget->GetEnumWidgetValue("type"),
+ gazebo::msgs::Joint_Type_Name(
+ gazebo::msgs::Joint_Type_BALL));
+
+ // pose
+ gazebo::math::Vector3 pos(-2.0, 1.0, 2.0);
+ gazebo::math::Quaternion quat(0.0, 0.0, 0.0);
+ QCOMPARE(jointConfigWidget->GetPoseWidgetValue("pose"),
+ gazebo::math::Pose(pos, quat));
+
+ // other joint physics properties
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("cfm"), 0.19);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("bounce"), 0.18);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("velocity"), 2.7);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("fudge_factor"), 0.26);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("limit_cfm"), 0.15);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("limit_erp"), 0.24);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("suspension_cfm"), 0.13);
+ QCOMPARE(jointConfigWidget->GetDoubleWidgetValue("suspension_erp"), 0.12);
+ }
+
+ // verify updates in new msg
+ {
+ gazebo::msgs::Joint *retJointMsg =
+ dynamic_cast<gazebo::msgs::Joint *>(jointConfigWidget->GetMsg());
+ QVERIFY(retJointMsg != NULL);
+
+ // joint
+ QVERIFY(retJointMsg->name() == "test_joint_updated2");
+ QCOMPARE(retJointMsg->id(), 2222222u);
+ QVERIFY(retJointMsg->parent() == "test_joint_parent_updated2");
+ QCOMPARE(retJointMsg->parent_id(), 10u);
+ QVERIFY(retJointMsg->child() == "test_joint_child_updated2");
+ QCOMPARE(retJointMsg->child_id(), 20u);
+
+ // type
+ QCOMPARE(retJointMsg->type(), gazebo::msgs::ConvertJointType("ball"));
+
+ // pose
+ const gazebo::msgs::Pose poseMsg = retJointMsg->pose();
+ const gazebo::msgs::Vector3d posMsg = poseMsg.position();
+ QCOMPARE(posMsg.x(), -2.0);
+ QCOMPARE(posMsg.y(), 1.0);
+ QCOMPARE(posMsg.z(), 2.0);
+ const gazebo::msgs::Quaternion quatMsg = poseMsg.orientation();
+ gazebo::math::Quaternion quat(quatMsg.w(), quatMsg.x(), quatMsg.y(),
+ quatMsg.z());
+ QCOMPARE(quat.GetAsEuler().x, 0.0);
+ QCOMPARE(quat.GetAsEuler().y, 0.0);
+ QCOMPARE(quat.GetAsEuler().z, 0.0);
+
+ // other joint physics properties
+ QCOMPARE(retJointMsg->cfm(), 0.19);
+ QCOMPARE(retJointMsg->bounce(), 0.18);
+ QCOMPARE(retJointMsg->velocity(), 2.7);
+ QCOMPARE(retJointMsg->fudge_factor(), 0.26);
+ QCOMPARE(retJointMsg->limit_cfm(), 0.15);
+ QCOMPARE(retJointMsg->limit_erp(), 0.24);
+ QCOMPARE(retJointMsg->suspension_cfm(), 0.13);
+ QCOMPARE(retJointMsg->suspension_erp(), 0.12);
+ }
+}
+
+/////////////////////////////////////////////////
void ConfigWidget_TEST::VisualMsgWidget()
{
// create a visual message with test values
@@ -71,12 +473,13 @@ void ConfigWidget_TEST::VisualMsgWidget()
visualMsg.set_delete_me(false);
visualMsg.set_is_static(false);
gazebo::msgs::Set(visualMsg.mutable_scale(),
- gazebo::math::Vector3(1.0, 1.0, 1.0));
+ ignition::math::Vector3d(1.0, 1.0, 1.0));
// pose
- gazebo::math::Vector3 pos(2.0, 3.0, 4.0);
- gazebo::math::Quaternion quat(1.57, 0.0, 0.0);
- gazebo::msgs::Set(visualMsg.mutable_pose(), gazebo::math::Pose(pos, quat));
+ ignition::math::Vector3d pos(2.0, 3.0, 4.0);
+ ignition::math::Quaterniond quat(1.57, 0.0, 0.0);
+ gazebo::msgs::Set(visualMsg.mutable_pose(),
+ ignition::math::Pose3d(pos, quat));
// geometry
gazebo::msgs::Geometry *geometryMsg = visualMsg.mutable_geometry();
@@ -251,8 +654,9 @@ void ConfigWidget_TEST::VisualMsgWidget()
// geometry
gazebo::math::Vector3 dimensions;
- QVERIFY(visualConfigWidget->GetGeometryWidgetValue("geometry", dimensions)
- == "box");
+ std::string uri;
+ QVERIFY(visualConfigWidget->GetGeometryWidgetValue("geometry", dimensions,
+ uri) == "box");
QCOMPARE(dimensions, gazebo::math::Vector3(5.0, 3.0, 4.0));
// material
@@ -364,9 +768,10 @@ void ConfigWidget_TEST::ConfigWidgetVisible()
visualMsg.set_id(12345u);
// pose
- gazebo::math::Vector3 pos(2.0, 3.0, 4.0);
- gazebo::math::Quaternion quat(1.57, 0.0, 0.0);
- gazebo::msgs::Set(visualMsg.mutable_pose(), gazebo::math::Pose(pos, quat));
+ ignition::math::Vector3d pos(2.0, 3.0, 4.0);
+ ignition::math::Quaterniond quat(1.57, 0.0, 0.0);
+ gazebo::msgs::Set(visualMsg.mutable_pose(),
+ ignition::math::Pose3d(pos, quat));
// geometry
gazebo::msgs::Geometry *geometryMsg = visualMsg.mutable_geometry();
@@ -447,9 +852,10 @@ void ConfigWidget_TEST::ConfigWidgetReadOnly()
visualMsg.set_id(12345u);
// pose
- gazebo::math::Vector3 pos(2.0, 3.0, 4.0);
- gazebo::math::Quaternion quat(1.57, 0.0, 0.0);
- gazebo::msgs::Set(visualMsg.mutable_pose(), gazebo::math::Pose(pos, quat));
+ ignition::math::Vector3d pos(2.0, 3.0, 4.0);
+ ignition::math::Quaterniond quat(1.57, 0.0, 0.0);
+ gazebo::msgs::Set(visualMsg.mutable_pose(),
+ ignition::math::Pose3d(pos, quat));
// geometry
gazebo::msgs::Geometry *geometryMsg = visualMsg.mutable_geometry();
diff --git a/gazebo/gui/ConfigWidget_TEST.hh b/gazebo/gui/ConfigWidget_TEST.hh
index 509b592..948f16d 100644
--- a/gazebo/gui/ConfigWidget_TEST.hh
+++ b/gazebo/gui/ConfigWidget_TEST.hh
@@ -28,6 +28,9 @@ class ConfigWidget_TEST : public QTestFixture
/// \brief Test creating config widgets from empty messages.
private slots: void EmptyMsgWidget();
+ /// \brief Test creating a config widget from a joint message.
+ private slots: void JointMsgWidget();
+
/// \brief Test creating a config widget from a visual message.
private slots: void VisualMsgWidget();
diff --git a/gazebo/gui/CylinderMaker.cc b/gazebo/gui/CylinderMaker.cc
index 3f19e07..85d3422 100644
--- a/gazebo/gui/CylinderMaker.cc
+++ b/gazebo/gui/CylinderMaker.cc
@@ -14,6 +14,13 @@
* 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 <sstream>
#include "gazebo/msgs/msgs.hh"
@@ -46,7 +53,7 @@ CylinderMaker::CylinderMaker()
this->visualMsg->mutable_material()->mutable_script()->set_name(
"Gazebo/TurquoiseGlowOutline");
msgs::Set(this->visualMsg->mutable_pose()->mutable_orientation(),
- math::Quaternion());
+ ignition::math::Quaterniond());
}
CylinderMaker::~CylinderMaker()
@@ -86,7 +93,7 @@ void CylinderMaker::OnMousePush(const common::MouseEvent &_event)
if (this->state == 0)
return;
- this->mousePushPos = _event.pressPos;
+ this->mousePushPos = _event.PressPos();
}
void CylinderMaker::OnMouseRelease(const common::MouseEvent &_event)
@@ -95,7 +102,7 @@ void CylinderMaker::OnMouseRelease(const common::MouseEvent &_event)
return;
this->state++;
- this->mouseReleasePos = _event.pos;
+ this->mouseReleasePos = _event.Pos();
if (this->state == 3)
{
@@ -119,15 +126,15 @@ void CylinderMaker::OnMouseMove(const common::MouseEvent &_event)
this->visualMsg->pose().position().y(),
this->visualMsg->pose().position().z());
- double size = (this->mouseReleasePos.y - _event.pos.y) * 0.01;
- if (!_event.shift)
+ double size = (this->mouseReleasePos.y - _event.Pos().Y()) * 0.01;
+ if (!_event.Shift())
size = rint(size);
this->visualMsg->mutable_geometry()->mutable_cylinder()->set_length(size);
p.z = size / 2.0;
- msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p);
+ msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p.Ign());
this->visPub->Publish(*this->visualMsg);
}
@@ -153,7 +160,7 @@ void CylinderMaker::OnMouseDrag(const common::MouseEvent &_event)
p1.Round();
if (!this->camera->GetWorldPointOnPlane(
- _event.pos.x, _event.pos.y, math::Plane(norm), p2))
+ _event.Pos().X(), _event.Pos().Y(), math::Plane(norm), p2))
{
gzerr << "Invalid mouse point\n";
return;
@@ -162,7 +169,7 @@ void CylinderMaker::OnMouseDrag(const common::MouseEvent &_event)
p2 = this->GetSnappedPoint(p2);
if (this->state == 1)
- msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p1);
+ msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p1.Ign());
math::Vector3 p(this->visualMsg->pose().position().x(),
this->visualMsg->pose().position().y(),
@@ -179,48 +186,26 @@ void CylinderMaker::OnMouseDrag(const common::MouseEvent &_event)
this->visualMsg->mutable_geometry()->mutable_cylinder()->set_length(0.01);
}
- msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p);
+ msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p.Ign());
this->visPub->Publish(*this->visualMsg);
}
/////////////////////////////////////////////////
std::string CylinderMaker::GetSDFString()
{
- std::ostringstream newModelStr;
-
- newModelStr
- << "<sdf version ='" << SDF_VERSION << "'>"
- << " <model name ='unit_cylinder_" << counter << "'>"
- << " <pose>0 0 0.5 0 0 0</pose>"
- << " <link name='link'>"
- << " <inertial><mass>1.0</mass></inertial>"
- << " <collision name='collision'>"
- << " <geometry>"
- << " <cylinder>"
- << " <radius>0.5</radius>"
- << " <length>1.0</length>"
- << " </cylinder>"
- << " </geometry>"
- << " </collision>"
- << " <visual name='visual'>"
- << " <geometry>"
- << " <cylinder>"
- << " <radius>0.5</radius>"
- << " <length>1.0</length>"
- << " </cylinder>"
- << " </geometry>"
- << " <material>"
- << " <script>"
- << " <uri>file://media/materials/scripts/gazebo.material</uri>"
- << " <name>Gazebo/Grey</name>"
- << " </script>"
- << " </material>"
- << " </visual>"
- << " </link>"
- << " </model>"
- << "</sdf>";
-
- return newModelStr.str();
+ msgs::Model model;
+ {
+ std::ostringstream modelName;
+ modelName << "unit_cylinder_" << counter;
+ model.set_name(modelName.str());
+ }
+ msgs::Set(model.mutable_pose(), ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0));
+ msgs::AddCylinderLink(model, 1.0, 0.5, 1.0);
+ model.mutable_link(0)->set_name("link");
+
+ return "<sdf version='" + std::string(SDF_VERSION) + "'>"
+ + msgs::ModelToSDF(model)->ToString("")
+ + "</sdf>";
}
/////////////////////////////////////////////////
diff --git a/gazebo/gui/DataLogger.cc b/gazebo/gui/DataLogger.cc
index a97b6be..853f351 100644
--- a/gazebo/gui/DataLogger.cc
+++ b/gazebo/gui/DataLogger.cc
@@ -15,9 +15,20 @@
*
*/
+#ifdef _WIN32
+ // Ensure that Winsock2.h is included before Windows.h, which can get
+ // pulled in by anybody (e.g., Boost).
+ #include <Winsock2.h>
+ // For _access()
+ #include <io.h>
+#endif
+
#include <boost/filesystem.hpp>
#include <stdio.h>
+#include "gazebo/common/CommonIface.hh"
+#include "gazebo/common/SystemPaths.hh"
+
#include "gazebo/msgs/msgs.hh"
#include "gazebo/transport/transport.hh"
#include "gazebo/gui/DataLogger.hh"
@@ -33,6 +44,8 @@ DataLogger::DataLogger(QWidget *_parent)
this->setObjectName("dataLogger");
this->setWindowIcon(QIcon(":/images/gazebo.svg"));
this->setWindowTitle(tr("Gazebo: Data Logger"));
+ // Tool stays on top of parent window
+ this->setWindowFlags(this->windowFlags() & Qt::Tool);
// The record button allows the user to start and pause data recording
this->recordButton = new QToolButton(this);
@@ -45,17 +58,10 @@ DataLogger::DataLogger(QWidget *_parent)
connect(this->recordButton, SIGNAL(toggled(bool)),
this, SLOT(OnRecord(bool)));
- // Create the status frame, which contains the duration label, size
- // label, and a one word text message.
- // {
- QFrame *statusFrame = new QFrame;
- statusFrame->setFixedWidth(240);
- statusFrame->setObjectName("dataLoggerStatusFrame");
-
// Textual status information
this->statusLabel = new QLabel("Ready");
this->statusLabel->setObjectName("dataLoggerStatusLabel");
- this->statusLabel->setFixedWidth(70);
+ this->statusLabel->setFixedWidth(80);
// Duration of logging
this->timeLabel = new QLabel("00:00:00.000");
@@ -63,144 +69,115 @@ DataLogger::DataLogger(QWidget *_parent)
this->timeLabel->setFixedWidth(90);
// Size of log file
- this->sizeLabel = new QLabel("0.00 B");
+ this->sizeLabel = new QLabel("(0.00 B)");
this->sizeLabel->setObjectName("dataLoggerSizeLabel");
- QHBoxLayout *timeLayout = new QHBoxLayout;
- timeLayout->addStretch(1);
- timeLayout->addWidget(this->statusLabel);
- timeLayout->addSpacing(5);
- timeLayout->addWidget(this->timeLabel);
-
- QHBoxLayout *sizeLayout = new QHBoxLayout;
- sizeLayout->addStretch(1);
- sizeLayout->addWidget(this->sizeLabel);
-
- QVBoxLayout *statusFrameLayout = new QVBoxLayout;
- statusFrameLayout->addLayout(timeLayout);
- statusFrameLayout->addLayout(sizeLayout);
+ // Horizontal separator
+ QFrame *separator = new QFrame();
+ separator->setFrameShape(QFrame::HLine);
+ separator->setLineWidth(1);
+ separator->setFixedHeight(30);
- statusFrame->setLayout(statusFrameLayout);
- // }
-
- // Create the settings frame, where the user can input a save-to location
- // {
- QFrame *settingsMasterFrame = new QFrame;
- settingsMasterFrame->setObjectName("dataLoggerSettingFrame");
+ // Address label
+ QLabel *uriLabel = new QLabel("Address: ");
+ uriLabel->setStyleSheet(
+ "QLabel {color: #aeaeae; font-size: 11px; background: transparent}");
- this->logList = new QTextBrowser(this);
- this->logList->setObjectName("dataLoggerRecordingsList");
+ // Address URI Line Edit
+ this->destURI = new QLineEdit;
+ this->destURI->setReadOnly(true);
+ this->destURI->setObjectName("dataLoggerDestnationURILabel");
+ this->destURI->setStyleSheet(
+ "QLineEdit {color: #aeaeae; font-size: 11px; background: transparent}");
- QVBoxLayout *logListLayout = new QVBoxLayout;
- logListLayout->addWidget(this->logList);
+ // "Save to" label
+ QLabel *pathLabel = new QLabel("Save to: ");
+ pathLabel->setStyleSheet(
+ "QLabel {color: #aeaeae; font-size: 11px; background: transparent}");
- /*QHBoxLayout *filenameLayout = new QHBoxLayout;
- this->filenameEdit = new QLineEdit;
- this->filenameEdit->setText("~/.gazebo/log");
- filenameLayout->addWidget(new QLabel("Save to:"));
- filenameLayout->addWidget(this->filenameEdit);
+ // Destination path Line Edit
+ this->destPath = new QLineEdit();
+ this->destPath->setObjectName("dataLoggerDestnationPathLabel");
+ this->destPath->setMinimumWidth(300);
+ this->destPath->setReadOnly(true);
+ this->destPath->setStyleSheet(
+ "QLineEdit {color: #aeaeae; font-size: 11px; background: transparent}");
- this->browseButton = new QPushButton("Browse");
- this->browseButton->setFixedHeight(23);
- this->browseButton->setFocusPolicy(Qt::NoFocus);
+ // Browser button
+ QPushButton *browseButton = new QPushButton("Browse");
+ browseButton->setFixedWidth(100);
+ browseButton->setFocusPolicy(Qt::NoFocus);
connect(browseButton, SIGNAL(clicked()), this, SLOT(OnBrowse()));
- filenameLayout->addWidget(browseButton);
- */
-
- QHBoxLayout *settingExpandLayout = new QHBoxLayout;
-
- this->settingExpandButton = new QPushButton("Recordings");
- this->settingExpandButton->setObjectName("expandButton");
- this->settingExpandButton->setCheckable(true);
- this->settingExpandButton->setChecked(false);
- this->settingExpandButton->setFocusPolicy(Qt::NoFocus);
- connect(settingExpandButton, SIGNAL(toggled(bool)),
- this, SLOT(OnToggleSettings(bool)));
-
- settingExpandLayout->setContentsMargins(0, 0, 0, 0);
- settingExpandLayout->addWidget(this->settingExpandButton);
- settingExpandLayout->addStretch(1);
+ // Button which toggles recordings
+ QRadioButton *recordingsButton = new QRadioButton();
+ recordingsButton->setChecked(false);
+ recordingsButton->setFocusPolicy(Qt::NoFocus);
+ recordingsButton->setText("Recordings");
+ recordingsButton->setStyleSheet(
+ "QRadioButton {\
+ color: #d0d0d0;\
+ }\
+ QRadioButton::indicator::unchecked {\
+ image: url(:/images/right_arrow.png);\
+ }\
+ QRadioButton::indicator::checked {\
+ image: url(:/images/down_arrow.png);\
+ }");
+ connect(recordingsButton, SIGNAL(toggled(bool)), this,
+ SLOT(OnToggleSettings(bool)));
+
+ // Insert widgets in the top layout
+ QGridLayout *topLayout = new QGridLayout();
+ topLayout->addWidget(this->recordButton, 0, 0, 2, 1);
+ topLayout->addWidget(this->statusLabel, 0, 1, 2, 2);
+ topLayout->addWidget(this->timeLabel, 0, 3);
+ topLayout->addWidget(this->sizeLabel, 1, 3);
+ topLayout->addWidget(separator, 2, 0, 1, 4);
+ topLayout->addWidget(uriLabel, 3, 0);
+ topLayout->addWidget(this->destURI, 3, 1, 1, 3);
+ topLayout->addWidget(pathLabel, 4, 0);
+ topLayout->addWidget(this->destPath, 4, 1, 1, 2);
+ topLayout->addWidget(browseButton, 4, 3);
+ topLayout->addWidget(recordingsButton, 5, 0, 1, 4);
+
+ // Align widgets within layout
+ topLayout->setAlignment(this->statusLabel, Qt::AlignCenter);
+ topLayout->setAlignment(this->timeLabel, Qt::AlignRight);
+ topLayout->setAlignment(this->sizeLabel, Qt::AlignRight | Qt::AlignTop);
+ topLayout->setAlignment(uriLabel, Qt::AlignRight);
+ topLayout->setAlignment(pathLabel, Qt::AlignRight);
+
+ // Put the layout in a widget to be able to control size
+ QWidget *topWidget = new QWidget();
+ topWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ topWidget->setLayout(topLayout);
+
+ // List of recorded logs
+ this->logList = new QTextBrowser(this);
+ this->logList->setObjectName("dataLoggerRecordingsList");
- /// Create the frame that can be hidden by toggling the setting's button
- this->settingsFrame = new QFrame;
+ // Layout to hold the list
QVBoxLayout *settingsLayout = new QVBoxLayout;
settingsLayout->setContentsMargins(2, 2, 2, 2);
- settingsLayout->addLayout(logListLayout);
+ settingsLayout->addWidget(logList);
+
+ // Frame that can be hidden by toggling the expand button
+ this->settingsFrame = new QFrame();
+ this->settingsFrame->setObjectName("dataLoggerSettingFrame");
this->settingsFrame->setLayout(settingsLayout);
+ this->settingsFrame->setSizePolicy(QSizePolicy::Expanding,
+ QSizePolicy::Expanding);
this->settingsFrame->hide();
- QVBoxLayout *settingsMasterFrameLayout = new QVBoxLayout;
- settingsMasterFrameLayout->setContentsMargins(2, 2, 2, 2);
-
- settingsMasterFrameLayout->addLayout(settingExpandLayout);
- settingsMasterFrameLayout->addWidget(this->settingsFrame);
- settingsMasterFrame->setLayout(settingsMasterFrameLayout);
- // }
-
- // Layout to position the record button vertically
- QVBoxLayout *buttonLayout = new QVBoxLayout;
- buttonLayout->setContentsMargins(0, 0, 0, 0);
- buttonLayout->addWidget(this->recordButton);
- buttonLayout->addStretch(4);
-
- // Layout to position the status information vertically
- QVBoxLayout *statusLayout = new QVBoxLayout;
- statusLayout->setContentsMargins(0, 0, 0, 0);
- statusLayout->addWidget(statusFrame);
-
- // Horizontal layout for the record button and status information
- QHBoxLayout *topLayout = new QHBoxLayout;
- topLayout->setContentsMargins(0, 0, 0, 0);
- topLayout->addLayout(buttonLayout);
- topLayout->addStretch(4);
- topLayout->addLayout(statusLayout);
-
- QHBoxLayout *destPathLayout = new QHBoxLayout;
- this->destPath = new QLineEdit;
- this->destPath->setReadOnly(true);
- this->destPath->setObjectName("dataLoggerDestnationPathLabel");
- this->destPath->setStyleSheet(
- "QLineEdit {color: #aeaeae; font-size: 11px; "
- "background-color: transparent;}");
-
- QLabel *pathLabel = new QLabel("Path: ");
- pathLabel->setStyleSheet(
- "QLabel {color: #aeaeae; font-size: 11px; background: transparent}");
-
- destPathLayout->setContentsMargins(0, 0, 0, 0);
- destPathLayout->addSpacing(4);
- destPathLayout->addWidget(pathLabel);
- destPathLayout->addWidget(this->destPath);
-
- QHBoxLayout *destURILayout = new QHBoxLayout;
- this->destURI = new QLineEdit;
- this->destURI->setReadOnly(true);
- this->destURI->setObjectName("dataLoggerDestnationURILabel");
- this->destURI->setStyleSheet(
- "QLineEdit {color: #aeaeae; font-size: 11px; background: transparent}");
- QLabel *uriLabel = new QLabel("Address: ");
- uriLabel->setStyleSheet(
- "QLabel {color: #aeaeae; font-size: 11px; background: transparent}");
- destURILayout->setContentsMargins(0, 0, 0, 0);
- destURILayout->addSpacing(4);
- destURILayout->addWidget(uriLabel);
- destURILayout->addWidget(this->destURI);
-
// Mainlayout for the whole widget
- // Create the main layout for this widget
QVBoxLayout *mainLayout = new QVBoxLayout;
- mainLayout->addLayout(topLayout);
- mainLayout->addLayout(destURILayout);
- mainLayout->addLayout(destPathLayout);
- mainLayout->addWidget(settingsMasterFrame);
-
- // Let the stylesheet handle the margin sizes
- mainLayout->setContentsMargins(2, 2, 2, 2);
+ mainLayout->setSizeConstraint(QLayout::SetMinAndMaxSize);
+ mainLayout->addWidget(topWidget);
+ mainLayout->addWidget(this->settingsFrame);
// Assign the mainlayout to this widget
this->setLayout(mainLayout);
- this->layout()->setSizeConstraint(QLayout::SetFixedSize);
// Create a QueuedConnection to set time. This is used for thread safety.
connect(this, SIGNAL(SetTime(QString)),
@@ -220,6 +197,19 @@ DataLogger::DataLogger(QWidget *_parent)
connect(this, SIGNAL(SetDestinationURI(QString)),
this, SLOT(OnSetDestinationURI(QString)), Qt::QueuedConnection);
+ connect(this, SIGNAL(rejected()), this, SLOT(OnCancel()));
+
+ // Timer used to blink the status label
+ this->statusTimer = new QTimer();
+ connect(this->statusTimer, SIGNAL(timeout()), this, SLOT(OnBlinkStatus()));
+ this->statusTime = 0;
+
+ // Timer used to hide the confirmation dialog
+ this->confirmationDialog = NULL;
+ this->confirmationTimer = new QTimer(this);
+ connect(this->confirmationTimer, SIGNAL(timeout()), this,
+ SLOT(OnConfirmationTimeout()));
+
// Create a node from communication.
this->node = transport::NodePtr(new transport::Node());
this->node->Init();
@@ -232,6 +222,29 @@ DataLogger::DataLogger(QWidget *_parent)
// messages.
this->sub = this->node->Subscribe<msgs::LogStatus>("~/log/status",
&DataLogger::OnStatus, this);
+
+ // Fill the path with the home folder - duplicated from util/LogRecord
+#ifndef _WIN32
+ const char *homePath = common::getEnv("HOME");
+#else
+ const char *homePath = common::getEnv("HOMEPATH");
+#endif
+
+ GZ_ASSERT(homePath, "HOME environment variable is missing");
+
+ if (!homePath)
+ {
+ common::SystemPaths *paths = common::SystemPaths::Instance();
+ this->basePath = QString::fromStdString(paths->GetTmpPath() + "/gazebo");
+ }
+ else
+ {
+ this->basePath =
+ QString::fromStdString(boost::filesystem::path(homePath).string());
+ }
+
+ this->basePath = this->basePath + "/.gazebo/log/";
+ this->SetDestinationPath(this->basePath);
}
/////////////////////////////////////////////////
@@ -248,7 +261,8 @@ void DataLogger::OnRecord(bool _toggle)
// Switch the icon
this->recordButton->setIcon(QPixmap(":/images/record_stop.png"));
- this->statusLabel->setText("Recording");
+ this->statusLabel->setText("Recording...");
+ this->statusTimer->start(100);
// Tell the server to start data logging
msgs::LogControl msg;
@@ -261,7 +275,35 @@ void DataLogger::OnRecord(bool _toggle)
// Switch the icon
this->recordButton->setIcon(QPixmap(":/images/record.png"));
+ // Display confirmation
+ this->confirmationTimer->start(2000);
+ QLabel *confirmationLabel = new QLabel("Saved to \n" +
+ this->destPath->text());
+ confirmationLabel->setObjectName("dataLoggerConfirmationLabel");
+ QHBoxLayout *confirmationLayout = new QHBoxLayout();
+ confirmationLayout->addWidget(confirmationLabel);
+
+ if (this->confirmationDialog)
+ this->confirmationDialog->close();
+ this->confirmationDialog = new QDialog(this, Qt::FramelessWindowHint);
+ this->confirmationDialog->setObjectName("dataLoggerConfirmationDialog");
+ this->confirmationDialog->setLayout(confirmationLayout);
+ this->confirmationDialog->setStyleSheet(
+ "QDialog {background-color: #eee}\
+ QLabel {color: #111}");
+ this->confirmationDialog->setModal(false);
+ this->confirmationDialog->show();
+ this->confirmationDialog->move(this->mapToGlobal(
+ QPoint((this->width()-this->confirmationDialog->width())*0.5,
+ this->height() + 10)));
+
+ // Change the status
this->statusLabel->setText("Ready");
+ this->statusLabel->setStyleSheet("QLabel{color: #aeaeae}");
+ this->statusTimer->stop();
+
+ // Change the Save to box
+ this->SetDestinationPath(this->basePath);
// Tell the server to stop data logging
msgs::LogControl msg;
@@ -300,18 +342,17 @@ void DataLogger::OnStatus(ConstLogStatusPtr &_msg)
// If there is log file information in the message...
if (_msg->has_log_file())
{
- // If there is file name information...
- if (_msg->log_file().has_base_path())
+ // If there is file name information and we're recording...
+ if (_msg->log_file().has_base_path() && this->recordButton->isChecked())
{
- std::string basePath = _msg->log_file().base_path();
+ std::string logBasePath = _msg->log_file().base_path();
- // Display the leaf log filename
- if (_msg->log_file().has_full_path() && !basePath.empty())
+ // Display the log path
+ if (_msg->log_file().has_full_path() && !logBasePath.empty())
{
- std::string leaf = _msg->log_file().full_path();
- if (!leaf.empty())
- leaf = leaf.substr(basePath.size());
- this->SetDestinationPath(QString::fromStdString(leaf));
+ std::string fullPath = _msg->log_file().full_path();
+ if (!fullPath.empty())
+ this->SetDestinationPath(QString::fromStdString(fullPath));
}
}
@@ -325,30 +366,30 @@ void DataLogger::OnStatus(ConstLogStatusPtr &_msg)
if (_msg->log_file().has_size() && _msg->log_file().has_size_units())
{
// Get the size of the log file.
- stream << std::fixed << std::setprecision(2) << _msg->log_file().size();
-
+ stream << std::fixed << std::setprecision(2) << "(" <<
+ _msg->log_file().size();
// Get the size units.
switch (_msg->log_file().size_units())
{
case msgs::LogStatus::LogFile::BYTES:
- stream << "B";
+ stream << "B)";
break;
case msgs::LogStatus::LogFile::K_BYTES:
- stream << "KB";
+ stream << "KB)";
break;
case msgs::LogStatus::LogFile::M_BYTES:
- stream << "MB";
+ stream << "MB)";
break;
default:
- stream << "GB";
+ stream << "GB)";
break;
}
this->SetSize(QString::fromStdString(stream.str()));
}
else
- this->SetSize("0.00 B");
+ this->SetSize("(0.00 B)");
}
}
@@ -357,8 +398,6 @@ void DataLogger::OnSetDestinationPath(QString _filename)
{
if (!_filename.isEmpty())
this->destPath->setText(_filename);
- else
- this->destPath->setText("");
}
/////////////////////////////////////////////////
@@ -382,10 +421,20 @@ void DataLogger::OnToggleSettings(bool _checked)
/////////////////////////////////////////////////
void DataLogger::OnBrowse()
{
- boost::filesystem::path path = QFileDialog::getExistingDirectory(this,
- tr("Set log directory"), this->filenameEdit->text(),
- QFileDialog::ShowDirsOnly |
- QFileDialog::DontResolveSymlinks).toStdString();
+ QFileDialog fileDialog(this, tr("Set log directory"), QDir::homePath());
+ fileDialog.setFileMode(QFileDialog::Directory);
+ fileDialog.setFilter(QDir::AllDirs | QDir::Hidden);
+ fileDialog.setOptions(QFileDialog::ShowDirsOnly
+ | QFileDialog::DontResolveSymlinks);
+
+ if (fileDialog.exec() != QDialog::Accepted)
+ return;
+
+ QStringList selected = fileDialog.selectedFiles();
+ if (selected.empty())
+ return;
+
+ boost::filesystem::path path = selected[0].toStdString();
// Make sure the directory exists
if (!boost::filesystem::exists(path))
@@ -412,7 +461,13 @@ void DataLogger::OnBrowse()
// Make sure the path is writable.
// Note: This is not cross-platform compatible.
+#ifdef _WIN32
+ // Check for write-only (2) and read-write (6)
+ if ((_access(path.string().c_str(), 2) != 0) &&
+ (_access(path.string().c_str(), 6) != 0))
+#else
if (access(path.string().c_str(), W_OK) != 0)
+#endif
{
QMessageBox msgBox(this);
std::ostringstream stream;
@@ -427,5 +482,36 @@ void DataLogger::OnBrowse()
msg.set_base_path(path.string());
this->pub->Publish(msg);
- this->SetFilename(QString::fromStdString(path.string()));
+ this->basePath = QString::fromStdString(path.string());
+ this->SetDestinationPath(this->basePath);
+}
+
+/////////////////////////////////////////////////
+void DataLogger::OnBlinkStatus()
+{
+ this->statusTime += 1.0/10;
+
+ if (this->statusTime >= 1)
+ this->statusTime = 0;
+
+ this->statusLabel->setStyleSheet(QString::fromStdString(
+ "QLabel{color: rgb("+
+ std::to_string(255+(128*(this->statusTime-1)))+", "+
+ std::to_string(255+(128*(this->statusTime-1)))+", "+
+ std::to_string(255+(128*(this->statusTime-1)))+
+ ")}"));
+}
+
+/////////////////////////////////////////////////
+void DataLogger::OnConfirmationTimeout()
+{
+ this->confirmationDialog->close();
+ this->confirmationTimer->stop();
+}
+
+/////////////////////////////////////////////////
+void DataLogger::OnCancel()
+{
+ if (this->recordButton->isChecked())
+ this->recordButton->click();
}
diff --git a/gazebo/gui/DataLogger.hh b/gazebo/gui/DataLogger.hh
index 9fdac22..db7feb9 100644
--- a/gazebo/gui/DataLogger.hh
+++ b/gazebo/gui/DataLogger.hh
@@ -50,10 +50,6 @@ namespace gazebo
/// \param[in] _string String representation of size.
signals: void SetSize(QString _string);
- /// \brief A signal used to set the filename.
- /// \param[in] _string The log filename
- signals: void SetFilename(QString _string);
-
/// \brief A signal used to set the destination path label.
/// \param[in] _string The log destination directory
signals: void SetDestinationPath(QString _string);
@@ -82,6 +78,15 @@ namespace gazebo
/// state.
private slots: void OnToggleSettings(bool _checked);
+ /// \brief QT callback for blinking the status message.
+ private slots: void OnBlinkStatus();
+
+ /// \brief QT callback for timing out the confirmation message.
+ private slots: void OnConfirmationTimeout();
+
+ /// \brief QT callback when the dialog is closed.
+ private slots: void OnCancel();
+
/// \brief Callback for log status messages.
/// \param[in] _msg Log status message.
private: void OnStatus(ConstLogStatusPtr &_msg);
@@ -98,16 +103,13 @@ namespace gazebo
/// \brief The button used to start and pause logging.
private: QToolButton *recordButton;
- /// \brief The button used to show/hide the settings frame.
- private: QPushButton *settingExpandButton;
-
- /// \brief The button used to stop logging.
- private: QToolButton *stopButton;
-
/// \brief Label to display the log time.
private: QLabel *timeLabel;
- /// \brief Label to display the log destination path.
+ /// \brief Log base path.
+ private: QString basePath;
+
+ /// \brief Line edit to display the log destination path.
private: QLineEdit *destPath;
/// \brief Label to display the log destination uri.
@@ -119,17 +121,26 @@ namespace gazebo
/// \brief Label to display status information.
private: QLabel *statusLabel;
+ /// \brief Timer used to blink the status label.
+ private: QTimer *statusTimer;
+
+ /// \brief Keep track of the time the status label blinks.
+ private: double statusTime;
+
/// \brief Name of the log file path
private: QLineEdit *filenameEdit;
/// \brief Frame that holds settings.
private: QFrame *settingsFrame;
- /// \brief Button to browse for a log recording directory
- private: QPushButton *browseButton;
-
// private: QListWidget *logList;
private: QTextBrowser *logList;
+
+ /// \brief Dialog that displays confirmation after saving.
+ private: QDialog *confirmationDialog;
+
+ /// \brief Timer used to timeout confirmation dialog.
+ private: QTimer *confirmationTimer;
};
/// \}
}
diff --git a/gazebo/gui/DataLogger_TEST.cc b/gazebo/gui/DataLogger_TEST.cc
index e78f9cc..3a1f891 100644
--- a/gazebo/gui/DataLogger_TEST.cc
+++ b/gazebo/gui/DataLogger_TEST.cc
@@ -34,22 +34,22 @@ void DataLogger_TEST::RecordButton()
QCoreApplication::processEvents();
// Get the record button
- QToolButton *recordButton = dataLogger->findChild<QToolButton*>(
+ QToolButton *recordButton = dataLogger->findChild<QToolButton *>(
"dataLoggerRecordButton");
// Get the destination label
- QLineEdit *destPathLabel = dataLogger->findChild<QLineEdit*>(
+ QLineEdit *destPathLabel = dataLogger->findChild<QLineEdit *>(
"dataLoggerDestnationPathLabel");
// Get the time label
- QLabel *timeLabel = dataLogger->findChild<QLabel*>("dataLoggerTimeLabel");
+ QLabel *timeLabel = dataLogger->findChild<QLabel *>("dataLoggerTimeLabel");
// Get the status label
QLabel *statusLabel =
- dataLogger->findChild<QLabel*>("dataLoggerStatusLabel");
+ dataLogger->findChild<QLabel *>("dataLoggerStatusLabel");
// Get the size label
- QLabel *sizeLabel = dataLogger->findChild<QLabel*>("dataLoggerSizeLabel");
+ QLabel *sizeLabel = dataLogger->findChild<QLabel *>("dataLoggerSizeLabel");
QVERIFY(recordButton != NULL);
QVERIFY(destPathLabel != NULL);
@@ -57,11 +57,22 @@ void DataLogger_TEST::RecordButton()
QVERIFY(timeLabel != NULL);
QVERIFY(statusLabel != NULL);
+ std::string txt;
+
+ // Make sure the initial size is zero
+ txt = sizeLabel->text().toStdString();
+ QVERIFY(txt == "(0.00 B)");
+
+ // Make sure the initial time is zero
+ txt = timeLabel->text().toStdString();
+ QVERIFY(txt == "00:00:00.000");
+
// Toggle the record button, which starts logging.
recordButton->toggle();
// Wait for a log status return message
- while (destPathLabel->text().toStdString().empty())
+ while (destPathLabel->text().toStdString().find(".log") ==
+ std::string::npos)
{
// The following line tell QT to process its events. This is vital for
// all tests, but it must be run in the main thread.
@@ -69,41 +80,64 @@ void DataLogger_TEST::RecordButton()
gazebo::common::Time::MSleep(100);
}
- std::string txt;
-
// Make sure the destination log file is correct.
txt = destPathLabel->text().toStdString();
- QVERIFY(txt.find("test/state.log") != std::string::npos);
-
- // Make sure the initial size is zero
- txt = sizeLabel->text().toStdString();
- QVERIFY(txt == "0.00 B");
-
- // Make sure the initial time is zero
- txt = timeLabel->text().toStdString();
- QVERIFY(txt == "00:00:00.000");
+ QVERIFY(txt.find("state.log") != std::string::npos);
// Make sure the status label says "Recording"
txt = statusLabel->text().toStdString();
- QVERIFY(txt == "Recording");
-
+ QVERIFY(txt == "Recording...");
// Toggle the record button, which stops logging.
recordButton->toggle();
- // Make sure the initial size is zero
+ // Wait for a log status return message
+ while (destPathLabel->text().toStdString().find(".log") !=
+ std::string::npos)
+ {
+ QCoreApplication::processEvents();
+ gazebo::common::Time::MSleep(100);
+ }
+
+ // Make sure there's no log file (only path)
+ txt = destPathLabel->text().toStdString();
+ QVERIFY(txt.find(".log") == std::string::npos);
+
+ // Make sure size is back to zero
txt = sizeLabel->text().toStdString();
- QVERIFY(txt == "0.00 B");
+ QVERIFY(txt == "(0.00 B)");
- // Make sure the initial time is zero
+ // Make sure time is back to zero
txt = timeLabel->text().toStdString();
QVERIFY(txt == "00:00:00.000");
-
// Make sure the status label says "Ready"
txt = statusLabel->text().toStdString();
QVERIFY(txt == "Ready");
+ // Get the confirmation dialog
+ QDialog *confirmationDialog = dataLogger->findChild<QDialog *>(
+ "dataLoggerConfirmationDialog");
+ QVERIFY(confirmationDialog != NULL);
+ QVERIFY(confirmationDialog->isVisible());
+
+ // Get the confirmation label
+ QLabel *confirmationLabel = confirmationDialog->findChild<QLabel *>(
+ "dataLoggerConfirmationLabel");
+ QVERIFY(confirmationLabel != NULL);
+
+ // Make sure the confirmation label contains the correct file
+ txt = confirmationLabel->text().toStdString();
+ QVERIFY(txt.find("test/state.log"));
+
+ // Make sure the confirmation disappears after 2s
+ for (size_t i = 0; i < 21; ++i)
+ {
+ QCoreApplication::processEvents();
+ gazebo::common::Time::MSleep(100);
+ }
+ QVERIFY(!confirmationDialog->isVisible());
+
dataLogger->hide();
}
}
diff --git a/gazebo/gui/EntityMaker.cc b/gazebo/gui/EntityMaker.cc
index 5441780..18c7664 100644
--- a/gazebo/gui/EntityMaker.cc
+++ b/gazebo/gui/EntityMaker.cc
@@ -14,6 +14,13 @@
* 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 "gazebo/transport/TransportIface.hh"
#include "gazebo/transport/Node.hh"
#include "gazebo/gui/EntityMaker.hh"
diff --git a/gazebo/gui/GLWidget.cc b/gazebo/gui/GLWidget.cc
index 09df11d..edf1a1c 100644
--- a/gazebo/gui/GLWidget.cc
+++ b/gazebo/gui/GLWidget.cc
@@ -14,6 +14,12 @@
* 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 <math.h>
#include "gazebo/common/Assert.hh"
@@ -33,7 +39,6 @@
#include "gazebo/rendering/UserCamera.hh"
#include "gazebo/rendering/OrbitViewController.hh"
#include "gazebo/rendering/FPSViewController.hh"
-#include "gazebo/rendering/SelectionObj.hh"
#include "gazebo/gui/ModelAlign.hh"
#include "gazebo/gui/ModelSnap.hh"
@@ -58,15 +63,15 @@ GLWidget::GLWidget(QWidget *_parent)
{
this->setObjectName("GLWidget");
this->state = "select";
- this->sceneCreated = false;
this->copyEntityName = "";
+ this->modelEditorEnabled = false;
this->setFocusPolicy(Qt::StrongFocus);
this->windowId = -1;
- setAttribute(Qt::WA_OpaquePaintEvent, true);
- setAttribute(Qt::WA_PaintOnScreen, true);
+ this->setAttribute(Qt::WA_OpaquePaintEvent, true);
+ this->setAttribute(Qt::WA_PaintOnScreen, true);
this->renderFrame = new QFrame;
this->renderFrame->setFrameShape(QFrame::NoFrame);
@@ -74,16 +79,13 @@ GLWidget::GLWidget(QWidget *_parent)
QSizePolicy::Expanding);
this->renderFrame->setContentsMargins(0, 0, 0, 0);
this->renderFrame->show();
+
QVBoxLayout *mainLayout = new QVBoxLayout;
mainLayout->addWidget(this->renderFrame);
mainLayout->setContentsMargins(0, 0, 0, 0);
this->setLayout(mainLayout);
this->connections.push_back(
- rendering::Events::ConnectCreateScene(
- boost::bind(&GLWidget::OnCreateScene, this, _1)));
-
- this->connections.push_back(
rendering::Events::ConnectRemoveScene(
boost::bind(&GLWidget::OnRemoveScene, this, _1)));
@@ -126,10 +128,6 @@ GLWidget::GLWidget(QWidget *_parent)
this->factoryPub = this->node->Advertise<msgs::Factory>("~/factory");
- // Subscribes to selection messages.
- this->selectionSub = this->node->Subscribe("~/selection",
- &GLWidget::OnSelectionMsg, this);
-
// Publishes information about user selections.
this->selectionPub =
this->node->Advertise<msgs::Selection>("~/selection");
@@ -157,6 +155,34 @@ GLWidget::GLWidget(QWidget *_parent)
connect(g_editModelAct, SIGNAL(toggled(bool)), this,
SLOT(OnModelEditor(bool)));
+
+ // Connect the ortho action
+ connect(g_cameraOrthoAct, SIGNAL(triggered()), this,
+ SLOT(OnOrtho()));
+
+ // Connect the perspective action
+ connect(g_cameraPerspectiveAct, SIGNAL(triggered()), this,
+ SLOT(OnPerspective()));
+
+ // Create the scene. This must be done in the constructor so that
+ // we can then create a user camera.
+ this->scene = rendering::create_scene(gui::get_world(), true);
+
+ if (!this->scene)
+ {
+ gzerr << "GLWidget could not create a scene. This will likely result "
+ << "in a blank screen.\n";
+ }
+ else
+ {
+ // This will ultimately create a user camera. We need to create a user
+ // camera in the constructor so that communications (such as via the
+ // ~/gui topic) can work properly (see MainWindow::OnGUI).
+ //
+ // All of this means that we must have a GL Context by this point. So,
+ // we have to create a dummy 1x1 window in RenderEngine::Load.
+ this->OnCreateScene(this->scene->GetName());
+ }
}
/////////////////////////////////////////////////
@@ -170,10 +196,17 @@ GLWidget::~GLWidget()
this->connections.clear();
this->node.reset();
this->modelPub.reset();
- this->selectionSub.reset();
this->selectionPub.reset();
+ ModelManipulator::Instance()->Clear();
+ ModelSnap::Instance()->Clear();
+ ModelAlign::Instance()->Clear();
+
+ if (this->userCamera)
+ this->userCamera->Fini();
+
this->userCamera.reset();
+ this->scene.reset();
}
/////////////////////////////////////////////////
@@ -191,21 +224,25 @@ bool GLWidget::eventFilter(QObject * /*_obj*/, QEvent *_event)
/////////////////////////////////////////////////
void GLWidget::showEvent(QShowEvent *_event)
{
+ // These two functions are most applicable for Linux.
QApplication::flush();
+ QApplication::syncX();
- if (this->windowId < 0)
- {
- this->windowId = rendering::RenderEngine::Instance()->GetWindowManager()->
- CreateWindow(this->GetOgreHandle(), this->width(), this->height());
- if (this->userCamera)
- {
- rendering::RenderEngine::Instance()->GetWindowManager()->SetCamera(
- this->windowId, this->userCamera);
- }
- }
+ // Get the window handle in a form that OGRE can use.
+ std::string winHandle = this->GetOgreHandle();
+
+ // Create the OGRE render window
+ this->windowId = rendering::RenderEngine::Instance()->GetWindowManager()->
+ CreateWindow(winHandle, this->width(), this->height());
+ // Attach the user camera to the window
+ rendering::RenderEngine::Instance()->GetWindowManager()->SetCamera(
+ this->windowId, this->userCamera);
+
+ // Let QT continue processing the show event.
QWidget::showEvent(_event);
+ // Grab focus.
this->setFocus();
}
@@ -229,11 +266,6 @@ void GLWidget::moveEvent(QMoveEvent *_e)
/////////////////////////////////////////////////
void GLWidget::paintEvent(QPaintEvent *_e)
{
- // Timing may cause GLWidget to miss the OnCreateScene event. So, we check
- // here to make sure it's handled.
- if (!this->sceneCreated && rendering::get_scene())
- this->OnCreateScene(rendering::get_scene()->GetName());
-
rendering::UserCameraPtr cam = gui::get_active_camera();
if (cam && cam->GetInitialized())
{
@@ -244,6 +276,12 @@ void GLWidget::paintEvent(QPaintEvent *_e)
event::Events::postRender();
}
+ else
+ {
+ event::Events::preRender();
+ }
+
+ this->update();
_e->accept();
}
@@ -251,14 +289,13 @@ void GLWidget::paintEvent(QPaintEvent *_e)
/////////////////////////////////////////////////
void GLWidget::resizeEvent(QResizeEvent *_e)
{
- if (!this->scene)
- return;
-
if (this->windowId >= 0)
{
rendering::RenderEngine::Instance()->GetWindowManager()->Resize(
this->windowId, _e->size().width(), _e->size().height());
- this->userCamera->Resize(_e->size().width(), _e->size().height());
+
+ if (this->userCamera)
+ this->userCamera->Resize(_e->size().width(), _e->size().height());
}
}
@@ -286,7 +323,8 @@ void GLWidget::keyPressEvent(QKeyEvent *_event)
// Trigger a model delete if the Delete key was pressed, and a model
// is currently selected.
- if (_event->key() == Qt::Key_Delete)
+ if (_event->key() == Qt::Key_Delete &&
+ this->selectionLevel == SelectionLevels::MODEL)
{
boost::mutex::scoped_lock lock(this->selectedVisMutex);
while (!this->selectedVisuals.empty())
@@ -323,11 +361,11 @@ void GLWidget::keyPressEvent(QKeyEvent *_event)
this->keyEvent.alt =
this->keyModifiers & Qt::AltModifier ? true : false;
- this->mouseEvent.control = this->keyEvent.control;
- this->mouseEvent.shift = this->keyEvent.shift;
- this->mouseEvent.alt = this->keyEvent.alt;
+ this->mouseEvent.SetControl(this->keyEvent.control);
+ this->mouseEvent.SetShift(this->keyEvent.shift);
+ this->mouseEvent.SetAlt(this->keyEvent.alt);
- if (this->mouseEvent.control)
+ if (this->mouseEvent.Control())
{
if (_event->key() == Qt::Key_C && !this->selectedVisuals.empty()
&& !this->modelEditorEnabled)
@@ -344,6 +382,12 @@ void GLWidget::keyPressEvent(QKeyEvent *_event)
// Process Key Events
if (!KeyEventHandler::Instance()->HandlePress(this->keyEvent))
{
+ // model editor exit pop-up message is modal so can block event propagation.
+ // So using hotkeys to exit will leave the control variable in a bad state.
+ // Manually override and reset the control value.
+ if (this->modelEditorEnabled && this->mouseEvent.Control())
+ this->mouseEvent.SetControl(false);
+
ModelManipulator::Instance()->OnKeyPressEvent(this->keyEvent);
this->userCamera->HandleKeyPressEvent(this->keyText);
}
@@ -389,9 +433,9 @@ void GLWidget::keyReleaseEvent(QKeyEvent *_event)
this->keyEvent.alt =
this->keyModifiers & Qt::AltModifier ? true : false;
- this->mouseEvent.control = this->keyEvent.control;
- this->mouseEvent.shift = this->keyEvent.shift;
- this->mouseEvent.alt = this->keyEvent.alt;
+ this->mouseEvent.SetControl(this->keyEvent.control);
+ this->mouseEvent.SetShift(this->keyEvent.shift);
+ this->mouseEvent.SetAlt(this->keyEvent.alt);
ModelManipulator::Instance()->OnKeyReleaseEvent(this->keyEvent);
this->keyText = "";
@@ -408,28 +452,18 @@ void GLWidget::mouseDoubleClickEvent(QMouseEvent *_event)
if (!this->scene)
return;
- this->mouseEvent.pressPos.Set(_event->pos().x(), _event->pos().y());
- this->mouseEvent.prevPos = this->mouseEvent.pressPos;
+ this->mouseEvent.SetPressPos(_event->pos().x(), _event->pos().y());
+ this->mouseEvent.SetPrevPos(this->mouseEvent.PressPos());
/// Set the button which cause the press event
- if (_event->button() == Qt::LeftButton)
- this->mouseEvent.button = common::MouseEvent::LEFT;
- else if (_event->button() == Qt::RightButton)
- this->mouseEvent.button = common::MouseEvent::RIGHT;
- else if (_event->button() == Qt::MidButton)
- this->mouseEvent.button = common::MouseEvent::MIDDLE;
+ this->SetMouseEventButton(_event->button());
- this->mouseEvent.buttons = common::MouseEvent::NO_BUTTON;
- this->mouseEvent.type = common::MouseEvent::PRESS;
+ this->mouseEvent.SetButtons(common::MouseEvent::NO_BUTTON);
+ this->mouseEvent.SetType(common::MouseEvent::PRESS);
- this->mouseEvent.buttons |= _event->buttons() & Qt::LeftButton ?
- common::MouseEvent::LEFT : 0x0;
- this->mouseEvent.buttons |= _event->buttons() & Qt::RightButton ?
- common::MouseEvent::RIGHT : 0x0;
- this->mouseEvent.buttons |= _event->buttons() & Qt::MidButton ?
- common::MouseEvent::MIDDLE : 0x0;
+ this->SetMouseEventButtons(_event->buttons());
- this->mouseEvent.dragging = false;
+ this->mouseEvent.SetDragging(false);
// Process Mouse Events
MouseEventHandler::Instance()->HandleDoubleClick(this->mouseEvent);
@@ -441,28 +475,18 @@ void GLWidget::mousePressEvent(QMouseEvent *_event)
if (!this->scene)
return;
- this->mouseEvent.pressPos.Set(_event->pos().x(), _event->pos().y());
- this->mouseEvent.prevPos = this->mouseEvent.pressPos;
+ this->mouseEvent.SetPressPos(_event->pos().x(), _event->pos().y());
+ this->mouseEvent.SetPrevPos(this->mouseEvent.PressPos());
/// Set the button which cause the press event
- if (_event->button() == Qt::LeftButton)
- this->mouseEvent.button = common::MouseEvent::LEFT;
- else if (_event->button() == Qt::RightButton)
- this->mouseEvent.button = common::MouseEvent::RIGHT;
- else if (_event->button() == Qt::MidButton)
- this->mouseEvent.button = common::MouseEvent::MIDDLE;
+ this->SetMouseEventButton(_event->button());
- this->mouseEvent.buttons = common::MouseEvent::NO_BUTTON;
- this->mouseEvent.type = common::MouseEvent::PRESS;
+ this->mouseEvent.SetButtons(common::MouseEvent::NO_BUTTON);
+ this->mouseEvent.SetType(common::MouseEvent::PRESS);
- this->mouseEvent.buttons |= _event->buttons() & Qt::LeftButton ?
- common::MouseEvent::LEFT : 0x0;
- this->mouseEvent.buttons |= _event->buttons() & Qt::RightButton ?
- common::MouseEvent::RIGHT : 0x0;
- this->mouseEvent.buttons |= _event->buttons() & Qt::MidButton ?
- common::MouseEvent::MIDDLE : 0x0;
+ this->SetMouseEventButtons(_event->buttons());
- this->mouseEvent.dragging = false;
+ this->mouseEvent.SetDragging(false);
// Process Mouse Events
MouseEventHandler::Instance()->HandlePress(this->mouseEvent);
@@ -489,10 +513,7 @@ bool GLWidget::OnMouseRelease(const common::MouseEvent & /*_event*/)
{
if (this->state == "make_entity")
this->OnMouseReleaseMakeEntity();
- // Auto switch to select mode if control is pressed to allow multi-object
- // selection. Remove this once multi-object manipulation is implemented in
- // RTS modes, issue #213
- else if (this->state == "select" || this->mouseEvent.control)
+ else if (this->state == "select")
this->OnMouseReleaseNormal();
else if (this->state == "translate" || this->state == "rotate"
|| this->state == "scale")
@@ -523,7 +544,9 @@ bool GLWidget::OnMouseMove(const common::MouseEvent & /*_event*/)
/////////////////////////////////////////////////
bool GLWidget::OnMouseDoubleClick(const common::MouseEvent & /*_event*/)
{
- rendering::VisualPtr vis = this->userCamera->GetVisual(this->mouseEvent.pos);
+ rendering::VisualPtr vis =
+ this->userCamera->GetVisual(this->mouseEvent.Pos());
+
if (vis && gui::get_entity_id(vis->GetRootVisual()->GetName()))
{
if (vis->IsPlane())
@@ -531,7 +554,7 @@ bool GLWidget::OnMouseDoubleClick(const common::MouseEvent & /*_event*/)
math::Pose pose, camPose;
camPose = this->userCamera->GetWorldPose();
if (this->scene->GetFirstContact(this->userCamera,
- this->mouseEvent.pos, pose.pos))
+ this->mouseEvent.Pos(), pose.pos))
{
this->userCamera->SetFocalPoint(pose.pos);
math::Vector3 dir = pose.pos - camPose.pos;
@@ -557,7 +580,8 @@ void GLWidget::OnMousePressNormal()
if (!this->userCamera)
return;
- rendering::VisualPtr vis = this->userCamera->GetVisual(this->mouseEvent.pos);
+ rendering::VisualPtr vis = this->userCamera->GetVisual(
+ this->mouseEvent.Pos());
this->userCamera->HandleMouseEvent(this->mouseEvent);
}
@@ -575,14 +599,14 @@ void GLWidget::wheelEvent(QWheelEvent *_event)
if (!this->scene)
return;
- this->mouseEvent.scroll.y = _event->delta() > 0 ? -1 : 1;
- this->mouseEvent.type = common::MouseEvent::SCROLL;
- this->mouseEvent.buttons |= _event->buttons() & Qt::LeftButton ?
- common::MouseEvent::LEFT : 0x0;
- this->mouseEvent.buttons |= _event->buttons() & Qt::RightButton ?
- common::MouseEvent::RIGHT : 0x0;
- this->mouseEvent.buttons |= _event->buttons() & Qt::MidButton ?
- common::MouseEvent::MIDDLE : 0x0;
+ if (_event->delta() > 0)
+ this->mouseEvent.SetScroll(this->mouseEvent.Scroll().X(), -1);
+ else
+ this->mouseEvent.SetScroll(this->mouseEvent.Scroll().X(), 1);
+
+ this->mouseEvent.SetType(common::MouseEvent::SCROLL);
+
+ this->SetMouseEventButtons(_event->buttons());
this->userCamera->HandleMouseEvent(this->mouseEvent);
}
@@ -595,24 +619,20 @@ void GLWidget::mouseMoveEvent(QMouseEvent *_event)
this->setFocus(Qt::MouseFocusReason);
- this->mouseEvent.pos.Set(_event->pos().x(), _event->pos().y());
- this->mouseEvent.type = common::MouseEvent::MOVE;
- this->mouseEvent.buttons |= _event->buttons() & Qt::LeftButton ?
- common::MouseEvent::LEFT : 0x0;
- this->mouseEvent.buttons |= _event->buttons() & Qt::RightButton ?
- common::MouseEvent::RIGHT : 0x0;
- this->mouseEvent.buttons |= _event->buttons() & Qt::MidButton ?
- common::MouseEvent::MIDDLE : 0x0;
+ this->mouseEvent.SetPos(_event->pos().x(), _event->pos().y());
+ this->mouseEvent.SetType(common::MouseEvent::MOVE);
+
+ this->SetMouseEventButtons(_event->buttons());
if (_event->buttons())
- this->mouseEvent.dragging = true;
+ this->mouseEvent.SetDragging(true);
else
- this->mouseEvent.dragging = false;
+ this->mouseEvent.SetDragging(false);
// Process Mouse Events
MouseEventHandler::Instance()->HandleMove(this->mouseEvent);
- this->mouseEvent.prevPos = this->mouseEvent.pos;
+ this->mouseEvent.SetPrevPos(this->mouseEvent.Pos());
}
/////////////////////////////////////////////////
@@ -620,7 +640,7 @@ void GLWidget::OnMouseMoveMakeEntity()
{
if (this->entityMaker)
{
- if (this->mouseEvent.dragging)
+ if (this->mouseEvent.Dragging())
this->entityMaker->OnMouseDrag(this->mouseEvent);
else
this->entityMaker->OnMouseMove(this->mouseEvent);
@@ -633,7 +653,8 @@ void GLWidget::OnMouseMoveNormal()
if (!this->userCamera)
return;
- rendering::VisualPtr vis = this->userCamera->GetVisual(this->mouseEvent.pos);
+ rendering::VisualPtr vis = this->userCamera->GetVisual(
+ this->mouseEvent.Pos());
if (vis && !vis->IsPlane())
QApplication::setOverrideCursor(Qt::PointingHandCursor);
@@ -649,27 +670,15 @@ void GLWidget::mouseReleaseEvent(QMouseEvent *_event)
if (!this->scene)
return;
- this->mouseEvent.pos.Set(_event->pos().x(), _event->pos().y());
- this->mouseEvent.prevPos = this->mouseEvent.pos;
+ this->mouseEvent.SetPos(_event->pos().x(), _event->pos().y());
+ this->mouseEvent.SetPrevPos(this->mouseEvent.Pos());
- if (_event->button() == Qt::LeftButton)
- this->mouseEvent.button = common::MouseEvent::LEFT;
- else if (_event->button() == Qt::RightButton)
- this->mouseEvent.button = common::MouseEvent::RIGHT;
- else if (_event->button() == Qt::MidButton)
- this->mouseEvent.button = common::MouseEvent::MIDDLE;
+ this->SetMouseEventButton(_event->button());
- this->mouseEvent.buttons = common::MouseEvent::NO_BUTTON;
- this->mouseEvent.type = common::MouseEvent::RELEASE;
+ this->mouseEvent.SetButtons(common::MouseEvent::NO_BUTTON);
+ this->mouseEvent.SetType(common::MouseEvent::RELEASE);
- this->mouseEvent.buttons |= _event->buttons() & Qt::LeftButton ?
- common::MouseEvent::LEFT : 0x0;
-
- this->mouseEvent.buttons |= _event->buttons() & Qt::RightButton ?
- common::MouseEvent::RIGHT : 0x0;
-
- this->mouseEvent.buttons |= _event->buttons() & Qt::MidButton ?
- common::MouseEvent::MIDDLE : 0x0;
+ this->SetMouseEventButtons(_event->buttons());
// Process Mouse Events
MouseEventHandler::Instance()->HandleRelease(this->mouseEvent);
@@ -690,19 +699,83 @@ void GLWidget::OnMouseReleaseNormal()
if (!this->userCamera)
return;
- if (!this->mouseEvent.dragging)
+ if (!this->mouseEvent.Dragging())
{
rendering::VisualPtr vis =
- this->userCamera->GetVisual(this->mouseEvent.pos);
+ this->userCamera->GetVisual(this->mouseEvent.Pos());
+
if (vis)
{
- vis = vis->GetRootVisual();
- this->SetSelectedVisual(vis);
- event::Events::setSelectedEntity(vis->GetName(), "normal");
+ rendering::VisualPtr selectVis;
+ rendering::VisualPtr linkVis = vis->GetParent();
+ if (!linkVis)
+ {
+ gzerr << "Link visual not found, this should not happen." << std::endl;
+ return;
+ }
+ rendering::VisualPtr modelVis = vis->GetRootVisual();
+ if (!modelVis)
+ {
+ gzerr << "Model visual not found, this should not happen." << std::endl;
+ return;
+ }
+
+ // Flags to check if we should select a link or a model
+ bool rightButton = (this->mouseEvent.Button() ==
+ common::MouseEvent::RIGHT);
+ bool modelHighlighted = modelVis->GetHighlighted();
+ int linkCount = 0;
+ bool linkHighlighted = false;
+ for (unsigned int i = 0; i < modelVis->GetChildCount(); ++i)
+ {
+ // Find out if there's only one link in the model
+ uint32_t flags = modelVis->GetChild(i)->GetVisibilityFlags();
+ if ((flags != GZ_VISIBILITY_ALL) && (flags & GZ_VISIBILITY_GUI))
+ {
+ continue;
+ }
+ linkCount++;
+
+ // A link from the same model is currently selected
+ if (modelVis->GetChild(i)->GetHighlighted())
+ {
+ linkHighlighted = true;
+ }
+ }
- if (this->mouseEvent.button == common::MouseEvent::RIGHT)
+ // Select link
+ if (linkCount > 1 && !this->mouseEvent.Control() &&
+ ((modelHighlighted && !rightButton) || linkHighlighted))
{
- g_modelRightMenu->Run(vis->GetName(), QCursor::pos());
+ selectVis = linkVis;
+ this->selectionLevel = SelectionLevels::LINK;
+ }
+ // Select model
+ else
+ {
+ // Can't select a link and a model at the same time
+ if (this->selectionLevel == SelectionLevels::LINK)
+ this->DeselectAllVisuals();
+
+ selectVis = modelVis;
+ this->selectionLevel = SelectionLevels::MODEL;
+ }
+ this->SetSelectedVisual(selectVis);
+ event::Events::setSelectedEntity(selectVis->GetName(), "normal");
+
+ // Open context menu
+ if (rightButton)
+ {
+ if (selectVis == modelVis)
+ {
+ g_modelRightMenu->Run(selectVis->GetName(), QCursor::pos(),
+ ModelRightMenu::EntityTypes::MODEL);
+ }
+ else if (selectVis == linkVis)
+ {
+ g_modelRightMenu->Run(selectVis->GetName(), QCursor::pos(),
+ ModelRightMenu::EntityTypes::LINK);
+ }
}
}
else
@@ -749,9 +822,14 @@ void GLWidget::ViewScene(rendering::ScenePtr _scene)
gzerr << "Unable to connect to a running Gazebo master.\n";
if (_scene->GetUserCameraCount() == 0)
- this->userCamera = _scene->CreateUserCamera(cameraName);
+ {
+ this->userCamera = _scene->CreateUserCamera(cameraName,
+ gazebo::gui::getINIProperty<int>("rendering.stereo", 0));
+ }
else
+ {
this->userCamera = _scene->GetUserCamera(0);
+ }
gui::set_active_camera(this->userCamera);
this->scene = _scene;
@@ -763,14 +841,8 @@ void GLWidget::ViewScene(rendering::ScenePtr _scene)
double yaw = atan2(delta.y, delta.x);
double pitch = atan2(-delta.z, sqrt(delta.x*delta.x + delta.y*delta.y));
- this->userCamera->SetWorldPose(math::Pose(camPos,
+ this->userCamera->SetDefaultPose(math::Pose(camPos,
math::Vector3(0, pitch, yaw)));
-
- if (this->windowId >= 0)
- {
- rendering::RenderEngine::Instance()->GetWindowManager()->SetCamera(
- this->windowId, this->userCamera);
- }
}
/////////////////////////////////////////////////
@@ -786,11 +858,9 @@ void GLWidget::Clear()
this->userCamera.reset();
this->scene.reset();
this->SetSelectedVisual(rendering::VisualPtr());
- this->hoverVis.reset();
this->keyModifiers = 0;
}
-
//////////////////////////////////////////////////
rendering::UserCameraPtr GLWidget::GetCamera() const
{
@@ -802,20 +872,20 @@ std::string GLWidget::GetOgreHandle() const
{
std::string ogreHandle;
-#if defined(WIN32) || defined(__APPLE__)
- ogreHandle = boost::lexical_cast<std::string>(this->winId());
+#if defined(__APPLE__)
+ ogreHandle = std::to_string(this->winId());
+#elif defined(WIN32)
+ ogreHandle = std::to_string(
+ reinterpret_cast<uint32_t>(this->renderFrame->winId()));
#else
QX11Info info = x11Info();
QWidget *q_parent = dynamic_cast<QWidget*>(this->renderFrame);
- ogreHandle = boost::lexical_cast<std::string>(
- reinterpret_cast<uint64_t>(info.display()));
- ogreHandle += ":";
- ogreHandle += boost::lexical_cast<std::string>(
- static_cast<uint32_t>(info.screen()));
- ogreHandle += ":";
GZ_ASSERT(q_parent, "q_parent is null");
- ogreHandle += boost::lexical_cast<std::string>(
- static_cast<uint64_t>(q_parent->winId()));
+
+ ogreHandle =
+ std::to_string(reinterpret_cast<uint64_t>(info.display())) + ":" +
+ std::to_string(static_cast<uint32_t>(info.screen())) + ":" +
+ std::to_string(static_cast<uint64_t>(q_parent->winId()));
#endif
return ogreHandle;
@@ -833,7 +903,6 @@ void GLWidget::OnRemoveScene(const std::string &_name)
/////////////////////////////////////////////////
void GLWidget::OnCreateScene(const std::string &_name)
{
- this->hoverVis.reset();
this->SetSelectedVisual(rendering::VisualPtr());
this->ViewScene(rendering::get_scene(_name));
@@ -841,8 +910,6 @@ void GLWidget::OnCreateScene(const std::string &_name)
ModelManipulator::Instance()->Init();
ModelSnap::Instance()->Init();
ModelAlign::Instance()->Init();
-
- this->sceneCreated = true;
}
/////////////////////////////////////////////////
@@ -928,41 +995,35 @@ void GLWidget::OnOrbit()
}
/////////////////////////////////////////////////
-void GLWidget::OnSelectionMsg(ConstSelectionPtr &_msg)
+std::vector<rendering::VisualPtr> GLWidget::SelectedVisuals() const
{
- if (_msg->has_selected() && _msg->selected())
- {
- this->OnSetSelectedEntity(_msg->name(), "normal");
- }
+ return this->selectedVisuals;
}
/////////////////////////////////////////////////
void GLWidget::SetSelectedVisual(rendering::VisualPtr _vis)
{
- boost::mutex::scoped_lock lock(this->selectedVisMutex);
-
- msgs::Selection msg;
-
// deselect all if not in multi-selection mode.
- if (!this->mouseEvent.control)
+ if (!this->mouseEvent.Control())
{
- for (unsigned int i = 0; i < this->selectedVisuals.size(); ++i)
- {
- this->selectedVisuals[i]->SetHighlighted(false);
- msg.set_id(this->selectedVisuals[i]->GetId());
- msg.set_name(this->selectedVisuals[i]->GetName());
- msg.set_selected(false);
- this->selectionPub->Publish(msg);
- }
- this->selectedVisuals.clear();
+ this->DeselectAllVisuals();
}
+ boost::mutex::scoped_lock lock(this->selectedVisMutex);
+
+ msgs::Selection msg;
+
if (_vis && !_vis->IsPlane())
{
+ if (_vis == _vis->GetRootVisual())
+ this->selectionLevel = SelectionLevels::MODEL;
+ else
+ this->selectionLevel = SelectionLevels::LINK;
+
_vis->SetHighlighted(true);
// enable multi-selection if control is pressed
- if (this->selectedVisuals.empty() || this->mouseEvent.control)
+ if (this->selectedVisuals.empty() || this->mouseEvent.Control())
{
std::vector<rendering::VisualPtr>::iterator it =
std::find(this->selectedVisuals.begin(),
@@ -984,12 +1045,30 @@ void GLWidget::SetSelectedVisual(rendering::VisualPtr _vis)
msg.set_selected(true);
this->selectionPub->Publish(msg);
}
- else
+ else if (g_copyAct)
{
g_copyAct->setEnabled(false);
}
- g_alignAct->setEnabled(this->selectedVisuals.size() > 1);
+ if (g_alignAct)
+ g_alignAct->setEnabled(this->selectedVisuals.size() > 1);
+}
+
+/////////////////////////////////////////////////
+void GLWidget::DeselectAllVisuals()
+{
+ boost::mutex::scoped_lock lock(this->selectedVisMutex);
+
+ msgs::Selection msg;
+ for (unsigned int i = 0; i < this->selectedVisuals.size(); ++i)
+ {
+ this->selectedVisuals[i]->SetHighlighted(false);
+ msg.set_id(this->selectedVisuals[i]->GetId());
+ msg.set_name(this->selectedVisuals[i]->GetName());
+ msg.set_selected(false);
+ this->selectionPub->Publish(msg);
+ }
+ this->selectedVisuals.clear();
}
/////////////////////////////////////////////////
@@ -1089,12 +1168,6 @@ void GLWidget::Paste(const std::string &_name)
/////////////////////////////////////////////////
void GLWidget::ClearSelection()
{
- if (this->hoverVis)
- {
- this->hoverVis->SetEmissive(common::Color(0, 0, 0));
- this->hoverVis.reset();
- }
-
this->SetSelectedVisual(rendering::VisualPtr());
this->scene->SelectVisual("", "normal");
@@ -1127,8 +1200,6 @@ void GLWidget::OnSetSelectedEntity(const std::string &_name,
this->SetSelectedVisual(rendering::VisualPtr());
this->scene->SelectVisual("", _mode);
}
-
- this->hoverVis.reset();
}
/////////////////////////////////////////////////
@@ -1151,7 +1222,7 @@ void GLWidget::PopHistory()
msg.set_id(gui::get_entity_id(this->moveHistory.back().first));
msg.set_name(this->moveHistory.back().first);
- msgs::Set(msg.mutable_pose(), this->moveHistory.back().second);
+ msgs::Set(msg.mutable_pose(), this->moveHistory.back().second.Ign());
this->scene->GetVisual(this->moveHistory.back().first)->SetWorldPose(
this->moveHistory.back().second);
@@ -1175,6 +1246,7 @@ void GLWidget::OnRequest(ConstRequestPtr &_msg)
{
if ((*it)->GetName() == _msg->data())
{
+ ModelManipulator::Instance()->Detach();
this->selectedVisuals.erase(it);
break;
}
@@ -1204,11 +1276,75 @@ void GLWidget::OnModelEditor(bool _checked)
g_arrowAct->trigger();
event::Events::setSelectedEntity("", "normal");
- boost::mutex::scoped_lock lock(this->selectedVisMutex);
// Manually deselect, in case the editor was opened with Ctrl
- for (unsigned int i = 0; i < this->selectedVisuals.size(); ++i)
+ this->DeselectAllVisuals();
+}
+
+/////////////////////////////////////////////////
+void GLWidget::OnOrtho()
+{
+ // Disable view control options when in ortho projection
+ g_fpsAct->setEnabled(false);
+ g_orbitAct->setEnabled(false);
+ this->userCamera->SetProjectionType("orthographic");
+}
+
+/////////////////////////////////////////////////
+void GLWidget::OnPerspective()
+{
+ // Enable view control options when in perspective projection
+ g_fpsAct->setEnabled(true);
+ g_orbitAct->setEnabled(true);
+ this->userCamera->SetProjectionType("perspective");
+}
+
+/////////////////////////////////////////////////
+QPaintEngine *GLWidget::paintEngine() const
+{
+ return NULL;
+}
+
+/////////////////////////////////////////////////
+void GLWidget::SetMouseEventButtons(const Qt::MouseButtons &_buttons)
+{
+ if (_buttons & Qt::LeftButton)
{
- this->selectedVisuals[i]->SetHighlighted(false);
+ this->mouseEvent.SetButtons(
+ this->mouseEvent.Buttons() | common::MouseEvent::LEFT);
}
- this->selectedVisuals.clear();
+ else
+ {
+ this->mouseEvent.SetButtons(this->mouseEvent.Buttons() | 0x0);
+ }
+
+ if (_buttons & Qt::RightButton)
+ {
+ this->mouseEvent.SetButtons(
+ this->mouseEvent.Buttons() | common::MouseEvent::RIGHT);
+ }
+ else
+ {
+ this->mouseEvent.SetButtons(this->mouseEvent.Buttons() | 0x0);
+ }
+
+ if (_buttons & Qt::MidButton)
+ {
+ this->mouseEvent.SetButtons(
+ this->mouseEvent.Buttons() | common::MouseEvent::MIDDLE);
+ }
+ else
+ {
+ this->mouseEvent.SetButtons(this->mouseEvent.Buttons() | 0x0);
+ }
+}
+
+/////////////////////////////////////////////////
+void GLWidget::SetMouseEventButton(const Qt::MouseButton &_button)
+{
+ if (_button == Qt::LeftButton)
+ this->mouseEvent.SetButton(common::MouseEvent::LEFT);
+ else if (_button == Qt::RightButton)
+ this->mouseEvent.SetButton(common::MouseEvent::RIGHT);
+ else if (_button == Qt::MidButton)
+ this->mouseEvent.SetButton(common::MouseEvent::MIDDLE);
}
diff --git a/gazebo/gui/GLWidget.hh b/gazebo/gui/GLWidget.hh
index 592e26b..7d2b2bd 100644
--- a/gazebo/gui/GLWidget.hh
+++ b/gazebo/gui/GLWidget.hh
@@ -22,6 +22,8 @@
#include <utility>
#include <list>
+#include <boost/thread/mutex.hpp>
+
#include "gazebo/gui/qt.h"
#include "gazebo/rendering/RenderTypes.hh"
@@ -51,6 +53,15 @@ namespace gazebo
{
Q_OBJECT
+ /// \enum SelectionLevels
+ /// \brief Unique identifiers for all selection levels supported.
+ public: enum SelectionLevels {
+ /// \brief Model level
+ MODEL,
+ /// \brief Link level
+ LINK
+ };
+
public: GLWidget(QWidget *_parent = 0);
public: virtual ~GLWidget();
@@ -65,13 +76,26 @@ namespace gazebo
public: void Clear();
+ /// \brief Returns the list of selected visuals.
+ /// \return List with pointers to selected visuals.
+ public: std::vector<rendering::VisualPtr> SelectedVisuals() const;
+
signals: void clicked();
+ /// \brief QT signal to notify when we received a selection msg.
+ /// \param[in] _name Name of the selected entity.
+ signals: void selectionMsgReceived(const QString &_name);
protected: virtual void moveEvent(QMoveEvent *_e);
protected: virtual void paintEvent(QPaintEvent *_e);
protected: virtual void resizeEvent(QResizeEvent *_e);
+
+ /// \brief Custom processing for the QT showEvent. Based on empirical
+ /// evidence, we believe Mac needs to create the render window in this
+ /// function.
+ /// \param[in] _e The QT show event information.
protected: virtual void showEvent(QShowEvent *_e);
+
protected: virtual void enterEvent(QEvent * event);
@@ -83,6 +107,11 @@ namespace gazebo
protected: void mouseMoveEvent(QMouseEvent *_event);
protected: void mouseReleaseEvent(QMouseEvent *_event);
+ /// \brief Override paintEngine to stop Qt From trying to draw on top of
+ /// OGRE.
+ /// \return NULL.
+ protected: virtual QPaintEngine *paintEngine() const;
+
private: std::string GetOgreHandle() const;
/// \brief Callback for a mouse move event.
@@ -137,8 +166,6 @@ namespace gazebo
private: void OnSetSelectedEntity(const std::string &_name,
const std::string &_mode);
- private: void OnSelectionMsg(ConstSelectionPtr &_msg);
-
private: bool eventFilter(QObject *_obj, QEvent *_event);
private: void ClearSelection();
@@ -151,6 +178,9 @@ namespace gazebo
/// visual
private: void SetSelectedVisual(rendering::VisualPtr _vis);
+ /// \brief Deselect all visuals, removing highlight and publishing message
+ private: void DeselectAllVisuals();
+
/// \brief Callback when a specific alignment configuration is set.
/// \param[in] _axis Axis of alignment: x, y, or z.
/// \param[in] _config Configuration: min, center, or max.
@@ -179,6 +209,26 @@ namespace gazebo
/// \param[in] _checked True if the model editor was checked.
private slots: void OnModelEditor(bool _checked);
+ /// \brief QT Callback that turns on orthographic projection
+ private slots: void OnOrtho();
+
+ /// \brief QT Callback that turns on perspective projection
+ private slots: void OnPerspective();
+
+ /// \brief Set this->mouseEvent's Buttons property to the value of
+ /// _event->buttons(). Note that this is different from the
+ /// SetMouseEventButtons, plural, function.
+ /// \sa SetMouseEventButtons
+ /// \param[in] _button The QT mouse button
+ private: void SetMouseEventButton(const Qt::MouseButton &_button);
+
+ /// \brief Set this->mouseEvent's Button property to the value of
+ /// _event->button(). Note that this is different from the
+ /// SetMouseEventButton, singular, function.
+ /// \sa SetMouseEventButton
+ /// \param[in] _button The QT mouse buttons
+ private: void SetMouseEventButtons(const Qt::MouseButtons &_buttons);
+
private: int windowId;
private: rendering::UserCameraPtr userCamera;
@@ -204,18 +254,19 @@ namespace gazebo
/// \brief Light maker
private: LightMaker lightMaker;
- private: rendering::VisualPtr hoverVis;
-
/// \brief A list of selected visuals.
private: std::vector<rendering::VisualPtr> selectedVisuals;
+ /// \brief Indicates how deep into the model to select.
+ private: SelectionLevels selectionLevel;
+
private: transport::NodePtr node;
private: transport::PublisherPtr modelPub, factoryPub;
/// \brief Publishes information about user selections.
private: transport::PublisherPtr selectionPub;
- private: transport::SubscriberPtr selectionSub, requestSub;
+ private: transport::SubscriberPtr requestSub;
private: std::string keyText;
private: Qt::KeyboardModifiers keyModifiers;
@@ -230,10 +281,6 @@ namespace gazebo
/// \brief Name of entity that is being copied.
private: std::string copyEntityName;
- /// \brief Flag that is set to true when GLWidget has responded to
- /// OnCreateScene
- private: bool sceneCreated;
-
/// \brief True if the model editor is up, false otherwise
private: bool modelEditorEnabled;
diff --git a/gazebo/gui/GLWidget_TEST.cc b/gazebo/gui/GLWidget_TEST.cc
index 809967e..2140a41 100644
--- a/gazebo/gui/GLWidget_TEST.cc
+++ b/gazebo/gui/GLWidget_TEST.cc
@@ -48,22 +48,16 @@ void GLWidget_TEST::SelectObject()
gazebo::transport::SubscriberPtr sub;
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init();
- sub = node->Subscribe("~/selection", &OnSelection, this);
+ sub = node->Subscribe("~/selection", &OnSelection, true);
// Create the main window.
gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
QVERIFY(mainWindow != NULL);
mainWindow->Load();
-
- gazebo::rendering::create_scene(
- gazebo::physics::get_world()->GetName(), false);
-
mainWindow->Init();
mainWindow->show();
- gazebo::rendering::Events::createScene("default");
-
// Process some events, and draw the screen
for (unsigned int i = 0; i < 10; ++i)
{
@@ -85,6 +79,28 @@ void GLWidget_TEST::SelectObject()
// Verify the box was selected
QVERIFY(g_gotBoxSelection);
+ // Check the selected visuals list
+ std::vector<gazebo::rendering::VisualPtr> selectedVisuals =
+ glWidget->SelectedVisuals();
+
+ QVERIFY(selectedVisuals.size() == 1u);
+ QVERIFY(selectedVisuals[0]->GetName() == "box");
+
+ // Delete the selected object. This is here to make sure the GUI does not
+ // segfault if an object is deleted.
+ {
+ std::string name = selectedVisuals.back()->GetName();
+ gazebo::transport::requestNoReply(node, "entity_delete", name);
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+ }
+
mainWindow->close();
delete mainWindow;
}
diff --git a/gazebo/gui/GLWidget_TEST2.cc b/gazebo/gui/GLWidget_TEST2.cc
index 6569c06..ebca311 100644
--- a/gazebo/gui/GLWidget_TEST2.cc
+++ b/gazebo/gui/GLWidget_TEST2.cc
@@ -47,15 +47,9 @@ void GLWidget_TEST2::KeyPresses()
gazebo::gui::MainWindow mainWindow;
mainWindow.Load();
-
- gazebo::rendering::create_scene(
- gazebo::physics::get_world()->GetName(), false);
-
mainWindow.Init();
mainWindow.show();
- gazebo::rendering::Events::createScene("default");
-
// Process some events, and draw the screen
for (unsigned int i = 0; i < 10; ++i)
{
diff --git a/gazebo/gui/GuiEvents.cc b/gazebo/gui/GuiEvents.cc
index 743d52e..40031aa 100644
--- a/gazebo/gui/GuiEvents.cc
+++ b/gazebo/gui/GuiEvents.cc
@@ -26,6 +26,7 @@ event::EventT<void (std::string)> Events::manipMode;
event::EventT<void (std::string, std::string, std::string, bool)>
Events::alignMode;
event::EventT<void (bool)> Events::fullScreen;
+event::EventT<void (bool)> Events::showToolbars;
event::EventT<void ()> Events::fps;
event::EventT<void ()> Events::orbit;
event::EventT<void (std::string)> Events::keyPress;
@@ -35,3 +36,6 @@ event::EventT<void (int)> Events::inputStepSize;
event::EventT<void (const std::string &)> Events::follow;
event::EventT<void (bool)> Events::leftPaneVisibility;
event::EventT<void ()> Events::mainWindowReady;
+event::EventT<void (const std::string &)> Events::editModel;
+event::EventT<void (const std::string &, const math::Vector3 &)>
+ Events::scaleEntity;
diff --git a/gazebo/gui/GuiEvents.hh b/gazebo/gui/GuiEvents.hh
index a511f5f..0e7b7fe 100644
--- a/gazebo/gui/GuiEvents.hh
+++ b/gazebo/gui/GuiEvents.hh
@@ -80,6 +80,17 @@ namespace gazebo
{ fullScreen.Disconnect(_subscriber); }
//////////////////////////////////////////////////////////////////////////
+ /// \brief Connect a signal to the show toolbars signal
+ public: template<typename T>
+ static event::ConnectionPtr ConnectShowToolbars(T _subscriber)
+ { return showToolbars.Connect(_subscriber); }
+
+ /// \brief Disconnect a signal from the show toolbars signal
+ public: static void DisconnectShowToolbars(event::ConnectionPtr
+ _subscriber)
+ { showToolbars.Disconnect(_subscriber); }
+
+ //////////////////////////////////////////////////////////////////////////
/// \brief Connect a signal to the view FPS signal
public: template<typename T>
static event::ConnectionPtr ConnectFPS(T _subscriber)
@@ -154,6 +165,17 @@ namespace gazebo
{ follow.Disconnect(_subscriber); }
//////////////////////////////////////////////////////////////////////////
+ /// \brief Connect a signal to the edit model signal
+ public: template<typename T>
+ static event::ConnectionPtr ConnectEditModel(T _subscriber)
+ { return editModel.Connect(_subscriber); }
+
+ /// \brief Disconnect a signal from the edit model signal
+ public: static void DisconnectEditModel(
+ event::ConnectionPtr _subscriber)
+ { editModel.Disconnect(_subscriber); }
+
+ //////////////////////////////////////////////////////////////////////////
/// \brief Connect a signal to the main window ready signal
public: template<typename T>
static event::ConnectionPtr ConnectMainWindowReady(T _subscriber)
@@ -176,6 +198,17 @@ namespace gazebo
event::ConnectionPtr _subscriber)
{ leftPaneVisibility.Disconnect(_subscriber); }
+ //////////////////////////////////////////////////////////////////////////
+ /// \brief Connect a signal to the scale entity signal
+ public: template<typename T>
+ static event::ConnectionPtr ConnectScaleEntity(T _subscriber)
+ { return scaleEntity.Connect(_subscriber); }
+
+ /// \brief Disconnect a signal from the scale entity signal
+ public: static void DisconnectScaleEntity(
+ event::ConnectionPtr _subscriber)
+ { scaleEntity.Disconnect(_subscriber); }
+
/// \brief Indicates the user is moving the camera
public: static event::EventT<void (bool)> moveMode;
@@ -199,6 +232,9 @@ namespace gazebo
/// \brief An event to trigger full screen mode.
public: static event::EventT<void (bool)> fullScreen;
+ /// \brief An event to trigger show toolbars.
+ public: static event::EventT<void (bool)> showToolbars;
+
/// \brief An event to enable first-person-shooter view control.
public: static event::EventT<void ()> fps;
@@ -209,6 +245,10 @@ namespace gazebo
/// name is given as the function parameter.
public: static event::EventT<void (const std::string &)> follow;
+ /// \brief Event triggered when the user selects edit a model. The model
+ /// name is given as the function parameter.
+ public: static event::EventT<void (const std::string &)> editModel;
+
/// \brief Event triggered when a key is pressed
public: static event::EventT<void (std::string)> keyPress;
@@ -220,6 +260,10 @@ namespace gazebo
/// \brief Main window ready event.
public: static event::EventT<void ()> mainWindowReady;
+
+ /// \brief Scale entity event.
+ public: static event::EventT<void (const std::string &,
+ const math::Vector3 &)> scaleEntity;
};
}
}
diff --git a/gazebo/gui/GuiIface.cc b/gazebo/gui/GuiIface.cc
index ccc9551..9a8755e 100644
--- a/gazebo/gui/GuiIface.cc
+++ b/gazebo/gui/GuiIface.cc
@@ -14,18 +14,26 @@
* 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>
+ #define snprintf _snprintf
+#endif
#include <signal.h>
#include <boost/program_options.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include "gazebo/gui/qt.h"
-#include "gazebo/gazebo.hh"
+#include "gazebo/gazebo_client.hh"
+#include "gazebo/common/Time.hh"
#include "gazebo/common/ModelDatabase.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Plugin.hh"
#include "gazebo/common/CommonTypes.hh"
+#include "gazebo/gui/SplashScreen.hh"
#include "gazebo/gui/MainWindow.hh"
#include "gazebo/gui/ModelRightMenu.hh"
#include "gazebo/gui/GuiIface.hh"
@@ -54,10 +62,15 @@ gui::ModelRightMenu *g_modelRightMenu = NULL;
std::string g_worldname = "default";
QApplication *g_app;
+gui::SplashScreen *g_splashScreen = NULL;
gui::MainWindow *g_main_win = NULL;
rendering::UserCameraPtr g_active_camera;
bool g_fullscreen = false;
+// This makes it possible to use common::Time in QT signals and slots.
+// qRegisterMetaType is also required, see below.
+Q_DECLARE_METATYPE(common::Time)
+
//////////////////////////////////////////////////
void print_usage()
{
@@ -71,7 +84,7 @@ void print_usage()
void signal_handler(int)
{
gazebo::gui::stop();
- gazebo::shutdown();
+ gazebo::client::shutdown();
}
//////////////////////////////////////////////////
@@ -113,7 +126,7 @@ bool parse_args(int _argc, char **_argv)
if (vm.count("verbose"))
{
- gazebo::printVersion();
+ gazebo::client::printVersion();
gazebo::common::Console::SetQuiet(false);
}
@@ -126,7 +139,7 @@ bool parse_args(int _argc, char **_argv)
for (std::vector<std::string>::iterator iter = pp.begin();
iter != pp.end(); ++iter)
{
- gazebo::addPlugin(*iter);
+ gazebo::client::addPlugin(*iter);
}
}
@@ -162,7 +175,6 @@ namespace gazebo
void gui::init()
{
g_modelRightMenu->Init();
- g_main_win->show();
g_main_win->Init();
}
@@ -229,6 +241,7 @@ bool gui::load()
g_modelRightMenu = new gui::ModelRightMenu();
+ // Load the rendering engine.
rendering::load();
rendering::init();
@@ -242,10 +255,15 @@ bool gui::load()
g_app = new QApplication(g_argc, g_argv);
set_style();
+ // Register common::Time as a type that can be used in signals and slots.
+ // Q_DECLARE_METATYPE is also required, see above.
+ qRegisterMetaType<common::Time>();
+
+ g_splashScreen = new gui::SplashScreen();
+
g_main_win = new gui::MainWindow();
g_main_win->Load();
- g_main_win->resize(1024, 768);
return true;
}
@@ -271,7 +289,7 @@ bool gui::run(int _argc, char **_argv)
if (!parse_args(_argc, _argv))
return false;
- if (!gazebo::setupClient(_argc, _argv))
+ if (!gazebo::client::setup(_argc, _argv))
return false;
if (!gazebo::gui::load())
@@ -279,6 +297,7 @@ bool gui::run(int _argc, char **_argv)
gazebo::gui::init();
+#ifndef _WIN32
// Now that we're about to run, install a signal handler to allow for
// graceful shutdown on Ctrl-C.
struct sigaction sigact;
@@ -288,18 +307,22 @@ bool gui::run(int _argc, char **_argv)
std::cerr << "signal(2) failed while setting up for SIGINT" << std::endl;
return false;
}
+#endif
g_app->exec();
gazebo::gui::fini();
- gazebo::shutdown();
+ gazebo::client::shutdown();
+
+ delete g_splashScreen;
+ delete g_main_win;
return true;
}
/////////////////////////////////////////////////
void gui::stop()
{
- gazebo::shutdown();
+ gazebo::client::shutdown();
g_active_camera.reset();
g_app->quit();
}
diff --git a/gazebo/gui/IncrementalPlot.cc b/gazebo/gui/IncrementalPlot.cc
index 23c56f2..023586a 100644
--- a/gazebo/gui/IncrementalPlot.cc
+++ b/gazebo/gui/IncrementalPlot.cc
@@ -15,6 +15,12 @@
*
*/
+#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 <qwt/qwt_plot.h>
#include <qwt/qwt_scale_widget.h>
#include <qwt/qwt_plot_panner.h>
diff --git a/gazebo/gui/InsertModelWidget.cc b/gazebo/gui/InsertModelWidget.cc
index 0d5e70d..41cf8e1 100644
--- a/gazebo/gui/InsertModelWidget.cc
+++ b/gazebo/gui/InsertModelWidget.cc
@@ -15,7 +15,15 @@
*
*/
+#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 <fstream>
+
+#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <sdf/sdf.hh>
@@ -83,7 +91,19 @@ InsertModelWidget::InsertModelWidget(QWidget *_parent)
if (!additionalPaths.empty())
{
common::SystemPaths::Instance()->AddModelPaths(additionalPaths);
- this->UpdateLocalPath(additionalPaths);
+
+ // Get each path in the : separated list
+ std::string delim(":");
+ size_t pos1 = 0;
+ size_t pos2 = additionalPaths.find(delim);
+ while (pos2 != std::string::npos)
+ {
+ this->UpdateLocalPath(additionalPaths.substr(pos1, pos2-pos1));
+ pos1 = pos2+1;
+ pos2 = additionalPaths.find(delim, pos2+1);
+ }
+ this->UpdateLocalPath(additionalPaths.substr(pos1,
+ additionalPaths.size()-pos1));
}
// Connect callbacks now that everything else is initialized
@@ -152,6 +172,7 @@ void InsertModelWidget::Update()
}
this->dataPtr->modelBuffer.clear();
+ this->dataPtr->getModelsConnection.reset();
}
else
QTimer::singleShot(1000, this, SLOT(Update()));
@@ -165,7 +186,6 @@ void InsertModelWidget::OnModels(
{
boost::mutex::scoped_lock lock(this->dataPtr->mutex);
this->dataPtr->modelBuffer = _models;
- this->dataPtr->getModelsConnection.reset();
}
/////////////////////////////////////////////////
diff --git a/gazebo/gui/JointControlWidget.cc b/gazebo/gui/JointControlWidget.cc
index 956f1aa..977ff90 100644
--- a/gazebo/gui/JointControlWidget.cc
+++ b/gazebo/gui/JointControlWidget.cc
@@ -14,6 +14,13 @@
* 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 "gazebo/transport/Node.hh"
#include "gazebo/transport/TransportIface.hh"
#include "gazebo/gui/GuiIface.hh"
diff --git a/gazebo/gui/LaserVisualization_TEST.cc b/gazebo/gui/LaserVisualization_TEST.cc
new file mode 100644
index 0000000..881601f
--- /dev/null
+++ b/gazebo/gui/LaserVisualization_TEST.cc
@@ -0,0 +1,99 @@
+/*
+ * 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/common/Time.hh"
+#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/MainWindow.hh"
+#include "gazebo/rendering/RenderEvents.hh"
+#include "gazebo/gui/LaserVisualization_TEST.hh"
+#include "test_config.h"
+
+/////////////////////////////////////////////////
+void LaserVisualization_TEST::Lines()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/laser_lines_test.world", false, false, false);
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+
+ // Create the main window.
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ cam->SetCaptureData(true);
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get camera data
+ const unsigned char *data = cam->GetImageData();
+ unsigned int width = cam->GetImageWidth();
+ unsigned int height = cam->GetImageHeight();
+ unsigned int depth = cam->GetImageDepth();
+
+ // Make sure there are darker lines in the laser rendering
+ int lightBlueCount = 0;
+ int darkBlueTransition = 0;
+
+ int rPrev = 0;
+ int gPrev = 0;
+ int bPrev = 0;
+
+ // Read a horizontal line in the middle of the screen
+ unsigned int y = height / 2;
+ for (unsigned int x = 0; x < width*depth; x += depth)
+ {
+ int r = data[y*(width*depth) + x];
+ int g = data[y*(width*depth) + x+1];
+ int b = data[y*(width*depth) + x+2];
+
+ if (r < 189 && g < 189 && b == 255 &&
+ rPrev >= 204 && gPrev >= 204 && bPrev >= 255)
+ ++darkBlueTransition;
+
+ if (r >= 204 && g >= 204 && b == 255)
+ ++lightBlueCount;
+
+ rPrev = r;
+ bPrev = b;
+ gPrev = g;
+ }
+
+ // Make sure there are 2 dark blue lines.
+ QVERIFY(darkBlueTransition == 2);
+
+ // Make sure there is a bunch of light blue pixels
+ QVERIFY(lightBlueCount > 500);
+
+ mainWindow->close();
+ delete mainWindow;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(LaserVisualization_TEST)
diff --git a/gazebo/gui/LaserVisualization_TEST.hh b/gazebo/gui/LaserVisualization_TEST.hh
new file mode 100644
index 0000000..47b26c3
--- /dev/null
+++ b/gazebo/gui/LaserVisualization_TEST.hh
@@ -0,0 +1,32 @@
+/*
+ * 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_LASERVISUALIZATION_TEST_HH_
+#define _GAZEBO_LASERVISUALIZATION_TEST_HH_
+
+#include "gazebo/gui/QTestFixture.hh"
+
+/// \brief A test class for laser visualization.
+class LaserVisualization_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Test that laser visualization draws lines correctly.
+ private slots: void Lines();
+};
+
+#endif
diff --git a/gazebo/gui/LayersWidget.cc b/gazebo/gui/LayersWidget.cc
new file mode 100644
index 0000000..21402ab
--- /dev/null
+++ b/gazebo/gui/LayersWidget.cc
@@ -0,0 +1,89 @@
+/*
+ * 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 <sstream>
+
+#include "gazebo/rendering/RenderEvents.hh"
+#include "gazebo/gui/LayersWidgetPrivate.hh"
+#include "gazebo/gui/LayersWidget.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+LayersWidget::LayersWidget(QWidget *_parent)
+ : QWidget(_parent), dataPtr(new LayersWidgetPrivate)
+{
+ this->setObjectName("layersList");
+
+ QVBoxLayout *mainLayout = new QVBoxLayout;
+ this->dataPtr->layerList = new QListWidget(this);
+
+ mainLayout->addWidget(this->dataPtr->layerList);
+
+ this->setLayout(mainLayout);
+ this->layout()->setContentsMargins(0, 0, 0, 0);
+
+ connect(this->dataPtr->layerList, SIGNAL(itemChanged(QListWidgetItem *)),
+ this, SLOT(OnLayerSelected(QListWidgetItem *)));
+
+ this->dataPtr->connections.push_back(
+ rendering::Events::ConnectNewLayer(
+ boost::bind(&LayersWidget::OnNewLayer, this, _1)));
+}
+
+/////////////////////////////////////////////////
+LayersWidget::~LayersWidget()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+void LayersWidget::OnLayerSelected(QListWidgetItem *_layer)
+{
+ rendering::Events::toggleLayer(_layer->data(Qt::UserRole).toInt());
+}
+
+/////////////////////////////////////////////////
+void LayersWidget::OnNewLayer(const int32_t _layer)
+{
+ std::ostringstream stream;
+ stream << "Layer " << _layer;
+
+ auto found = this->dataPtr->layerList->findItems(stream.str().c_str(),
+ Qt::MatchExactly);
+
+ // Only add layers that do not already exist.
+ if (found.empty())
+ {
+ // Disconnect temporarily to prevent bad signals
+ disconnect(this->dataPtr->layerList, SIGNAL(itemChanged(QListWidgetItem *)),
+ this, SLOT(OnLayerSelected(QListWidgetItem *)));
+
+ QListWidgetItem *item = new QListWidgetItem(stream.str().c_str(),
+ this->dataPtr->layerList);
+
+ item->setCheckState(Qt::Checked);
+ item->setData(Qt::UserRole, QVariant(_layer));
+ item->setFlags(item->flags() | Qt::ItemIsUserCheckable);
+ this->dataPtr->layerList->addItem(item);
+
+ // Reconnect.
+ connect(this->dataPtr->layerList, SIGNAL(itemChanged(QListWidgetItem *)),
+ this, SLOT(OnLayerSelected(QListWidgetItem *)));
+ }
+}
diff --git a/gazebo/gui/LayersWidget.hh b/gazebo/gui/LayersWidget.hh
new file mode 100644
index 0000000..960f362
--- /dev/null
+++ b/gazebo/gui/LayersWidget.hh
@@ -0,0 +1,58 @@
+/*
+ * 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_LAYERS_WIDGET_HH_
+#define _GAZEBO_LAYERS_WIDGET_HH_
+
+#include "gazebo/common/Events.hh"
+#include "gazebo/util/system.hh"
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ // Forward declare private class
+ class LayersWidgetPrivate;
+
+ /// \brief A widget that manages visualization layers. This widget is
+ /// added to the left-hand tabset.
+ class GZ_GUI_VISIBLE LayersWidget : public QWidget
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ /// \param[in] _parent Parent widget pointer.
+ public: LayersWidget(QWidget *_parent = 0);
+
+ /// \brief Destructor
+ public: virtual ~LayersWidget();
+
+ /// \brief QT callback, triggered when a layer is selected.
+ /// \param[in] _layer Pointer to the widget that was selected.
+ private slots: void OnLayerSelected(QListWidgetItem *_layer);
+
+ /// \brief Gazebo callback, triggered when a new layer is added.
+ /// \param[in] _layer Number of the new layer
+ private: void OnNewLayer(const int32_t _layer);
+
+ /// \internal
+ /// \brief Pointer to private data.
+ private: LayersWidgetPrivate *dataPtr;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/LayersWidgetPrivate.hh b/gazebo/gui/LayersWidgetPrivate.hh
new file mode 100644
index 0000000..a00bbe4
--- /dev/null
+++ b/gazebo/gui/LayersWidgetPrivate.hh
@@ -0,0 +1,39 @@
+/*
+ * 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_LAYERS_WIDGET_PRIVATE_HH_
+#define _GAZEBO_LAYERS_WIDGET_PRIVATE_HH_
+
+#include <vector>
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \internal
+ /// \brief Private data for the LayersWidget class.
+ class LayersWidgetPrivate
+ {
+ /// \brief List of all the layers
+ public: QListWidget *layerList;
+
+ /// \brief Event connections
+ public: std::vector<event::ConnectionPtr> connections;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/LayersWidget_TEST.cc b/gazebo/gui/LayersWidget_TEST.cc
new file mode 100644
index 0000000..b8a5957
--- /dev/null
+++ b/gazebo/gui/LayersWidget_TEST.cc
@@ -0,0 +1,152 @@
+/*
+ * 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/gui/GuiIface.hh"
+#include "gazebo/gui/MainWindow.hh"
+#include "gazebo/rendering/RenderEvents.hh"
+#include "gazebo/gui/LayersWidget_TEST.hh"
+#include "test_config.h"
+
+
+/////////////////////////////////////////////////
+void LayersWidget_TEST::ToggleLayers()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/test_layers.world", false, false, false);
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+
+ // Create the main window.
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ cam->SetCaptureData(true);
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ const unsigned char *data = cam->GetImageData();
+ unsigned int width = cam->GetImageWidth();
+ unsigned int height = cam->GetImageHeight();
+ unsigned int depth = cam->GetImageDepth();
+
+ // Part 1 : Make sure the box is rendered. The white box should fill the
+ // entire field of view.
+ {
+ int blackCount = 0;
+
+ // Get the number of black pixels
+ for (unsigned int y = 0; y < height; ++y)
+ {
+ for (unsigned int x = 0; x < width*depth; ++x)
+ {
+ if (data[y*(width*depth) + x] <= 10)
+ blackCount++;
+ }
+ }
+
+ // Make sure the black count is zero. This means the visual
+ // is visible and compelty fills the camera field of view.
+ QVERIFY(blackCount == 0);
+ }
+
+ // PART 2: Disable the white box. Only a black background should be
+ // visible
+ {
+ // Disable the visual. This should hide the visual.
+ gazebo::rendering::Events::toggleLayer(0);
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get the new image data
+ data = cam->GetImageData();
+
+ int whiteCount = 0;
+
+ // Get the number of white pixels
+ for (unsigned int y = 0; y < height; ++y)
+ {
+ for (unsigned int x = 0; x < width*depth; ++x)
+ {
+ if (data[y*(width*depth) + x] >= 250)
+ whiteCount++;
+ }
+ }
+
+ // Make sure the white count is zero. This means the visual is not
+ // visible.
+ QVERIFY(whiteCount == 0);
+ }
+
+ // PART 3: Re-enable the white box, which again should fill the entire
+ // field of view.
+ {
+ // Re-enable the visual. This should make the visual visible again.
+ gazebo::rendering::Events::toggleLayer(0);
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get the new image data
+ data = cam->GetImageData();
+
+ int blackCount = 0;
+
+ // Get the number of black pixels
+ for (unsigned int y = 0; y < height; ++y)
+ {
+ for (unsigned int x = 0; x < width*depth; ++x)
+ {
+ if (data[y*(width*depth) + x] <= 10)
+ blackCount++;
+ }
+ }
+
+ // Make sure the black count is zero. This means the visual
+ // is visible and completely fills the camera field of view.
+ QVERIFY(blackCount == 0);
+ }
+
+ mainWindow->close();
+ delete mainWindow;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(LayersWidget_TEST)
diff --git a/gazebo/gui/LayersWidget_TEST.hh b/gazebo/gui/LayersWidget_TEST.hh
new file mode 100644
index 0000000..115378b
--- /dev/null
+++ b/gazebo/gui/LayersWidget_TEST.hh
@@ -0,0 +1,32 @@
+/*
+ * 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_LAYERSWIDGET_TEST_HH_
+#define _GAZEBO_LAYERSWIDGET_TEST_HH_
+
+#include "gazebo/gui/QTestFixture.hh"
+
+/// \brief A test class for the Layers widget.
+class LayersWidget_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Test toggling layers.
+ private slots: void ToggleLayers();
+};
+
+#endif
diff --git a/gazebo/gui/LightMaker.cc b/gazebo/gui/LightMaker.cc
index b85971e..7b5823a 100644
--- a/gazebo/gui/LightMaker.cc
+++ b/gazebo/gui/LightMaker.cc
@@ -15,6 +15,12 @@
*
*/
+#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 <iostream>
#include <sstream>
@@ -151,9 +157,9 @@ void LightMaker::OnMousePush(const common::MouseEvent &/*_event*/)
void LightMaker::CreateTheEntity()
{
msgs::Set(this->msg.mutable_pose()->mutable_position(),
- this->light->GetPosition());
+ this->light->GetPosition().Ign());
msgs::Set(this->msg.mutable_pose()->mutable_orientation(),
- math::Quaternion());
+ ignition::math::Quaterniond());
this->lightPub->Publish(this->msg);
this->camera.reset();
}
@@ -161,7 +167,7 @@ void LightMaker::CreateTheEntity()
/////////////////////////////////////////////////
void LightMaker::OnMouseRelease(const common::MouseEvent &_event)
{
- if (_event.button == common::MouseEvent::LEFT && !_event.dragging)
+ if (_event.Button() == common::MouseEvent::LEFT && !_event.Dragging())
{
this->CreateTheEntity();
this->Stop();
@@ -176,7 +182,7 @@ void LightMaker::OnMouseMove(const common::MouseEvent &_event)
math::Vector3 origin1, dir1, p1;
// Cast two rays from the camera into the world
- this->camera->GetCameraToViewportRay(_event.pos.x, _event.pos.y,
+ this->camera->GetCameraToViewportRay(_event.Pos().X(), _event.Pos().Y(),
origin1, dir1);
// Compute the distance from the camera to plane of translation
@@ -184,22 +190,13 @@ void LightMaker::OnMouseMove(const common::MouseEvent &_event)
double dist1 = plane.Distance(origin1, dir1);
- // Compute two points on the plane. The first point is the current
- // mouse position, the second is the previous mouse position
+ // Calculate position
p1 = origin1 + dir1 * dist1;
- if (!_event.shift)
- {
- if (ceil(p1.x) - p1.x <= .4)
- p1.x = ceil(p1.x);
- else if (p1.x - floor(p1.x) <= .4)
- p1.x = floor(p1.x);
-
- if (ceil(p1.y) - p1.y <= .4)
- p1.y = ceil(p1.y);
- else if (p1.y - floor(p1.y) <= .4)
- p1.y = floor(p1.y);
- }
+ // Get snap point
+ if (!_event.Shift())
+ p1 = this->GetSnappedPoint(p1);
+
p1.z = this->light->GetPosition().z;
this->light->SetPosition(p1);
diff --git a/gazebo/gui/LightMaker.hh b/gazebo/gui/LightMaker.hh
index a99e1fc..13334c6 100644
--- a/gazebo/gui/LightMaker.hh
+++ b/gazebo/gui/LightMaker.hh
@@ -83,7 +83,7 @@ namespace gazebo
{
this->msg.set_type(msgs::Light::SPOT);
msgs::Set(this->msg.mutable_direction(),
- math::Vector3(0, 0, -1));
+ ignition::math::Vector3d(0, 0, -1));
this->msg.set_cast_shadows(false);
this->msg.set_spot_inner_angle(0.6);
@@ -99,7 +99,7 @@ namespace gazebo
{
this->msg.set_type(msgs::Light::DIRECTIONAL);
msgs::Set(this->msg.mutable_direction(),
- math::Vector3(.1, .1, -0.9));
+ ignition::math::Vector3d(.1, .1, -0.9));
this->msg.set_cast_shadows(true);
this->lightTypename = "directional";
diff --git a/gazebo/gui/LogPlayWidget.cc b/gazebo/gui/LogPlayWidget.cc
new file mode 100644
index 0000000..5308634
--- /dev/null
+++ b/gazebo/gui/LogPlayWidget.cc
@@ -0,0 +1,466 @@
+/*
+ * 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/common/Console.hh"
+#include "gazebo/common/Time.hh"
+#include "gazebo/gui/Actions.hh"
+#include "gazebo/gui/LogPlayWidget.hh"
+#include "gazebo/gui/LogPlayWidgetPrivate.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+LogPlayWidget::LogPlayWidget(QWidget *_parent)
+ : QWidget(_parent), dataPtr(new LogPlayWidgetPrivate)
+{
+ this->setObjectName("logPlayWidget");
+
+ this->dataPtr->timePanel = dynamic_cast<TimePanel *>(_parent);
+
+ QSize bigSize(70, 70);
+ QSize bigIconSize(40, 40);
+ QSize smallSize(50, 50);
+ QSize smallIconSize(30, 30);
+
+ // Empty space on the left
+ QWidget *leftSpacer = new QWidget();
+ leftSpacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
+
+ // Play
+ QToolButton *playButton = new QToolButton(this);
+ playButton->setFixedSize(bigSize);
+ playButton->setCheckable(false);
+ playButton->setIcon(QPixmap(":/images/log_play.png"));
+ playButton->setIconSize(bigIconSize);
+ playButton->setStyleSheet(
+ QString("border-radius: %1px").arg(bigSize.width()/2-2));
+ connect(playButton, SIGNAL(clicked()), this, SLOT(OnPlay()));
+ connect(this, SIGNAL(ShowPlay()), playButton, SLOT(show()));
+ connect(this, SIGNAL(HidePlay()), playButton, SLOT(hide()));
+
+ // Pause
+ QToolButton *pauseButton = new QToolButton(this);
+ pauseButton->setFixedSize(bigSize);
+ pauseButton->setCheckable(false);
+ pauseButton->setIcon(QPixmap(":/images/log_pause.png"));
+ pauseButton->setIconSize(bigIconSize);
+ pauseButton->setStyleSheet(
+ QString("border-radius: %1px").arg(bigSize.width()/2-2));
+ connect(pauseButton, SIGNAL(clicked()), this, SLOT(OnPause()));
+ connect(this, SIGNAL(ShowPause()), pauseButton, SLOT(show()));
+ connect(this, SIGNAL(HidePause()), pauseButton, SLOT(hide()));
+
+ // Step forward
+ QToolButton *stepForwardButton = new QToolButton(this);
+ stepForwardButton->setFixedSize(smallSize);
+ stepForwardButton->setCheckable(false);
+ stepForwardButton->setIcon(QPixmap(":/images/log_step_forward.png"));
+ stepForwardButton->setIconSize(smallIconSize);
+ stepForwardButton->setStyleSheet(
+ QString("border-radius: %1px").arg(smallSize.width()/2-2));
+ connect(stepForwardButton, SIGNAL(clicked()), this, SLOT(OnStepForward()));
+
+ // Play layout
+ QHBoxLayout *playLayout = new QHBoxLayout();
+ playLayout->addWidget(playButton);
+ playLayout->addWidget(pauseButton);
+ playLayout->addWidget(stepForwardButton);
+
+ // View
+ this->dataPtr->view = new LogPlayView(this);
+ connect(this, SIGNAL(SetCurrentTime(common::Time)), this->dataPtr->view,
+ SLOT(SetCurrentTime(common::Time)));
+ connect(this, SIGNAL(SetStartTime(common::Time)), this->dataPtr->view,
+ SLOT(SetStartTime(common::Time)));
+ connect(this, SIGNAL(SetEndTime(common::Time)), this->dataPtr->view,
+ SLOT(SetEndTime(common::Time)));
+
+ // Time
+ QLineEdit *currentTime = new QLineEdit();
+ currentTime->setObjectName("logPlayCurrentTime");
+ currentTime->setMaximumWidth(110);
+ currentTime->setAlignment(Qt::AlignRight);
+ connect(this, SIGNAL(SetCurrentTime(const QString &)), currentTime,
+ SLOT(setText(const QString &)));
+
+ QLabel *endTime = new QLabel();
+ connect(this, SIGNAL(SetEndTime(const QString &)), endTime,
+ SLOT(setText(const QString &)));
+
+ QHBoxLayout *timeLayout = new QHBoxLayout();
+ timeLayout->addWidget(currentTime);
+ timeLayout->addWidget(endTime);
+
+ // Empty space on the right
+ QWidget *rightSpacer = new QWidget();
+ rightSpacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
+
+ // Main layout
+ QHBoxLayout *mainLayout = new QHBoxLayout;
+ mainLayout->addWidget(leftSpacer);
+ mainLayout->addLayout(playLayout);
+ mainLayout->addWidget(this->dataPtr->view);
+ mainLayout->addLayout(timeLayout);
+ mainLayout->addWidget(rightSpacer);
+
+ this->setLayout(mainLayout);
+ mainLayout->setAlignment(playLayout, Qt::AlignRight);
+ mainLayout->setAlignment(timeLayout, Qt::AlignLeft);
+
+ this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ this->layout()->setContentsMargins(0, 0, 0, 0);
+}
+
+/////////////////////////////////////////////////
+LogPlayWidget::~LogPlayWidget()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+bool LogPlayWidget::IsPaused() const
+{
+ return this->dataPtr->paused;
+}
+
+/////////////////////////////////////////////////
+void LogPlayWidget::SetPaused(const bool _paused)
+{
+ this->dataPtr->paused = _paused;
+
+ if (_paused)
+ {
+ emit ShowPlay();
+ emit HidePause();
+ }
+ else
+ {
+ emit HidePlay();
+ emit ShowPause();
+ }
+}
+
+/////////////////////////////////////////////////
+void LogPlayWidget::OnPlay()
+{
+ g_playAct->trigger();
+}
+
+/////////////////////////////////////////////////
+void LogPlayWidget::OnPause()
+{
+ g_pauseAct->trigger();
+}
+
+/////////////////////////////////////////////////
+void LogPlayWidget::OnStepForward()
+{
+ g_stepAct->trigger();
+}
+
+/////////////////////////////////////////////////
+void LogPlayWidget::EmitSetCurrentTime(const common::Time &_time)
+{
+ // Make sure it's within limits
+ common::Time time = std::max(_time, this->dataPtr->startTime);
+ if (this->dataPtr->endTime != common::Time::Zero)
+ time = std::min(time, this->dataPtr->endTime);
+
+ this->dataPtr->currentTime = time;
+
+ // Update current time line edit
+ if (this->dataPtr->lessThan1h)
+ {
+ this->SetCurrentTime(QString::fromStdString(time.FormattedString(
+ common::Time::FormatOption::MINUTES)));
+ }
+ else
+ {
+ this->SetCurrentTime(QString::fromStdString(time.FormattedString()));
+ }
+
+ // Update current time item in view
+ this->SetCurrentTime(time);
+}
+
+/////////////////////////////////////////////////
+void LogPlayWidget::EmitSetStartTime(const common::Time &_time)
+{
+ if (this->dataPtr->endTime != common::Time::Zero &&
+ _time >= this->dataPtr->endTime)
+ {
+ gzwarn << "Start time [" << _time << "] after end time [" <<
+ this->dataPtr->endTime << "]. Not updating." << std::endl;
+ return;
+ }
+
+ this->dataPtr->startTime = _time;
+
+ // Update start time in view
+ this->SetStartTime(this->dataPtr->startTime);
+
+ // Keep current time within bounds
+ if (this->dataPtr->startTime > this->dataPtr->currentTime)
+ this->EmitSetCurrentTime(this->dataPtr->startTime);
+}
+
+/////////////////////////////////////////////////
+void LogPlayWidget::EmitSetEndTime(const common::Time &_time)
+{
+ if (_time <= this->dataPtr->startTime)
+ {
+ gzwarn << "End time [" << _time << "] before start time [" <<
+ this->dataPtr->startTime << "]. Not updating." << std::endl;
+ return;
+ }
+
+ this->dataPtr->endTime = _time;
+
+ // Use shorter string if less than 1h
+ if (_time < common::Time::Hour)
+ this->dataPtr->lessThan1h = true;
+
+ // Update end time label
+ std::string timeString;
+ if (this->dataPtr->lessThan1h)
+ {
+ timeString = _time.FormattedString(common::Time::FormatOption::MINUTES);
+ }
+ else
+ {
+ timeString = _time.FormattedString();
+ }
+
+ timeString = "/ " + timeString;
+
+ this->SetEndTime(QString::fromStdString(timeString));
+
+ // Update end time in view
+ this->SetEndTime(_time);
+
+ // Keep current time within bounds
+ if (this->dataPtr->endTime < this->dataPtr->currentTime)
+ this->EmitSetCurrentTime(this->dataPtr->endTime);
+}
+
+/////////////////////////////////////////////////
+LogPlayView::LogPlayView(LogPlayWidget *_parent)
+ : QGraphicsView(_parent), dataPtr(new LogPlayViewPrivate)
+{
+ this->setSizePolicy(QSizePolicy::Expanding,
+ QSizePolicy::Expanding);
+
+ this->dataPtr->sceneWidth = 1000;
+ this->dataPtr->sceneHeight = 120;
+ this->dataPtr->margin = 50;
+
+ QGraphicsScene *graphicsScene = new QGraphicsScene();
+ graphicsScene->setBackgroundBrush(QColor(128, 128, 128));
+ this->setScene(graphicsScene);
+ this->setMinimumWidth(this->dataPtr->sceneHeight);
+ this->setViewportUpdateMode(QGraphicsView::FullViewportUpdate);
+ this->setStyleSheet("QGraphicsView{border-style: none;}");
+ this->setSceneRect(0, 0,
+ this->dataPtr->sceneWidth, this->dataPtr->sceneHeight);
+
+ // Time line
+ QGraphicsLineItem *line = new QGraphicsLineItem(this->dataPtr->margin,
+ this->dataPtr->sceneHeight/2,
+ this->dataPtr->sceneWidth - this->dataPtr->margin,
+ this->dataPtr->sceneHeight/2);
+ line->setPen(QPen(QColor(50, 50, 50, 255), 2));
+ graphicsScene->addItem(line);
+
+ // Current time item
+ this->dataPtr->currentTimeItem = new CurrentTimeItem();
+ this->dataPtr->currentTimeItem->setPos(this->dataPtr->margin,
+ this->dataPtr->sceneHeight/2);
+ graphicsScene->addItem(this->dataPtr->currentTimeItem);
+
+ this->dataPtr->startTimeSet = false;
+ this->dataPtr->endTimeSet = false;
+}
+
+/////////////////////////////////////////////////
+void LogPlayView::SetCurrentTime(const common::Time &_time)
+{
+ common::Time totalTime = this->dataPtr->endTime - this->dataPtr->startTime;
+
+ if (totalTime == common::Time::Zero)
+ return;
+
+ double relPos = ((_time - this->dataPtr->startTime) / totalTime).Double();
+
+ this->dataPtr->currentTimeItem->setPos(this->dataPtr->margin +
+ (this->dataPtr->sceneWidth - 2 * this->dataPtr->margin)*relPos,
+ this->dataPtr->sceneHeight/2);
+}
+
+/////////////////////////////////////////////////
+void LogPlayView::SetStartTime(const common::Time &_time)
+{
+ this->dataPtr->startTime = _time;
+ this->dataPtr->startTimeSet = true;
+}
+
+/////////////////////////////////////////////////
+void LogPlayView::SetEndTime(const common::Time &_time)
+{
+ this->dataPtr->endTime = _time;
+ this->dataPtr->endTimeSet = true;
+
+ if (this->dataPtr->startTimeSet)
+ this->DrawTimeline();
+}
+
+/////////////////////////////////////////////////
+void LogPlayView::DrawTimeline()
+{
+ if (this->dataPtr->timelineDrawn || !this->dataPtr->startTimeSet ||
+ !this->dataPtr->endTimeSet)
+ return;
+
+ common::Time totalTime = this->dataPtr->endTime - this->dataPtr->startTime;
+
+ if (totalTime == common::Time::Zero)
+ return;
+
+ // Aim for this number, but some samples might be added/removed
+ int intervals = 10;
+
+ // All ticks are shifted from a round number of seconds (no msec or nsec)
+ common::Time roundStartTime = common::Time(this->dataPtr->startTime.sec, 0);
+
+ // Time between ticks (round seconds)
+ common::Time interval = totalTime/intervals;
+ interval.nsec = 0;
+
+ if (interval == common::Time::Zero)
+ interval = common::Time::Second;
+
+ // Time line
+ int tickHeight = 15;
+ common::Time tickTime = this->dataPtr->startTime;
+ int i = 0;
+ while (tickTime >= this->dataPtr->startTime &&
+ tickTime < this->dataPtr->endTime)
+ {
+ // Intermediate samples have a round number
+ if (i != 0)
+ {
+ tickTime = roundStartTime + interval * i;
+ }
+
+ // If first interval too close, shift by 1s
+ common::Time endSpace = interval * 0.9;
+ if (tickTime != this->dataPtr->startTime &&
+ tickTime < this->dataPtr->startTime + endSpace)
+ {
+ roundStartTime += common::Time::Second;
+ tickTime = roundStartTime + interval * i;
+ }
+
+ // If last interval too close, skip to end
+ if (tickTime > this->dataPtr->endTime - endSpace)
+ {
+ tickTime = this->dataPtr->endTime;
+ }
+ ++i;
+
+ // Relative position
+ double relPos = ((tickTime - this->dataPtr->startTime) / totalTime)
+ .Double();
+
+ // Tick vertical line
+ QGraphicsLineItem *tick = new QGraphicsLineItem(0, -tickHeight, 0, 0);
+ tick->setPos(this->dataPtr->margin +
+ (this->dataPtr->sceneWidth - 2 * this->dataPtr->margin)*relPos,
+ this->dataPtr->sceneHeight/2);
+ tick->setPen(QPen(QColor(50, 50, 50, 255), 2));
+ this->scene()->addItem(tick);
+
+ // Text
+ std::string timeText;
+ if (tickTime == this->dataPtr->startTime ||
+ tickTime == this->dataPtr->endTime)
+ {
+ timeText = tickTime.FormattedString(common::Time::FormatOption::MINUTES);
+ }
+ else
+ {
+ timeText = tickTime.FormattedString(common::Time::FormatOption::MINUTES,
+ common::Time::FormatOption::SECONDS);
+ }
+
+ QGraphicsSimpleTextItem *tickText = new QGraphicsSimpleTextItem(
+ QString::fromStdString(timeText));
+ tickText->setBrush(QBrush(QColor(50, 50, 50, 255)));
+ tickText->setPos(
+ this->dataPtr->margin - tickText->boundingRect().width()*0.5 +
+ (this->dataPtr->sceneWidth - 2 * this->dataPtr->margin)*relPos,
+ this->dataPtr->sceneHeight/2 - 3 * tickHeight);
+ this->scene()->addItem(tickText);
+ }
+
+ this->dataPtr->timelineDrawn = true;
+}
+
+/////////////////////////////////////////////////
+CurrentTimeItem::CurrentTimeItem()
+{
+ this->setEnabled(true);
+ this->setZValue(10);
+}
+
+/////////////////////////////////////////////////
+void CurrentTimeItem::paint(QPainter *_painter,
+ const QStyleOptionGraphicsItem */*_option*/, QWidget */*_widget*/)
+{
+ int lineHeight = 50;
+ int lineWidth = 3;
+
+ // Vertical line
+ QLineF vLine(-lineWidth/10.0, -lineHeight/2.0,
+ -lineWidth/10.0, +lineHeight/2.0);
+
+ QPen linePen;
+ linePen.setColor(QColor(50, 50, 50, 255));
+ linePen.setWidth(lineWidth);
+
+ _painter->setPen(linePen);
+ _painter->drawLine(vLine);
+
+ // Triangle
+ QVector<QPointF> trianglePts;
+ trianglePts.push_back(QPointF(-8, -lineHeight/2 - 1));
+ trianglePts.push_back(QPointF(8, -lineHeight/2 - 1));
+ trianglePts.push_back(QPointF(0, -lineHeight/2 + 10));
+ QPolygonF triangle(trianglePts);
+
+ QPen whitePen(Qt::white, 0);
+ QPen orangePen(QColor(245, 129, 19, 255), 0);
+ QBrush whiteBrush(Qt::white);
+ QBrush orangeBrush(QColor(245, 129, 19, 255));
+
+ _painter->setPen(orangePen);
+ _painter->setBrush(orangeBrush);
+
+ _painter->drawPolygon(triangle);
+}
+
diff --git a/gazebo/gui/LogPlayWidget.hh b/gazebo/gui/LogPlayWidget.hh
new file mode 100644
index 0000000..d5145f2
--- /dev/null
+++ b/gazebo/gui/LogPlayWidget.hh
@@ -0,0 +1,160 @@
+/*
+ * 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_LOG_PLAY_WIDGET_HH_
+#define _GAZEBO_LOG_PLAY_WIDGET_HH_
+
+#include "gazebo/common/Time.hh"
+#include "gazebo/gui/qt.h"
+#include "gazebo/gui/TimePanel.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class LogPlayWidgetPrivate;
+ class LogPlayViewPrivate;
+ class TimePanel;
+
+ /// \class LogPlayWidget LogPlayWidget.hh
+ /// \brief Widget which displays log playback options.
+ class GAZEBO_VISIBLE LogPlayWidget : public QWidget
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ /// \param[in] _parent Parent widget, commonly a TimePanel.
+ public: LogPlayWidget(QWidget *_parent = 0);
+
+ /// \brief Destructor
+ public: virtual ~LogPlayWidget();
+
+ /// \brief Returns if the simulation is displayed as paused.
+ /// \return True if paused, false otherwise.
+ public: bool IsPaused() const;
+
+ /// \brief Set whether to display the simulation as paused.
+ /// \param[in] _p True to display the simulation as paused. False
+ /// indicates the simulation is running
+ public: void SetPaused(const bool _paused);
+
+ /// \brief Emit signal to set current time.
+ /// \param[in] _time Current time.
+ public: void EmitSetCurrentTime(const common::Time &_time);
+
+ /// \brief Emit signal to set start time.
+ /// \param[in] _time Start time.
+ public: void EmitSetStartTime(const common::Time &_time);
+
+ /// \brief Emit signal to set end time.
+ /// \param[in] _time End time.
+ public: void EmitSetEndTime(const common::Time &_time);
+
+ /// \brief Play simulation.
+ public slots: void OnPlay();
+
+ /// \brief Pause simulation.
+ public slots: void OnPause();
+
+ /// \brief Step simulation forward.
+ public slots: void OnStepForward();
+
+ /// \brief Qt signal to show the play button.
+ signals: void ShowPlay();
+
+ /// \brief Qt signal to hide the play button.
+ signals: void HidePlay();
+
+ /// \brief Qt signal to show the pause button.
+ signals: void ShowPause();
+
+ /// \brief Qt signal to hide the pause button.
+ signals: void HidePause();
+
+ /// \brief Qt signal used to set the current time line edit.
+ /// \param[in] _string String representation of current time.
+ signals: void SetCurrentTime(const QString &);
+
+ /// \brief Qt signal used to set the end time line edit.
+ /// \param[in] _string String representation of current time.
+ signals: void SetEndTime(const QString &);
+
+ /// \brief Qt signal used to set the current time in the view.
+ /// \param[in] _time Current time.
+ signals: void SetCurrentTime(const common::Time &_time);
+
+ /// \brief Qt signal used to set the start time in the view.
+ /// \param[in] _time Start time.
+ signals: void SetStartTime(const common::Time &_time);
+
+ /// \brief Qt signal used to set the end time in the view.
+ /// \param[in] _time End time.
+ signals: void SetEndTime(const common::Time &_time);
+
+ /// \internal
+ /// \brief Pointer to private data.
+ private: LogPlayWidgetPrivate *dataPtr;
+ };
+
+ /// \class LogPlayView LogPlayView.hh
+ /// \brief View for the timeline.
+ class GAZEBO_VISIBLE LogPlayView: public QGraphicsView
+ {
+ Q_OBJECT
+
+ /// \brief Constructor;
+ /// \param[in] _parent Parent widget.
+ public: LogPlayView(LogPlayWidget *_parent = 0);
+
+ /// \brief Set the position of the current time item.
+ /// \param[in] _time Current time.
+ public slots: void SetCurrentTime(const common::Time &_time);
+
+ /// \brief Set the log start time.
+ /// \param[in] _time Start time.
+ public slots: void SetStartTime(const common::Time &_time);
+
+ /// \brief Set the log end time.
+ /// \param[in] _time End time.
+ public slots: void SetEndTime(const common::Time &_time);
+
+ /// \brief Draw the timeline.
+ public slots: void DrawTimeline();
+
+ /// \internal
+ /// \brief Pointer to private data.
+ private: LogPlayViewPrivate *dataPtr;
+ };
+
+ /// \class CurrentTimeItem CurrentTimeItem.hh
+ /// \brief Item which represents the current time within the view.
+ class GAZEBO_VISIBLE CurrentTimeItem: public QObject,
+ public QGraphicsRectItem
+ {
+ Q_OBJECT
+
+ /// \brief Constructor;
+ public: CurrentTimeItem();
+
+ // Documentation inherited
+ private: virtual void paint(QPainter *_painter,
+ const QStyleOptionGraphicsItem *_option, QWidget *_widget);
+ };
+ }
+}
+
+#endif
diff --git a/gazebo/gui/LogPlayWidgetPrivate.hh b/gazebo/gui/LogPlayWidgetPrivate.hh
new file mode 100644
index 0000000..099cdf2
--- /dev/null
+++ b/gazebo/gui/LogPlayWidgetPrivate.hh
@@ -0,0 +1,85 @@
+/*
+ * 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_LOG_PLAY_WIDGET_PRIVATE_HH_
+#define _GAZEBO_LOG_PLAY_WIDGET_PRIVATE_HH_
+
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \internal
+ /// \brief Private data for the LogPlayWidget class
+ class LogPlayWidgetPrivate
+ {
+ /// \brief Paused state of the simulation.
+ public: bool paused;
+
+ /// \brief Paused state of the simulation.
+ public: TimePanel *timePanel;
+
+ /// \brief Log start time.
+ public: common::Time startTime;
+
+ /// \brief Log end time.
+ public: common::Time endTime;
+
+ /// \brief Log current time.
+ public: common::Time currentTime;
+
+ /// \brief If log is less than 1 hour long.
+ public: bool lessThan1h;
+
+ /// \brief View which containes the timeline.
+ public: LogPlayView *view;
+ };
+
+ /// \class LogPlayViewPrivate LogPlayViewPrivate.hh
+ /// \brief Private data for the LogPlayView class
+ class LogPlayViewPrivate
+ {
+ /// \brief Item which indicates the current time.
+ public: CurrentTimeItem *currentTimeItem;
+
+ /// \brief Start time in milliseconds.
+ public: common::Time startTime;
+
+ /// \brief End time in milliseconds.
+ public: common::Time endTime;
+
+ /// \brief Log start time has been set or not.
+ public: bool startTimeSet;
+
+ /// \brief Log end time has been set or not.
+ public: bool endTimeSet;
+
+ /// \brief Width of this view's scene.
+ public: int sceneWidth;
+
+ /// \brief Height of this view's scene.
+ public: int sceneHeight;
+
+ /// \brief Margin from the ends.
+ public: int margin;
+
+ /// \brief Whether the timeline has already been drawn.
+ public: bool timelineDrawn = false;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/MainWindow.cc b/gazebo/gui/MainWindow.cc
index 01145dc..b506e5c 100644
--- a/gazebo/gui/MainWindow.cc
+++ b/gazebo/gui/MainWindow.cc
@@ -14,6 +14,12 @@
* 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 <sdf/sdf.hh>
#include <boost/scoped_ptr.hpp>
@@ -27,7 +33,7 @@
#include "gazebo/gui/viewers/TopicView.hh"
#include "gazebo/gui/viewers/ImageView.hh"
-#include "gazebo/gazebo.hh"
+#include "gazebo/gazebo_client.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Events.hh"
@@ -44,11 +50,14 @@
#include "gazebo/gui/Actions.hh"
#include "gazebo/gui/GuiIface.hh"
#include "gazebo/gui/InsertModelWidget.hh"
+#include "gazebo/gui/LayersWidget.hh"
#include "gazebo/gui/ModelListWidget.hh"
#include "gazebo/gui/RenderWidget.hh"
#include "gazebo/gui/ToolsWidget.hh"
#include "gazebo/gui/GLWidget.hh"
#include "gazebo/gui/AlignWidget.hh"
+#include "gazebo/gui/ViewAngleWidget.hh"
+#include "gazebo/gui/TimePanel.hh"
#include "gazebo/gui/MainWindow.hh"
#include "gazebo/gui/GuiEvents.hh"
#include "gazebo/gui/SpaceNav.hh"
@@ -103,11 +112,13 @@ MainWindow::MainWindow()
this->modelListWidget = new ModelListWidget(this);
InsertModelWidget *insertModel = new InsertModelWidget(this);
+ LayersWidget *layersWidget = new LayersWidget(this);
this->tabWidget = new QTabWidget();
this->tabWidget->setObjectName("mainTab");
this->tabWidget->addTab(this->modelListWidget, "World");
this->tabWidget->addTab(insertModel, "Insert");
+ this->tabWidget->addTab(layersWidget, "Layers");
this->tabWidget->setSizePolicy(QSizePolicy::Expanding,
QSizePolicy::Expanding);
this->tabWidget->setMinimumWidth(MINIMUM_TAB_WIDTH);
@@ -127,11 +138,18 @@ MainWindow::MainWindow()
this->splitter->addWidget(this->toolsWidget);
this->splitter->setContentsMargins(0, 0, 0, 0);
+#ifdef _WIN32
+ // The splitter appears solid white in Windows, so we make it transparent.
+ this->splitter->setStyleSheet(
+ "QSplitter { color: #ffffff; background-color: transparent; }"
+ "QSplitter::handle { color: #ffffff; background-color: transparent; }");
+#endif
+
QList<int> sizes;
sizes.push_back(MINIMUM_TAB_WIDTH);
sizes.push_back(this->width() - MINIMUM_TAB_WIDTH);
sizes.push_back(0);
- splitter->setSizes(sizes);
+ this->splitter->setSizes(sizes);
this->splitter->setStretchFactor(0, 0);
this->splitter->setStretchFactor(1, 2);
@@ -167,6 +185,10 @@ MainWindow::MainWindow()
boost::bind(&MainWindow::OnFullScreen, this, _1)));
this->connections.push_back(
+ gui::Events::ConnectShowToolbars(
+ boost::bind(&MainWindow::OnShowToolbars, this, _1)));
+
+ this->connections.push_back(
gui::Events::ConnectMoveMode(
boost::bind(&MainWindow::OnMoveMode, this, _1)));
@@ -200,11 +222,18 @@ MainWindow::MainWindow()
// Use a signal/slot to load plugins. This makes the process thread safe.
connect(this, SIGNAL(AddPlugins()),
this, SLOT(OnAddPlugins()), Qt::QueuedConnection);
+
+ // Create data logger dialog
+ this->dataLogger = new gui::DataLogger(this);
+ connect(dataLogger, SIGNAL(rejected()), this, SLOT(OnDataLoggerClosed()));
+
+ this->show();
}
/////////////////////////////////////////////////
MainWindow::~MainWindow()
{
+ this->DeleteActions();
}
/////////////////////////////////////////////////
@@ -241,10 +270,8 @@ void MainWindow::Load()
/////////////////////////////////////////////////
void MainWindow::Init()
{
- this->renderWidget->show();
-
// Default window size is entire desktop.
- QSize winSize = QApplication::desktop()->size();
+ QSize winSize = QApplication::desktop()->screenGeometry().size();
// Get the size properties from the INI file.
int winWidth = getINIProperty<int>("geometry.width", winSize.width());
@@ -271,9 +298,6 @@ void MainWindow::Init()
this->lightSub = this->node->Subscribe("~/light", &MainWindow::OnLight, this);
- this->statsSub =
- this->node->Subscribe("~/world_stats", &MainWindow::OnStats, this);
-
this->requestPub = this->node->Advertise<msgs::Request>("~/request");
this->responseSub = this->node->Subscribe("~/response",
&MainWindow::OnResponse, this);
@@ -296,12 +320,6 @@ void MainWindow::closeEvent(QCloseEvent * /*_event*/)
this->connections.clear();
- delete this->renderWidget;
-
- // Cleanup the space navigator
- delete this->spacenav;
- this->spacenav = NULL;
-
#ifdef HAVE_OCULUS
if (this->oculusWindow)
{
@@ -309,8 +327,15 @@ void MainWindow::closeEvent(QCloseEvent * /*_event*/)
this->oculusWindow = NULL;
}
#endif
+ delete this->renderWidget;
+
+ // Cleanup the space navigator
+ delete this->spacenav;
+ this->spacenav = NULL;
+
+ emit Close();
- gazebo::shutdown();
+ gazebo::client::shutdown();
}
/////////////////////////////////////////////////
@@ -410,17 +435,22 @@ void MainWindow::SaveINI()
/////////////////////////////////////////////////
void MainWindow::SaveAs()
{
- std::string filename = QFileDialog::getSaveFileName(this,
- tr("Save World"), QString(),
- tr("SDF Files (*.xml *.sdf *.world)")).toStdString();
+ QFileDialog fileDialog(this, tr("Save World"), QDir::homePath(),
+ tr("SDF Files (*.xml *.sdf *.world)"));
+ fileDialog.setAcceptMode(QFileDialog::AcceptSave);
- // Return if the user has canceled.
- if (filename.empty())
- return;
+ if (fileDialog.exec() == QDialog::Accepted)
+ {
+ QStringList selected = fileDialog.selectedFiles();
+ if (selected.empty())
+ return;
+
+ std::string filename = selected[0].toStdString();
- g_saveAct->setEnabled(true);
- this->saveFilename = filename;
- this->Save();
+ g_saveAct->setEnabled(true);
+ this->saveFilename = filename;
+ this->Save();
+ }
}
/////////////////////////////////////////////////
@@ -443,9 +473,9 @@ void MainWindow::Save()
sdf::SDF sdf_parsed;
sdf_parsed.SetFromString(msg.data());
// Check that sdf contains world
- if (sdf_parsed.root->HasElement("world"))
+ if (sdf_parsed.Root()->HasElement("world"))
{
- sdf::ElementPtr world = sdf_parsed.root->GetElement("world");
+ sdf::ElementPtr world = sdf_parsed.Root()->GetElement("world");
sdf::ElementPtr guiElem = world->GetElement("gui");
if (guiElem->HasAttribute("fullscreen"))
@@ -457,8 +487,11 @@ void MainWindow::Save()
cameraElem->GetElement("pose")->Set(cam->GetWorldPose());
cameraElem->GetElement("view_controller")->Set(
cam->GetViewControllerTypeString());
+
+ cameraElem->GetElement("projection_type")->Set(cam->GetProjectionType());
+
// TODO: export track_visual properties as well.
- msgData = sdf_parsed.root->ToString("");
+ msgData = sdf_parsed.Root()->ToString("");
}
else
{
@@ -562,8 +595,6 @@ void MainWindow::Play()
msgs::WorldControl msg;
msg.set_pause(false);
- g_pauseAct->setVisible(true);
- g_playAct->setVisible(false);
this->worldControlPub->Publish(msg);
}
@@ -573,8 +604,6 @@ void MainWindow::Pause()
msgs::WorldControl msg;
msg.set_pause(true);
- g_pauseAct->setVisible(false);
- g_playAct->setVisible(true);
this->worldControlPub->Publish(msg);
}
@@ -751,20 +780,24 @@ void MainWindow::OnFullScreen(bool _value)
this->toolsWidget->show();
this->menuBar->show();
}
+ g_fullScreenAct->setChecked(_value);
+ g_fullscreen = _value;
}
/////////////////////////////////////////////////
-void MainWindow::Reset()
+void MainWindow::OnShowToolbars(bool _value)
{
- rendering::UserCameraPtr cam = gui::get_active_camera();
-
- math::Vector3 camPos(5, -5, 2);
- math::Vector3 lookAt(0, 0, 0);
- math::Vector3 delta = camPos - lookAt;
-
- double yaw = atan2(delta.x, delta.y);
- double pitch = atan2(delta.z, sqrt(delta.x*delta.x + delta.y*delta.y));
- cam->SetWorldPose(math::Pose(camPos, math::Vector3(0, pitch, yaw)));
+ if (_value)
+ {
+ this->GetRenderWidget()->GetTimePanel()->show();
+ this->GetRenderWidget()->GetToolbar()->show();
+ }
+ else
+ {
+ this->GetRenderWidget()->GetTimePanel()->hide();
+ this->GetRenderWidget()->GetToolbar()->hide();
+ }
+ g_showToolbarsAct->setChecked(_value);
}
/////////////////////////////////////////////////
@@ -788,6 +821,15 @@ void MainWindow::ShowGrid()
}
/////////////////////////////////////////////////
+void MainWindow::ShowOrigin()
+{
+ msgs::Scene msg;
+ msg.set_name(gui::get_world());
+ msg.set_origin_visual(g_showOriginAct->isChecked());
+ this->scenePub->Publish(msg);
+}
+
+/////////////////////////////////////////////////
void MainWindow::ShowJoints()
{
if (g_showJointsAct->isChecked())
@@ -821,6 +863,12 @@ void MainWindow::SetWireframe()
}
/////////////////////////////////////////////////
+void MainWindow::ShowGUIOverlays()
+{
+ this->GetRenderWidget()->SetOverlaysVisible(g_overlayAct->isChecked());
+}
+
+/////////////////////////////////////////////////
void MainWindow::ShowCOM()
{
if (g_showCOMAct->isChecked())
@@ -832,6 +880,32 @@ void MainWindow::ShowCOM()
}
/////////////////////////////////////////////////
+void MainWindow::ShowInertia()
+{
+ if (g_showInertiaAct->isChecked())
+ transport::requestNoReply(this->node->GetTopicNamespace(),
+ "show_inertia", "all");
+ else
+ transport::requestNoReply(this->node->GetTopicNamespace(),
+ "hide_inertia", "all");
+}
+
+/////////////////////////////////////////////////
+void MainWindow::ShowLinkFrame()
+{
+ if (g_showLinkFrameAct->isChecked())
+ {
+ transport::requestNoReply(this->node->GetTopicNamespace(),
+ "show_link_frame", "all");
+ }
+ else
+ {
+ transport::requestNoReply(this->node->GetTopicNamespace(),
+ "hide_link_frame", "all");
+ }
+}
+
+/////////////////////////////////////////////////
void MainWindow::ShowContacts()
{
if (g_showContactsAct->isChecked())
@@ -850,6 +924,12 @@ void MainWindow::FullScreen()
}
/////////////////////////////////////////////////
+void MainWindow::ShowToolbars()
+{
+ gui::Events::showToolbars(g_showToolbarsAct->isChecked());
+}
+
+/////////////////////////////////////////////////
void MainWindow::FPS()
{
gui::Events::fps();
@@ -895,8 +975,21 @@ void MainWindow::ViewOculus()
/////////////////////////////////////////////////
void MainWindow::DataLogger()
{
- gui::DataLogger *dataLogger = new gui::DataLogger(this);
- dataLogger->show();
+ if (g_dataLoggerAct->isChecked())
+ {
+ this->dataLogger->show();
+ }
+ else
+ {
+ this->dataLogger->close();
+ }
+}
+
+/////////////////////////////////////////////////
+void MainWindow::OnDataLoggerClosed()
+{
+ // Uncheck action on toolbar when user closes dialog
+ g_dataLoggerAct->setChecked(false);
}
/////////////////////////////////////////////////
@@ -961,12 +1054,14 @@ void MainWindow::CreateActions()
g_resetModelsAct = new QAction(tr("&Reset Model Poses"), this);
g_resetModelsAct->setShortcut(tr("Ctrl+Shift+R"));
+ this->addAction(g_resetModelsAct);
g_resetModelsAct->setStatusTip(tr("Reset model poses"));
connect(g_resetModelsAct, SIGNAL(triggered()), this,
SLOT(OnResetModelOnly()));
g_resetWorldAct = new QAction(tr("&Reset World"), this);
g_resetWorldAct->setShortcut(tr("Ctrl+R"));
+ this->addAction(g_resetWorldAct);
g_resetWorldAct->setStatusTip(tr("Reset the world"));
connect(g_resetWorldAct, SIGNAL(triggered()), this, SLOT(OnResetWorld()));
@@ -998,13 +1093,12 @@ void MainWindow::CreateActions()
g_stepAct->setStatusTip(tr("Step the world"));
connect(g_stepAct, SIGNAL(triggered()), this, SLOT(Step()));
this->CreateDisabledIcon(":/images/end.png", g_stepAct);
+ g_stepAct->setEnabled(false);
g_playAct = new QAction(QIcon(":/images/play.png"), tr("Play"), this);
g_playAct->setStatusTip(tr("Run the world"));
g_playAct->setVisible(false);
connect(g_playAct, SIGNAL(triggered()), this, SLOT(Play()));
- connect(g_playAct, SIGNAL(changed()), this, SLOT(OnPlayActionChanged()));
- this->OnPlayActionChanged();
g_pauseAct = new QAction(QIcon(":/images/pause.png"), tr("Pause"), this);
g_pauseAct->setStatusTip(tr("Pause the world"));
@@ -1075,7 +1169,6 @@ void MainWindow::CreateActions()
SLOT(CreateMesh()));
this->CreateDisabledIcon(":/images/cylinder.png", g_meshCreateAct);
-
g_pointLghtCreateAct = new QAction(QIcon(":/images/pointlight.png"),
tr("Point Light"), this);
g_pointLghtCreateAct->setStatusTip(tr("Create a point light"));
@@ -1100,10 +1193,8 @@ void MainWindow::CreateActions()
SLOT(CreateDirectionalLight()));
this->CreateDisabledIcon(":/images/directionallight.png", g_dirLghtCreateAct);
- g_resetAct = new QAction(tr("Reset Camera"), this);
- g_resetAct->setStatusTip(tr("Move camera to pose"));
- connect(g_resetAct, SIGNAL(triggered()), this,
- SLOT(Reset()));
+ g_resetAct = new QAction(tr("Reset View Angle"), this);
+ g_resetAct->setStatusTip(tr("Move camera to initial pose"));
g_showCollisionsAct = new QAction(tr("Collisions"), this);
g_showCollisionsAct->setStatusTip(tr("Show Collisions"));
@@ -1119,6 +1210,13 @@ void MainWindow::CreateActions()
connect(g_showGridAct, SIGNAL(triggered()), this,
SLOT(ShowGrid()));
+ g_showOriginAct = new QAction(tr("Origin"), this);
+ g_showOriginAct->setStatusTip(tr("Show World Origin"));
+ g_showOriginAct->setCheckable(true);
+ g_showOriginAct->setChecked(true);
+ connect(g_showOriginAct, SIGNAL(triggered()), this,
+ SLOT(ShowOrigin()));
+
g_transparentAct = new QAction(tr("Transparent"), this);
g_transparentAct->setStatusTip(tr("Transparent"));
g_transparentAct->setCheckable(true);
@@ -1133,13 +1231,27 @@ void MainWindow::CreateActions()
connect(g_viewWireframeAct, SIGNAL(triggered()), this,
SLOT(SetWireframe()));
- g_showCOMAct = new QAction(tr("Center of Mass / Inertia"), this);
- g_showCOMAct->setStatusTip(tr("Show COM/MOI"));
+ g_showCOMAct = new QAction(tr("Center of Mass"), this);
+ g_showCOMAct->setStatusTip(tr("Show center of mass"));
g_showCOMAct->setCheckable(true);
g_showCOMAct->setChecked(false);
connect(g_showCOMAct, SIGNAL(triggered()), this,
SLOT(ShowCOM()));
+ g_showInertiaAct = new QAction(tr("Inertias"), this);
+ g_showInertiaAct->setStatusTip(tr("Show moments of inertia"));
+ g_showInertiaAct->setCheckable(true);
+ g_showInertiaAct->setChecked(false);
+ connect(g_showInertiaAct, SIGNAL(triggered()), this,
+ SLOT(ShowInertia()));
+
+ g_showLinkFrameAct = new QAction(tr("Link Frames"), this);
+ g_showLinkFrameAct->setStatusTip(tr("Show link frames"));
+ g_showLinkFrameAct->setCheckable(true);
+ g_showLinkFrameAct->setChecked(false);
+ connect(g_showLinkFrameAct, SIGNAL(triggered()), this,
+ SLOT(ShowLinkFrame()));
+
g_showContactsAct = new QAction(tr("Contacts"), this);
g_showContactsAct->setStatusTip(tr("Show Contacts"));
g_showContactsAct->setCheckable(true);
@@ -1154,8 +1266,19 @@ void MainWindow::CreateActions()
connect(g_showJointsAct, SIGNAL(triggered()), this,
SLOT(ShowJoints()));
+ g_showToolbarsAct = new QAction(tr("Show Toolbars"), this);
+ g_showToolbarsAct->setStatusTip(
+ tr("Show or hide the top and bottom toolbars"));
+ g_showToolbarsAct->setShortcut(tr("Ctrl+H"));
+ this->addAction(g_showToolbarsAct);
+ g_showToolbarsAct->setCheckable(true);
+ g_showToolbarsAct->setChecked(true);
+ connect(g_showToolbarsAct, SIGNAL(triggered()), this,
+ SLOT(ShowToolbars()));
+
g_fullScreenAct = new QAction(tr("Full Screen"), this);
- g_fullScreenAct->setStatusTip(tr("Full Screen(F-11 to exit)"));
+ g_fullScreenAct->setStatusTip(tr("Full Screen (F-11 to exit)"));
+ g_fullScreenAct->setShortcut(tr("F11"));
connect(g_fullScreenAct, SIGNAL(triggered()), this,
SLOT(FullScreen()));
@@ -1171,6 +1294,13 @@ void MainWindow::CreateActions()
g_orbitAct->setChecked(true);
connect(g_orbitAct, SIGNAL(triggered()), this, SLOT(Orbit()));
+ g_overlayAct = new QAction(tr("Show GUI Overlays"), this);
+ g_overlayAct->setStatusTip(tr("Show GUI Overlays"));
+ g_overlayAct->setEnabled(false);
+ g_overlayAct->setCheckable(true);
+ g_overlayAct->setChecked(false);
+ connect(g_overlayAct, SIGNAL(triggered()), this, SLOT(ShowGUIOverlays()));
+
QActionGroup *viewControlActionGroup = new QActionGroup(this);
viewControlActionGroup->addAction(g_fpsAct);
viewControlActionGroup->addAction(g_orbitAct);
@@ -1183,9 +1313,28 @@ void MainWindow::CreateActions()
g_viewOculusAct->setEnabled(false);
#endif
- g_dataLoggerAct = new QAction(tr("&Log Data"), this);
+ g_cameraOrthoAct = new QAction(tr("Orthographic"), this);
+ g_cameraOrthoAct->setStatusTip(tr("Orthographic Projection"));
+ g_cameraOrthoAct->setCheckable(true);
+ g_cameraOrthoAct->setChecked(false);
+
+ g_cameraPerspectiveAct = new QAction(tr("Perspective"), this);
+ g_cameraPerspectiveAct->setStatusTip(tr("Perspective Projection"));
+ g_cameraPerspectiveAct->setCheckable(true);
+ g_cameraPerspectiveAct->setChecked(true);
+
+ QActionGroup *projectionActionGroup = new QActionGroup(this);
+ projectionActionGroup->addAction(g_cameraOrthoAct);
+ projectionActionGroup->addAction(g_cameraPerspectiveAct);
+ projectionActionGroup->setExclusive(true);
+
+ g_dataLoggerAct = new QAction(QIcon(":images/log_record.png"),
+ tr("&Log Data"), this);
g_dataLoggerAct->setShortcut(tr("Ctrl+D"));
g_dataLoggerAct->setStatusTip(tr("Data Logging Utility"));
+ g_dataLoggerAct->setToolTip(tr("Log Data (Ctrl+D)"));
+ g_dataLoggerAct->setCheckable(true);
+ g_dataLoggerAct->setChecked(false);
connect(g_dataLoggerAct, SIGNAL(triggered()), this, SLOT(DataLogger()));
g_screenshotAct = new QAction(QIcon(":/images/screenshot.png"),
@@ -1284,6 +1433,40 @@ void MainWindow::CreateActions()
g_alignAct->setDefaultWidget(alignWidget);
g_alignAct->setEnabled(false);
connect(g_alignAct, SIGNAL(triggered()), this, SLOT(Align()));
+
+ // set up view angle actions and widget
+ QAction *viewAngleTop = new QAction(QIcon(":/images/view_angle_top.png"),
+ tr("View from the top"), this);
+ QAction *viewAngleBottom = new QAction(
+ QIcon(":/images/view_angle_bottom.png"),
+ tr("View from the bottom"), this);
+ QAction *viewAngleFront = new QAction(QIcon(":/images/view_angle_front.png"),
+ tr("View from the front"), this);
+ QAction *viewAngleBack = new QAction(QIcon(":/images/view_angle_back.png"),
+ tr("View from the back"), this);
+ QAction *viewAngleLeft = new QAction(QIcon(":/images/view_angle_left.png"),
+ tr("View from the left"), this);
+ QAction *viewAngleRight = new QAction(QIcon(":/images/view_angle_right.png"),
+ tr("View from the right"), this);
+
+ // Create another action instead of using g_resetAct here directly because
+ // we don't want the icon on the menu.
+ QAction *viewAngleReset = new QAction(QIcon(":/images/view_angle_home.png"),
+ tr("Reset View Angle"), this);
+ connect(g_resetAct, SIGNAL(triggered()), viewAngleReset, SLOT(trigger()));
+
+ ViewAngleWidget *viewAngleWidget = new ViewAngleWidget(this);
+ viewAngleWidget->setObjectName("viewAngleWidget");
+ viewAngleWidget->Add(ViewAngleWidget::TOP, viewAngleTop);
+ viewAngleWidget->Add(ViewAngleWidget::BOTTOM, viewAngleBottom);
+ viewAngleWidget->Add(ViewAngleWidget::FRONT, viewAngleFront);
+ viewAngleWidget->Add(ViewAngleWidget::BACK, viewAngleBack);
+ viewAngleWidget->Add(ViewAngleWidget::LEFT, viewAngleLeft);
+ viewAngleWidget->Add(ViewAngleWidget::RIGHT, viewAngleRight);
+ viewAngleWidget->Add(ViewAngleWidget::RESET, viewAngleReset);
+
+ g_viewAngleAct = new QWidgetAction(this);
+ g_viewAngleAct->setDefaultWidget(viewAngleWidget);
}
/////////////////////////////////////////////////
@@ -1304,6 +1487,7 @@ void MainWindow::ShowMenuBar(QMenuBar *_bar)
this->menuBar->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
this->setMenuBar(this->menuBar);
+ // populate main window's menu bar with menus from normal simulation mode
this->CreateMenuBar();
}
@@ -1316,12 +1500,20 @@ void MainWindow::ShowMenuBar(QMenuBar *_bar)
// Note: for some reason we can not call menuBar() again,
// so manually retrieving the menubar from the mainwindow.
QList<QMenuBar *> menuBars = this->findChildren<QMenuBar *>();
- newMenuBar = menuBars[0];
+ if (!menuBars.empty())
+ newMenuBar = menuBars[0];
}
else
{
newMenuBar = _bar;
}
+
+ if (!newMenuBar)
+ {
+ gzerr << "Unable to set NULL menu bar" << std::endl;
+ return;
+ }
+
QList<QMenu *> menus = newMenuBar->findChildren<QMenu *>();
for (int i = 0; i < menus.size(); ++i)
{
@@ -1335,6 +1527,170 @@ void MainWindow::ShowMenuBar(QMenuBar *_bar)
}
/////////////////////////////////////////////////
+void MainWindow::DeleteActions()
+{
+ delete g_topicVisAct;
+ g_topicVisAct = 0;
+
+ delete g_openAct;
+ g_openAct = 0;
+
+ delete g_saveAct;
+ g_saveAct = 0;
+
+ delete g_saveAsAct;
+ g_saveAsAct = 0;
+
+ delete g_saveCfgAct;
+ g_saveCfgAct = 0;
+
+ delete g_cloneAct;
+ g_cloneAct = 0;
+
+ delete g_aboutAct;
+ g_aboutAct = 0;
+
+ delete g_quitAct;
+ g_quitAct = 0;
+
+ delete g_resetModelsAct;
+ g_resetModelsAct = 0;
+
+ delete g_resetWorldAct;
+ g_resetWorldAct = 0;
+
+ delete g_editBuildingAct;
+ g_editBuildingAct = 0;
+
+ delete g_editTerrainAct;
+ g_editTerrainAct = 0;
+
+ delete g_editModelAct;
+ g_editModelAct = 0;
+
+ delete g_stepAct;
+ g_stepAct = 0;
+
+ delete g_playAct;
+ g_playAct = 0;
+
+ delete g_pauseAct;
+ g_pauseAct = 0;
+
+ delete g_arrowAct;
+ g_arrowAct = 0,
+
+ delete g_translateAct;
+ g_translateAct = 0;
+
+ delete g_rotateAct;
+ g_rotateAct = 0;
+
+ delete g_scaleAct;
+ g_scaleAct = 0;
+
+ delete g_boxCreateAct;
+ g_boxCreateAct = 0;
+
+ delete g_sphereCreateAct;
+ g_sphereCreateAct = 0;
+
+ delete g_cylinderCreateAct;
+ g_cylinderCreateAct = 0;
+
+ delete g_meshCreateAct;
+ g_meshCreateAct = 0;
+
+ delete g_pointLghtCreateAct;
+ g_pointLghtCreateAct = 0;
+
+ delete g_spotLghtCreateAct;
+ g_spotLghtCreateAct = 0;
+
+ delete g_dirLghtCreateAct;
+ g_dirLghtCreateAct = 0;
+
+ delete g_resetAct;
+ g_resetAct = 0;
+
+ delete g_showCollisionsAct;
+ g_showCollisionsAct = 0;
+
+ delete g_showGridAct;
+ g_showGridAct = 0;
+
+ delete g_showOriginAct;
+ g_showOriginAct = 0;
+
+ delete g_transparentAct;
+ g_transparentAct = 0;
+
+ delete g_viewWireframeAct;
+ g_viewWireframeAct = 0;
+
+ delete g_showCOMAct;
+ g_showCOMAct = 0;
+
+ delete g_showInertiaAct;
+ g_showInertiaAct = 0;
+
+ delete g_showLinkFrameAct;
+ g_showLinkFrameAct = 0;
+
+ delete g_showContactsAct;
+ g_showContactsAct = 0;
+
+ delete g_showJointsAct;
+ g_showJointsAct = 0;
+
+ delete g_showToolbarsAct;
+ g_showToolbarsAct = 0;
+
+ delete g_fullScreenAct;
+ g_fullScreenAct = 0;
+
+ delete g_fpsAct;
+ g_fpsAct = 0;
+
+ delete g_orbitAct;
+ g_orbitAct = 0;
+
+ delete g_overlayAct;
+ g_overlayAct = 0;
+
+ delete g_viewOculusAct;
+ g_viewOculusAct = 0;
+
+ delete g_dataLoggerAct;
+ g_dataLoggerAct = 0;
+
+ delete g_screenshotAct;
+ g_screenshotAct = 0;
+
+ delete g_copyAct;
+ g_copyAct = 0;
+
+ delete g_pasteAct;
+ g_pasteAct = 0;
+
+ delete g_snapAct;
+ g_snapAct = 0;
+
+ delete g_alignAct;
+ g_alignAct = 0;
+
+ delete g_cameraOrthoAct;
+ g_cameraOrthoAct = 0;
+
+ delete g_cameraPerspectiveAct;
+ g_cameraPerspectiveAct = 0;
+
+ delete g_viewAngleAct;
+ g_viewAngleAct = 0;
+}
+
+
+/////////////////////////////////////////////////
void MainWindow::CreateMenuBar()
{
// main window's menu bar
@@ -1355,16 +1711,25 @@ void MainWindow::CreateMenuBar()
this->editMenu = bar->addMenu(tr("&Edit"));
editMenu->addAction(g_resetModelsAct);
editMenu->addAction(g_resetWorldAct);
+ editMenu->addSeparator();
editMenu->addAction(g_editBuildingAct);
+ editMenu->addAction(g_editModelAct);
// \TODO: Add this back in when implementing the full Terrain Editor spec.
// editMenu->addAction(g_editTerrainAct);
- // \TODO: Add this back in when implementing the full Model Editor spec.
- editMenu->addAction(g_editModelAct);
+ QMenu *cameraMenu = bar->addMenu(tr("&Camera"));
+ cameraMenu->addAction(g_cameraOrthoAct);
+ cameraMenu->addAction(g_cameraPerspectiveAct);
+ cameraMenu->addSeparator();
+ cameraMenu->addAction(g_fpsAct);
+ cameraMenu->addAction(g_orbitAct);
+ cameraMenu->addSeparator();
+ cameraMenu->addAction(g_resetAct);
QMenu *viewMenu = bar->addMenu(tr("&View"));
viewMenu->addAction(g_showGridAct);
+ viewMenu->addAction(g_showOriginAct);
viewMenu->addSeparator();
viewMenu->addAction(g_transparentAct);
@@ -1373,22 +1738,18 @@ void MainWindow::CreateMenuBar()
viewMenu->addAction(g_showCollisionsAct);
viewMenu->addAction(g_showJointsAct);
viewMenu->addAction(g_showCOMAct);
+ viewMenu->addAction(g_showInertiaAct);
viewMenu->addAction(g_showContactsAct);
- viewMenu->addSeparator();
-
- viewMenu->addAction(g_resetAct);
- viewMenu->addAction(g_fullScreenAct);
- viewMenu->addSeparator();
-
- viewMenu->addAction(g_fpsAct);
- viewMenu->addAction(g_orbitAct);
+ viewMenu->addAction(g_showLinkFrameAct);
QMenu *windowMenu = bar->addMenu(tr("&Window"));
windowMenu->addAction(g_topicVisAct);
windowMenu->addSeparator();
- windowMenu->addAction(g_dataLoggerAct);
-
windowMenu->addAction(g_viewOculusAct);
+ windowMenu->addSeparator();
+ windowMenu->addAction(g_overlayAct);
+ windowMenu->addAction(g_showToolbarsAct);
+ windowMenu->addAction(g_fullScreenAct);
#ifdef HAVE_QWT
// windowMenu->addAction(g_diagnosticsAct);
@@ -1401,6 +1762,30 @@ void MainWindow::CreateMenuBar()
}
/////////////////////////////////////////////////
+void MainWindow::AddMenu(QMenu *_menu)
+{
+ if (!_menu)
+ return;
+
+ // Get the main window's menubar
+ // Note: for some reason we can not call menuBar() again,
+ // so manually retrieving the menubar from the mainwindow.
+ QList<QMenuBar *> menuBars = this->findChildren<QMenuBar *>();
+ if (!menuBars.empty())
+ {
+ // Note: addMenu(QMenu *) works the first time but when
+ // ShowMenuBar() is called more than once which results in menus being
+ // re-added, (e.g. when switching between model editor and simulation modes)
+ // _menu does not show up in the menu bar.
+ // So workaround is to use addMenu(QString)
+ QMenu *newMenu = menuBars[0]->addMenu(_menu->title());
+
+ for (auto &menuAct : _menu->actions())
+ newMenu->addAction(menuAct);
+ }
+}
+
+/////////////////////////////////////////////////
void MainWindow::CreateMenus()
{
this->ShowMenuBar();
@@ -1413,15 +1798,6 @@ void MainWindow::CreateMenus()
}
/////////////////////////////////////////////////
-void MainWindow::CreateToolbars()
-{
- this->playToolbar = this->addToolBar(tr("Play"));
- this->playToolbar->addAction(g_playAct);
- this->playToolbar->addAction(g_pauseAct);
- this->playToolbar->addAction(g_stepAct);
-}
-
-/////////////////////////////////////////////////
void MainWindow::OnMoveMode(bool _mode)
{
if (_mode)
@@ -1465,7 +1841,7 @@ void MainWindow::OnGUI(ConstGUIPtr &_msg)
math::Pose cam_pose(cam_pose_pos, cam_pose_rot);
- cam->SetWorldPose(cam_pose);
+ cam->SetDefaultPose(cam_pose);
cam->SetUseSDFPose(true);
}
@@ -1474,6 +1850,15 @@ void MainWindow::OnGUI(ConstGUIPtr &_msg)
cam->SetViewController(_msg->camera().view_controller());
}
+ if (_msg->camera().has_projection_type())
+ {
+ cam->SetProjectionType(_msg->camera().projection_type());
+ g_cameraOrthoAct->setChecked(true);
+ // Disable view control options when in ortho projection
+ g_fpsAct->setEnabled(false);
+ g_orbitAct->setEnabled(false);
+ }
+
if (_msg->camera().has_track())
{
std::string name = _msg->camera().track().name();
@@ -1535,6 +1920,9 @@ void MainWindow::OnAddPlugins()
}
}
this->pluginMsgs.clear();
+
+ g_overlayAct->setChecked(true);
+ g_overlayAct->setEnabled(true);
}
/////////////////////////////////////////////////
@@ -1675,36 +2063,6 @@ void MainWindow::OnSetSelectedEntity(const std::string &_name,
}
/////////////////////////////////////////////////
-void MainWindow::OnStats(ConstWorldStatisticsPtr &_msg)
-{
- if (_msg->paused() && g_pauseAct->isVisible())
- {
- g_pauseAct->setVisible(false);
- g_playAct->setVisible(true);
- }
- else if (!_msg->paused() && !g_playAct->isVisible())
- {
- g_pauseAct->setVisible(true);
- g_playAct->setVisible(false);
- }
-}
-
-/////////////////////////////////////////////////
-void MainWindow::OnPlayActionChanged()
-{
- if (g_playAct->isVisible())
- {
- g_stepAct->setToolTip("Step the world");
- g_stepAct->setEnabled(true);
- }
- else
- {
- g_stepAct->setToolTip("Pause the world before stepping");
- g_stepAct->setEnabled(false);
- }
-}
-
-/////////////////////////////////////////////////
void MainWindow::ItemSelected(QTreeWidgetItem *_item, int)
{
_item->setExpanded(!_item->isExpanded());
@@ -1736,16 +2094,28 @@ RenderWidget *MainWindow::GetRenderWidget() const
}
/////////////////////////////////////////////////
+bool MainWindow::IsPaused() const
+{
+ if (this->renderWidget)
+ {
+ TimePanel *timePanel = this->renderWidget->GetTimePanel();
+ if (timePanel)
+ return timePanel->IsPaused();
+ }
+ return false;
+}
+
+/////////////////////////////////////////////////
void MainWindow::CreateEditors()
{
// Create a Terrain Editor
- this->editors.push_back(new TerrainEditor(this));
+ this->editors["terrain"] = new TerrainEditor(this);
// Create a Building Editor
- this->editors.push_back(new BuildingEditor(this));
+ this->editors["building"] = new BuildingEditor(this);
// Create a Model Editor
- this->editors.push_back(new ModelEditor(this));
+ this->editors["model"] = new ModelEditor(this);
}
/////////////////////////////////////////////////
@@ -1789,3 +2159,39 @@ void MainWindow::OnEditorGroup(QAction *_action)
}
}
}
+
+/////////////////////////////////////////////////
+Editor *MainWindow::GetEditor(const std::string &_name) const
+{
+ auto iter = this->editors.find(_name);
+ if (iter != this->editors.end())
+ return iter->second;
+
+ return NULL;
+}
+
+/////////////////////////////////////////////////
+QAction *MainWindow::CloneAction(QAction *_action, QObject *_parent)
+{
+ if (!_action || !_parent)
+ {
+ gzwarn << "Missing action or parent. Not cloning action." << std::endl;
+ return NULL;
+ }
+
+ QAction *actionClone = new QAction(_action->text(), _parent);
+
+ // Copy basic information from original action.
+ actionClone->setStatusTip(_action->statusTip());
+ actionClone->setCheckable(_action->isCheckable());
+ actionClone->setChecked(_action->isChecked());
+
+ // Do not copy shortcut to avoid overlaps. Instead, connect actions.
+ // Cloned action will trigger original action, which does the desired effect.
+ connect(actionClone, SIGNAL(triggered()), _action, SLOT(trigger()));
+ // Then the original action reports its checked state to the cloned action
+ // without triggering it circularly.
+ connect(_action, SIGNAL(toggled(bool)), actionClone, SLOT(setChecked(bool)));
+
+ return actionClone;
+}
diff --git a/gazebo/gui/MainWindow.hh b/gazebo/gui/MainWindow.hh
index a55d32c..4520b2a 100644
--- a/gazebo/gui/MainWindow.hh
+++ b/gazebo/gui/MainWindow.hh
@@ -22,8 +22,11 @@
#include <vector>
#include <list>
+#include <boost/thread/mutex.hpp>
+
#include "gazebo/gazebo_config.h"
#include "gazebo/gui/qt.h"
+#include "gazebo/gui/DataLogger.hh"
#include "gazebo/common/Event.hh"
#include "gazebo/msgs/MessageTypes.hh"
#include "gazebo/transport/TransportTypes.hh"
@@ -72,6 +75,10 @@ namespace gazebo
/// \return A pointer to the render widget.
public: RenderWidget *GetRenderWidget() const;
+ /// \brief Returns the state of the simulation, true if paused.
+ /// \return True if paused, false otherwise.
+ public: bool IsPaused() const;
+
/// \brief Play simulation.
public slots: void Play();
@@ -82,9 +89,37 @@ namespace gazebo
/// \param[in] _on True to show the left pane, false to hide.
public: void SetLeftPaneVisibility(bool _on);
+ /// \brief Add a menu to the main window menu bar.
+ /// \param[in] _menu Menu to be added.
+ public: void AddMenu(QMenu *_menu);
+
+ /// \brief Show a custom menubar. If NULL is used, the default menubar
+ /// is shown.
+ /// \param[in] _bar The menubar to show. NULL will show the default
+ /// menubar.
+ public: void ShowMenuBar(QMenuBar *_bar = NULL);
+
+ /// \brief Create a new action with information from the given action,
+ /// such as text and tooltip. The new action triggers the original action
+ /// and follows its checked state. This is used for example to have the
+ /// "same" action on the main window menu and the model editor menu,
+ /// since an action can't be added to 2 different menus.
+ /// \param[in] _action Action to be cloned.
+ /// \param[in] _parent Parent for the new action.
+ /// \return The new action.
+ public: QAction *CloneAction(QAction *_action, QObject *_parent);
+
+ /// \brief Get an editor by name
+ /// \param[in] _name Name of the editor.
+ /// \return Pointer to the editor.
+ public: Editor *GetEditor(const std::string &_name) const;
+
/// \brief A signal to trigger loading of GUI plugins.
signals: void AddPlugins();
+ /// \brief A signal to indicate the main window is about to close.
+ signals: void Close();
+
protected: void closeEvent(QCloseEvent *_event);
private: void OnGUI(ConstGUIPtr &_msg);
@@ -134,13 +169,37 @@ namespace gazebo
private slots: void CaptureScreenshot();
private slots: void InsertModel();
+
+ /// \brief Qt callback when the show grid action is triggered.
private slots: void ShowGrid();
+
+ /// \brief Qt callback when the show origin action is triggered.
+ private slots: void ShowOrigin();
+
+ /// \brief Qt callback when the show collisions action is triggered.
private slots: void ShowCollisions();
+
+ /// \brief Qt callback when the show joints action is triggered.
private slots: void ShowJoints();
+
+ /// \brief Qt callback when the show contacts action is triggered.
private slots: void ShowContacts();
+
+ /// \brief Qt callback when the show center of mass action is triggered.
private slots: void ShowCOM();
- private slots: void Reset();
+
+ /// \brief Qt callback when the show inertia action is triggered.
+ private slots: void ShowInertia();
+
+ /// \brief Qt callback when the show link frame action is triggered.
+ private slots: void ShowLinkFrame();
+
+ /// \brief Qt callback when the full screen action is triggered.
private slots: void FullScreen();
+
+ /// \brief Qt callback when the show toolbars action is triggered.
+ private slots: void ShowToolbars();
+
private slots: void FPS();
private slots: void Orbit();
private slots: void ViewOculus();
@@ -149,12 +208,15 @@ namespace gazebo
private slots: void SetTransparent();
private slots: void SetWireframe();
- /// \brief Qt call back when the play action state changes
- private slots: void OnPlayActionChanged();
+ /// \brief Qt callback when the show GUI overlays action is triggered.
+ private slots: void ShowGUIOverlays();
/// \brief QT slot to open the data logger utility
private slots: void DataLogger();
+ /// \brief QT callback when the data logger is shut down.
+ private slots: void OnDataLoggerClosed();
+
/// \brief Callback when topic selection action.
private slots: void SelectTopic();
@@ -171,29 +233,28 @@ namespace gazebo
/// \brief Toggle full screen display.
/// \param[in] _value True to display in full screen mode.
private: void OnFullScreen(bool _value);
+
+ /// \brief Toggle toolbars display.
+ /// \param[in] _value True to display toolbars.
+ private: void OnShowToolbars(bool _value);
+
private: void OnMoveMode(bool _mode);
/// \brief Create most of the actions.
private: void CreateActions();
+ /// \brief Delete the actions created in CreateActions.
+ private: void DeleteActions();
+
/// \brief Create menus.
private: void CreateMenus();
- /// \brief Create the toolbars.
- private: void CreateToolbars();
-
/// \brief Create the main menu bar.
private: void CreateMenuBar();
/// \brief Create all the editors.
private: void CreateEditors();
- /// \brief Show a custom menubar. If NULL is used, the default menubar
- /// is shown.
- /// \param[in] _bar The menubar to show. NULL will show the default
- /// menubar.
- public: void ShowMenuBar(QMenuBar *_bar = NULL);
-
private: void OnModel(ConstModelPtr &_msg);
/// \brief Light message callback.
@@ -205,7 +266,6 @@ namespace gazebo
private: void OnManipMode(const std::string &_mode);
private: void OnSetSelectedEntity(const std::string &_name,
const std::string &_mode);
- private: void OnStats(ConstWorldStatisticsPtr &_msg);
/// \brief Handle event for changing the manual step size.
/// \param[in] _value New input step size.
@@ -276,8 +336,8 @@ namespace gazebo
/// \brief User specified step size for manually stepping the world
private: int inputStepSize;
- /// \brief List of all the editors.
- private: std::list<Editor*> editors;
+ /// \brief Map of all the editors to their names.
+ private: std::map<std::string, Editor *> editors;
/// \brief List of all the align action groups.
private: std::vector<QActionGroup *> alignActionGroups;
@@ -297,6 +357,9 @@ namespace gazebo
/// \brief Splitter for the main window.
private: QSplitter *splitter;
+
+ /// \brief Data logger dialog.
+ private: gui::DataLogger *dataLogger;
};
}
}
diff --git a/gazebo/gui/MainWindow_TEST.cc b/gazebo/gui/MainWindow_TEST.cc
index ca3f421..aa2cb10 100644
--- a/gazebo/gui/MainWindow_TEST.cc
+++ b/gazebo/gui/MainWindow_TEST.cc
@@ -21,7 +21,6 @@
#include "gazebo/gui/Actions.hh"
#include "gazebo/gui/GuiIface.hh"
#include "gazebo/gui/MainWindow.hh"
-#include "gazebo/gui/TimePanel.hh"
#include "gazebo/gui/GLWidget.hh"
#include "gazebo/gui/MainWindow_TEST.hh"
@@ -35,7 +34,7 @@ void OnRequest(ConstRequestPtr &_msg)
}
/////////////////////////////////////////////////
-void MainWindow_TEST::CopyPaste()
+void MainWindow_TEST::StepState()
{
this->resMaxPercentChange = 5.0;
this->shareMaxPercentChange = 2.0;
@@ -44,13 +43,241 @@ void MainWindow_TEST::CopyPaste()
gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
QVERIFY(mainWindow != NULL);
+ // Create the main window.
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ QVERIFY(gazebo::gui::g_stepAct != NULL);
+ QVERIFY(!gazebo::gui::g_stepAct->isEnabled());
+ QVERIFY(!mainWindow->IsPaused());
+
+
+ // toggle pause and play step and check if the step action is properly
+ // enabled / disabled.
+ 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());
+ QVERIFY(gazebo::gui::g_stepAct->isEnabled());
+
+ 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());
+ QVERIFY(!gazebo::gui::g_stepAct->isEnabled());
+
+ 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());
+ QVERIFY(gazebo::gui::g_stepAct->isEnabled());
+
+ mainWindow->close();
+ delete mainWindow;
+}
+
+/////////////////////////////////////////////////
+void MainWindow_TEST::Selection()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/shapes.world", false, false, false);
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
// Create the main window.
mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
- gazebo::rendering::create_scene(
- gazebo::physics::get_world()->GetName(), false);
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ gazebo::gui::GLWidget *glWidget =
+ mainWindow->findChild<gazebo::gui::GLWidget *>("GLWidget");
+ QVERIFY(glWidget != NULL);
+
+ gazebo::math::Vector2i glWidgetCenter(
+ glWidget->width()*0.5, glWidget->height()*0.5);
+
+ // get model at center of window - should get the box
+ gazebo::rendering::VisualPtr vis =
+ cam->GetVisual(glWidgetCenter);
+ QVERIFY(vis != NULL);
+ QVERIFY(vis->GetRootVisual()->GetName() == "box");
+
+ // move camera to look at the box
+ gazebo::math::Pose cameraPose(gazebo::math::Vector3(-1, 0, 0.5),
+ gazebo::math::Vector3(0, 0, 0));
+ cam->SetWorldPose(cameraPose);
+ QVERIFY(cam->GetWorldPose() == cameraPose);
+
+ // verify we get a box
+ gazebo::rendering::VisualPtr vis2 =
+ cam->GetVisual(gazebo::math::Vector2i(0, 0));
+ QVERIFY(vis2 != NULL);
+ QVERIFY(vis2->GetRootVisual()->GetName() == "box");
+
+ // look upwards
+ gazebo::math::Quaternion pitch90(gazebo::math::Vector3(0, -1.57, 0));
+ cam->SetWorldRotation(pitch90);
+ QVERIFY(cam->GetWorldRotation() == pitch90);
+
+ // verify there is nothing in the middle of the window
+ gazebo::rendering::VisualPtr vis3 = cam->GetVisual(glWidgetCenter);
+ QVERIFY(vis3 == NULL);
+
+ // reset orientation
+ gazebo::math::Quaternion identityRot(gazebo::math::Vector3(0, 0, 0));
+ cam->SetWorldRotation(identityRot);
+ QVERIFY(cam->GetWorldRotation() == identityRot);
+
+ // verify we can still get the box
+ gazebo::rendering::VisualPtr vis4 =
+ cam->GetVisual(gazebo::math::Vector2i(0, 0));
+ QVERIFY(vis4 != NULL);
+ QVERIFY(vis4->GetRootVisual()->GetName() == "box");
+
+ // hide the box
+ vis4->SetVisible(false);
+ gazebo::rendering::VisualPtr vis5 = cam->GetVisual(glWidgetCenter);
+
+ // verify we don't get anything now
+ QVERIFY(vis5 == NULL);
+
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+}
+
+/////////////////////////////////////////////////
+void MainWindow_TEST::SceneDestruction()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/shapes.world", false, false, false);
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+
+ // Create the main window.
+ mainWindow->Load();
+
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+ gazebo::rendering::ScenePtr scene = cam->GetScene();
+ QVERIFY(scene != NULL);
+
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+
+ // verify that this test case has the only scene shared pointer remaining.
+ QVERIFY(scene.use_count() == 1u);
+}
+
+/////////////////////////////////////////////////
+void MainWindow_TEST::UserCameraFPS()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/shapes.world", false, false, true);
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ // Create the main window.
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ // Wait a little bit for the average FPS to even out.
+ for (unsigned int i = 0; i < 10000; ++i)
+ {
+ gazebo::common::Time::NSleep(500000);
+ QCoreApplication::processEvents();
+ }
+
+ std::cerr << "\nFPS[" << cam->GetAvgFPS() << "]\n" << std::endl;
+ QVERIFY(cam->GetAvgFPS() > 55.0);
+ QVERIFY(cam->GetAvgFPS() < 75.0);
+
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+}
+
+/////////////////////////////////////////////////
+void MainWindow_TEST::CopyPaste()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/shapes.world", false, false, false);
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+
+ // Create the main window.
+ mainWindow->Load();
mainWindow->Init();
mainWindow->show();
@@ -98,23 +325,28 @@ void MainWindow_TEST::CopyPaste()
}
QVERIFY(modelVis->GetHighlighted());
+ // 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(gazebo::gui::g_copyAct != NULL);
+ QVERIFY(gazebo::gui::g_pasteAct != NULL);
+
// Copy the model
- QTest::keyClick(glWidget, Qt::Key_C, Qt::ControlModifier);
- QTest::qWait(500);
+ QTest::keyClick(glWidget, Qt::Key_C, Qt::ControlModifier, 100);
// Move to center of the screen
QPoint moveTo(glWidget->width()/2, glWidget->height()/2);
- QTest::mouseMove(glWidget, moveTo);
- QTest::qWait(500);
+ QTest::mouseMove(glWidget, moveTo, 100);
// Paste the model
- QTest::keyClick(glWidget, Qt::Key_V, Qt::ControlModifier);
- QTest::qWait(500);
+ QTest::keyClick(glWidget, Qt::Key_V, Qt::ControlModifier, 100);
// Release and spawn the model
- QTest::mouseClick(glWidget, Qt::LeftButton, Qt::NoModifier, moveTo);
- QTest::qWait(500);
-
+ QTest::mouseClick(glWidget, Qt::LeftButton, Qt::NoModifier, moveTo, 100);
QCoreApplication::processEvents();
// Verify there is a clone of the model
@@ -124,7 +356,7 @@ void MainWindow_TEST::CopyPaste()
while (!modelVisClone && sleep < maxSleep)
{
modelVisClone = scene->GetVisual(modelName + "_clone");
- QTest::qWait(30);
+ QTest::qWait(100);
sleep++;
}
QVERIFY(modelVisClone != NULL);
@@ -150,22 +382,20 @@ void MainWindow_TEST::CopyPaste()
QVERIFY(lightVis->GetHighlighted());
// Copy the light
- QTest::keyClick(glWidget, Qt::Key_C, Qt::ControlModifier);
- QTest::qWait(500);
+ QTest::keyClick(glWidget, Qt::Key_C, Qt::ControlModifier, 500);
+ QCoreApplication::processEvents();
// Move to center of the screen
QPoint moveTo(glWidget->width()/2, glWidget->height()/2);
- QTest::mouseMove(glWidget, moveTo);
- QTest::qWait(500);
+ QTest::mouseMove(glWidget, moveTo, 500);
+ QCoreApplication::processEvents();
// Paste the light
- QTest::keyClick(glWidget, Qt::Key_V, Qt::ControlModifier);
- QTest::qWait(500);
+ QTest::keyClick(glWidget, Qt::Key_V, Qt::ControlModifier, 500);
+ QCoreApplication::processEvents();
// Release and spawn the model
- QTest::mouseClick(glWidget, Qt::LeftButton, Qt::NoModifier, moveTo);
- QTest::qWait(500);
-
+ QTest::mouseClick(glWidget, Qt::LeftButton, Qt::NoModifier, moveTo, 500);
QCoreApplication::processEvents();
// Verify there is a clone of the light
@@ -202,7 +432,7 @@ void MainWindow_TEST::Wireframe()
node = gazebo::transport::NodePtr(new gazebo::transport::Node());
node->Init();
- sub = node->Subscribe("~/request", &OnRequest, this);
+ sub = node->Subscribe("~/request", &OnRequest, true);
// Create the main window.
gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
@@ -476,5 +706,371 @@ void MainWindow_TEST::UserCameraJoystick()
delete mainWindow;
}
+/////////////////////////////////////////////////
+void MainWindow_TEST::ActionCreationDestruction()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/empty.world", false, false, true);
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ // Create the main window.
+ mainWindow->Load();
+ mainWindow->Init();
+
+ QVERIFY(gazebo::gui::g_topicVisAct);
+
+ QVERIFY(gazebo::gui::g_openAct);
+
+ QVERIFY(gazebo::gui::g_saveAct);
+
+ QVERIFY(gazebo::gui::g_saveAsAct);
+
+ QVERIFY(gazebo::gui::g_saveCfgAct);
+
+ QVERIFY(gazebo::gui::g_cloneAct);
+
+ QVERIFY(gazebo::gui::g_aboutAct);
+
+ QVERIFY(gazebo::gui::g_quitAct);
+
+ QVERIFY(gazebo::gui::g_resetModelsAct);
+
+ QVERIFY(gazebo::gui::g_resetWorldAct);
+
+ QVERIFY(gazebo::gui::g_editBuildingAct);
+
+ QVERIFY(gazebo::gui::g_editTerrainAct);
+
+ QVERIFY(gazebo::gui::g_editModelAct);
+
+ QVERIFY(gazebo::gui::g_stepAct);
+
+ QVERIFY(gazebo::gui::g_playAct);
+
+ QVERIFY(gazebo::gui::g_pauseAct);
+
+ QVERIFY(gazebo::gui::g_arrowAct);
+
+ QVERIFY(gazebo::gui::g_translateAct);
+
+ QVERIFY(gazebo::gui::g_rotateAct);
+
+ QVERIFY(gazebo::gui::g_scaleAct);
+
+ QVERIFY(gazebo::gui::g_boxCreateAct);
+
+ QVERIFY(gazebo::gui::g_sphereCreateAct);
+
+ QVERIFY(gazebo::gui::g_cylinderCreateAct);
+
+ QVERIFY(gazebo::gui::g_meshCreateAct);
+
+ QVERIFY(gazebo::gui::g_pointLghtCreateAct);
+
+ QVERIFY(gazebo::gui::g_spotLghtCreateAct);
+
+ QVERIFY(gazebo::gui::g_dirLghtCreateAct);
+
+ QVERIFY(gazebo::gui::g_resetAct);
+
+ QVERIFY(gazebo::gui::g_showCollisionsAct);
+
+ QVERIFY(gazebo::gui::g_showGridAct);
+
+ QVERIFY(gazebo::gui::g_showOriginAct);
+
+ QVERIFY(gazebo::gui::g_showLinkFrameAct);
+
+ QVERIFY(gazebo::gui::g_transparentAct);
+
+ QVERIFY(gazebo::gui::g_viewWireframeAct);
+
+ QVERIFY(gazebo::gui::g_showCOMAct);
+
+ QVERIFY(gazebo::gui::g_showInertiaAct);
+
+ QVERIFY(gazebo::gui::g_showContactsAct);
+
+ QVERIFY(gazebo::gui::g_showJointsAct);
+
+ QVERIFY(gazebo::gui::g_showToolbarsAct);
+
+ QVERIFY(gazebo::gui::g_fullScreenAct);
+
+ QVERIFY(gazebo::gui::g_fpsAct);
+
+ QVERIFY(gazebo::gui::g_orbitAct);
+
+ QVERIFY(gazebo::gui::g_overlayAct);
+
+ QVERIFY(gazebo::gui::g_viewOculusAct);
+
+ QVERIFY(gazebo::gui::g_dataLoggerAct);
+
+ QVERIFY(gazebo::gui::g_screenshotAct);
+
+ QVERIFY(gazebo::gui::g_copyAct);
+
+ QVERIFY(gazebo::gui::g_pasteAct);
+
+ QVERIFY(gazebo::gui::g_snapAct);
+
+ QVERIFY(gazebo::gui::g_alignAct);
+
+ QVERIFY(gazebo::gui::g_viewAngleAct);
+
+ QVERIFY(gazebo::gui::g_cameraOrthoAct);
+
+ QVERIFY(gazebo::gui::g_cameraPerspectiveAct);
+
+ mainWindow->close();
+ delete mainWindow;
+
+ QVERIFY(!gazebo::gui::g_topicVisAct);
+
+ QVERIFY(!gazebo::gui::g_openAct);
+
+ QVERIFY(!gazebo::gui::g_saveAct);
+
+ QVERIFY(!gazebo::gui::g_saveAsAct);
+
+ QVERIFY(!gazebo::gui::g_saveCfgAct);
+
+ QVERIFY(!gazebo::gui::g_cloneAct);
+
+ QVERIFY(!gazebo::gui::g_aboutAct);
+
+ QVERIFY(!gazebo::gui::g_quitAct);
+
+ QVERIFY(!gazebo::gui::g_resetModelsAct);
+
+ QVERIFY(!gazebo::gui::g_resetWorldAct);
+
+ QVERIFY(!gazebo::gui::g_editBuildingAct);
+
+ QVERIFY(!gazebo::gui::g_editTerrainAct);
+
+ QVERIFY(!gazebo::gui::g_editModelAct);
+
+ QVERIFY(!gazebo::gui::g_stepAct);
+
+ QVERIFY(!gazebo::gui::g_playAct);
+
+ QVERIFY(!gazebo::gui::g_pauseAct);
+
+ QVERIFY(!gazebo::gui::g_arrowAct);
+
+ QVERIFY(!gazebo::gui::g_translateAct);
+
+ QVERIFY(!gazebo::gui::g_rotateAct);
+
+ QVERIFY(!gazebo::gui::g_scaleAct);
+
+ QVERIFY(!gazebo::gui::g_boxCreateAct);
+
+ QVERIFY(!gazebo::gui::g_sphereCreateAct);
+
+ QVERIFY(!gazebo::gui::g_cylinderCreateAct);
+
+ QVERIFY(!gazebo::gui::g_meshCreateAct);
+
+ QVERIFY(!gazebo::gui::g_pointLghtCreateAct);
+
+ QVERIFY(!gazebo::gui::g_spotLghtCreateAct);
+
+ QVERIFY(!gazebo::gui::g_dirLghtCreateAct);
+
+ QVERIFY(!gazebo::gui::g_resetAct);
+
+ QVERIFY(!gazebo::gui::g_showCollisionsAct);
+
+ QVERIFY(!gazebo::gui::g_showGridAct);
+
+ QVERIFY(!gazebo::gui::g_showOriginAct);
+
+ QVERIFY(!gazebo::gui::g_showLinkFrameAct);
+
+ QVERIFY(!gazebo::gui::g_transparentAct);
+
+ QVERIFY(!gazebo::gui::g_viewWireframeAct);
+
+ QVERIFY(!gazebo::gui::g_showCOMAct);
+
+ QVERIFY(!gazebo::gui::g_showInertiaAct);
+
+ QVERIFY(!gazebo::gui::g_showContactsAct);
+
+ QVERIFY(!gazebo::gui::g_showJointsAct);
+
+ QVERIFY(!gazebo::gui::g_showToolbarsAct);
+
+ QVERIFY(!gazebo::gui::g_fullScreenAct);
+
+ QVERIFY(!gazebo::gui::g_fpsAct);
+
+ QVERIFY(!gazebo::gui::g_orbitAct);
+
+ QVERIFY(!gazebo::gui::g_overlayAct);
+
+ QVERIFY(!gazebo::gui::g_viewOculusAct);
+
+ QVERIFY(!gazebo::gui::g_dataLoggerAct);
+
+ QVERIFY(!gazebo::gui::g_screenshotAct);
+
+ QVERIFY(!gazebo::gui::g_copyAct);
+
+ QVERIFY(!gazebo::gui::g_pasteAct);
+
+ QVERIFY(!gazebo::gui::g_snapAct);
+
+ QVERIFY(!gazebo::gui::g_alignAct);
+
+ QVERIFY(!gazebo::gui::g_viewAngleAct);
+
+ QVERIFY(!gazebo::gui::g_cameraOrthoAct);
+
+ QVERIFY(!gazebo::gui::g_cameraPerspectiveAct);
+}
+
+/////////////////////////////////////////////////
+void MainWindow_TEST::SetUserCameraPoseSDF()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/usercamera_test.world", false, false, false);
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+
+ // Create the main window.
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ cam->SetCaptureData(true);
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ const unsigned char *data = cam->GetImageData();
+ unsigned int width = cam->GetImageWidth();
+ unsigned int height = cam->GetImageHeight();
+ unsigned int depth = cam->GetImageDepth();
+
+ // Part 1 : The user camera should be positioned so that it sees only
+ // a white box
+ {
+ int blackCount = 0;
+
+ // Get the number of black pixels
+ for (unsigned int y = 0; y < height; ++y)
+ {
+ for (unsigned int x = 0; x < width*depth; ++x)
+ {
+ if (data[y*(width*depth) + x] <= 10)
+ blackCount++;
+ }
+ }
+
+ // Make sure the black count is zero. This means the camera is
+ // positioned correctly
+ QVERIFY(blackCount == 0);
+ }
+
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+}
+
+/////////////////////////////////////////////////
+void MainWindow_TEST::MenuBar()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/empty.world", false, false, false);
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+
+ // Create the main window.
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Get the user camera
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ QList<QMenuBar *> menuBars = mainWindow->findChildren<QMenuBar *>();
+ QVERIFY(!menuBars.empty());
+
+ std::set<std::string> mainMenus;
+ mainMenus.insert("&File");
+ mainMenus.insert("&Edit");
+ mainMenus.insert("&Camera");
+ mainMenus.insert("&View");
+ mainMenus.insert("&Window");
+ mainMenus.insert("&Help");
+
+ // verify all menus are created in the menu bar.
+ std::set<std::string> mainMenusCopy = mainMenus;
+ QMenuBar *menuBar = menuBars[0];
+ QList<QMenu *> menus = menuBar->findChildren<QMenu *>();
+ for (auto &m : menus)
+ {
+ auto it = mainMenusCopy.find(m->title().toStdString());
+ QVERIFY(it != mainMenus.end());
+ mainMenusCopy.erase(it);
+ }
+
+ // test adding a new menu to the menu bar
+ QMenu newMenu(tr("&TEST"));
+ mainWindow->AddMenu(&newMenu);
+
+ QList<QMenu *> newMenus = menuBar->findChildren<QMenu *>();
+ mainMenusCopy = mainMenus;
+ mainMenusCopy.insert("&TEST");
+ for (auto &m : menus)
+ {
+ std::string title = m->title().toStdString();
+ auto it = mainMenusCopy.find(title);
+ QVERIFY(it != mainMenus.end());
+ mainMenusCopy.erase(it);
+ }
+
+ // test calling ShowMenuBar and verify all menus remain the same
+ mainWindow->ShowMenuBar();
+
+ menus = menuBar->findChildren<QMenu *>();
+ mainMenusCopy = mainMenus;
+ mainMenusCopy.insert("TEST");
+ for (auto &m : menus)
+ {
+ std::string title = m->title().toStdString();
+ auto it = mainMenusCopy.find(title);
+ QVERIFY(it != mainMenus.end());
+ mainMenusCopy.erase(title);
+ }
+
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+}
+
// Generate a main function for the test
QTEST_MAIN(MainWindow_TEST)
diff --git a/gazebo/gui/MainWindow_TEST.hh b/gazebo/gui/MainWindow_TEST.hh
index 9b2e135..a9bd9db 100644
--- a/gazebo/gui/MainWindow_TEST.hh
+++ b/gazebo/gui/MainWindow_TEST.hh
@@ -25,6 +25,21 @@ class MainWindow_TEST : public QTestFixture
{
Q_OBJECT
+ /// \brief Test the step action state when simulation is running and paused.
+ private slots: void StepState();
+
+ /// \brief Test that Actions created in MainWindow get destroyed.
+ private slots: void ActionCreationDestruction();
+
+ /// \brief Test scene destruction on shutdown
+ private slots: void SceneDestruction();
+
+ /// \brief Test user camera entity selection
+ private slots: void Selection();
+
+ /// \brief Test user camera frames per second
+ private slots: void UserCameraFPS();
+
/// \brief Test copying and pasting a model and a light
private slots: void CopyPaste();
@@ -37,6 +52,12 @@ class MainWindow_TEST : public QTestFixture
/// \brief Test moving the user camera via a joystick message.
private slots: void UserCameraJoystick();
+
+ /// \brief Test Set user camera pose via SDF
+ private slots: void SetUserCameraPoseSDF();
+
+ /// \brief Test that menus are created in the main window menu bar.
+ private slots: void MenuBar();
};
#endif
diff --git a/gazebo/gui/MeshMaker.cc b/gazebo/gui/MeshMaker.cc
index 5ab376d..e2bf2bc 100644
--- a/gazebo/gui/MeshMaker.cc
+++ b/gazebo/gui/MeshMaker.cc
@@ -14,6 +14,13 @@
* 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 <sstream>
#include "gazebo/msgs/msgs.hh"
@@ -43,9 +50,9 @@ MeshMaker::MeshMaker()
this->visualMsg = new msgs::Visual();
this->visualMsg->mutable_geometry()->set_type(msgs::Geometry::MESH);
msgs::Set(this->visualMsg->mutable_pose()->mutable_orientation(),
- math::Quaternion());
+ ignition::math::Quaterniond());
msgs::Set(this->visualMsg->mutable_pose()->mutable_position(),
- math::Vector3());
+ ignition::math::Vector3d());
}
/////////////////////////////////////////////////
@@ -109,13 +116,13 @@ void MeshMaker::OnMouseRelease(const common::MouseEvent &/*_event*/)
/////////////////////////////////////////////////
void MeshMaker::OnMouseMove(const common::MouseEvent &_event)
{
- math::Pose pose = msgs::Convert(this->visualMsg->pose());
+ ignition::math::Pose3d pose = msgs::ConvertIgn(this->visualMsg->pose());
math::Vector3 origin1, dir1, p1;
math::Vector3 origin2, dir2, p2;
// Cast two rays from the camera into the world
- this->camera->GetCameraToViewportRay(_event.pos.x, _event.pos.y,
+ this->camera->GetCameraToViewportRay(_event.Pos().X(), _event.Pos().Y(),
origin1, dir1);
// Compute the distance from the camera to plane of translation
@@ -126,24 +133,24 @@ void MeshMaker::OnMouseMove(const common::MouseEvent &_event)
// Compute two points on the plane. The first point is the current
// mouse position, the second is the previous mouse position
p1 = origin1 + dir1 * dist1;
- pose.pos = p1;
+ pose.Pos() = p1.Ign();
- if (!_event.shift)
+ if (!_event.Shift())
{
- if (ceil(pose.pos.x) - pose.pos.x <= .4)
- pose.pos.x = ceil(pose.pos.x);
- else if (pose.pos.x - floor(pose.pos.x) <= .4)
- pose.pos.x = floor(pose.pos.x);
-
- if (ceil(pose.pos.y) - pose.pos.y <= .4)
- pose.pos.y = ceil(pose.pos.y);
- else if (pose.pos.y - floor(pose.pos.y) <= .4)
- pose.pos.y = floor(pose.pos.y);
-
- if (ceil(pose.pos.z) - pose.pos.z <= .4)
- pose.pos.z = ceil(pose.pos.z);
- else if (pose.pos.z - floor(pose.pos.z) <= .4)
- pose.pos.z = floor(pose.pos.z);
+ if (ceil(pose.Pos().X()) - pose.Pos().X() <= .4)
+ pose.Pos().X() = ceil(pose.Pos().X());
+ else if (pose.Pos().X() - floor(pose.Pos().X()) <= .4)
+ pose.Pos().X() = floor(pose.Pos().X());
+
+ if (ceil(pose.Pos().Y()) - pose.Pos().Y() <= .4)
+ pose.Pos().Y() = ceil(pose.Pos().Y());
+ else if (pose.Pos().Y() - floor(pose.Pos().Y()) <= .4)
+ pose.Pos().Y() = floor(pose.Pos().Y());
+
+ if (ceil(pose.Pos().Z()) - pose.Pos().Z() <= .4)
+ pose.Pos().Z() = ceil(pose.Pos().Z());
+ else if (pose.Pos().Z() - floor(pose.Pos().Z()) <= .4)
+ pose.Pos().Z() = floor(pose.Pos().Z());
}
msgs::Set(this->visualMsg->mutable_pose(), pose);
diff --git a/gazebo/gui/ModelAlign.cc b/gazebo/gui/ModelAlign.cc
index 7652520..aceb83b 100644
--- a/gazebo/gui/ModelAlign.cc
+++ b/gazebo/gui/ModelAlign.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/transport/transport.hh"
#include "gazebo/rendering/RenderTypes.hh"
@@ -46,12 +52,26 @@ ModelAlign::ModelAlign()
/////////////////////////////////////////////////
ModelAlign::~ModelAlign()
{
- this->dataPtr->modelPub.reset();
+ this->Clear();
delete this->dataPtr;
this->dataPtr = NULL;
}
/////////////////////////////////////////////////
+void ModelAlign::Clear()
+{
+ this->dataPtr->targetVis.reset();
+ this->dataPtr->userCamera.reset();
+ this->dataPtr->scene.reset();
+ this->dataPtr->node.reset();
+ this->dataPtr->modelPub.reset();
+ this->dataPtr->selectedVisuals.clear();
+ this->dataPtr->connections.clear();
+ this->dataPtr->originalVisualPose.clear();
+ this->dataPtr->initialized = false;
+}
+
+/////////////////////////////////////////////////
void ModelAlign::Init()
{
if (this->dataPtr->initialized)
@@ -172,8 +192,7 @@ void ModelAlign::AlignVisuals(std::vector<rendering::VisualPtr> _visuals,
if (it->first)
{
it->first->SetWorldPose(it->second);
- it->first->SetTransparency(std::abs(
- it->first->GetTransparency()*2.0-1.0));
+ this->SetHighlighted(it->first, false);
}
}
this->dataPtr->originalVisualPose.clear();
@@ -245,7 +264,7 @@ void ModelAlign::AlignVisuals(std::vector<rendering::VisualPtr> _visuals,
this->dataPtr->originalVisualPose.end())
{
this->dataPtr->originalVisualPose[vis] = vis->GetWorldPose();
- vis->SetTransparency((1.0 - vis->GetTransparency()) * 0.5);
+ this->SetHighlighted(vis, true);
}
// prevent the visual pose from being updated by the server
if (this->dataPtr->scene)
@@ -286,7 +305,34 @@ void ModelAlign::PublishVisualPose(rendering::VisualPtr _vis)
msg.set_id(gui::get_entity_id(_vis->GetName()));
msg.set_name(_vis->GetName());
- msgs::Set(msg.mutable_pose(), _vis->GetWorldPose());
+ msgs::Set(msg.mutable_pose(), _vis->GetWorldPose().Ign());
this->dataPtr->modelPub->Publish(msg);
}
}
+
+/////////////////////////////////////////////////
+void ModelAlign::SetHighlighted(rendering::VisualPtr _vis, bool _highlight)
+{
+ if (_vis->GetChildCount() != 0)
+ {
+ for (unsigned int j = 0; j < _vis->GetChildCount(); ++j)
+ {
+ this->SetHighlighted(_vis->GetChild(j), _highlight);
+ }
+ }
+ else
+ {
+ // Highlighting increases transparency for opaque visuals (0 < t < 0.3) and
+ // decreases transparency for semi-transparent visuals (0.3 < t < 1).
+ // A visual will never become fully transparent (t = 1) when highlighted.
+ if (_highlight)
+ {
+ _vis->SetTransparency((1.0 - _vis->GetTransparency()) * 0.5);
+ }
+ // The inverse operation restores the original transparency value.
+ else
+ {
+ _vis->SetTransparency(std::abs(_vis->GetTransparency()*2.0-1.0));
+ }
+ }
+}
diff --git a/gazebo/gui/ModelAlign.hh b/gazebo/gui/ModelAlign.hh
index 246f31e..0432030 100644
--- a/gazebo/gui/ModelAlign.hh
+++ b/gazebo/gui/ModelAlign.hh
@@ -47,10 +47,15 @@ namespace gazebo
/// \brief Initialize the model alignment tool.
public: void Init();
+ /// \brief Clear the model alignment tool. This explicity cleans up the
+ /// internal state of the singleton and prepares it for exit.
+ public: void Clear();
+
/// \brief Callback when a specific alignment configuration is set.
/// \param[in] _visuals Visuals to be aligned.
/// \param[in] _axis Axis of alignment: x, y, or z.
- /// \param[in] _config Configuration: min, center, or max.
+ /// \param[in] _config Either a configuration (min, center, max),
+ /// or "reset" to restore the original pose.
/// \param[in] _publish True to publish new alignment pose
public: void AlignVisuals(std::vector<rendering::VisualPtr> _visuals,
const std::string &_axis, const std::string &_config,
@@ -75,6 +80,13 @@ namespace gazebo
private: void Transform(math::Box _bbox, math::Pose _worldPose,
std::vector<math::Vector3> &_vertices);
+ /// \brief Change the transparency of the visual's leaf children to
+ /// indicate a highlighted state or not. Must do it for each leaf as
+ /// they might have different transparencies.
+ /// \param[in] _vis Visual to be highlighted.
+ /// \param[in] _highlight Whether to highlight or not.
+ private: void SetHighlighted(rendering::VisualPtr _vis, bool _highlight);
+
/// \brief This is a singleton class.
private: friend class SingletonT<ModelAlign>;
diff --git a/gazebo/gui/ModelAlignPrivate.hh b/gazebo/gui/ModelAlignPrivate.hh
index 4e4fd63..7ebda8d 100644
--- a/gazebo/gui/ModelAlignPrivate.hh
+++ b/gazebo/gui/ModelAlignPrivate.hh
@@ -48,15 +48,6 @@ namespace gazebo
/// \brief Pointer to the scene where models are in.
public: rendering::ScenePtr scene;
- /// \brief Ray query for selecting a surface of an entity.
- public: rendering::RayQueryPtr rayQuery;
-
- /// \brief Current mouse event.
- public: common::MouseEvent mouseEvent;
-
- /// \brief Current key event.
- public: common::KeyEvent keyEvent;
-
/// \brief The last selected visual which will be used for alignment.
public: rendering::VisualPtr targetVis;
diff --git a/gazebo/gui/ModelAlign_TEST.cc b/gazebo/gui/ModelAlign_TEST.cc
index 8c7613d..6e88bdd 100644
--- a/gazebo/gui/ModelAlign_TEST.cc
+++ b/gazebo/gui/ModelAlign_TEST.cc
@@ -19,6 +19,7 @@
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/MainWindow.hh"
#include "gazebo/gui/ModelAlign.hh"
#include "gazebo/rendering/RenderEvents.hh"
@@ -706,7 +707,6 @@ void ModelAlign_TEST::AlignZMax()
}
}
-
/////////////////////////////////////////////////
void ModelAlign_TEST::AlignScale()
{
@@ -789,5 +789,137 @@ void ModelAlign_TEST::AlignScale()
}
}
+/////////////////////////////////////////////////
+void ModelAlign_TEST::SetHighlighted()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/blank.world", false, false, true);
+
+ gazebo::rendering::ScenePtr scene;
+ scene = gazebo::rendering::get_scene("default");
+ QVERIFY(scene != NULL);
+
+ // Create a deeply nested visual where each level has a different transparency
+ gazebo::rendering::VisualPtr vis1;
+ vis1.reset(new gazebo::rendering::Visual("vis1", scene->GetWorldVisual()));
+ vis1->Load();
+ double vis1Transp = 0.0;
+ QVERIFY(gazebo::math::equal(
+ static_cast<double>(vis1->GetTransparency()), vis1Transp, 1e-5));
+
+ gazebo::rendering::VisualPtr vis2;
+ vis2.reset(new gazebo::rendering::Visual("vis2", vis1));
+ vis2->Load();
+ double vis2Transp = 0.6;
+ vis2->SetTransparency(vis2Transp);
+ QVERIFY(gazebo::math::equal(
+ static_cast<double>(vis2->GetTransparency()), vis2Transp, 1e-5));
+
+ gazebo::rendering::VisualPtr vis3_1;
+ vis3_1.reset(new gazebo::rendering::Visual("vis3_1", vis2));
+ vis3_1->Load();
+ double vis3_1Transp = 0.25;
+ vis3_1->SetTransparency(vis3_1Transp);
+ QVERIFY(gazebo::math::equal(
+ static_cast<double>(vis3_1->GetTransparency()), vis3_1Transp, 1e-5));
+
+ gazebo::rendering::VisualPtr vis3_2;
+ vis3_2.reset(new gazebo::rendering::Visual("vis3_2_LEAF", vis2));
+ vis3_2->Load();
+ double vis3_2Transp = 1.0;
+ vis3_2->SetTransparency(vis3_2Transp);
+ QVERIFY(gazebo::math::equal(
+ static_cast<double>(vis3_2->GetTransparency()), vis3_2Transp, 1e-5));
+
+ gazebo::rendering::VisualPtr vis4;
+ vis4.reset(new gazebo::rendering::Visual("vis4_LEAF", vis3_1));
+ vis4->Load();
+ double vis4Transp = 0.9;
+ vis4->SetTransparency(vis4Transp);
+ QVERIFY(gazebo::math::equal(
+ static_cast<double>(vis4->GetTransparency()), vis4Transp, 1e-5));
+
+ // Create another model just to align them
+ gazebo::rendering::VisualPtr otherVis;
+ otherVis.reset(
+ new gazebo::rendering::Visual("otherVis", scene->GetWorldVisual()));
+ otherVis->Load();
+
+ // Align preview
+ std::vector<gazebo::rendering::VisualPtr> modelVisuals;
+ modelVisuals.push_back(vis1);
+ modelVisuals.push_back(otherVis);
+
+ gazebo::gui::ModelAlign::Instance()->Init();
+ gazebo::gui::ModelAlign::Instance()->AlignVisuals(
+ modelVisuals, "x", "min", "last", false);
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ }
+
+ // Check that the transparency of the leaves have changed
+ QVERIFY(!gazebo::math::equal(
+ static_cast<double>(vis3_2->GetTransparency()), vis3_2Transp, 1e-5));
+ QVERIFY(!gazebo::math::equal(
+ static_cast<double>(vis4->GetTransparency()), vis4Transp, 1e-5));
+
+ // Reset
+ gazebo::gui::ModelAlign::Instance()->AlignVisuals(
+ modelVisuals, "x", "reset", "last", false);
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ }
+
+ // Check that the transparency of the leaves have the original value
+ QVERIFY(gazebo::math::equal(
+ static_cast<double>(vis3_2->GetTransparency()), vis3_2Transp, 1e-5));
+ QVERIFY(gazebo::math::equal(
+ static_cast<double>(vis4->GetTransparency()), vis4Transp, 1e-5));
+
+ // Align preview again
+ gazebo::gui::ModelAlign::Instance()->AlignVisuals(
+ modelVisuals, "z", "max", "last", false);
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ }
+
+ // Check that the transparency of the leaves have changed
+ QVERIFY(!gazebo::math::equal(
+ static_cast<double>(vis3_2->GetTransparency()), vis3_2Transp, 1e-5));
+ QVERIFY(!gazebo::math::equal(
+ static_cast<double>(vis4->GetTransparency()), vis4Transp, 1e-5));
+
+ // Publish the position
+ gazebo::gui::ModelAlign::Instance()->AlignVisuals(
+ modelVisuals, "z", "max", "last", true);
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ }
+
+ // Check that the transparency of the leaves have the original value
+ QVERIFY(gazebo::math::equal(
+ static_cast<double>(vis3_2->GetTransparency()), vis3_2Transp, 1e-5));
+ QVERIFY(gazebo::math::equal(
+ static_cast<double>(vis4->GetTransparency()), vis4Transp, 1e-5));
+}
+
// Generate a main function for the test
QTEST_MAIN(ModelAlign_TEST)
diff --git a/gazebo/gui/ModelAlign_TEST.hh b/gazebo/gui/ModelAlign_TEST.hh
index 9dc7165..565fce1 100644
--- a/gazebo/gui/ModelAlign_TEST.hh
+++ b/gazebo/gui/ModelAlign_TEST.hh
@@ -54,6 +54,9 @@ class ModelAlign_TEST : public QTestFixture
/// \brief Test aligning models with non unit scale.
private slots: void AlignScale();
+
+ /// \brief Test if the highlighting is properly reset for nested visuals.
+ private slots: void SetHighlighted();
};
#endif
diff --git a/gazebo/gui/ModelBuilderWidget.cc b/gazebo/gui/ModelBuilderWidget.cc
index 1294ee5..e3d44b2 100644
--- a/gazebo/gui/ModelBuilderWidget.cc
+++ b/gazebo/gui/ModelBuilderWidget.cc
@@ -14,6 +14,13 @@
* 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 "gazebo/rendering/RenderingIface.hh"
#include "gazebo/rendering/UserCamera.hh"
diff --git a/gazebo/gui/ModelEditWidget.cc b/gazebo/gui/ModelEditWidget.cc
index 422004d..11a4cf1 100644
--- a/gazebo/gui/ModelEditWidget.cc
+++ b/gazebo/gui/ModelEditWidget.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/transport/Node.hh"
#include "gazebo/transport/Publisher.hh"
diff --git a/gazebo/gui/ModelListWidget.cc b/gazebo/gui/ModelListWidget.cc
index c10c6bb..1c29622 100644
--- a/gazebo/gui/ModelListWidget.cc
+++ b/gazebo/gui/ModelListWidget.cc
@@ -14,6 +14,13 @@
* 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 <google/protobuf/descriptor.h>
#include <google/protobuf/message.h>
@@ -93,6 +100,8 @@ ModelListWidget::ModelListWidget(QWidget *_parent)
this->variantManager = new QtVariantPropertyManager();
this->propTreeBrowser = new QtTreePropertyBrowser();
this->propTreeBrowser->setObjectName("propTreeBrowser");
+ this->propTreeBrowser->setStyleSheet(
+ "QTreeView::branch:selected:active { background-color: transparent; }");
this->variantFactory = new QtVariantEditorFactory();
this->propTreeBrowser->setFactoryForManager(this->variantManager,
this->variantFactory);
@@ -189,7 +198,42 @@ void ModelListWidget::OnModelSelection(QTreeWidgetItem *_item, int /*_column*/)
this->selectedEntityName);
this->requestPub->Publish(*this->requestMsg);
}
+ else if (name == "GUI")
+ {
+ QtVariantProperty *item = NULL;
+
+ rendering::UserCameraPtr cam = gui::get_active_camera();
+ if (!cam)
+ return;
+
+ // Create a camera item
+ QtProperty *topItem = this->variantManager->addProperty(
+ QtVariantPropertyManager::groupTypeId(), tr("camera"));
+ auto cameraBrowser = this->propTreeBrowser->addProperty(topItem);
+
+ // Create and set the gui camera name
+ std::string cameraName = cam->GetName();
+ item = this->variantManager->addProperty(QVariant::String, tr("name"));
+ item->setValue(cameraName.c_str());
+ topItem->addSubProperty(item);
+ item->setEnabled(false);
+
+ // Create and set the gui camera pose
+ item = this->variantManager->addProperty(
+ QtVariantPropertyManager::groupTypeId(), tr("pose"));
+ {
+ topItem->addSubProperty(item);
+ ignition::math::Pose3d cameraPose = cam->GetWorldPose().Ign();
+ this->FillPoseProperty(msgs::Convert(cameraPose), item);
+ // set expanded to true by default for easier viewing
+ this->propTreeBrowser->setExpanded(cameraBrowser, true);
+ for (auto browser : cameraBrowser->children())
+ {
+ this->propTreeBrowser->setExpanded(browser, true);
+ }
+ }
+ }
else
{
this->propTreeBrowser->clear();
@@ -434,7 +478,7 @@ void ModelListWidget::OnResponse(ConstResponsePtr &_msg)
}
else if (_msg->has_type() && _msg->type() == "error")
{
- if (_msg->response() == "nonexistant")
+ if (_msg->response() == "nonexistent")
{
this->removeEntityList.push_back(this->selectedEntityName);
}
@@ -509,7 +553,8 @@ void ModelListWidget::OnCustomContextMenu(const QPoint &_pt)
if (i >= 0)
{
g_modelRightMenu->Run(item->text(0).toStdString(),
- this->modelTreeWidget->mapToGlobal(_pt));
+ this->modelTreeWidget->mapToGlobal(_pt),
+ ModelRightMenu::EntityTypes::MODEL);
return;
}
@@ -518,7 +563,16 @@ void ModelListWidget::OnCustomContextMenu(const QPoint &_pt)
if (i >= 0)
{
g_modelRightMenu->Run(item->text(0).toStdString(),
- this->modelTreeWidget->mapToGlobal(_pt));
+ this->modelTreeWidget->mapToGlobal(_pt),
+ ModelRightMenu::EntityTypes::LIGHT);
+ }
+
+ // Check to see if the selected item is a link
+ if (item->data(3, Qt::UserRole).toString().toStdString() == "Link")
+ {
+ g_modelRightMenu->Run(item->data(0, Qt::UserRole).toString().toStdString(),
+ this->modelTreeWidget->mapToGlobal(_pt),
+ ModelRightMenu::EntityTypes::LINK);
}
}
@@ -555,6 +609,8 @@ void ModelListWidget::OnPropertyChanged(QtProperty *_item)
this->ScenePropertyChanged(_item);
else if (currentItem == this->physicsItem)
this->PhysicsPropertyChanged(_item);
+ else if (currentItem == this->guiItem)
+ this->GUIPropertyChanged(_item);
}
/////////////////////////////////////////////////
@@ -571,7 +627,7 @@ void ModelListWidget::LightPropertyChanged(QtProperty * /*_item*/)
(*iter)).toString().toStdString());
else if ((*iter)->propertyName().toStdString() == "pose")
{
- math::Pose pose;
+ ignition::math::Pose3d pose;
pose.Set(this->variantManager->value(
this->GetChildItem((*iter), "x")).toDouble(),
this->variantManager->value(
@@ -619,6 +675,37 @@ void ModelListWidget::LightPropertyChanged(QtProperty * /*_item*/)
}
/////////////////////////////////////////////////
+void ModelListWidget::GUIPropertyChanged(QtProperty *_item)
+{
+ // Only camera pose editable for now
+ QtProperty *cameraProperty = this->GetChildItem("camera");
+ if (!cameraProperty)
+ return;
+
+ QtProperty *cameraPoseProperty = this->GetChildItem(cameraProperty, "pose");
+ if (!cameraPoseProperty)
+ return;
+
+ if (cameraPoseProperty)
+ {
+ std::string changedProperty = _item->propertyName().toStdString();
+ if (changedProperty == "x"
+ || changedProperty == "y"
+ || changedProperty == "z"
+ || changedProperty == "roll"
+ || changedProperty == "pitch"
+ || changedProperty == "yaw")
+ {
+ msgs::Pose poseMsg;
+ this->FillPoseMsg(cameraPoseProperty, &poseMsg, poseMsg.GetDescriptor());
+ rendering::UserCameraPtr cam = gui::get_active_camera();
+ if (cam)
+ cam->SetWorldPose(msgs::ConvertIgn(poseMsg));
+ }
+ }
+}
+
+/////////////////////////////////////////////////
void ModelListWidget::PhysicsPropertyChanged(QtProperty * /*_item*/)
{
msgs::Physics msg;
@@ -629,6 +716,8 @@ void ModelListWidget::PhysicsPropertyChanged(QtProperty * /*_item*/)
{
if ((*iter)->propertyName().toStdString() == "gravity")
this->FillVector3Msg((*iter), msg.mutable_gravity());
+ else if ((*iter)->propertyName().toStdString() == "magnetic field")
+ this->FillVector3Msg((*iter), msg.mutable_magnetic_field());
else if ((*iter)->propertyName().toStdString() == "enable physics")
msg.set_enable_physics(this->variantManager->value((*iter)).toBool());
else if ((*iter)->propertyName().toStdString() == "solver")
@@ -665,7 +754,7 @@ void ModelListWidget::PhysicsPropertyChanged(QtProperty * /*_item*/)
}
/////////////////////////////////////////////////
-void ModelListWidget::ScenePropertyChanged(QtProperty * /*_item*/)
+void ModelListWidget::ScenePropertyChanged(QtProperty */*_item*/)
{
msgs::Scene msg;
@@ -1520,6 +1609,7 @@ void ModelListWidget::FillPropertyTree(const msgs::Link &_msg,
else
item->setValue(true);
this->AddProperty(item, _parent);
+ item->setEnabled(false);
// gravity
item = this->variantManager->addProperty(QVariant::Bool, tr("gravity"));
@@ -2057,6 +2147,14 @@ void ModelListWidget::FillPropertyTree(const msgs::Model &_msg,
item->setEnabled(false);
this->propTreeBrowser->addProperty(item);
+ item = this->variantManager->addProperty(QVariant::Bool, tr("self_collide"));
+ if (_msg.has_self_collide())
+ item->setValue(_msg.self_collide());
+ else
+ item->setValue(false);
+ item->setEnabled(false);
+ this->propTreeBrowser->addProperty(item);
+
topItem = this->variantManager->addProperty(
QtVariantPropertyManager::groupTypeId(), tr("pose"));
QtBrowserItem *bItem = this->propTreeBrowser->addProperty(topItem);
@@ -2089,8 +2187,8 @@ void ModelListWidget::FillVector3dProperty(const msgs::Vector3d &_msg,
}
QtVariantProperty *item;
- math::Vector3 value;
- value = msgs::Convert(_msg);
+ ignition::math::Vector3d value;
+ value = msgs::ConvertIgn(_msg);
value.Round(6);
// Add X value
@@ -2103,7 +2201,7 @@ void ModelListWidget::FillVector3dProperty(const msgs::Vector3d &_msg,
static_cast<QtVariantPropertyManager*>
(this->variantFactory->propertyManager(item))->setAttribute(
item, "decimals", 6);
- item->setValue(value.x);
+ item->setValue(value.X());
// Add Y value
item = static_cast<QtVariantProperty*>(this->GetChildItem(_parent, "y"));
@@ -2114,7 +2212,7 @@ void ModelListWidget::FillVector3dProperty(const msgs::Vector3d &_msg,
}
static_cast<QtVariantPropertyManager*>(this->variantFactory->propertyManager(
item))->setAttribute(item, "decimals", 6);
- item->setValue(value.y);
+ item->setValue(value.Y());
// Add Z value
item = static_cast<QtVariantProperty*>(this->GetChildItem(_parent, "z"));
@@ -2125,7 +2223,7 @@ void ModelListWidget::FillVector3dProperty(const msgs::Vector3d &_msg,
}
static_cast<QtVariantPropertyManager*>(this->variantFactory->propertyManager(
item))->setAttribute(item, "decimals", 6);
- item->setValue(value.z);
+ item->setValue(value.Z());
}
/////////////////////////////////////////////////
@@ -2140,11 +2238,11 @@ void ModelListWidget::FillPoseProperty(const msgs::Pose &_msg,
}
QtVariantProperty *item;
- math::Pose value;
- value = msgs::Convert(_msg);
+ ignition::math::Pose3d value;
+ value = msgs::ConvertIgn(_msg);
value.Round(6);
- math::Vector3 rpy = value.rot.GetAsEuler();
+ ignition::math::Vector3d rpy = value.Rot().Euler();
rpy.Round(6);
this->FillVector3dProperty(_msg.position(), _parent);
@@ -2155,10 +2253,14 @@ void ModelListWidget::FillPoseProperty(const msgs::Pose &_msg,
{
item = this->variantManager->addProperty(QVariant::Double, "roll");
_parent->addSubProperty(item);
+ static_cast<QtVariantPropertyManager *>(
+ this->variantFactory->propertyManager(
+ item))->setAttribute(item, "decimals", 6);
+ static_cast<QtVariantPropertyManager *>(
+ this->variantFactory->propertyManager(
+ item))->setAttribute(item, "singleStep", 0.05);
}
- static_cast<QtVariantPropertyManager*>(this->variantFactory->propertyManager(
- item))->setAttribute(item, "decimals", 6);
- item->setValue(rpy.x);
+ item->setValue(rpy.X());
// Add Pitch value
item = static_cast<QtVariantProperty*>(this->GetChildItem(_parent, "pitch"));
@@ -2166,10 +2268,14 @@ void ModelListWidget::FillPoseProperty(const msgs::Pose &_msg,
{
item = this->variantManager->addProperty(QVariant::Double, "pitch");
_parent->addSubProperty(item);
+ static_cast<QtVariantPropertyManager *>(
+ this->variantFactory->propertyManager(
+ item))->setAttribute(item, "decimals", 6);
+ static_cast<QtVariantPropertyManager *>(
+ this->variantFactory->propertyManager(
+ item))->setAttribute(item, "singleStep", 0.05);
}
- static_cast<QtVariantPropertyManager*>(this->variantFactory->propertyManager(
- item))->setAttribute(item, "decimals", 6);
- item->setValue(rpy.y);
+ item->setValue(rpy.Y());
// Add Yaw value
item = static_cast<QtVariantProperty*>(this->GetChildItem(_parent, "yaw"));
@@ -2177,10 +2283,14 @@ void ModelListWidget::FillPoseProperty(const msgs::Pose &_msg,
{
item = this->variantManager->addProperty(QVariant::Double, "yaw");
_parent->addSubProperty(item);
- }
- static_cast<QtVariantPropertyManager*>(this->variantFactory->propertyManager(
- item))->setAttribute(item, "decimals", 6);
- item->setValue(rpy.z);
+ static_cast<QtVariantPropertyManager *>(
+ this->variantFactory->propertyManager(
+ item))->setAttribute(item, "decimals", 6);
+ static_cast<QtVariantPropertyManager *>(
+ this->variantFactory->propertyManager(
+ item))->setAttribute(item, "singleStep", 0.05);
+ }
+ item->setValue(rpy.Z());
}
/////////////////////////////////////////////////
@@ -2273,8 +2383,17 @@ void ModelListWidget::ResetTree()
// Create the top level of items in the tree widget
{
+ // GUI item
+ this->guiItem = new QTreeWidgetItem(
+ static_cast<QTreeWidgetItem*>(0),
+ QStringList(QString("%1").arg(tr("GUI"))));
+ this->guiItem->setData(0, Qt::UserRole, QVariant(tr("GUI")));
+ this->modelTreeWidget->addTopLevelItem(this->guiItem);
+
+ // Scene item
this->ResetScene();
+ // Spherical coordinates item
this->sphericalCoordItem = new QTreeWidgetItem(
static_cast<QTreeWidgetItem*>(0),
QStringList(QString("%1").arg(tr("Spherical Coordinates"))));
@@ -2282,18 +2401,21 @@ void ModelListWidget::ResetTree()
Qt::UserRole, QVariant(tr("Spherical Coordinates")));
this->modelTreeWidget->addTopLevelItem(this->sphericalCoordItem);
+ // Physics item
this->physicsItem = new QTreeWidgetItem(
static_cast<QTreeWidgetItem*>(0),
QStringList(QString("%1").arg(tr("Physics"))));
this->physicsItem->setData(0, Qt::UserRole, QVariant(tr("Physics")));
this->modelTreeWidget->addTopLevelItem(this->physicsItem);
+ // Models item
this->modelsItem = new QTreeWidgetItem(
static_cast<QTreeWidgetItem*>(0),
QStringList(QString("%1").arg(tr("Models"))));
this->modelsItem->setData(0, Qt::UserRole, QVariant(tr("Models")));
this->modelTreeWidget->addTopLevelItem(this->modelsItem);
+ // Lights item
this->lightsItem = new QTreeWidgetItem(
static_cast<QTreeWidgetItem*>(0),
QStringList(QString("%1").arg(tr("Lights"))));
@@ -2470,6 +2592,20 @@ void ModelListWidget::FillPropertyTree(const msgs::Physics &_msg,
this->FillVector3dProperty(xyz, gravityItem);
}
+ QtProperty *magneticFieldItem = this->variantManager->addProperty(
+ QtVariantPropertyManager::groupTypeId(), tr("magnetic field"));
+ this->propTreeBrowser->addProperty(magneticFieldItem);
+ if (_msg.has_magnetic_field())
+ this->FillVector3dProperty(_msg.magnetic_field(), magneticFieldItem);
+ else
+ {
+ msgs::Vector3d xyz;
+ xyz.set_x(0.0);
+ xyz.set_y(0.0);
+ xyz.set_z(0.0);
+ this->FillVector3dProperty(xyz, magneticFieldItem);
+ }
+
QtProperty *solverItem = this->variantManager->addProperty(
QtVariantPropertyManager::groupTypeId(), tr("solver"));
this->propTreeBrowser->addProperty(solverItem);
@@ -2547,7 +2683,7 @@ void ModelListWidget::FillPropertyTree(const msgs::Light &_msg,
if (_msg.has_pose())
this->FillPoseProperty(_msg.pose(), topItem);
else
- this->FillPoseProperty(msgs::Convert(math::Pose()), topItem);
+ this->FillPoseProperty(msgs::Convert(ignition::math::Pose3d()), topItem);
// Create and set the diffuse color property
item = this->variantManager->addProperty(QVariant::Color, tr("diffuse"));
diff --git a/gazebo/gui/ModelListWidget.hh b/gazebo/gui/ModelListWidget.hh
index 5799132..69a5e2c 100644
--- a/gazebo/gui/ModelListWidget.hh
+++ b/gazebo/gui/ModelListWidget.hh
@@ -198,6 +198,10 @@ namespace gazebo
/// \param[in] _item The item that was changed.
private: void PhysicsPropertyChanged(QtProperty *_item);
+ /// \brief Called when a GUI property is changed by the user.
+ /// \param[in] _item The item that was changed.
+ private: void GUIPropertyChanged(QtProperty *_item);
+
private: QTreeWidget *modelTreeWidget;
private: QtTreePropertyBrowser *propTreeBrowser;
@@ -211,10 +215,22 @@ namespace gazebo
private: transport::SubscriberPtr responseSub;
private: transport::SubscriberPtr requestSub;
+ /// \brief GUI tree item.
+ private: QTreeWidgetItem *guiItem;
+
+ /// \brief Scene tree item.
private: QTreeWidgetItem *sceneItem;
+
+ /// \brief Physics tree item.
private: QTreeWidgetItem *physicsItem;
+
+ /// \brief Models tree item.
private: QTreeWidgetItem *modelsItem;
+
+ /// \brief Lights tree item.
private: QTreeWidgetItem *lightsItem;
+
+ /// \brief Spherical coordinates tree item.
private: QTreeWidgetItem *sphericalCoordItem;
private: QtVariantPropertyManager *variantManager;
diff --git a/gazebo/gui/ModelListWidget_TEST.cc b/gazebo/gui/ModelListWidget_TEST.cc
index 263ed84..95524a3 100644
--- a/gazebo/gui/ModelListWidget_TEST.cc
+++ b/gazebo/gui/ModelListWidget_TEST.cc
@@ -610,6 +610,7 @@ void ModelListWidget_TEST::LinkProperties()
gazebo::gui::ModelListWidget *modelListWidget
= new gazebo::gui::ModelListWidget;
modelListWidget->show();
+ modelListWidget->setGeometry(0, 0, 400, 800);
QCoreApplication::processEvents();
this->Load("worlds/multilink_shape.world");
@@ -713,6 +714,7 @@ void ModelListWidget_TEST::LinkProperties()
QRect boxLinkRect = modelTreeWidget->visualItemRect(boxLinkItem);
QTest::mouseClick(modelTreeWidget->viewport(), Qt::LeftButton, 0,
boxLinkRect.center() );
+
QCoreApplication::processEvents();
sleep = 0;
maxSleep = 5;
@@ -787,10 +789,12 @@ void ModelListWidget_TEST::LinkProperties()
QTest::mouseClick(modelTreeWidget->viewport(), Qt::LeftButton, 0,
sphereLinkRect.center() );
QCoreApplication::processEvents();
+ QTest::qWait(100);
sleep = 0;
maxSleep = 5;
while (!sphereLinkItem->isSelected() && sleep < maxSleep)
{
+ QCoreApplication::processEvents();
QTest::qWait(10);
sleep++;
}
diff --git a/gazebo/gui/ModelMaker.cc b/gazebo/gui/ModelMaker.cc
index 308a8cd..8250442 100644
--- a/gazebo/gui/ModelMaker.cc
+++ b/gazebo/gui/ModelMaker.cc
@@ -14,6 +14,13 @@
* 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 <sstream>
#include "gazebo/msgs/msgs.hh"
@@ -146,10 +153,10 @@ bool ModelMaker::Init()
math::Pose modelPose, linkPose, visualPose;
sdf::ElementPtr modelElem;
- if (this->modelSDF->root->HasElement("model"))
- modelElem = this->modelSDF->root->GetElement("model");
- else if (this->modelSDF->root->HasElement("light"))
- modelElem = this->modelSDF->root->GetElement("light");
+ if (this->modelSDF->Root()->HasElement("model"))
+ modelElem = this->modelSDF->Root()->GetElement("model");
+ else if (this->modelSDF->Root()->HasElement("light"))
+ modelElem = this->modelSDF->Root()->GetElement("light");
else
{
gzerr << "No model or light in SDF\n";
@@ -170,8 +177,6 @@ bool ModelMaker::Init()
modelName = this->modelVisual->GetName();
modelElem->GetAttribute("name")->Set(modelName);
- scene->AddVisual(this->modelVisual);
-
if (modelElem->GetName() == "model")
{
sdf::ElementPtr linkElem = modelElem->GetElement("link");
@@ -251,7 +256,8 @@ void ModelMaker::Stop()
{
// Remove the temporary visual from the scene
rendering::ScenePtr scene = gui::get_active_camera()->GetScene();
- scene->RemoveVisual(this->modelVisual);
+ for (auto vis : this->visuals)
+ scene->RemoveVisual(vis);
this->modelVisual.reset();
this->visuals.clear();
this->modelSDF.reset();
@@ -274,12 +280,12 @@ void ModelMaker::OnMousePush(const common::MouseEvent &/*_event*/)
/////////////////////////////////////////////////
void ModelMaker::OnMouseRelease(const common::MouseEvent &_event)
{
- if (_event.button == common::MouseEvent::LEFT)
+ if (_event.Button() == common::MouseEvent::LEFT)
{
// Place if not dragging, or if dragged for less than 50 pixels.
// The 50 pixels is used to account for accidental mouse movement
// when placing an object.
- if (!_event.dragging || _event.pressPos.Distance(_event.pos) < 50)
+ if (!_event.Dragging() || _event.PressPos().Distance(_event.Pos()) < 50)
{
this->CreateTheEntity();
this->Stop();
@@ -293,7 +299,7 @@ void ModelMaker::OnMouseMove(const common::MouseEvent &_event)
math::Pose pose = this->modelVisual->GetWorldPose();
pose.pos = ModelManipulator::GetMousePositionOnPlane(this->camera, _event);
- if (!_event.shift)
+ if (!_event.Shift())
{
pose.pos = ModelManipulator::SnapPoint(pose.pos);
}
@@ -317,14 +323,14 @@ void ModelMaker::CreateTheEntity()
sdf::ElementPtr modelElem;
bool isModel = false;
bool isLight = false;
- if (this->modelSDF->root->HasElement("model"))
+ if (this->modelSDF->Root()->HasElement("model"))
{
- modelElem = this->modelSDF->root->GetElement("model");
+ modelElem = this->modelSDF->Root()->GetElement("model");
isModel = true;
}
- else if (this->modelSDF->root->HasElement("light"))
+ else if (this->modelSDF->Root()->HasElement("light"))
{
- modelElem = this->modelSDF->root->GetElement("light");
+ modelElem = this->modelSDF->Root()->GetElement("light");
isLight = true;
}
@@ -353,7 +359,7 @@ void ModelMaker::CreateTheEntity()
}
else
{
- msgs::Set(msg.mutable_pose(), this->modelVisual->GetWorldPose());
+ msgs::Set(msg.mutable_pose(), this->modelVisual->GetWorldPose().Ign());
msg.set_clone_model_name(this->modelVisual->GetName().substr(0,
this->modelVisual->GetName().find("_clone_tmp")));
}
diff --git a/gazebo/gui/ModelManipulator.cc b/gazebo/gui/ModelManipulator.cc
index 4be1d17..f29826f 100644
--- a/gazebo/gui/ModelManipulator.cc
+++ b/gazebo/gui/ModelManipulator.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/transport/transport.hh"
#include "gazebo/rendering/RenderEvents.hh"
@@ -26,6 +32,7 @@
#include "gazebo/rendering/SelectionObj.hh"
#include "gazebo/gui/qt.h"
+#include "gazebo/gui/GuiEvents.hh"
#include "gazebo/gui/MouseEventHandler.hh"
#include "gazebo/gui/GuiIface.hh"
@@ -39,24 +46,36 @@ using namespace gui;
ModelManipulator::ModelManipulator()
: dataPtr(new ModelManipulatorPrivate)
{
- this->dataPtr->initialized = false;
- this->dataPtr->selectionObj.reset();
- this->dataPtr->mouseMoveVis.reset();
-
this->dataPtr->manipMode = "";
this->dataPtr->globalManip = false;
+ this->dataPtr->initialized = false;
}
/////////////////////////////////////////////////
ModelManipulator::~ModelManipulator()
{
- this->dataPtr->modelPub.reset();
- this->dataPtr->selectionObj.reset();
+ this->Clear();
delete this->dataPtr;
this->dataPtr = NULL;
}
/////////////////////////////////////////////////
+void ModelManipulator::Clear()
+{
+ this->dataPtr->modelPub.reset();
+ this->dataPtr->lightPub.reset();
+ this->dataPtr->selectionObj.reset();
+ this->dataPtr->userCamera.reset();
+ this->dataPtr->scene.reset();
+ this->dataPtr->node.reset();
+ this->dataPtr->mouseMoveVis.reset();
+ this->dataPtr->mouseChildVisualScale.clear();
+ this->dataPtr->manipMode = "";
+ this->dataPtr->globalManip = false;
+ this->dataPtr->initialized = false;
+}
+
+/////////////////////////////////////////////////
void ModelManipulator::Init()
{
if (this->dataPtr->initialized)
@@ -116,14 +135,14 @@ void ModelManipulator::RotateEntity(rendering::VisualPtr &_vis,
math::Vector3 pressPoint;
this->dataPtr->userCamera->GetWorldPointOnPlane(
- this->dataPtr->mouseEvent.pressPos.x,
- this->dataPtr->mouseEvent.pressPos.y,
+ this->dataPtr->mouseEvent.PressPos().X(),
+ this->dataPtr->mouseEvent.PressPos().Y(),
math::Plane(normal, offset), pressPoint);
math::Vector3 newPoint;
this->dataPtr->userCamera->GetWorldPointOnPlane(
- this->dataPtr->mouseEvent.pos.x,
- this->dataPtr->mouseEvent.pos.y,
+ this->dataPtr->mouseEvent.Pos().X(),
+ this->dataPtr->mouseEvent.Pos().Y(),
math::Plane(normal, offset), newPoint);
math::Vector3 v1 = pressPoint - this->dataPtr->mouseMoveVisStartPose.pos;
@@ -136,7 +155,9 @@ void ModelManipulator::RotateEntity(rendering::VisualPtr &_vis,
if (signTest < 0 )
angle *= -1;
- if (this->dataPtr->mouseEvent.control)
+ // Using Qt control modifier instead of Gazebo's for now.
+ // See GLWidget::keyPressEvent
+ if (QApplication::keyboardModifiers() & Qt::ControlModifier)
angle = rint(angle / (M_PI * 0.25)) * (M_PI * 0.25);
math::Quaternion rot(_axis, angle);
@@ -157,7 +178,7 @@ math::Vector3 ModelManipulator::GetMousePositionOnPlane(
math::Vector3 origin1, dir1, p1;
// Cast ray from the camera into the world
- _camera->GetCameraToViewportRay(_event.pos.x, _event.pos.y,
+ _camera->GetCameraToViewportRay(_event.Pos().X(), _event.Pos().Y(),
origin1, dir1);
// Compute the distance from the camera to plane of translation
@@ -303,8 +324,9 @@ math::Vector3 ModelManipulator::GetMouseMoveDistance(const math::Pose &_pose,
const math::Vector3 &_axis, bool _local) const
{
return GetMouseMoveDistance(this->dataPtr->userCamera,
- this->dataPtr->mouseStart, math::Vector2i(this->dataPtr->mouseEvent.pos.x,
- this->dataPtr->mouseEvent.pos.y), _pose, _axis, _local);
+ this->dataPtr->mouseStart,
+ math::Vector2i(this->dataPtr->mouseEvent.Pos().X(),
+ this->dataPtr->mouseEvent.Pos().Y()), _pose, _axis, _local);
}
/////////////////////////////////////////////////
@@ -319,10 +341,127 @@ void ModelManipulator::ScaleEntity(rendering::VisualPtr &_vis,
math::Vector3 scale = (bboxSize + pose.rot.RotateVectorReverse(distance))
/ bboxSize;
- // a bit hacky to check for unit sphere and cylinder simple shapes in order
- // to restrict the scaling dimensions.
- if (this->dataPtr->keyEvent.key == Qt::Key_Shift ||
- _vis->GetName().find("unit_sphere") != std::string::npos)
+ // extended scaling to work in model editor mode by checking geometry
+ // type of first visual child.
+ std::string geomType;
+ if (_vis == _vis->GetRootVisual())
+ {
+ // link-level visuals
+ for (unsigned int i = 0; i < _vis->GetChildCount(); ++i)
+ {
+ rendering::VisualPtr childVis = _vis->GetChild(i);
+
+ if (childVis->GetPose().pos != math::Vector3::Zero)
+ {
+ gzwarn << "Scaling is currently limited to simple shapes with their "
+ << "origin in the centroid." << std::endl;
+ return;
+ }
+ // visual/collision level visuals
+ for (unsigned int j = 0; j < childVis->GetChildCount(); ++j)
+ {
+ rendering::VisualPtr grandChildVis = childVis->GetChild(j);
+ std::string thisGeomType = grandChildVis->GetGeometryType();
+
+ if (grandChildVis->GetPose().pos != math::Vector3::Zero)
+ {
+ gzwarn << "Scaling is currently limited to simple shapes with their "
+ << "origin in the centroid." << std::endl;
+ return;
+ }
+
+ if (thisGeomType == "")
+ continue;
+
+ if (geomType == "")
+ {
+ geomType = thisGeomType;
+ }
+ else if (thisGeomType != geomType)
+ {
+ gzwarn << "Scaling is currently limited to models consisting of a " <<
+ "single simple geometry type." << std::endl;
+ return;
+ }
+ }
+ }
+
+ if (this->dataPtr->keyEvent.key == Qt::Key_Shift || geomType == "sphere")
+ {
+ scale = this->UpdateScale(_axis, scale, "sphere");
+ }
+ else if (geomType == "cylinder")
+ {
+ scale = this->UpdateScale(_axis, scale, "cylinder");
+ }
+ else if (geomType == "box")
+ {
+ // keep new scale as it is
+ }
+ else
+ {
+ // TODO scaling for complex models are not yet functional.
+ // Limit scaling to simple shapes for now.
+ gzwarn << " Scaling is currently limited to simple shapes." << std::endl;
+ return;
+ }
+
+ math::Vector3 newScale = this->dataPtr->mouseVisualScale * scale.GetAbs();
+
+ if (QApplication::keyboardModifiers() & Qt::ControlModifier)
+ {
+ newScale = SnapPoint(newScale);
+ // prevent setting zero scale
+ newScale.x = std::max(1e-4, newScale.x);
+ newScale.y = std::max(1e-4, newScale.y);
+ newScale.z = std::max(1e-4, newScale.z);
+ }
+ _vis->SetScale(newScale);
+ Events::scaleEntity(_vis->GetName(), newScale);
+ }
+ else
+ {
+ // model editor mode -> apply scaling to individual visuals
+ if (this->dataPtr->mouseChildVisualScale.size() != _vis->GetChildCount())
+ {
+ gzerr << "Incorrect number of child visuals to be scaled. " <<
+ "This should not happen" << std::endl;
+ return;
+ }
+
+ for (unsigned int i = 0; i < _vis->GetChildCount(); ++i)
+ {
+ rendering::VisualPtr childVis = _vis->GetChild(i);
+ geomType = childVis->GetGeometryType();
+ if (childVis != this->dataPtr->selectionObj &&
+ geomType != "" && geomType != "mesh")
+ {
+ math::Vector3 geomScale = this->UpdateScale(_axis, scale,
+ childVis->GetGeometryType());
+ math::Vector3 newScale = this->dataPtr->mouseChildVisualScale[i]
+ * geomScale.GetAbs();
+
+ if (QApplication::keyboardModifiers() & Qt::ControlModifier)
+ {
+ newScale = SnapPoint(newScale);
+ // prevent setting zero scale
+ newScale.x = std::max(1e-4, newScale.x);
+ newScale.y = std::max(1e-4, newScale.y);
+ newScale.z = std::max(1e-4, newScale.z);
+ }
+ childVis->SetScale(newScale);
+ Events::scaleEntity(childVis->GetName(), newScale);
+ }
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+math::Vector3 ModelManipulator::UpdateScale(const math::Vector3 &_axis,
+ const math::Vector3 &_scale, const std::string &_geom)
+{
+ math::Vector3 scale = _scale;
+ if (_geom == "sphere")
{
if (_axis.x > 0)
{
@@ -340,7 +479,7 @@ void ModelManipulator::ScaleEntity(rendering::VisualPtr &_vis,
scale.y = scale.z;
}
}
- else if (_vis->GetName().find("unit_cylinder") != std::string::npos)
+ else if (_geom == "cylinder")
{
if (_axis.x > 0)
{
@@ -351,29 +490,8 @@ void ModelManipulator::ScaleEntity(rendering::VisualPtr &_vis,
scale.x = scale.y;
}
}
- else if (_vis->GetName().find("unit_box") != std::string::npos)
- {
- }
- else
- {
- // TODO scaling for complex models are not yet functional.
- // Limit scaling to simple shapes for now.
- gzwarn << " Scaling is currently limited to simple shapes." << std::endl;
- return;
- }
-
- math::Vector3 newScale = this->dataPtr->mouseVisualScale * scale.GetAbs();
- if (this->dataPtr->mouseEvent.control)
- {
- newScale = SnapPoint(newScale);
- // prevent setting zero scale
- newScale.x = std::max(1e-4, newScale.x);
- newScale.y = std::max(1e-4, newScale.y);
- newScale.z = std::max(1e-4, newScale.z);
- }
-
- _vis->SetScale(newScale);
+ return scale;
}
/////////////////////////////////////////////////
@@ -385,7 +503,7 @@ void ModelManipulator::TranslateEntity(rendering::VisualPtr &_vis,
pose.pos = this->dataPtr->mouseMoveVisStartPose.pos + distance;
- if (this->dataPtr->mouseEvent.control)
+ if (QApplication::keyboardModifiers() & Qt::ControlModifier)
{
pose.pos = SnapPoint(pose.pos);
}
@@ -408,7 +526,7 @@ void ModelManipulator::PublishVisualPose(rendering::VisualPtr _vis)
msg.set_id(gui::get_entity_id(_vis->GetName()));
msg.set_name(_vis->GetName());
- msgs::Set(msg.mutable_pose(), _vis->GetWorldPose());
+ msgs::Set(msg.mutable_pose(), _vis->GetWorldPose().Ign());
this->dataPtr->modelPub->Publish(msg);
}
// Otherwise, check to see if the visual is a light
@@ -416,7 +534,7 @@ void ModelManipulator::PublishVisualPose(rendering::VisualPtr _vis)
{
msgs::Light msg;
msg.set_name(_vis->GetName());
- msgs::Set(msg.mutable_pose(), _vis->GetWorldPose());
+ msgs::Set(msg.mutable_pose(), _vis->GetWorldPose().Ign());
this->dataPtr->lightPub->Publish(msg);
}
}
@@ -434,7 +552,7 @@ void ModelManipulator::PublishVisualScale(rendering::VisualPtr _vis)
msg.set_id(gui::get_entity_id(_vis->GetName()));
msg.set_name(_vis->GetName());
- msgs::Set(msg.mutable_scale(), _vis->GetScale());
+ msgs::Set(msg.mutable_scale(), _vis->GetScale().Ign());
this->dataPtr->modelPub->Publish(msg);
_vis->SetScale(this->dataPtr->mouseVisualScale);
}
@@ -445,12 +563,12 @@ void ModelManipulator::PublishVisualScale(rendering::VisualPtr _vis)
void ModelManipulator::OnMousePressEvent(const common::MouseEvent &_event)
{
this->dataPtr->mouseEvent = _event;
- this->dataPtr->mouseStart = _event.pressPos;
+ this->dataPtr->mouseStart = _event.PressPos();
this->SetMouseMoveVisual(rendering::VisualPtr());
rendering::VisualPtr vis;
rendering::VisualPtr mouseVis
- = this->dataPtr->userCamera->GetVisual(this->dataPtr->mouseEvent.pos);
+ = this->dataPtr->userCamera->GetVisual(this->dataPtr->mouseEvent.Pos());
// set the new mouse vis only if there are no modifier keys pressed and the
// entity was different from the previously selected one.
if (!this->dataPtr->keyEvent.key && (this->dataPtr->selectionObj->GetMode() ==
@@ -465,18 +583,33 @@ void ModelManipulator::OnMousePressEvent(const common::MouseEvent &_event)
}
if (vis && !vis->IsPlane() &&
- this->dataPtr->mouseEvent.button == common::MouseEvent::LEFT)
+ this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT)
{
+ // Root visual
rendering::VisualPtr rootVis = vis->GetRootVisual();
+
+ // Root visual's immediate child
+ rendering::VisualPtr topLevelVis = vis->GetNthAncestor(2);
+
+ // If the root visual's ID can be found, it is a model in the main window
if (gui::get_entity_id(rootVis->GetName()))
{
// select model
vis = rootVis;
}
- else if (vis->GetParent() != rootVis)
+ // If it is not a model and its parent is either a direct child or
+ // grandchild of the world, this is a light, so just keep vis = vis
+ else if (vis->GetParent() == rootVis ||
+ vis->GetParent() == this->dataPtr->scene->GetWorldVisual())
{
- // select link
- vis = vis->GetParent();
+ // select light
+ }
+ // Otherwise, this is a visual in the model editor, so we want to get its
+ // top level visual below the root.
+ else
+ {
+ // select link / nested model
+ vis = topLevelVis;
}
this->dataPtr->mouseMoveVisStartPose = vis->GetWorldPose();
@@ -507,10 +640,10 @@ void ModelManipulator::OnMousePressEvent(const common::MouseEvent &_event)
void ModelManipulator::OnMouseMoveEvent(const common::MouseEvent &_event)
{
this->dataPtr->mouseEvent = _event;
- if (this->dataPtr->mouseEvent.dragging)
+ if (this->dataPtr->mouseEvent.Dragging())
{
if (this->dataPtr->mouseMoveVis &&
- this->dataPtr->mouseEvent.button == common::MouseEvent::LEFT)
+ this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT)
{
math::Vector3 axis = math::Vector3::Zero;
if (this->dataPtr->keyEvent.key == Qt::Key_X)
@@ -616,7 +749,7 @@ void ModelManipulator::OnMouseMoveEvent(const common::MouseEvent &_event)
else
{
std::string manipState;
- this->dataPtr->userCamera->GetVisual(this->dataPtr->mouseEvent.pos,
+ this->dataPtr->userCamera->GetVisual(this->dataPtr->mouseEvent.Pos(),
manipState);
this->dataPtr->selectionObj->SetState(manipState);
@@ -625,7 +758,7 @@ void ModelManipulator::OnMouseMoveEvent(const common::MouseEvent &_event)
else
{
rendering::VisualPtr vis = this->dataPtr->userCamera->GetVisual(
- this->dataPtr->mouseEvent.pos);
+ this->dataPtr->mouseEvent.Pos());
if (vis && !vis->IsPlane())
QApplication::setOverrideCursor(Qt::OpenHandCursor);
@@ -640,7 +773,7 @@ void ModelManipulator::OnMouseMoveEvent(const common::MouseEvent &_event)
void ModelManipulator::OnMouseReleaseEvent(const common::MouseEvent &_event)
{
this->dataPtr->mouseEvent = _event;
- if (this->dataPtr->mouseEvent.dragging)
+ if (this->dataPtr->mouseEvent.Dragging())
{
// If we were dragging a visual around, then publish its new pose to the
// server
@@ -660,10 +793,10 @@ void ModelManipulator::OnMouseReleaseEvent(const common::MouseEvent &_event)
}
else
{
- if (this->dataPtr->mouseEvent.button == common::MouseEvent::LEFT)
+ if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT)
{
rendering::VisualPtr vis =
- this->dataPtr->userCamera->GetVisual(this->dataPtr->mouseEvent.pos);
+ this->dataPtr->userCamera->GetVisual(this->dataPtr->mouseEvent.Pos());
if (vis && vis->IsPlane())
{
this->dataPtr->selectionObj->SetMode(
@@ -713,6 +846,14 @@ void ModelManipulator::SetMouseMoveVisual(rendering::VisualPtr _vis)
if (_vis)
{
this->dataPtr->mouseVisualScale = _vis->GetScale();
+ this->dataPtr->mouseChildVisualScale.clear();
+ // keep track of all child visual scale for scaling to work in
+ // model editor mode.
+ for (unsigned int i = 0; i < _vis->GetChildCount(); ++i)
+ {
+ rendering::VisualPtr childVis = _vis->GetChild(i);
+ this->dataPtr->mouseChildVisualScale.push_back(childVis->GetScale());
+ }
this->dataPtr->mouseVisualBbox = _vis->GetBoundingBox();
}
else
@@ -731,7 +872,7 @@ void ModelManipulator::OnKeyPressEvent(const common::KeyEvent &_event)
if (_event.key == Qt::Key_X || _event.key == Qt::Key_Y
|| _event.key == Qt::Key_Z)
{
- this->dataPtr->mouseStart = this->dataPtr->mouseEvent.pos;
+ this->dataPtr->mouseStart = this->dataPtr->mouseEvent.Pos();
if (this->dataPtr->mouseMoveVis)
{
this->dataPtr->mouseMoveVisStartPose =
@@ -758,7 +899,7 @@ void ModelManipulator::OnKeyReleaseEvent(const common::KeyEvent &_event)
if (_event.key == Qt::Key_X || _event.key == Qt::Key_Y
|| _event.key == Qt::Key_Z)
{
- this->dataPtr->mouseStart = this->dataPtr->mouseEvent.pos;
+ this->dataPtr->mouseStart = this->dataPtr->mouseEvent.Pos();
if (this->dataPtr->mouseMoveVis)
{
this->dataPtr->mouseMoveVisStartPose =
diff --git a/gazebo/gui/ModelManipulator.hh b/gazebo/gui/ModelManipulator.hh
index 6632bb2..17770b7 100644
--- a/gazebo/gui/ModelManipulator.hh
+++ b/gazebo/gui/ModelManipulator.hh
@@ -47,6 +47,10 @@ namespace gazebo
/// \brief Initialize the model manipulator.
public: void Init();
+ /// \brief Clear the model manipulator. This explicity cleans up the
+ /// internal state of the singleton and prepares it for exit.
+ public: void Clear();
+
/// \brief Detach the manipulator from an entity
public: void Detach();
@@ -123,6 +127,8 @@ namespace gazebo
/// \brief Helper function to get the distance moved by the mouse.
/// \param[in] _camera Pointer to user camera.
+ /// \param[in] _start Start point.
+ /// \param[in] _end End point.
/// \param[in] _pose Pose of origin.
/// \param[in] _axis Movement axis.
/// \param[in] _local True to get distance in local frame.
@@ -153,6 +159,15 @@ namespace gazebo
/// \param[in] _vis Pointer to the visual whose scale is to be published.
private: void PublishVisualScale(rendering::VisualPtr _vis);
+ /// \brief Helper function to constrain the scale dimensions for simple
+ /// shapes
+ /// \param[in] _axis Scaling axis.
+ /// \param[in] _scale Input scale to be updated.
+ /// \param[in] _geom Type of geometry.
+ /// \return Updated scale.
+ private: math::Vector3 UpdateScale(const math::Vector3 &_axis,
+ const math::Vector3 &_scale, const std::string &_geom);
+
/// \brief This is a singleton class.
private: friend class SingletonT<ModelManipulator>;
diff --git a/gazebo/gui/ModelManipulatorPrivate.hh b/gazebo/gui/ModelManipulatorPrivate.hh
index f58d5e2..0476621 100644
--- a/gazebo/gui/ModelManipulatorPrivate.hh
+++ b/gazebo/gui/ModelManipulatorPrivate.hh
@@ -18,6 +18,7 @@
#define _MODEL_MANIPULATOR_PRIVATE_HH_
#include <string>
+#include <vector>
#include "gazebo/common/MouseEvent.hh"
#include "gazebo/common/KeyEvent.hh"
@@ -46,9 +47,6 @@ namespace gazebo
/// \brief Keep track of the mouse start screen position.
public: math::Vector2i mouseStart;
- /// \brief The current selected visual.
- public: rendering::VisualPtr selectedVis;
-
/// \brief The current visual attached to the mouse.
public: rendering::VisualPtr mouseMoveVis;
@@ -79,6 +77,9 @@ namespace gazebo
/// \brief Scale of the visual attached to the mouse.
public: math::Vector3 mouseVisualScale;
+ /// \brief Scale of all the child visuals attached to the mouse.
+ public: std::vector<math::Vector3> mouseChildVisualScale;
+
/// \brief Bounding box of the visual attached to the mouse (for scaling).
public: math::Box mouseVisualBbox;
diff --git a/gazebo/gui/ModelRightMenu.cc b/gazebo/gui/ModelRightMenu.cc
index adc181b..4956524 100644
--- a/gazebo/gui/ModelRightMenu.cc
+++ b/gazebo/gui/ModelRightMenu.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/transport/transport.hh"
#include "gazebo/rendering/UserCamera.hh"
#include "gazebo/rendering/Scene.hh"
@@ -24,6 +30,7 @@
#include "gazebo/gui/GuiEvents.hh"
#include "gazebo/gui/Actions.hh"
#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/ApplyWrenchDialog.hh"
#include "gazebo/gui/ModelRightMenu.hh"
using namespace gazebo;
@@ -43,6 +50,14 @@ ModelRightMenu::ModelRightMenu()
this->followAct->setStatusTip(tr("Follow the selection"));
connect(this->followAct, SIGNAL(triggered()), this, SLOT(OnFollow()));
+ this->editAct = new QAction(tr("Edit model"), this);
+ this->editAct->setStatusTip(tr("Open on Model Editor"));
+ connect(this->editAct, SIGNAL(triggered()), this, SLOT(OnEdit()));
+
+ this->applyWrenchAct = new QAction(tr("Apply Force/Torque"), this);
+ this->applyWrenchAct->setStatusTip(tr("Apply force and torque to a link"));
+ connect(this->applyWrenchAct, SIGNAL(triggered()), this,
+ SLOT(OnApplyWrench()));
// \todo Reimplement
// this->snapBelowAct = new QAction(tr("Snap"), this);
@@ -93,6 +108,20 @@ ModelRightMenu::ModelRightMenu()
connect(state->action, SIGNAL(triggered()), state, SLOT(Callback()));
this->viewStates.push_back(state);
+ state = new ViewState(this, "show_inertia", "hide_inertia");
+ state->action = new QAction(tr("Inertia"), this);
+ state->action->setStatusTip(tr("Show moments of inertia"));
+ state->action->setCheckable(true);
+ connect(state->action, SIGNAL(triggered()), state, SLOT(Callback()));
+ this->viewStates.push_back(state);
+
+ state = new ViewState(this, "show_link_frame", "hide_link_frame");
+ state->action = new QAction(tr("Link Frames"), this);
+ state->action->setStatusTip(tr("Show link frames"));
+ state->action->setCheckable(true);
+ connect(state->action, SIGNAL(triggered()), state, SLOT(Callback()));
+ this->viewStates.push_back(state);
+
// \todo Reimplement
// this->skeletonAction = new QAction(tr("Skeleton"), this);
// this->skeletonAction->setStatusTip(tr("Show model skeleton"));
@@ -132,42 +161,71 @@ ModelRightMenu::~ModelRightMenu()
}
/////////////////////////////////////////////////
-void ModelRightMenu::Run(const std::string &_modelName, const QPoint &_pt)
+void ModelRightMenu::Run(const std::string &_entityName, const QPoint &_pt,
+ EntityTypes _type)
{
- this->modelName = _modelName.substr(0, _modelName.find("::"));
+ if (_type == EntityTypes::MODEL || _type == EntityTypes::LIGHT)
+ {
+ this->entityName = _entityName.substr(0, _entityName.find("::"));
+ }
+ else if (_type == EntityTypes::LINK)
+ {
+ this->entityName = _entityName;
+ }
QMenu menu;
+
menu.addAction(this->moveToAct);
menu.addAction(this->followAct);
- // menu.addAction(this->snapBelowAct);
+ if (_type == EntityTypes::MODEL || _type == EntityTypes::LINK)
+ menu.addAction(this->applyWrenchAct);
- // Create the view menu
- QMenu *viewMenu = menu.addMenu(tr("View"));
- for (std::vector<ViewState*>::iterator iter = this->viewStates.begin();
- iter != this->viewStates.end(); ++iter)
+ if (_type == EntityTypes::MODEL)
{
- viewMenu->addAction((*iter)->action);
+ // disable editing planes for now
+ rendering::UserCameraPtr cam = gui::get_active_camera();
+ rendering::ScenePtr scene = cam->GetScene();
+ rendering::VisualPtr vis = scene->GetVisual(this->entityName);
+ if (vis && !vis->IsPlane())
+ {
+ menu.addSeparator();
+ menu.addAction(this->editAct);
+ menu.addSeparator();
+ }
+
+ // menu.addAction(this->snapBelowAct);
- std::map<std::string, bool>::iterator modelIter =
- (*iter)->modelStates.find(this->modelName);
+ // Create the view menu
+ QMenu *viewMenu = menu.addMenu(tr("View"));
+ for (std::vector<ViewState*>::iterator iter = this->viewStates.begin();
+ iter != this->viewStates.end(); ++iter)
+ {
+ viewMenu->addAction((*iter)->action);
+
+ std::map<std::string, bool>::iterator modelIter =
+ (*iter)->modelStates.find(this->entityName);
- if (modelIter == (*iter)->modelStates.end())
- (*iter)->action->setChecked((*iter)->globalEnable);
- else
- (*iter)->action->setChecked(modelIter->second);
+ if (modelIter == (*iter)->modelStates.end())
+ (*iter)->action->setChecked((*iter)->globalEnable);
+ else
+ (*iter)->action->setChecked(modelIter->second);
+ }
}
- if (g_copyAct && g_pasteAct)
+ if (_type == EntityTypes::MODEL || _type == EntityTypes::LIGHT)
{
+ if (g_copyAct && g_pasteAct)
+ {
+ menu.addSeparator();
+ menu.addAction(g_copyAct);
+ menu.addAction(g_pasteAct);
+ }
+
menu.addSeparator();
- menu.addAction(g_copyAct);
- menu.addAction(g_pasteAct);
+ menu.addAction(g_deleteAct);
}
- menu.addSeparator();
- menu.addAction(g_deleteAct);
-
// \todo Reimplement these features.
// menu.addAction(this->skeletonAction);
@@ -178,15 +236,52 @@ void ModelRightMenu::Run(const std::string &_modelName, const QPoint &_pt)
void ModelRightMenu::OnMoveTo()
{
rendering::UserCameraPtr cam = gui::get_active_camera();
- cam->MoveToVisual(this->modelName);
+ cam->MoveToVisual(this->entityName);
}
/////////////////////////////////////////////////
void ModelRightMenu::OnFollow()
{
rendering::UserCameraPtr cam = gui::get_active_camera();
- cam->TrackVisual(this->modelName);
- gui::Events::follow(this->modelName);
+ cam->TrackVisual(this->entityName);
+ gui::Events::follow(this->entityName);
+}
+
+/////////////////////////////////////////////////
+void ModelRightMenu::OnEdit()
+{
+ g_editModelAct->trigger();
+ gui::Events::editModel(this->entityName);
+}
+
+/////////////////////////////////////////////////
+void ModelRightMenu::OnApplyWrench()
+{
+ ApplyWrenchDialog *applyWrenchDialog = new ApplyWrenchDialog();
+
+ rendering::VisualPtr vis = gui::get_active_camera()->GetScene()->
+ GetVisual(this->entityName);
+
+ if (!vis)
+ {
+ gzerr << "Can't find entity " << this->entityName << std::endl;
+ return;
+ }
+
+ std::string modelName, linkName;
+ if (vis == vis->GetRootVisual())
+ {
+ modelName = this->entityName;
+ // If model selected just take the first link
+ linkName = vis->GetChild(0)->GetName();
+ }
+ else
+ {
+ modelName = vis->GetRootVisual()->GetName();
+ linkName = this->entityName;
+ }
+
+ applyWrenchDialog->Init(modelName, linkName);
}
/////////////////////////////////////////////////
@@ -199,7 +294,7 @@ void ModelRightMenu::OnFollow()
// if (!cam->GetScene())
// gzerr << "Invalid user camera scene\n";
//
-// // cam->GetScene()->SnapVisualToNearestBelow(this->modelName);
+// // cam->GetScene()->SnapVisualToNearestBelow(this->entityName);
// }
/////////////////////////////////////////////////
@@ -207,7 +302,7 @@ void ModelRightMenu::OnDelete(const std::string &_name)
{
std::string name = _name;
if (name.empty())
- name = this->modelName;
+ name = this->entityName;
// Delete the entity
if (!name.empty())
@@ -270,19 +365,19 @@ ViewState::ViewState(ModelRightMenu *_parent,
void ViewState::Callback()
{
// Store the check state for the model
- this->modelStates[this->parent->modelName] = this->action->isChecked();
+ this->modelStates[this->parent->entityName] = this->action->isChecked();
// Send a message with the new check state. The Scene listens to these
// messages and updates the visualizations accordingly.
if (this->action->isChecked())
{
transport::requestNoReply(this->parent->node, this->checkRequest,
- this->parent->modelName);
+ this->parent->entityName);
}
else
{
transport::requestNoReply(this->parent->node, this->uncheckRequest,
- this->parent->modelName);
+ this->parent->entityName);
}
}
@@ -290,17 +385,19 @@ void ViewState::Callback()
/////////////////////////////////////////////////
// void ModelRightMenu::OnSkeleton()
// {
-// this->skeletonActionState[this->modelName] =
+// this->skeletonActionState[this->entityName] =
// this->skeletonAction->isChecked();
//
// if (this->skeletonAction->isChecked())
// {
-// this->requestMsg = msgs::CreateRequest("show_skeleton", this->modelName);
+// this->requestMsg = msgs::CreateRequest("show_skeleton",
+// this->entityName);
// this->requestMsg->set_dbl_data(1.0);
// }
// else
// {
-// this->requestMsg = msgs::CreateRequest("show_skeleton", this->modelName);
+// this->requestMsg = msgs::CreateRequest("show_skeleton",
+// this->entityName);
// this->requestMsg->set_dbl_data(0.0);
// }
//
diff --git a/gazebo/gui/ModelRightMenu.hh b/gazebo/gui/ModelRightMenu.hh
index 3e63ed3..5749375 100644
--- a/gazebo/gui/ModelRightMenu.hh
+++ b/gazebo/gui/ModelRightMenu.hh
@@ -23,6 +23,7 @@
#include "gazebo/common/KeyEvent.hh"
#include "gazebo/gui/qt.h"
+#include "gazebo/gui/ApplyWrenchDialog.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/transport/TransportTypes.hh"
#include "gazebo/util/system.hh"
@@ -39,6 +40,18 @@ namespace gazebo
{
Q_OBJECT
+ /// \enum EntityTypes
+ /// \brief Unique identifiers for the type of entity this menu is
+ /// attached to.
+ public: enum EntityTypes {
+ /// \brief Model
+ MODEL,
+ /// \brief Light
+ LIGHT,
+ /// \brief Link
+ LINK
+ };
+
/// \brief Constructor
public: ModelRightMenu();
@@ -46,10 +59,12 @@ namespace gazebo
public: virtual ~ModelRightMenu();
/// \brief Show the right menu.
- /// \param[in] _modelName Name of the model that is active.
+ /// \param[in] _entityName Name of the entity that is active.
/// \param[in] _pt Point on the GUI that has received the right-click
/// request.
- public: void Run(const std::string &_modelName, const QPoint &_pt);
+ /// \param[in] _type Type of the entity clicked.
+ public: void Run(const std::string &_entityName, const QPoint &_pt,
+ EntityTypes _type = MODEL);
/// \brief QT callback when move to has been selected.
private slots: void OnMoveTo();
@@ -57,8 +72,14 @@ namespace gazebo
/// \brief QT callback when follow has been selected.
private slots: void OnFollow();
+ /// \brief QT callback when edit has been selected.
+ private slots: void OnEdit();
+
+ /// \brief QT callback when apply force/torque has been selected.
+ private slots: void OnApplyWrench();
+
/// \brief QT callback when delete has been selected.
- /// \param[in] _name Name of the model to delete.
+ /// \param[in] _name Name of the entity to delete.
private slots: void OnDelete(const std::string &_name="");
/// \brief QT callback when snap below has been selected.
@@ -81,15 +102,21 @@ namespace gazebo
/// \brief Subscriber to request messages.
private: transport::SubscriberPtr requestSub;
- /// \brief Name of the active model.
- private: std::string modelName;
+ /// \brief Name of the active entity.
+ private: std::string entityName;
/// \brief Action for moving the camera to an object.
private: QAction *moveToAct;
- /// \brief Action for attaching the camera to a model.
+ /// \brief Action for attaching the camera to an entity.
private: QAction *followAct;
+ /// \brief Action for opening entity on Model Editor.
+ private: QAction *editAct;
+
+ /// \brief Action for applying force and torque to a model.
+ private: QAction *applyWrenchAct;
+
/// \brief Action for snapping an object to another object below the
/// first.
// private: QAction *snapBelowAct;
diff --git a/gazebo/gui/ModelSnap.cc b/gazebo/gui/ModelSnap.cc
index 38f1800..0ffe156 100644
--- a/gazebo/gui/ModelSnap.cc
+++ b/gazebo/gui/ModelSnap.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/transport/transport.hh"
#include "gazebo/rendering/RenderTypes.hh"
@@ -46,29 +52,58 @@ ModelSnap::ModelSnap()
this->dataPtr->selectedTriangleDirty = false;
this->dataPtr->hoverTriangleDirty = false;
this->dataPtr->snapLines = NULL;
-
- this->dataPtr->updateMutex = new boost::recursive_mutex();
+ this->dataPtr->updateMutex = NULL;
}
/////////////////////////////////////////////////
ModelSnap::~ModelSnap()
{
+ this->Clear();
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+void ModelSnap::Clear()
+{
+ this->dataPtr->selectedTriangleDirty = false;
+ this->dataPtr->hoverTriangleDirty = false;
+ this->dataPtr->selectedTriangle.clear();
+ this->dataPtr->hoverTriangle.clear();
+ this->dataPtr->selectedVis.reset();
+ this->dataPtr->hoverVis.reset();
+
+ this->dataPtr->node.reset();
this->dataPtr->modelPub.reset();
+ if (this->dataPtr->updateMutex)
{
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->updateMutex);
- if (this->dataPtr->snapLines)
- delete this->dataPtr->snapLines;
+ delete this->dataPtr->snapLines;
+ this->dataPtr->snapLines = NULL;
this->dataPtr->snapVisual.reset();
+
+ if (this->dataPtr->snapHighlight != NULL &&
+ this->dataPtr->highlightVisual != NULL)
+ {
+ this->dataPtr->highlightVisual->
+ DeleteDynamicLine(this->dataPtr->snapHighlight);
+ }
+ this->dataPtr->highlightVisual.reset();
}
- event::Events::DisconnectRender(this->dataPtr->renderConnection);
+ if (this->dataPtr->renderConnection)
+ event::Events::DisconnectRender(this->dataPtr->renderConnection);
this->dataPtr->renderConnection.reset();
delete this->dataPtr->updateMutex;
+ this->dataPtr->updateMutex = NULL;
- delete this->dataPtr;
- this->dataPtr = NULL;
+ this->dataPtr->scene.reset();
+ this->dataPtr->userCamera.reset();
+ this->dataPtr->rayQuery.reset();
+
+ this->dataPtr->initialized = false;
}
/////////////////////////////////////////////////
@@ -87,6 +122,8 @@ void ModelSnap::Init()
this->dataPtr->userCamera = cam;
this->dataPtr->scene = cam->GetScene();
+ this->dataPtr->updateMutex = new boost::recursive_mutex();
+
this->dataPtr->node = transport::NodePtr(new transport::Node());
this->dataPtr->node->Init();
this->dataPtr->modelPub =
@@ -149,14 +186,14 @@ void ModelSnap::OnMouseMoveEvent(const common::MouseEvent &_event)
this->dataPtr->mouseEvent = _event;
rendering::VisualPtr vis = this->dataPtr->userCamera->GetVisual(
- this->dataPtr->mouseEvent.pos);
+ this->dataPtr->mouseEvent.Pos());
if (vis && !vis->IsPlane())
{
// get the triangle being hovered so that it can be highlighted
math::Vector3 intersect;
std::vector<math::Vector3> hoverTriangle;
- this->dataPtr->rayQuery->SelectMeshTriangle(_event.pos.x, _event.pos.y,
- vis->GetRootVisual(), intersect, hoverTriangle);
+ this->dataPtr->rayQuery->SelectMeshTriangle(_event.Pos().X(),
+ _event.Pos().Y(), vis->GetRootVisual(), intersect, hoverTriangle);
if (!hoverTriangle.empty())
{
@@ -190,14 +227,16 @@ void ModelSnap::OnMouseReleaseEvent(const common::MouseEvent &_event)
this->dataPtr->mouseEvent = _event;
rendering::VisualPtr vis = this->dataPtr->userCamera->GetVisual(
- this->dataPtr->mouseEvent.pos);
+ this->dataPtr->mouseEvent.Pos());
if (vis && !vis->IsPlane() &&
- this->dataPtr->mouseEvent.button == common::MouseEvent::LEFT)
+ this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT)
{
// Parent model or parent link
rendering::VisualPtr currentParent = vis->GetRootVisual();
rendering::VisualPtr previousParent;
+ rendering::VisualPtr topLevelVis = vis->GetNthAncestor(2);
+
if (gui::get_entity_id(currentParent->GetName()))
{
if (this->dataPtr->selectedVis)
@@ -205,9 +244,11 @@ void ModelSnap::OnMouseReleaseEvent(const common::MouseEvent &_event)
}
else
{
- currentParent = vis->GetParent();
+ currentParent = topLevelVis;
if (this->dataPtr->selectedVis)
- previousParent = this->dataPtr->selectedVis->GetParent();
+ {
+ previousParent = this->dataPtr->selectedVis->GetNthAncestor(2);
+ }
}
// Select first triangle on any mesh
@@ -215,8 +256,9 @@ void ModelSnap::OnMouseReleaseEvent(const common::MouseEvent &_event)
if (!this->dataPtr->selectedVis || (currentParent == previousParent))
{
math::Vector3 intersect;
- this->dataPtr->rayQuery->SelectMeshTriangle(_event.pos.x, _event.pos.y,
- currentParent, intersect, this->dataPtr->selectedTriangle);
+ this->dataPtr->rayQuery->SelectMeshTriangle(_event.Pos().X(),
+ _event.Pos().Y(), currentParent, intersect,
+ this->dataPtr->selectedTriangle);
if (!this->dataPtr->selectedTriangle.empty())
{
@@ -234,8 +276,8 @@ void ModelSnap::OnMouseReleaseEvent(const common::MouseEvent &_event)
// select triangle on the target
math::Vector3 intersect;
std::vector<math::Vector3> vertices;
- this->dataPtr->rayQuery->SelectMeshTriangle(_event.pos.x, _event.pos.y,
- currentParent, intersect, vertices);
+ this->dataPtr->rayQuery->SelectMeshTriangle(_event.Pos().X(),
+ _event.Pos().Y(), currentParent, intersect, vertices);
if (!vertices.empty())
{
@@ -315,7 +357,7 @@ void ModelSnap::PublishVisualPose(rendering::VisualPtr _vis)
msg.set_id(gui::get_entity_id(_vis->GetName()));
msg.set_name(_vis->GetName());
- msgs::Set(msg.mutable_pose(), _vis->GetWorldPose());
+ msgs::Set(msg.mutable_pose(), _vis->GetWorldPose().Ign());
this->dataPtr->modelPub->Publish(msg);
}
}
diff --git a/gazebo/gui/ModelSnap.hh b/gazebo/gui/ModelSnap.hh
index 2f88d33..445a5b1 100644
--- a/gazebo/gui/ModelSnap.hh
+++ b/gazebo/gui/ModelSnap.hh
@@ -48,6 +48,10 @@ namespace gazebo
/// \brief Initialize the model snapping tool.
public: void Init();
+ /// \brief Clear the model snapping tool. This explicity cleans up the
+ /// internal state of the singleton and prepares it for exit.
+ public: void Clear();
+
/// \brief Reset the model snapping tool.
public: void Reset();
diff --git a/gazebo/gui/ModelSnapPrivate.hh b/gazebo/gui/ModelSnapPrivate.hh
index ec6e640..540a4e9 100644
--- a/gazebo/gui/ModelSnapPrivate.hh
+++ b/gazebo/gui/ModelSnapPrivate.hh
@@ -53,9 +53,6 @@ namespace gazebo
/// \brief Current mouse event.
public: common::MouseEvent mouseEvent;
- /// \brief Current key event.
- public: common::KeyEvent keyEvent;
-
/// \brief True if the model align tool is initialized.
public: bool initialized;
diff --git a/gazebo/gui/OculusWindow.cc b/gazebo/gui/OculusWindow.cc
index 39248a5..26b7270 100644
--- a/gazebo/gui/OculusWindow.cc
+++ b/gazebo/gui/OculusWindow.cc
@@ -59,6 +59,7 @@ OculusWindow::OculusWindow(int _x, int _y, const std::string &_visual,
/////////////////////////////////////////////////
OculusWindow::~OculusWindow()
{
+ this->scene.reset();
if (this->attachCameraThread)
this->attachCameraThread->join();
delete this->attachCameraThread;
diff --git a/gazebo/gui/Projection_TEST.cc b/gazebo/gui/Projection_TEST.cc
new file mode 100644
index 0000000..c5d60bd
--- /dev/null
+++ b/gazebo/gui/Projection_TEST.cc
@@ -0,0 +1,108 @@
+/*
+ * 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/gui/GuiIface.hh"
+#include "gazebo/gui/MainWindow.hh"
+#include "gazebo/gui/Projection_TEST.hh"
+
+#include "test_config.h"
+
+/////////////////////////////////////////////////
+void Projection_TEST::Projection()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/ortho_box.world", false, false, false);
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+
+ // Create the main window.
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ cam->SetCaptureData(true);
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ const unsigned char *data = cam->GetImageData();
+ unsigned int width = cam->GetImageWidth();
+ unsigned int height = cam->GetImageHeight();
+ unsigned int depth = cam->GetImageDepth();
+
+ int topWidth = 0;
+ int bottomWidth = 0;
+
+ // Get the width of the white box at the top and bottom of the image
+ for (unsigned int x = 0; x < width*depth; ++x)
+ {
+ if ( data[x] >= 250)
+ topWidth++;
+
+ if ( data[((height-1)*width*depth)+x] >= 250)
+ bottomWidth++;
+ }
+
+ // Make sure the widths are roughly the same
+ QVERIFY(std::abs(topWidth - bottomWidth) < 5);
+
+ // Now change to Perspective projection
+ cam->SetProjectionType("perspective");
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ topWidth = 0;
+ bottomWidth = 0;
+
+ // Get the width of the white box at the top and bottom of the image
+ for (unsigned int x = 0; x < width*depth; ++x)
+ {
+ if (data[x] >= 250)
+ topWidth++;
+
+ if (data[((height-1)*width*depth)+x] >= 250)
+ bottomWidth++;
+ }
+
+ // Make sure the top and bottom width difference is large enough to
+ // indicate perspective rendering is used
+ QVERIFY(topWidth - bottomWidth < -200);
+
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(Projection_TEST)
diff --git a/gazebo/gui/Projection_TEST.hh b/gazebo/gui/Projection_TEST.hh
new file mode 100644
index 0000000..df1c53d
--- /dev/null
+++ b/gazebo/gui/Projection_TEST.hh
@@ -0,0 +1,31 @@
+/*
+ * 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_PROJECTION_TEST_HH_
+#define _GAZEBO_PROJECTION_TEST_HH_
+
+#include "gazebo/gui/QTestFixture.hh"
+
+/// \brief A test class for orthographic and perspective projections
+class Projection_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Test orthographic and perspective projections.
+ private slots: void Projection();
+};
+#endif
diff --git a/gazebo/gui/RenderWidget.cc b/gazebo/gui/RenderWidget.cc
index e573793..baacb75 100644
--- a/gazebo/gui/RenderWidget.cc
+++ b/gazebo/gui/RenderWidget.cc
@@ -14,6 +14,12 @@
* 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 <iomanip>
#include "gazebo/rendering/UserCamera.hh"
@@ -27,7 +33,6 @@
#include "gazebo/gui/GuiEvents.hh"
#include "gazebo/gui/TimePanel.hh"
#include "gazebo/gui/RenderWidget.hh"
-#include "gazebo/gui/building/BuildingEditorWidget.hh"
using namespace gazebo;
using namespace gui;
@@ -37,10 +42,6 @@ RenderWidget::RenderWidget(QWidget *_parent)
: QWidget(_parent)
{
this->setObjectName("renderWidget");
- this->show();
-
- this->clear = false;
- this->create = false;
QVBoxLayout *mainLayout = new QVBoxLayout;
this->mainFrame = new QFrame;
@@ -49,70 +50,128 @@ RenderWidget::RenderWidget(QWidget *_parent)
QVBoxLayout *frameLayout = new QVBoxLayout;
- QFrame *toolFrame = new QFrame;
- toolFrame->setObjectName("toolFrame");
- toolFrame->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Minimum);
+ this->toolFrame = new QFrame;
+ this->toolFrame->setObjectName("toolFrame");
+ this->toolFrame->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Minimum);
this->toolbar = new QToolBar;
QHBoxLayout *toolLayout = new QHBoxLayout;
toolLayout->setContentsMargins(0, 0, 0, 0);
- QActionGroup *actionGroup = new QActionGroup(toolFrame);
- actionGroup->addAction(g_arrowAct);
- actionGroup->addAction(g_translateAct);
- actionGroup->addAction(g_rotateAct);
- actionGroup->addAction(g_scaleAct);
- actionGroup->addAction(g_snapAct);
-
- this->toolbar->addAction(g_arrowAct);
- this->toolbar->addAction(g_translateAct);
- this->toolbar->addAction(g_rotateAct);
- this->toolbar->addAction(g_scaleAct);
+ // Manipulation modes
+ QActionGroup *actionGroup = new QActionGroup(this->toolFrame);
+ if (g_arrowAct)
+ {
+ actionGroup->addAction(g_arrowAct);
+ this->toolbar->addAction(g_arrowAct);
+ }
+ if (g_translateAct)
+ {
+ actionGroup->addAction(g_translateAct);
+ this->toolbar->addAction(g_translateAct);
+ }
+ if (g_rotateAct)
+ {
+ actionGroup->addAction(g_rotateAct);
+ this->toolbar->addAction(g_rotateAct);
+ }
+ if (g_scaleAct)
+ {
+ actionGroup->addAction(g_scaleAct);
+ this->toolbar->addAction(g_scaleAct);
+ }
this->toolbar->addSeparator();
- this->toolbar->addAction(g_boxCreateAct);
- this->toolbar->addAction(g_sphereCreateAct);
- this->toolbar->addAction(g_cylinderCreateAct);
- this->toolbar->addSeparator();
- this->toolbar->addAction(g_pointLghtCreateAct);
- this->toolbar->addAction(g_spotLghtCreateAct);
- this->toolbar->addAction(g_dirLghtCreateAct);
+
+ // Insert simple shapes
+ if (g_boxCreateAct)
+ this->toolbar->addAction(g_boxCreateAct);
+ if (g_sphereCreateAct)
+ this->toolbar->addAction(g_sphereCreateAct);
+ if (g_cylinderCreateAct)
+ this->toolbar->addAction(g_cylinderCreateAct);
this->toolbar->addSeparator();
- this->toolbar->addAction(g_screenshotAct);
+ // Insert lights
+ if (g_pointLghtCreateAct)
+ this->toolbar->addAction(g_pointLghtCreateAct);
+ if (g_spotLghtCreateAct)
+ this->toolbar->addAction(g_spotLghtCreateAct);
+ if (g_dirLghtCreateAct)
+ this->toolbar->addAction(g_dirLghtCreateAct);
this->toolbar->addSeparator();
- this->toolbar->addAction(g_copyAct);
- this->toolbar->addAction(g_pasteAct);
+
+ // Copy & Paste
+ if (g_copyAct)
+ this->toolbar->addAction(g_copyAct);
+ if (g_pasteAct)
+ this->toolbar->addAction(g_pasteAct);
this->toolbar->addSeparator();
- QToolButton *alignButton = new QToolButton;
- alignButton->setToolButtonStyle(Qt::ToolButtonIconOnly);
- alignButton->setIcon(QIcon(":/images/align.png"));
- alignButton->setToolTip(
- tr("In Selection Mode, hold Ctrl and select 2 objects to align"));
- alignButton->setArrowType(Qt::NoArrow);
- QMenu *alignMenu = new QMenu(alignButton);
- alignMenu->addAction(g_alignAct);
- alignButton->setMenu(alignMenu);
- alignButton->setPopupMode(QToolButton::InstantPopup);
- g_alignButtonAct = this->toolbar->addWidget(alignButton);
- connect(alignButton, SIGNAL(pressed()), g_alignAct, SLOT(trigger()));
+ // Align
+ if (g_alignAct)
+ {
+ QToolButton *alignButton = new QToolButton;
+ alignButton->setToolButtonStyle(Qt::ToolButtonIconOnly);
+ alignButton->setIcon(QIcon(":/images/align.png"));
+ alignButton->setToolTip(
+ tr("In Selection Mode, hold Ctrl and select 2 objects to align"));
+ alignButton->setArrowType(Qt::NoArrow);
+ QMenu *alignMenu = new QMenu(alignButton);
+ alignMenu->addAction(g_alignAct);
+ alignButton->setMenu(alignMenu);
+ alignButton->setPopupMode(QToolButton::InstantPopup);
+ g_alignButtonAct = this->toolbar->addWidget(alignButton);
+ connect(alignButton, SIGNAL(pressed()), g_alignAct, SLOT(trigger()));
+ }
+
+ // Snap
+ if (g_snapAct)
+ {
+ actionGroup->addAction(g_snapAct);
+ this->toolbar->addAction(g_snapAct);
+ }
this->toolbar->addSeparator();
- this->toolbar->addAction(g_snapAct);
+
+ // View angle
+ if (g_viewAngleAct)
+ {
+ QToolButton *viewAngleButton = new QToolButton;
+ viewAngleButton->setObjectName("viewAngleToolBarButton");
+ viewAngleButton->setStyleSheet(
+ "#viewAngleToolBarButton{padding-right:10px}");
+ viewAngleButton->setToolButtonStyle(Qt::ToolButtonIconOnly);
+ viewAngleButton->setIcon(QIcon(":/images/view_angle_front.png"));
+ viewAngleButton->setToolTip(tr("Change the view angle"));
+
+ QMenu *viewAngleMenu = new QMenu(viewAngleButton);
+ viewAngleMenu->addAction(g_viewAngleAct);
+
+ viewAngleButton->setMenu(viewAngleMenu);
+ viewAngleButton->setPopupMode(QToolButton::InstantPopup);
+ g_viewAngleButtonAct = this->toolbar->addWidget(viewAngleButton);
+ }
+
+ // Empty space to push whatever comes next to the right
+ QWidget *spacer = new QWidget();
+ spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
+ QAction *spacerAction = this->toolbar->addWidget(spacer);
+ spacerAction->setObjectName("toolbarSpacerAction");
+
+ // Screenshot / logging
+ if (g_screenshotAct)
+ this->toolbar->addAction(g_screenshotAct);
+ if (g_dataLoggerAct)
+ this->toolbar->addAction(g_dataLoggerAct);
toolLayout->addSpacing(10);
toolLayout->addWidget(this->toolbar);
- toolFrame->setLayout(toolLayout);
+ toolLayout->addSpacing(10);
+ this->toolFrame->setLayout(toolLayout);
this->glWidget = new GLWidget(this->mainFrame);
- rendering::ScenePtr scene = rendering::create_scene(gui::get_world(), true);
-
- this->buildingEditorWidget = new BuildingEditorWidget(this);
- this->buildingEditorWidget->setSizePolicy(QSizePolicy::Expanding,
- QSizePolicy::Expanding);
- this->buildingEditorWidget->hide();
this->msgOverlayLabel = new QLabel(this->glWidget);
this->msgOverlayLabel->setStyleSheet(
@@ -121,14 +180,14 @@ RenderWidget::RenderWidget(QWidget *_parent)
QHBoxLayout *bottomPanelLayout = new QHBoxLayout;
- TimePanel *timePanel = new TimePanel(this);
+ this->timePanel = new TimePanel(this);
this->bottomFrame = new QFrame;
this->bottomFrame->setObjectName("renderBottomFrame");
this->bottomFrame->setSizePolicy(QSizePolicy::Expanding,
QSizePolicy::Minimum);
- bottomPanelLayout->addWidget(timePanel, 0);
+ bottomPanelLayout->addWidget(this->timePanel, 0);
bottomPanelLayout->setSpacing(0);
bottomPanelLayout->setContentsMargins(0, 0, 0, 0);
this->bottomFrame->setLayout(bottomPanelLayout);
@@ -136,24 +195,21 @@ RenderWidget::RenderWidget(QWidget *_parent)
QFrame *render3DFrame = new QFrame;
render3DFrame->setObjectName("render3DFrame");
QVBoxLayout *render3DLayout = new QVBoxLayout;
- render3DLayout->addWidget(toolFrame);
+ render3DLayout->addWidget(this->toolFrame);
render3DLayout->addWidget(this->glWidget);
render3DLayout->setContentsMargins(0, 0, 0, 0);
render3DLayout->setSpacing(0);
render3DFrame->setLayout(render3DLayout);
- QSplitter *splitter = new QSplitter(this);
- splitter->addWidget(this->buildingEditorWidget);
- splitter->addWidget(render3DFrame);
+ this->splitter = new QSplitter(this);
+ this->splitter->addWidget(render3DFrame);
QList<int> sizes;
sizes.push_back(300);
- sizes.push_back(300);
- splitter->setSizes(sizes);
- splitter->setStretchFactor(0, 1);
- splitter->setStretchFactor(1, 1);
- splitter->setOrientation(Qt::Vertical);
+ this->splitter->setSizes(sizes);
+ this->splitter->setStretchFactor(0, 1);
+ this->splitter->setOrientation(Qt::Vertical);
- frameLayout->addWidget(splitter);
+ frameLayout->addWidget(this->splitter);
frameLayout->addWidget(this->bottomFrame);
frameLayout->setContentsMargins(0, 0, 0, 0);
frameLayout->setSpacing(0);
@@ -166,10 +222,6 @@ RenderWidget::RenderWidget(QWidget *_parent)
this->setLayout(mainLayout);
this->layout()->setContentsMargins(0, 0, 0, 0);
- this->timer = new QTimer(this);
- connect(this->timer, SIGNAL(timeout()), this, SLOT(update()));
- this->timer->start(44);
-
this->connections.push_back(
gui::Events::ConnectFollow(
boost::bind(&RenderWidget::OnFollow, this, _1)));
@@ -217,109 +269,61 @@ RenderWidget::~RenderWidget()
delete this->toolbar;
this->toolbar = NULL;
+
+ // we created the scene here we are responsible for removing it.
+ rendering::remove_scene(gui::get_world());
}
/////////////////////////////////////////////////
-void RenderWidget::update()
+void RenderWidget::InsertWidget(unsigned int _index, QWidget *_widget)
{
- if (this->clear)
- {
- rendering::remove_scene(this->clearName);
- this->clear = false;
- return;
- }
- else if (this->create)
- {
- rendering::create_scene(this->createName, true);
- this->create = false;
- return;
- }
-
- rendering::UserCameraPtr cam = this->glWidget->GetCamera();
-
- if (!cam || !cam->GetInitialized())
+ if (static_cast<int>(_index) <= this->splitter->count())
{
- event::Events::preRender();
- return;
+ // set equal size for now. There should always be at least one widget
+ // (render3DFrame) in the splitter.
+ int childCount = this->splitter->count();
+ GZ_ASSERT(childCount > 0,
+ "RenderWidget splitter has no child widget");
+
+ QSize widgetSize = this->size();
+ int newSize = widgetSize.height() / (this->splitter->count()+1);
+ QList<int> newSizes;
+ for (int i = 0; i < childCount+1; ++i)
+ newSizes.append(newSize);
+
+ this->splitter->insertWidget(_index, _widget);
+ this->splitter->setSizes(newSizes);
+ this->splitter->setStretchFactor(_index, 1);
}
-
- // float fps = cam->GetAvgFPS();
- // int triangleCount = cam->GetTriangleCount();
- // math::Pose pose = cam->GetWorldPose();
-
- // std::ostringstream stream;
-
- // stream << std::fixed << std::setprecision(2) << pose.pos.x;
- // this->xPosEdit->setText(tr(stream.str().c_str()));
- // stream.str("");
-
- // stream << std::fixed << std::setprecision(2) << pose.pos.y;
- // this->yPosEdit->setText(tr(stream.str().c_str()));
- // stream.str("");
-
- // stream << std::fixed << std::setprecision(2) << pose.pos.z;
- // this->zPosEdit->setText(tr(stream.str().c_str()));
- // stream.str("");
-
- // stream << std::fixed << std::setprecision(2)
- // << GZ_RTOD(pose.rot.GetAsEuler().x);
- // this->rollEdit->setText(tr(stream.str().c_str()));
- // stream.str("");
-
- // stream << std::fixed << std::setprecision(2)
- // << GZ_RTOD(pose.rot.GetAsEuler().y);
- // this->pitchEdit->setText(tr(stream.str().c_str()));
- // stream.str("");
-
- // stream << std::fixed << std::setprecision(2)
- // << GZ_RTOD(pose.rot.GetAsEuler().z);
- // this->yawEdit->setText(tr(stream.str().c_str()));
- // stream.str("");
-
- /*stream << std::fixed << std::setprecision(1) << fps;
- this->fpsEdit->setText(tr(stream.str().c_str()));
- stream.str("");
-
- stream << std::fixed << std::setprecision(2) << triangleCount;
- this->trianglesEdit->setText(tr(stream.str().c_str()));
- */
-
- this->glWidget->update();
+ else
+ gzerr << "Unable to add widget, index out of range " << std::endl;
}
/////////////////////////////////////////////////
-void RenderWidget::ShowEditor(bool _show)
+void RenderWidget::ShowTimePanel(bool _show)
{
if (_show)
- {
- this->buildingEditorWidget->show();
- this->baseOverlayMsg = "Building is view-only";
- this->OnClearOverlayMsg();
- this->bottomFrame->hide();
- this->ShowToolbar(false);
- }
- else
- {
- this->buildingEditorWidget->hide();
- this->baseOverlayMsg = "";
- this->OnClearOverlayMsg();
this->bottomFrame->show();
- this->ShowToolbar(true);
- }
+ else
+ this->bottomFrame->hide();
+}
+
+/////////////////////////////////////////////////
+TimePanel *RenderWidget::GetTimePanel() const
+{
+ return this->timePanel;
}
/////////////////////////////////////////////////
void RenderWidget::RemoveScene(const std::string &_name)
{
- this->clear = true;
- this->clearName = _name;
+ rendering::remove_scene(_name);
}
/////////////////////////////////////////////////
void RenderWidget::CreateScene(const std::string &_name)
{
- this->create = true;
- this->createName = _name;
+ rendering::create_scene(_name, true);
}
/////////////////////////////////////////////////
@@ -343,6 +347,13 @@ void RenderWidget::DisplayOverlayMsg(const std::string &_msg, int _duration)
}
/////////////////////////////////////////////////
+void RenderWidget::SetOverlaysVisible(const bool _visible)
+{
+ for (auto const &plugin : this->plugins)
+ plugin->setVisible(_visible);
+}
+
+/////////////////////////////////////////////////
std::string RenderWidget::GetOverlayMsg() const
{
return this->msgOverlayLabel->text().toStdString();
@@ -355,11 +366,11 @@ void RenderWidget::ShowToolbar(const bool _show)
{
if (_show)
{
- this->toolbar->show();
+ this->toolFrame->show();
}
else
{
- this->toolbar->hide();
+ this->toolFrame->hide();
}
}
}
diff --git a/gazebo/gui/RenderWidget.hh b/gazebo/gui/RenderWidget.hh
index f5ae80c..196878f 100644
--- a/gazebo/gui/RenderWidget.hh
+++ b/gazebo/gui/RenderWidget.hh
@@ -32,11 +32,10 @@ class QHBoxLayout;
namespace gazebo
{
- class GLWidget;
-
namespace gui
{
- class BuildingEditorWidget;
+ class GLWidget;
+ class TimePanel;
class GAZEBO_VISIBLE RenderWidget : public QWidget
{
@@ -47,9 +46,18 @@ namespace gazebo
public: void RemoveScene(const std::string &_name);
public: void CreateScene(const std::string &_name);
- /// \brief Show editor widget in the main window
- /// param[in] _show True to show the editor widget, false to hide it.
- public: void ShowEditor(bool _show);
+ /// \brief Add a widget inside the render widget
+ /// \param[in] _widget Widget to be added.
+ /// \param[in] _index Index in the splitter to add the widget at.
+ public: void InsertWidget(unsigned int _index, QWidget *_widget);
+
+ /// \brief Show the time panel.
+ /// \para[in] _show True to show the panel, false to hide it.
+ public: void ShowTimePanel(bool _show);
+
+ /// \brief Get the time panel widget.
+ /// \return the time panel widget.
+ public: TimePanel *GetTimePanel() const;
/// \brief Display an overlay message
/// \param[in] _msg Message to be displayed
@@ -57,6 +65,10 @@ namespace gazebo
public: void DisplayOverlayMsg(const std::string &_msg,
int _duration = -1);
+ /// \brief Show or hide all the GUI overlays.
+ /// \param[in] _visible True to show the GUI overlays, false to hide them.
+ public: void SetOverlaysVisible(const bool _visible);
+
/// \brief Get the overlay message being displayed
/// \return Message displayed in the render window
public: std::string GetOverlayMsg() const;
@@ -74,8 +86,6 @@ namespace gazebo
/// \param[in] _show Whether or not to show the toolbar.
public: void ShowToolbar(const bool _show);
- private slots: virtual void update();
-
/// \brief Qt callback to clear overlay message if a duration is
/// specified
private slots: void OnClearOverlayMsg();
@@ -92,9 +102,6 @@ namespace gazebo
/// \brief Widget used to draw the scene.
private: GLWidget *glWidget;
- /// \brief Building editor widget for creating a building model
- private: BuildingEditorWidget *buildingEditorWidget;
-
/// \brief Frame that holds the contents of this widget.
private: QFrame *mainFrame;
@@ -103,6 +110,10 @@ namespace gazebo
/// \brief Bottom frame that holds the play/pause widgets
private: QFrame *bottomFrame;
+
+ /// \brief Frame which holds the top toolbar.
+ private: QFrame *toolFrame;
+
private: QLabel *xyzLabel;
private: QLineEdit *xPosEdit;
private: QLineEdit *yPosEdit;
@@ -117,24 +128,24 @@ namespace gazebo
/// \brief Widget for the top toolbar
private: QToolBar *toolbar;
+
private: QToolBar *mouseToolbar;
private: QToolBar *editToolbar;
/// \brief An overlay label on the 3D render widget
private: QLabel *msgOverlayLabel;
- private: bool clear;
- private: std::string clearName;
-
- private: bool create;
- private: std::string createName;
- private: QTimer *timer;
-
/// \brief Base overlay message;
private: std::string baseOverlayMsg;
+ /// \brief Vertical splitter between widgets.
+ private: QSplitter *splitter;
+
/// \brief All the gui plugins
private: std::vector<gazebo::GUIPluginPtr> plugins;
+
+ /// \brief Time panel widget.
+ private: TimePanel *timePanel;
};
}
}
diff --git a/gazebo/gui/RenderWidget_TEST.cc b/gazebo/gui/RenderWidget_TEST.cc
new file mode 100644
index 0000000..1269a42
--- /dev/null
+++ b/gazebo/gui/RenderWidget_TEST.cc
@@ -0,0 +1,55 @@
+/*
+ * 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/gui/RenderWidget.hh"
+#include "gazebo/gui/RenderWidget_TEST.hh"
+
+/////////////////////////////////////////////////
+void RenderWidget_TEST::InsertWidget()
+{
+ this->Load("empty.world");
+
+ // Create a new render widget
+ gazebo::gui::RenderWidget *renderWidget = new gazebo::gui::RenderWidget;
+
+ // Get the splitter widget
+ QSplitter *splitter = renderWidget->findChild<QSplitter *>();
+ QVERIFY(splitter != NULL);
+
+ QVERIFY(splitter->count() == 1);
+ QWidget *render3DWidget = splitter->widget(0);
+ QVERIFY(render3DWidget->objectName().toStdString() == "render3DFrame");
+
+ // Prepend a widget
+ QWidget *testWidget = new QWidget(renderWidget);
+ testWidget->setObjectName("testWidget");
+ renderWidget->InsertWidget(0, testWidget);
+ QVERIFY(splitter->count() == 2);
+ QWidget *verifyTestWidget = splitter->widget(0);
+ QVERIFY(verifyTestWidget->objectName().toStdString() == "testWidget");
+
+ // Append a widget
+ QWidget *testWidget2 = new QWidget(renderWidget);
+ testWidget2->setObjectName("testWidget2");
+ renderWidget->InsertWidget(splitter->count(), testWidget2);
+ QVERIFY(splitter->count() == 3);
+ QWidget *verifyTestWidget2 = splitter->widget(2);
+ QVERIFY(verifyTestWidget2->objectName().toStdString() == "testWidget2");
+}
+
+// Generate a main function for the test
+QTEST_MAIN(RenderWidget_TEST)
diff --git a/gazebo/gui/RenderWidget_TEST.hh b/gazebo/gui/RenderWidget_TEST.hh
new file mode 100644
index 0000000..85093ae
--- /dev/null
+++ b/gazebo/gui/RenderWidget_TEST.hh
@@ -0,0 +1,32 @@
+/*
+ * 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 _RENDERWIDGET_TEST_HH_
+#define _RENDERWIDGET_TEST_HH_
+
+#include "gazebo/gui/QTestFixture.hh"
+
+/// \brief A test class for the RenderWidget.
+class RenderWidget_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Test inserting widgets to the render widget
+ private slots: void InsertWidget();
+};
+
+#endif
diff --git a/gazebo/gui/SaveDialog.cc b/gazebo/gui/SaveDialog.cc
index f1eb158..9e68726 100644
--- a/gazebo/gui/SaveDialog.cc
+++ b/gazebo/gui/SaveDialog.cc
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2015 Open Source Robotics Foundation
+ * Copyright (C) 2013-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,56 +15,161 @@
*
*/
+#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/filesystem.hpp>
+
+#include "gazebo/common/SystemPaths.hh"
#include "gazebo/common/Console.hh"
+
+#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/SaveDialogPrivate.hh"
#include "gazebo/gui/SaveDialog.hh"
+#include "gazebo/gazebo_config.h"
+
using namespace gazebo;
using namespace gui;
/////////////////////////////////////////////////
-SaveDialog::SaveDialog(QWidget *_parent) : QDialog(_parent)
+SaveDialog::SaveDialog(int _mode, QWidget *_parent)
+ : QDialog(_parent), dataPtr(new SaveDialogPrivate)
{
this->setObjectName("saveDialog");
- this->fileExtension = "";
-
- this->messageLabel = new QLabel;
- this->messageLabel->setText(
- tr("Please enter the name and location to save to"));
-
- QLabel *nameLabel = new QLabel;
- nameLabel->setText(tr("Name"));
- this->nameLineEdit = new QLineEdit;
- this->nameLineEdit->setText(tr("DefaultName"));
- QLabel *saveLocation = new QLabel;
- saveLocation->setText(tr("Location"));
- this->locationLineEdit = new QLineEdit;
- this->locationLineEdit->setText(QDir::homePath());
+ this->setWindowTitle(tr("Save Model"));
+
+ this->dataPtr->messageLabel = new QLabel;
+ this->dataPtr->messageLabel->setText(
+ tr("Pick a name and a location for your model.\n"
+ "All the model files will be saved in the model folder.\n"));
+
+ QLabel *modelLabel = new QLabel;
+ modelLabel->setText(tr("Model Name:"));
+ this->dataPtr->modelNameLineEdit = new QLineEdit;
+ connect(this->dataPtr->modelNameLineEdit, SIGNAL(textChanged(QString)), this,
+ SLOT(ModelNameChangedOnDialog(QString)));
+
+ QLabel *modelHeader = new QLabel;
+ modelHeader->setText(tr("<b>Model</b>"));
+
+ QLabel *modelLocation = new QLabel;
+ modelLocation->setText(tr("Location:"));
+ this->dataPtr->modelLocationLineEdit = new QLineEdit;
+ this->dataPtr->modelLocationLineEdit->setMinimumWidth(300);
+
+ // Try to get path to home folder
+ if (_mode == SaveMode::BUILDING)
+ {
+ this->dataPtr->modelLocationLineEdit->setText(QDir::homePath()+
+ "/building_editor_models/Untitled");
+ }
+ else if (_mode == SaveMode::MODEL)
+ {
+ this->dataPtr->modelLocationLineEdit->setText(QDir::homePath()+
+ "/model_editor_models/Untitled");
+ }
+
QPushButton *browseButton = new QPushButton(tr("Browse"));
connect(browseButton, SIGNAL(clicked()), this, SLOT(OnBrowse()));
+ QLabel *authorHeader = new QLabel;
+ authorHeader->setText(tr("<b>Author</b>"));
+ QLabel *modelAuthorName = new QLabel;
+ modelAuthorName->setText(tr(" Name:"));
+ this->dataPtr->modelAuthorNameLineEdit = new QLineEdit;
+ QLabel *modelAuthorEmail = new QLabel;
+ modelAuthorEmail->setText(tr(" Email:"));
+ this->dataPtr->modelAuthorEmailLineEdit = new QLineEdit;
+
+ QLabel *modelVersion = new QLabel;
+ modelVersion->setText(tr(" Version:"));
+ this->dataPtr->modelVersionLineEdit = new QLineEdit;
+ this->dataPtr->modelVersionLineEdit->setText(tr("1.0"));
+
+ QLabel *modelDescription = new QLabel;
+ modelDescription->setText(tr(" Description:"));
+ this->dataPtr->modelDescriptionLineEdit = new QLineEdit;
+
+// TODO Propshop integration
+/* QString contributeText(
+ tr("Contribute this model to the Model Database so that\n"
+ "the entire Gazebo community can benefit!\n"
+ "[This will open up a new tab in your browser]\n"));
+ QCheckBox *contributeCheckBox = new QCheckBox(contributeText);*/
+
QHBoxLayout *buttonsLayout = new QHBoxLayout;
QPushButton *cancelButton = new QPushButton(tr("&Cancel"));
connect(cancelButton, SIGNAL(clicked()), this, SLOT(OnCancel()));
- QPushButton *saveButton = new QPushButton("&Save");
+ std::string saveButtonText = "&Save";
+
+ QPushButton *saveButton = new QPushButton(tr(saveButtonText.c_str()));
saveButton->setDefault(true);
- connect(saveButton, SIGNAL(clicked()), this, SLOT(OnSave()));
+ connect(saveButton, SIGNAL(clicked()), this, SLOT(OnAcceptSave()));
buttonsLayout->addWidget(cancelButton);
buttonsLayout->addWidget(saveButton);
buttonsLayout->setAlignment(Qt::AlignRight);
- QGridLayout *gridLayout = new QGridLayout;
- gridLayout->addWidget(nameLabel, 0, 0);
- gridLayout->addWidget(nameLineEdit, 0, 1);
- gridLayout->addWidget(saveLocation, 1, 0);
- gridLayout->addWidget(this->locationLineEdit, 1, 1);
- gridLayout->addWidget(browseButton, 1, 2);
+ QHBoxLayout *modelNameLayout = new QHBoxLayout;
+ modelNameLayout->addWidget(modelLabel);
+ modelNameLayout->addWidget(this->dataPtr->modelNameLineEdit);
+
+ QHBoxLayout *locationLayout = new QHBoxLayout;
+ locationLayout->addWidget(modelLocation);
+ locationLayout->addWidget(this->dataPtr->modelLocationLineEdit);
+ locationLayout->addWidget(browseButton);
+
+ QRadioButton *advancedOptionsCollapser = new QRadioButton();
+ advancedOptionsCollapser->setChecked(false);
+ advancedOptionsCollapser->setText("Advanced Options");
+ advancedOptionsCollapser->setStyleSheet(
+ "QRadioButton {\
+ color: #d0d0d0;\
+ }\
+ QRadioButton::indicator::unchecked {\
+ image: url(:/images/right_arrow.png);\
+ }\
+ QRadioButton::indicator::checked {\
+ image: url(:/images/down_arrow.png);\
+ }");
+ // initialize as "closed" (unchecked)
+ // Button behavior: when "open", show advancedOptionsGrid
+ connect(advancedOptionsCollapser, SIGNAL(toggled(bool)), this,
+ SLOT(ToggleAdvancedOptions(bool)));
+
+ QHBoxLayout *advancedOptions = new QHBoxLayout();
+ advancedOptions->addWidget(advancedOptionsCollapser);
+
+ // Advanced options
+ QGridLayout *advancedOptionsGrid = new QGridLayout();
+
+ advancedOptionsGrid->addWidget(modelHeader, 1, 0);
+ advancedOptionsGrid->addWidget(modelVersion, 2, 0);
+ advancedOptionsGrid->addWidget(this->dataPtr->modelVersionLineEdit, 2, 1);
+ advancedOptionsGrid->addWidget(modelDescription, 3, 0);
+ advancedOptionsGrid->addWidget(this->dataPtr->modelDescriptionLineEdit, 3, 1);
+
+ advancedOptionsGrid->addWidget(authorHeader, 4, 0);
+ advancedOptionsGrid->addWidget(modelAuthorName, 5, 0);
+ advancedOptionsGrid->addWidget(this->dataPtr->modelAuthorNameLineEdit, 5, 1);
+ advancedOptionsGrid->addWidget(modelAuthorEmail, 6, 0);
+ advancedOptionsGrid->addWidget(this->dataPtr->modelAuthorEmailLineEdit, 6, 1);
+
+ this->dataPtr->advancedOptionsWidget = new QWidget();
+ this->dataPtr->advancedOptionsWidget->setLayout(advancedOptionsGrid);
+ this->dataPtr->advancedOptionsWidget->hide();
QVBoxLayout *mainLayout = new QVBoxLayout;
- mainLayout->addWidget(this->messageLabel);
- mainLayout->addLayout(gridLayout);
+ mainLayout->addWidget(this->dataPtr->messageLabel);
+ mainLayout->addLayout(modelNameLayout);
+ mainLayout->addLayout(locationLayout);
+ mainLayout->addLayout(advancedOptions);
+ mainLayout->addWidget(this->dataPtr->advancedOptionsWidget);
mainLayout->addLayout(buttonsLayout);
+ mainLayout->setSizeConstraint(QLayout::SetFixedSize);
this->setLayout(mainLayout);
}
@@ -72,52 +177,78 @@ SaveDialog::SaveDialog(QWidget *_parent) : QDialog(_parent)
/////////////////////////////////////////////////
SaveDialog::~SaveDialog()
{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
}
/////////////////////////////////////////////////
-std::string SaveDialog::GetSaveName() const
+std::string SaveDialog::GetModelName() const
{
- return this->nameLineEdit->text().toStdString();
+ return this->dataPtr->modelNameLineEdit->text().toStdString();
}
/////////////////////////////////////////////////
std::string SaveDialog::GetSaveLocation() const
{
- return this->locationLineEdit->text().toStdString();
+ return this->dataPtr->modelLocationLineEdit->text().toStdString();
}
/////////////////////////////////////////////////
-void SaveDialog::SetSaveName(const std::string &_name)
+std::string SaveDialog::GetAuthorName() const
{
- this->nameLineEdit->setText(tr(_name.c_str()));
+ return this->dataPtr->modelAuthorNameLineEdit->text().toStdString();
}
/////////////////////////////////////////////////
-void SaveDialog::SetSaveLocation(const std::string &_location)
+std::string SaveDialog::GetAuthorEmail() const
+{
+ return this->dataPtr->modelAuthorEmailLineEdit->text().toStdString();
+}
+
+/////////////////////////////////////////////////
+std::string SaveDialog::GetDescription() const
+{
+ return this->dataPtr->modelDescriptionLineEdit->text().toStdString();
+}
+
+/////////////////////////////////////////////////
+std::string SaveDialog::GetVersion() const
{
- this->locationLineEdit->setText(tr(_location.c_str()));
+ return this->dataPtr->modelVersionLineEdit->text().toStdString();
}
/////////////////////////////////////////////////
-void SaveDialog::SetMessage(const std::string &_msg)
+void SaveDialog::SetModelName(const std::string &_name)
{
- this->messageLabel->setText(tr(_msg.c_str()));
+ this->dataPtr->modelNameLineEdit->setText(tr(_name.c_str()));
}
/////////////////////////////////////////////////
-void SaveDialog::SetTitle(const std::string &_title)
+void SaveDialog::SetSaveLocation(const std::string &_location)
{
- this->setWindowTitle(tr(_title.c_str()));
+ this->dataPtr->modelLocationLineEdit->setText(tr(_location.c_str()));
}
/////////////////////////////////////////////////
void SaveDialog::OnBrowse()
{
- QString dir = QFileDialog::getExistingDirectory(this, tr("Open Directory"),
- QDir::homePath(), QFileDialog::ShowDirsOnly
- | QFileDialog::DontResolveSymlinks);
- if (!dir.isEmpty())
- this->locationLineEdit->setText(dir);
+ QFileDialog fileDialog(this, tr("Open Directory"), QDir::homePath());
+ fileDialog.setFileMode(QFileDialog::Directory);
+ fileDialog.setOptions(QFileDialog::ShowDirsOnly
+ | QFileDialog::DontResolveSymlinks);
+
+ if (fileDialog.exec() == QDialog::Accepted)
+ {
+ QStringList selected = fileDialog.selectedFiles();
+ if (selected.empty())
+ return;
+
+ // Substitute everything up to the model folder
+ std::string folder = this->GetSaveLocation();
+ folder = folder.substr(folder.rfind("/")+1);
+
+ this->SetSaveLocation(selected[0].toStdString() + "/" + folder);
+ }
}
/////////////////////////////////////////////////
@@ -127,53 +258,291 @@ void SaveDialog::OnCancel()
}
/////////////////////////////////////////////////
-void SaveDialog::OnSave()
+void SaveDialog::OnAcceptSave()
{
- boost::filesystem::path savePath(this->GetSaveLocation());
- savePath /= this->GetSaveName() + "." + this->fileExtension;
+ this->accept();
+}
- try
+/////////////////////////////////////////////////
+bool SaveDialog::OnSaveAs()
+{
+ if (this->exec() == QDialog::Accepted)
{
- if (boost::filesystem::exists(savePath.string()))
+ if (this->GetModelName().size() == 0)
+ {
+ QMessageBox msgBox(QMessageBox::Warning, QString("Empty Name"),
+ QString("Please give your model a non-empty name."));
+
+ msgBox.exec();
+ return this->OnSaveAs();
+ }
+ if (this->GetSaveLocation().size() == 0)
+ {
+ QMessageBox msgBox(QMessageBox::Warning, QString("Empty Location"),
+ QString("Please give a path to where your model will be saved."));
+
+ msgBox.exec();
+ return this->OnSaveAs();
+ }
+
+ boost::filesystem::path path;
+ path = path / this->GetSaveLocation();
+ if (!boost::filesystem::exists(path))
+ {
+ if (!boost::filesystem::create_directories(path))
+ {
+ gzerr << "Couldn't create folder for model files." << std::endl;
+ return false;
+ }
+ gzmsg << "Created folder " << path << " for model files." << std::endl;
+ }
+
+ boost::filesystem::path modelConfigPath = path / "model.config";
+
+ boost::filesystem::path sdfPath = path / "model.sdf";
+
+ // Before writing
+ if (boost::filesystem::exists(sdfPath) ||
+ boost::filesystem::exists(modelConfigPath))
{
- std::string msg = "A file named " + savePath.string() +
- " already exists.\nDo you wish to overwrite the existing file?";
- int ret = QMessageBox::warning(0, QString("File Exists"),
- QString(msg.c_str()), QMessageBox::Save | QMessageBox::Cancel,
- QMessageBox::Cancel);
+ std::string msg = "A model already exists in folder \n" +
+ path.string() + ".\n\n" +
+ "Do you wish to overwrite the existing model files?\n";
+
+ QMessageBox msgBox(QMessageBox::Warning, QString("Files Exist"),
+ QString(msg.c_str()));
- switch (ret)
+ QPushButton *cancelButton =
+ msgBox.addButton("Cancel", QMessageBox::RejectRole);
+ QPushButton *saveButton = msgBox.addButton("Save",
+ QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(saveButton);
+ msgBox.setEscapeButton(cancelButton);
+ msgBox.exec();
+ if (msgBox.clickedButton() != saveButton)
{
- case QMessageBox::Save:
- this->accept();
- break;
- case QMessageBox::Cancel:
- // Do nothing
- break;
- default:
- break;
+ return this->OnSaveAs();
}
}
+
+ this->AddDirToModelPaths(this->GetSaveLocation());
+ return true;
+ }
+ return false;
+}
+
+/////////////////////////////////////////////////
+void SaveDialog::ToggleAdvancedOptions(bool _checked)
+{
+ if (_checked)
+ {
+ this->dataPtr->advancedOptionsWidget->show();
+ }
+ else
+ {
+ this->dataPtr->advancedOptionsWidget->hide();
+ }
+}
+
+/////////////////////////////////////////////////
+void SaveDialog::AddDirToModelPaths(const std::string &_path)
+{
+ std::string parentDirectory = boost::filesystem::path(_path)
+ .parent_path().string();
+
+ std::list<std::string> modelPaths =
+ gazebo::common::SystemPaths::Instance()->GetModelPaths();
+ std::list<std::string>::iterator iter;
+ for (iter = modelPaths.begin();
+ iter != modelPaths.end(); ++iter)
+ {
+ if (iter->compare(parentDirectory) == 0)
+ {
+ break;
+ }
+ }
+
+ gazebo::common::SystemPaths::Instance()->
+ AddModelPathsUpdate(parentDirectory);
+
+ std::string additionalProperties =
+ gui::getINIProperty<std::string>("model_paths.filenames", "");
+ if (additionalProperties.find(parentDirectory) == std::string::npos)
+ {
+ // Append it to gui.ini
+ if (additionalProperties.empty())
+ additionalProperties = parentDirectory;
else
+ additionalProperties = additionalProperties + ":" + parentDirectory;
+
+ gui::setINIProperty("model_paths.filenames", additionalProperties);
+
+ // Save any changes that were made to the property tree
+ // TODO: check gui.ini env variable
+ char *home = getenv("HOME");
+ if (home)
{
- this->accept();
+ boost::filesystem::path guiINIPath = home;
+ guiINIPath = guiINIPath / ".gazebo" / "gui.ini";
+ saveINI(guiINIPath);
}
}
- catch(const boost::filesystem::filesystem_error &ex)
+}
+
+/////////////////////////////////////////////////
+std::string SaveDialog::GetTemplateConfigString()
+{
+ std::ostringstream newModelStr;
+ newModelStr << "<?xml version=\"1.0\"?>"
+ << "<model>"
+ << "<name>template_model</name>"
+ << "<version>1.0</version>"
+ << "<sdf version=\"" << SDF_VERSION << "\">model.sdf</sdf>"
+ << "<author>"
+ << "<name>author_name</name>"
+ << "<email>author_email</email>"
+ << "</author>"
+ << "<description>Made with Gazebo</description>"
+ << "</model>";
+ return newModelStr.str();
+}
+
+/////////////////////////////////////////////////
+void SaveDialog::GenerateConfig()
+{
+ // Create an xml config file
+ this->dataPtr->modelConfig.Clear();
+ this->dataPtr->modelConfig.Parse(this->GetTemplateConfigString().c_str());
+
+ TiXmlElement *modelXML = this->dataPtr
+ ->modelConfig.FirstChildElement("model");
+ if (!modelXML)
+ {
+ gzerr << "No model name in default config file" << std::endl;
+ return;
+ }
+ TiXmlElement *modelNameXML = modelXML->FirstChildElement("name");
+ modelNameXML->FirstChild()->SetValue(this->GetModelName());
+
+ TiXmlElement *versionXML = modelXML->FirstChildElement("version");
+ if (!versionXML)
+ {
+ gzerr << "Couldn't find model version" << std::endl;
+ versionXML->FirstChild()->SetValue("1.0");
+ }
+ else
+ {
+ versionXML->FirstChild()->SetValue(this->GetVersion());
+ }
+
+ TiXmlElement *descriptionXML = modelXML->FirstChildElement("description");
+ if (!descriptionXML)
+ {
+ gzerr << "Couldn't find model description" << std::endl;
+ descriptionXML->FirstChild()->SetValue("");
+ }
+ else
+ {
+ descriptionXML->FirstChild()->SetValue(this->GetDescription());
+ }
+
+ // TODO: Multiple authors
+ TiXmlElement *authorXML = modelXML->FirstChildElement("author");
+ if (!authorXML)
{
- gzerr << ex.what() << std::endl;
+ gzerr << "Couldn't find model author" << std::endl;
}
+ else
+ {
+ TiXmlElement *authorChild = authorXML->FirstChildElement("name");
+ if (!authorChild)
+ {
+ gzerr << "Couldn't find author name" << std::endl;
+ authorChild->FirstChild()->SetValue("");
+ }
+ else
+ {
+ authorChild->FirstChild()->SetValue(this->GetAuthorName());
+ }
+ authorChild = authorXML->FirstChildElement("email");
+ if (!authorChild)
+ {
+ gzerr << "Couldn't find author email" << std::endl;
+ authorChild->FirstChild()->SetValue("");
+ }
+ else
+ {
+ authorChild->FirstChild()->SetValue(this->GetAuthorEmail());
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void SaveDialog::SaveToConfig()
+{
+ boost::filesystem::path path(this->GetSaveLocation());
+ path = path / "model.config";
+ const char* modelConfigString = path.string().c_str();
+
+ this->dataPtr->modelConfig.SaveFile(modelConfigString);
+ gzdbg << "Saved file to " << modelConfigString << std::endl;
}
+/////////////////////////////////////////////////
+void SaveDialog::SaveToSDF(sdf::SDFPtr _modelSDF)
+{
+ std::ofstream savefile;
+ boost::filesystem::path path(this->GetSaveLocation());
+ path = path / "model.sdf";
+
+ // FIXME
+ savefile.open(path.string().c_str());
+ if (!savefile.is_open())
+ {
+ gzerr << "Couldn't open file for writing: " << path.string() << std::endl;
+ return;
+ }
+ savefile << _modelSDF->ToString();
+ savefile.close();
+ gzdbg << "Saved file to " << path.string() << std::endl;
+ this->AddDirToModelPaths(this->GetSaveLocation());
+}
/////////////////////////////////////////////////
-void SaveDialog::SetFileExtension(const std::string &_extension)
+std::string SaveDialog::GetFolderNameFromModelName(const std::string
+ &_modelName)
{
- this->fileExtension = _extension;
+ // Auto-generate folder name based on model name
+ std::string foldername = _modelName;
+
+ std::vector<std::pair<std::string, std::string> > replacePairs;
+ replacePairs.push_back(std::pair<std::string, std::string>(" ", "_"));
+
+ for (unsigned int i = 0; i < replacePairs.size(); ++i)
+ {
+ std::string forbiddenChar = replacePairs[i].first;
+ std::string replaceChar = replacePairs[i].second;
+ size_t index = foldername.find(forbiddenChar);
+ while (index != std::string::npos)
+ {
+ foldername.replace(index, forbiddenChar.size(), replaceChar);
+ index = foldername.find(forbiddenChar);
+ }
+ }
+
+ return foldername;
}
/////////////////////////////////////////////////
-void SaveDialog::showEvent(QShowEvent */*_event*/)
+void SaveDialog::ModelNameChangedOnDialog(QString _modelName)
{
- this->nameLineEdit->selectAll();
+ std::string folderName = this->GetFolderNameFromModelName(
+ _modelName.toStdString());
+
+ // Use current path and change only last folder name
+ std::string path = this->GetSaveLocation();
+ path = path.substr(0, path.rfind("/")+1);
+ path += folderName;
+
+ this->SetSaveLocation(path);
}
diff --git a/gazebo/gui/SaveDialog.hh b/gazebo/gui/SaveDialog.hh
index ef7ddf1..4c45551 100644
--- a/gazebo/gui/SaveDialog.hh
+++ b/gazebo/gui/SaveDialog.hh
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2015 Open Source Robotics Foundation
+ * Copyright (C) 2013-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.
@@ -19,6 +19,7 @@
#define _SAVE_DIALOG_HH_
#include <string>
+#include <sdf/sdf.hh>
#include "gazebo/gui/qt.h"
#include "gazebo/util/system.hh"
@@ -26,6 +27,8 @@ namespace gazebo
{
namespace gui
{
+ class SaveDialogPrivate;
+
/// \addtogroup gazebo_gui
/// \{
@@ -35,43 +38,72 @@ namespace gazebo
{
Q_OBJECT
+ /// \enum SaveMode
+ /// \brief Unique identifiers for all dialog modes.
+ public: enum SaveMode {
+ /// \brief Save model
+ MODEL,
+ /// \brief Save building
+ BUILDING
+ };
+
/// \brief Constructor.
+ /// \param[in] _mode Mode of the dialog.
/// \param[in] _parent Parent QWidget.
- public: SaveDialog(QWidget *_parent = 0);
+ public: SaveDialog(int _mode = 0, QWidget *_parent = 0);
/// \brief Destructor.
public: ~SaveDialog();
- /// \brief Get name of file.
- /// \return The name of file.
- public: std::string GetSaveName() const;
+ /// \brief Get the model name.
+ /// \return The model name.
+ public: std::string GetModelName() const;
/// \brief Get the save location.
/// \return Path of the save location.
public: std::string GetSaveLocation() const;
- /// \brief Set the name to save as.
- /// \param[in] _name Name of file.
- public: void SetSaveName(const std::string &_name);
+ /// \brief Set the model name.
+ /// \param[in] _name Name to set the model to.
+ public: void SetModelName(const std::string &_name);
/// \brief Set the save location.
/// \param[in] _location Location to save to.
public: void SetSaveLocation(const std::string &_location);
- /// \brief Set the message to be displayed.
- /// \param[in] _msg Message to be displayed.
- public: void SetMessage(const std::string &_msg);
+ /// \brief Get the model's author's name.
+ /// \return The author's name.
+ public: std::string GetAuthorName() const;
+
+ /// \brief Get the model's author's email.
+ /// \return The author's email.
+ public: std::string GetAuthorEmail() const;
+
+ /// \brief Get the model's description.
+ /// \return The model's description.
+ public: std::string GetDescription() const;
- /// \brief Set the tile of the dialog.
- /// \param[in] _title Title of dialog.
- public: void SetTitle(const std::string &_title);
+ /// \brief Get the model's version.
+ /// \return The model's version.
+ public: std::string GetVersion() const;
- /// \brief Set the file extension.
- /// \param[in] _extension File extension.
- public: void SetFileExtension(const std::string &_extension);
+ /// \brief Add the parent folder of _path to the model path represented
+ /// by SystemPaths, notify InsertModelWidget to display the model name in
+ /// the "Insert Models" tab, and write the parent folder filename to
+ /// gui.ini
+ /// \param[in] _path Path to be added.
+ public: void AddDirToModelPaths(const std::string &_path);
- /// \brief Qt event emitted showing the dialog
- protected: virtual void showEvent(QShowEvent *_event);
+ /// \brief Helper function to generate a valid folder name from a
+ /// human-readable model name.
+ /// \param[in] _modelName Human-readable model name.
+ /// \return Folder name.
+ public: std::string GetFolderNameFromModelName(const std::string
+ &_modelName);
+
+ /// \brief Call to execute the dialog.
+ /// \return True if the user accepted the dialog.
+ public: bool OnSaveAs();
/// \brief Qt callback when the file directory browse button is pressed.
private slots: void OnBrowse();
@@ -80,19 +112,32 @@ namespace gazebo
private slots: void OnCancel();
/// \brief Qt callback when the Save button is pressed.
- private slots: void OnSave();
+ private slots: void OnAcceptSave();
+
+ /// \brief Qt callback when the model name line edit changes.
+ /// \param[in] _modelName New name.
+ private slots: void ModelNameChangedOnDialog(QString _modelName);
+
+ /// \brief Get a template config file for a simple model.
+ private: std::string GetTemplateConfigString();
+
+ /// \brief Generate the config file.
+ public: void GenerateConfig();
- /// \brief Editable line that holds the name.
- private: QLineEdit *nameLineEdit;
+ /// \brief Save config file.
+ public: void SaveToConfig();
- /// \brief Editable line that holds the save location.
- private: QLineEdit *locationLineEdit;
+ /// \brief Save model to SDF format.
+ /// \param[in] _modelSDF Pointer to the model SDF.
+ public: void SaveToSDF(sdf::SDFPtr _modelSDF);
- /// \brief Message displayed in the dialog.
- private: QLabel *messageLabel;
+ /// \brief Qt callback to show/hide advanced model saving options.
+ /// \param[in] _checked Whether it is checked or not.
+ private slots: void ToggleAdvancedOptions(bool _checked);
- /// \brief File extension.
- private: std::string fileExtension;
+ /// \internal
+ /// \brief Pointer to private data.
+ private: SaveDialogPrivate *dataPtr;
};
/// \}
}
diff --git a/gazebo/gui/SaveDialogPrivate.hh b/gazebo/gui/SaveDialogPrivate.hh
new file mode 100644
index 0000000..39d1d9f
--- /dev/null
+++ b/gazebo/gui/SaveDialogPrivate.hh
@@ -0,0 +1,61 @@
+/*
+ * 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 _SAVE_DIALOG_PRIVATE_HH_
+#define _SAVE_DIALOG_PRIVATE_HH_
+
+#include <sdf/sdf.hh>
+
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \class SaveDialogPrivate SaveDialogPrivate.hh
+ /// \brief Private data for the SaveDialog class
+ class SaveDialogPrivate
+ {
+ /// \brief Widget container to hold advanced model saving options.
+ public: QWidget *advancedOptionsWidget;
+
+ /// \brief Label appearing at the top of the dialog box.
+ public: QLabel *messageLabel;
+
+ /// \brief Editable line that holds the model name.
+ public: QLineEdit* modelNameLineEdit;
+
+ /// \brief Editable line that holds the model's version.
+ public: QLineEdit* modelVersionLineEdit;
+
+ /// \brief Editable line that holds the model's description.
+ public: QLineEdit* modelDescriptionLineEdit;
+
+ /// \brief Editable line that holds the model's author's name.
+ public: QLineEdit* modelAuthorNameLineEdit;
+
+ /// \brief Editable line that holds the model's author's email.
+ public: QLineEdit* modelAuthorEmailLineEdit;
+
+ /// \brief Editable line that holds the model's save location.
+ public: QLineEdit* modelLocationLineEdit;
+
+ /// \brief The model's config file.
+ public: TiXmlDocument modelConfig;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/SaveDialog_TEST.cc b/gazebo/gui/SaveDialog_TEST.cc
new file mode 100644
index 0000000..36dd73c
--- /dev/null
+++ b/gazebo/gui/SaveDialog_TEST.cc
@@ -0,0 +1,86 @@
+/*
+ * 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/gui/SaveDialog.hh"
+#include "gazebo/gui/SaveDialog_TEST.hh"
+
+#include "test_config.h"
+
+/////////////////////////////////////////////////
+void SaveDialogTestHelper::CheckFileDialog()
+{
+ QVERIFY(this->dialog);
+
+ QFileDialog *fileDialog = this->dialog->findChild<QFileDialog *>();
+ QVERIFY(fileDialog);
+
+ // set default path to home dir.
+ fileDialog->setDirectory(QDir::homePath());
+
+ // hit enter to close dialog
+ QTest::keyClick(fileDialog, Qt::Key_Enter);
+
+ QVERIFY(!fileDialog->isVisible());
+}
+
+/////////////////////////////////////////////////
+void SaveDialog_TEST::SaveLocation()
+{
+ gazebo::gui::SaveDialog *saveDialog = new gazebo::gui::SaveDialog;
+ QCoreApplication::processEvents();
+
+ // Set the model name
+ std::string modelName = "model name";
+ saveDialog->SetModelName(modelName);
+
+ // Get folder name from model name
+ std::string folderName = saveDialog->GetFolderNameFromModelName(modelName);
+ const std::string expectedFolderName("model_name");
+ std::cout << "folderName: " << folderName << std::endl;
+ std::cout << "expectedFolderName: " << expectedFolderName << std::endl;
+ QVERIFY(folderName == expectedFolderName);
+
+ // find the browse button
+ QList<QPushButton *> pushButtons = saveDialog->findChildren<QPushButton *>();
+ QVERIFY(!pushButtons.empty());
+ QPushButton *browseButton = NULL;
+ for (int i = 0; i < pushButtons.size(); ++i)
+ {
+ QPushButton *button = pushButtons[i];
+ QVERIFY(button);
+ if (button->text().toLower().toStdString() == "browse")
+ browseButton = button;
+ }
+ QVERIFY(browseButton);
+
+ // set a path in the browse file dialog and verify value
+ SaveDialogTestHelper helper;
+ helper.dialog = saveDialog;
+ QTimer::singleShot(0, &helper, SLOT(CheckFileDialog()));
+ browseButton->click();
+ const std::string actualSaveLocation(saveDialog->GetSaveLocation());
+ const std::string expectedSaveLocation(
+ QDir::homePath().toStdString() + "/" + folderName);
+ std::cout << "actualSaveLocation: " << actualSaveLocation << std::endl;
+ std::cout << "expectedSaveLocation: " << expectedSaveLocation << std::endl;
+ QVERIFY(actualSaveLocation == expectedSaveLocation);
+
+ delete saveDialog;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(SaveDialog_TEST)
diff --git a/gazebo/gui/SaveDialog_TEST.hh b/gazebo/gui/SaveDialog_TEST.hh
new file mode 100644
index 0000000..2e50c9c
--- /dev/null
+++ b/gazebo/gui/SaveDialog_TEST.hh
@@ -0,0 +1,56 @@
+/*
+ * 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 _SAVEDIALOG_TEST_HH_
+#define _SAVEDIALOG_TEST_HH_
+
+#include <string>
+#include "gazebo/gui/QTestFixture.hh"
+
+
+/// \brief A test class for the SaveDialog.
+class SaveDialog_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Test setting save location
+ private slots: void SaveLocation();
+};
+
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class SaveDialog;
+ }
+}
+
+/// \brief A helper class for testing the SaveDialog.
+class SaveDialogTestHelper : public QObject
+{
+ Q_OBJECT
+
+ /// \brief Pointer to a SaveDiag
+ public: gazebo::gui::SaveDialog *dialog;
+
+ /// \brief Verify file dialog is working.
+ private slots: void CheckFileDialog();
+};
+
+
+#endif
diff --git a/gazebo/gui/SpaceNav.cc b/gazebo/gui/SpaceNav.cc
index 22a1f07..380430f 100644
--- a/gazebo/gui/SpaceNav.cc
+++ b/gazebo/gui/SpaceNav.cc
@@ -15,6 +15,12 @@
*
*/
+#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 <gazebo/gazebo_config.h>
#ifdef HAVE_SPNAV
#include <spnav.h>
@@ -32,6 +38,7 @@ using namespace gui;
// spnav_open() when spnav daemon is not running.
int spnav_test_daemon(void)
{
+#ifndef _WIN32
int s;
struct sockaddr_un addr;
@@ -49,6 +56,7 @@ int spnav_test_daemon(void)
}
close(s);
+#endif
return 0;
}
@@ -70,6 +78,10 @@ bool SpaceNav::Load()
{
bool result = true;
+ // reset button settings
+ this->dataPtr->buttons[0] = 0;
+ this->dataPtr->buttons[1] = 0;
+
#ifdef HAVE_SPNAV
// Read deadband from [spacenav] in gui.ini
this->dataPtr->deadbandTrans.x = getINIProperty<double>(
@@ -129,6 +141,10 @@ void SpaceNav::Run()
{
msgs::Joystick joystickMsg;
+ // add button state
+ for (unsigned int i = 0; i < 2; ++i)
+ joystickMsg.add_buttons(this->dataPtr->buttons[i]);
+
// bool joyStale = false;
bool queueEmpty = false;
@@ -139,6 +155,13 @@ void SpaceNav::Run()
queueEmpty = true;
break;
+ case SPNAV_EVENT_BUTTON:
+ // update button press
+ this->dataPtr->buttons[sev.button.bnum] = sev.button.press;
+ joystickMsg.mutable_buttons()->Set(sev.button.bnum, sev.button.press);
+ this->dataPtr->joyPub->Publish(joystickMsg);
+ break;
+
case SPNAV_EVENT_MOTION:
joystickMsg.mutable_translation()->set_x(
this->Deadband(this->dataPtr->deadbandTrans.x,
@@ -163,12 +186,6 @@ void SpaceNav::Run()
this->dataPtr->joyPub->Publish(joystickMsg);
break;
- case SPNAV_EVENT_BUTTON:
- joystickMsg.add_buttons(sev.button.bnum == 0 ? sev.button.press : 0);
- joystickMsg.add_buttons(sev.button.bnum == 1 ? sev.button.press : 0);
- this->dataPtr->joyPub->Publish(joystickMsg);
- break;
-
default:
break;
}
diff --git a/gazebo/gui/SpaceNavPrivate.hh b/gazebo/gui/SpaceNavPrivate.hh
index daf44b6..19283e0 100644
--- a/gazebo/gui/SpaceNavPrivate.hh
+++ b/gazebo/gui/SpaceNavPrivate.hh
@@ -34,6 +34,8 @@ namespace gazebo
{
this->pollThread = NULL;
this->stop = false;
+ this->buttons[0] = 0;
+ this->buttons[1] = 0;
}
/// \brief Destructor
@@ -63,6 +65,9 @@ namespace gazebo
/// \brief Rotation values below which joystick values return zero.
public: math::Vector3 deadbandRot;
+
+ /// \brief button states
+ public: int buttons[2];
};
}
}
diff --git a/gazebo/gui/SphereMaker.cc b/gazebo/gui/SphereMaker.cc
index 370bb8b..c23a71e 100644
--- a/gazebo/gui/SphereMaker.cc
+++ b/gazebo/gui/SphereMaker.cc
@@ -14,6 +14,13 @@
* 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 <sstream>
#include "gazebo/msgs/msgs.hh"
@@ -48,7 +55,7 @@ SphereMaker::SphereMaker()
this->visualMsg->mutable_material()->mutable_script()->set_name(
"Gazebo/TurquoiseGlowOutline");
msgs::Set(this->visualMsg->mutable_pose()->mutable_orientation(),
- math::Quaternion());
+ ignition::math::Quaterniond());
}
/////////////////////////////////////////////////
@@ -94,7 +101,7 @@ void SphereMaker::OnMousePush(const common::MouseEvent &_event)
if (this->state == 0)
return;
- this->mousePushPos = _event.pressPos;
+ this->mousePushPos = _event.PressPos();
}
/////////////////////////////////////////////////
@@ -134,7 +141,7 @@ void SphereMaker::OnMouseDrag(const common::MouseEvent &_event)
p1 = this->GetSnappedPoint(p1);
if (!this->camera->GetWorldPointOnPlane(
- _event.pos.x, _event.pos.y, math::Plane(norm), p2))
+ _event.Pos().X(), _event.Pos().Y(), math::Plane(norm), p2))
{
gzerr << "Invalid mouse point\n";
return;
@@ -142,7 +149,7 @@ void SphereMaker::OnMouseDrag(const common::MouseEvent &_event)
p2 = this->GetSnappedPoint(p2);
- msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p1);
+ msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p1.Ign());
double scale = p1.Distance(p2);
math::Vector3 p(this->visualMsg->pose().position().x(),
@@ -151,7 +158,7 @@ void SphereMaker::OnMouseDrag(const common::MouseEvent &_event)
p.z = scale;
- msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p);
+ msgs::Set(this->visualMsg->mutable_pose()->mutable_position(), p.Ign());
this->visualMsg->mutable_geometry()->mutable_sphere()->set_radius(scale);
this->visPub->Publish(*this->visualMsg);
}
@@ -159,33 +166,19 @@ void SphereMaker::OnMouseDrag(const common::MouseEvent &_event)
/////////////////////////////////////////////////
std::string SphereMaker::GetSDFString()
{
- std::ostringstream newModelStr;
- newModelStr << "<sdf version ='" << SDF_VERSION << "'>"
- << "<model name='unit_sphere_" << counter << "'>"
- << " <pose>0 0 0.5 0 0 0</pose>"
- << " <link name='link'>"
- << " <inertial><mass>1.0</mass></inertial>"
- << " <collision name='collision'>"
- << " <geometry>"
- << " <sphere><radius>0.5</radius></sphere>"
- << " </geometry>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << " <sphere><radius>0.5</radius></sphere>"
- << " </geometry>"
- << " <material>"
- << " <script>"
- << " <uri>file://media/materials/scripts/gazebo.material</uri>"
- << " <name>Gazebo/Grey</name>"
- << " </script>"
- << " </material>"
- << " </visual>"
- << " </link>"
- << " </model>"
- << "</sdf>";
-
- return newModelStr.str();
+ msgs::Model model;
+ {
+ std::ostringstream modelName;
+ modelName << "unit_sphere_" << counter;
+ model.set_name(modelName.str());
+ }
+ msgs::Set(model.mutable_pose(), ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0));
+ msgs::AddSphereLink(model, 1.0, 0.5);
+ model.mutable_link(0)->set_name("link");
+
+ return "<sdf version='" + std::string(SDF_VERSION) + "'>"
+ + msgs::ModelToSDF(model)->ToString("")
+ + "</sdf>";
}
diff --git a/gazebo/gui/SplashScreen.cc b/gazebo/gui/SplashScreen.cc
new file mode 100644
index 0000000..ac1e1c4
--- /dev/null
+++ b/gazebo/gui/SplashScreen.cc
@@ -0,0 +1,95 @@
+/*
+ * 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/rendering/UserCamera.hh"
+#include "gazebo/rendering/Scene.hh"
+
+#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/SplashScreen.hh"
+#include "gazebo/gui/SplashScreenPrivate.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+SplashScreen::SplashScreen()
+ : dataPtr(new SplashScreenPrivate)
+{
+ this->setObjectName("SplashScreen");
+ QPixmap pixmap(":/images/splash.svg");
+
+ std::string versionText;
+ versionText = "Version " + std::string(GAZEBO_VERSION);
+
+ QTextEdit *versionEdit = new QTextEdit(tr(versionText.c_str()));
+ versionEdit->setAcceptRichText(true);
+ versionEdit->setContentsMargins(0, 0, 0, 0);
+ versionEdit->setFrameStyle(QFrame::NoFrame);
+ versionEdit->setAlignment(Qt::AlignBottom | Qt::AlignRight);
+ versionEdit->setObjectName("splashVersionTextEdit");
+ versionEdit->setFixedHeight(20);
+
+ QHBoxLayout *versionLayout = new QHBoxLayout();
+ versionLayout->addWidget(versionEdit);
+
+ QVBoxLayout *textLayout = new QVBoxLayout();
+ textLayout->addSpacerItem(new QSpacerItem(1, pixmap.size().height(),
+ QSizePolicy::Expanding, QSizePolicy::MinimumExpanding));
+ textLayout->addLayout(versionLayout);
+
+ this->dataPtr->splashScreen = new QSplashScreen(pixmap);
+ this->dataPtr->splashScreen->setObjectName("splashScreenWidget");
+ this->dataPtr->splashScreen->setMask(pixmap.mask());
+ this->dataPtr->splashScreen->setWindowFlags(
+ this->dataPtr->splashScreen->windowFlags() | Qt::WindowStaysOnTopHint);
+ this->dataPtr->splashScreen->setLayout(textLayout);
+ this->dataPtr->splashScreen->setFixedSize(pixmap.size());
+
+ this->dataPtr->splashScreen->show();
+ this->dataPtr->splashScreen->repaint();
+
+ QTimer::singleShot(2000, this, SLOT(Update()));
+}
+
+/////////////////////////////////////////////////
+SplashScreen::~SplashScreen()
+{
+ delete this->dataPtr->splashScreen;
+ this->dataPtr->splashScreen = NULL;
+
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+void SplashScreen::Update()
+{
+ rendering::UserCameraPtr cam = get_active_camera();
+ if (cam && cam->GetScene() && cam->GetScene()->GetInitialized())
+ {
+ this->dataPtr->splashScreen->hide();
+ return;
+ }
+
+ QTimer::singleShot(100, this, SLOT(Update()));
+}
+
+/////////////////////////////////////////////////
+bool SplashScreen::Visible() const
+{
+ return this->dataPtr->splashScreen->isVisible();
+}
diff --git a/gazebo/gui/SplashScreen.hh b/gazebo/gui/SplashScreen.hh
new file mode 100644
index 0000000..a001cef
--- /dev/null
+++ b/gazebo/gui/SplashScreen.hh
@@ -0,0 +1,56 @@
+/*
+ * 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_SPLASH_SCREEN_HH_
+#define _GAZEBO_SPLASH_SCREEN_HH_
+
+#include <string>
+
+#include "gazebo/gui/qt.h"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class SplashScreenPrivate;
+
+ /// \brief Splash screen that displays an icon and a message.
+ class GAZEBO_VISIBLE SplashScreen : public QObject
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ public: SplashScreen();
+
+ /// \brief Destructor
+ public: virtual ~SplashScreen();
+
+ /// \brief Returns whether the splash screen is visible.
+ /// \return True if the splash screen is visible, false otherwise.
+ public: bool Visible() const;
+
+ /// \brief Qt callback to update the splash screen
+ private slots: void Update();
+
+ /// \internal
+ /// \brief Pointer to private data.
+ private: SplashScreenPrivate *dataPtr;
+ };
+ }
+}
+
+#endif
diff --git a/gazebo/gui/SplashScreenPrivate.hh b/gazebo/gui/SplashScreenPrivate.hh
new file mode 100644
index 0000000..4927214
--- /dev/null
+++ b/gazebo/gui/SplashScreenPrivate.hh
@@ -0,0 +1,37 @@
+/*
+ * 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_SPLASH_SCREEN_PRIVATE_HH_
+#define _GAZEBO_SPLASH_SCREEN_PRIVATE_HH_
+
+#include <vector>
+
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \internal
+ /// \brief Private data for the SplashScreen class
+ class SplashScreenPrivate
+ {
+ /// \brief Pointer to the splash screen widget.
+ public: QSplashScreen *splashScreen;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/SplashScreen_TEST.cc b/gazebo/gui/SplashScreen_TEST.cc
new file mode 100644
index 0000000..e350f81
--- /dev/null
+++ b/gazebo/gui/SplashScreen_TEST.cc
@@ -0,0 +1,59 @@
+/*
+ * 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/gui/SplashScreen.hh"
+#include "gazebo/gui/MainWindow.hh"
+#include "gazebo/gui/SplashScreen_TEST.hh"
+
+#include "test_config.h"
+
+/////////////////////////////////////////////////
+void SplashScreen_TEST::Show()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/empty.world", false, false, false);
+
+ gazebo::gui::SplashScreen *splashObj = new gazebo::gui::SplashScreen();
+ QVERIFY(splashObj != NULL);
+
+ QVERIFY(splashObj->Visible());
+
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+
+ QVERIFY(mainWindow != NULL);
+ // Create the main window.
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process events.
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(210);
+ QCoreApplication::processEvents();
+ }
+
+ QVERIFY(!splashObj->Visible());
+
+ mainWindow->close();
+ delete mainWindow;
+ delete splashObj;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(SplashScreen_TEST)
diff --git a/gazebo/gui/SplashScreen_TEST.hh b/gazebo/gui/SplashScreen_TEST.hh
new file mode 100644
index 0000000..afced21
--- /dev/null
+++ b/gazebo/gui/SplashScreen_TEST.hh
@@ -0,0 +1,32 @@
+/*
+ * 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_SPLASHSCREEN_TEST_HH_
+#define _GAZEBO_SPLASHSCREEN_TEST_HH_
+
+#include "gazebo/gui/QTestFixture.hh"
+
+/// \brief A test class for the SplashScreen.
+class SplashScreen_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Test the splash screen shows and disappears
+ private slots: void Show();
+};
+
+#endif
diff --git a/gazebo/gui/TimePanel.cc b/gazebo/gui/TimePanel.cc
index 247fa33..aecd279 100644
--- a/gazebo/gui/TimePanel.cc
+++ b/gazebo/gui/TimePanel.cc
@@ -14,171 +14,81 @@
* 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 <sstream>
#include "gazebo/transport/Node.hh"
#include "gazebo/gui/Actions.hh"
#include "gazebo/gui/GuiEvents.hh"
+#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/rendering/UserCamera.hh"
+#include "gazebo/gui/TimeWidget.hh"
+#include "gazebo/gui/LogPlayWidget.hh"
#include "gazebo/gui/TimePanel.hh"
+#include "gazebo/gui/TimePanelPrivate.hh"
using namespace gazebo;
using namespace gui;
/////////////////////////////////////////////////
TimePanel::TimePanel(QWidget *_parent)
- : QWidget(_parent)
+ : QWidget(_parent), dataPtr(new TimePanelPrivate)
{
this->setObjectName("timePanel");
- QHBoxLayout *mainLayout = new QHBoxLayout;
+ // Time Widget
+ this->dataPtr->timeWidget = new TimeWidget(this);
+ this->dataPtr->timeWidget->setObjectName("timeWidget");
+ connect(this, SIGNAL(SetTimeWidgetVisible(bool)),
+ this->dataPtr->timeWidget, SLOT(setVisible(bool)));
- QScrollArea *scrollArea = new QScrollArea(this);
- scrollArea->setLineWidth(1);
- scrollArea->setFrameShape(QFrame::NoFrame);
- scrollArea->setFrameShadow(QFrame::Plain);
- scrollArea->setWidgetResizable(true);
- scrollArea->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum);
- scrollArea->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
-
- // Play control (Play/Step/Pause)
- QSpinBox *stepSpinBox = new QSpinBox;
- stepSpinBox->setRange(1, 9999);
- connect(stepSpinBox, SIGNAL(valueChanged(int)), this,
- SLOT(OnStepValueChanged(int)));
-
- QWidget *stepWidget = new QWidget;
- QLabel *stepLabel = new QLabel(tr("Steps:"));
- QVBoxLayout *stepLayout = new QVBoxLayout;
- stepLayout->addWidget(stepLabel);
- stepLayout->addWidget(stepSpinBox);
- stepWidget->setLayout(stepLayout);
-
- QLabel *stepToolBarLabel = new QLabel(tr("Steps:"));
-
- QMenu *stepMenu = new QMenu;
- this->stepButton = new QToolButton;
- this->stepButton->setMaximumSize(35, stepButton->height());
- QWidgetAction *stepAction = new QWidgetAction(stepMenu);
- stepAction->setDefaultWidget(stepWidget);
- stepMenu->addAction(stepAction);
- this->stepButton->setMenu(stepMenu);
- this->stepButton->setPopupMode(QToolButton::InstantPopup);
- this->stepButton->setToolButtonStyle(Qt::ToolButtonTextOnly);
- this->stepButton->setContentsMargins(0, 0, 0, 0);
- this->OnStepValueChanged(1);
-
- connect(stepSpinBox, SIGNAL(editingFinished()), stepMenu,
- SLOT(hide()));
-
- QFrame *frame = new QFrame(scrollArea);
- frame->setFrameShape(QFrame::NoFrame);
- scrollArea->setWidget(frame);
-
- QToolBar *playToolbar = new QToolBar;
- playToolbar->setObjectName("playToolBar");
- playToolbar->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);
- playToolbar->addAction(g_playAct);
- playToolbar->addAction(g_pauseAct);
-
- QLabel *emptyLabel = new QLabel(tr(" "));
- playToolbar->addWidget(emptyLabel);
- playToolbar->addAction(g_stepAct);
- playToolbar->addWidget(stepToolBarLabel);
- playToolbar->addWidget(this->stepButton);
-
- this->percentRealTimeEdit = new QLineEdit;
- this->percentRealTimeEdit->setObjectName("timePanelPercentRealTime");
- this->percentRealTimeEdit->setReadOnly(true);
- this->percentRealTimeEdit->setFixedWidth(90);
-
- this->simTimeEdit = new QLineEdit;
- this->simTimeEdit->setObjectName("timePanelSimTime");
- this->simTimeEdit->setReadOnly(true);
- this->simTimeEdit->setFixedWidth(110);
-
- this->realTimeEdit = new QLineEdit;
- this->realTimeEdit->setObjectName("timePanelRealTime");
- this->realTimeEdit->setReadOnly(true);
- this->realTimeEdit->setFixedWidth(110);
-
- this->iterationsEdit = new QLineEdit;
- this->iterationsEdit->setReadOnly(true);
- this->iterationsEdit->setFixedWidth(110);
-
- QPushButton *timeResetButton = new QPushButton("Reset");
- timeResetButton->setFocusPolicy(Qt::NoFocus);
- connect(timeResetButton, SIGNAL(clicked()),
- this, SLOT(OnTimeReset()));
-
- QHBoxLayout *frameLayout = new QHBoxLayout;
- frameLayout->setContentsMargins(0, 0, 0, 0);
- frameLayout->addItem(new QSpacerItem(5, -1, QSizePolicy::Expanding,
- QSizePolicy::Minimum));
- frameLayout->addWidget(playToolbar);
-
- frameLayout->addWidget(new QLabel(tr("Real Time Factor:")));
- frameLayout->addWidget(this->percentRealTimeEdit);
-
- frameLayout->addWidget(new QLabel(tr("Sim Time:")));
- frameLayout->addWidget(this->simTimeEdit);
-
- frameLayout->addWidget(new QLabel(tr("Real Time:")));
- frameLayout->addWidget(this->realTimeEdit);
-
- frameLayout->addWidget(new QLabel(tr("Iterations:")));
- frameLayout->addWidget(this->iterationsEdit);
-
- frameLayout->addWidget(timeResetButton);
-
- frameLayout->addItem(new QSpacerItem(15, -1, QSizePolicy::Expanding,
- QSizePolicy::Minimum));
-
- frame->setLayout(frameLayout);
- frame->layout()->setContentsMargins(0, 0, 0, 0);
-
- mainLayout->addWidget(scrollArea);
+ // LogPlay Widget
+ this->dataPtr->logPlayWidget = new LogPlayWidget(this);
+ this->dataPtr->logPlayWidget->setObjectName("logPlayWidget");
+ connect(this, SIGNAL(SetLogPlayWidgetVisible(bool)),
+ this->dataPtr->logPlayWidget, SLOT(setVisible(bool)));
+
+ // Layout
+ QHBoxLayout *mainLayout = new QHBoxLayout;
+ mainLayout->addWidget(this->dataPtr->timeWidget);
+ mainLayout->addWidget(this->dataPtr->logPlayWidget);
this->setLayout(mainLayout);
this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
this->layout()->setContentsMargins(0, 0, 0, 0);
- this->node = transport::NodePtr(new transport::Node());
- this->node->Init();
+ // Transport
+ this->dataPtr->node = transport::NodePtr(new transport::Node());
+ this->dataPtr->node->Init();
- this->statsSub =
- this->node->Subscribe("~/world_stats", &TimePanel::OnStats, this);
- this->worldControlPub =
- this->node->Advertise<msgs::WorldControl>("~/world_control");
+ this->dataPtr->statsSub = this->dataPtr->node->Subscribe(
+ "~/world_stats", &TimePanel::OnStats, this);
+ this->dataPtr->worldControlPub = this->dataPtr->node->
+ Advertise<msgs::WorldControl>("~/world_control");
+
+ // Timer
QTimer *timer = new QTimer(this);
connect(timer, SIGNAL(timeout()), this, SLOT(Update()));
timer->start(33);
- this->connections.push_back(
+ // Connections
+ this->dataPtr->connections.push_back(
gui::Events::ConnectFullScreen(
- boost::bind(&TimePanel::OnFullScreen, this, _1)));
-
- this->show();
-
- // Create a QueuedConnection to set iterations.
- // This is used for thread safety.
- connect(this, SIGNAL(SetIterations(QString)),
- this->iterationsEdit, SLOT(setText(QString)), Qt::QueuedConnection);
-
- // Create a QueuedConnection to set sim time.
- // This is used for thread safety.
- connect(this, SIGNAL(SetSimTime(QString)),
- this->simTimeEdit, SLOT(setText(QString)), Qt::QueuedConnection);
+ boost::bind(&TimePanel::OnFullScreen, this, _1)));
- // Create a QueuedConnection to set real time.
- // This is used for thread safety.
- connect(this, SIGNAL(SetRealTime(QString)),
- this->realTimeEdit, SLOT(setText(QString)), Qt::QueuedConnection);
+ connect(g_playAct, SIGNAL(changed()), this, SLOT(OnPlayActionChanged()));
}
/////////////////////////////////////////////////
-void TimePanel::OnFullScreen(bool & /*_value*/)
+void TimePanel::OnFullScreen(bool /*_value*/)
{
/*if (_value)
this->hide();
@@ -190,91 +100,180 @@ void TimePanel::OnFullScreen(bool & /*_value*/)
/////////////////////////////////////////////////
TimePanel::~TimePanel()
{
- this->node.reset();
+ this->dataPtr->node.reset();
+
+ delete this->dataPtr;
+ this->dataPtr = NULL;
}
/////////////////////////////////////////////////
-void TimePanel::OnStats(ConstWorldStatisticsPtr &_msg)
+void TimePanel::OnPlayActionChanged()
{
- boost::mutex::scoped_lock lock(this->mutex);
-
- this->simTimes.push_back(msgs::Convert(_msg->sim_time()));
- if (this->simTimes.size() > 20)
- this->simTimes.pop_front();
+ // Tests don't see external actions
+ if (!g_stepAct)
+ return;
- this->realTimes.push_back(msgs::Convert(_msg->real_time()));
- if (this->realTimes.size() > 20)
- this->realTimes.pop_front();
-
- if (_msg->paused() && (g_playAct && !g_playAct->isVisible()))
+ if (this->IsPaused())
{
- g_playAct->setVisible(true);
- g_pauseAct->setVisible(false);
+ g_stepAct->setToolTip("Step the world");
+ g_stepAct->setEnabled(true);
}
- else if (!_msg->paused() && (g_pauseAct && !g_pauseAct->isVisible()))
+ else
{
- g_pauseAct->setVisible(true);
- g_playAct->setVisible(false);
+ g_stepAct->setToolTip("Pause the world before stepping");
+ g_stepAct->setEnabled(false);
}
+}
- // Set simulation time
- this->SetSimTime(QString::fromStdString(FormatTime(_msg->sim_time())));
+/////////////////////////////////////////////////
+void TimePanel::ShowRealTimeFactor(bool _show)
+{
+ if (this->dataPtr->timeWidget->isVisible())
+ this->dataPtr->timeWidget->ShowRealTimeFactor(_show);
+ else
+ gzwarn << "Time widget not visible" << std::endl;
+}
- // Set real time
- this->SetRealTime(QString::fromStdString(FormatTime(_msg->real_time())));
+/////////////////////////////////////////////////
+void TimePanel::ShowRealTime(bool _show)
+{
+ if (this->dataPtr->timeWidget->isVisible())
+ this->dataPtr->timeWidget->ShowRealTime(_show);
+ else
+ gzwarn << "Time widget not visible" << std::endl;
+}
- // Set the iterations
- this->SetIterations(QString::fromStdString(
- boost::lexical_cast<std::string>(_msg->iterations())));
+/////////////////////////////////////////////////
+void TimePanel::ShowSimTime(bool _show)
+{
+ if (this->dataPtr->timeWidget->isVisible())
+ this->dataPtr->timeWidget->ShowSimTime(_show);
+ else
+ gzwarn << "Time widget not visible" << std::endl;
}
/////////////////////////////////////////////////
-std::string TimePanel::FormatTime(const msgs::Time &_msg)
+void TimePanel::ShowIterations(bool _show)
{
- std::ostringstream stream;
- unsigned int day, hour, min, sec, msec;
+ if (this->dataPtr->timeWidget->isVisible())
+ this->dataPtr->timeWidget->ShowIterations(_show);
+ else
+ gzwarn << "Time widget not visible" << std::endl;
+}
- stream.str("");
+/////////////////////////////////////////////////
+void TimePanel::ShowFPS(bool _show)
+{
+ if (this->dataPtr->timeWidget->isVisible())
+ this->dataPtr->timeWidget->ShowFPS(_show);
+ else
+ gzwarn << "Time widget not visible" << std::endl;
+}
- sec = _msg.sec();
+/////////////////////////////////////////////////
+void TimePanel::ShowStepWidget(bool _show)
+{
+ if (this->dataPtr->timeWidget->isVisible())
+ this->dataPtr->timeWidget->ShowStepWidget(_show);
+ else
+ gzwarn << "Time widget not visible" << std::endl;
+}
- day = sec / 86400;
- sec -= day * 86400;
+/////////////////////////////////////////////////
+bool TimePanel::IsPaused() const
+{
+ return this->dataPtr->paused;
+}
- hour = sec / 3600;
- sec -= hour * 3600;
+/////////////////////////////////////////////////
+void TimePanel::SetPaused(bool _paused)
+{
+ this->dataPtr->paused = _paused;
- min = sec / 60;
- sec -= min * 60;
+ if (this->dataPtr->timeWidget->isVisible())
+ this->dataPtr->timeWidget->SetPaused(_paused);
+ else if (this->dataPtr->logPlayWidget->isVisible())
+ this->dataPtr->logPlayWidget->SetPaused(_paused);
+}
- msec = rint(_msg.nsec() * 1e-6);
+/////////////////////////////////////////////////
+void TimePanel::OnStats(ConstWorldStatisticsPtr &_msg)
+{
+ boost::mutex::scoped_lock lock(this->dataPtr->mutex);
- stream << std::setw(2) << std::setfill('0') << day << " ";
- stream << std::setw(2) << std::setfill('0') << hour << ":";
- stream << std::setw(2) << std::setfill('0') << min << ":";
- stream << std::setw(2) << std::setfill('0') << sec << ".";
- stream << std::setw(3) << std::setfill('0') << msec;
+ this->dataPtr->simTimes.push_back(msgs::Convert(_msg->sim_time()));
+ if (this->dataPtr->simTimes.size() > 20)
+ this->dataPtr->simTimes.pop_front();
- return stream.str();
-}
+ this->dataPtr->realTimes.push_back(msgs::Convert(_msg->real_time()));
+ if (this->dataPtr->realTimes.size() > 20)
+ this->dataPtr->realTimes.pop_front();
+
+ if (_msg->has_log_playback_stats())
+ {
+ this->SetTimeWidgetVisible(false);
+ this->SetLogPlayWidgetVisible(true);
+ }
+ else
+ {
+ this->SetTimeWidgetVisible(true);
+ this->SetLogPlayWidgetVisible(false);
+ }
+
+ if (_msg->has_paused())
+ this->SetPaused(_msg->paused());
+
+ if (this->dataPtr->timeWidget->isVisible())
+ {
+ // Set simulation time
+ this->dataPtr->timeWidget->EmitSetSimTime(QString::fromStdString(
+ msgs::Convert(_msg->sim_time()).FormattedString()));
+
+ // Set real time
+ this->dataPtr->timeWidget->EmitSetRealTime(QString::fromStdString(
+ msgs::Convert(_msg->real_time()).FormattedString()));
+
+ // Set the iterations
+ this->dataPtr->timeWidget->EmitSetIterations(QString::fromStdString(
+ boost::lexical_cast<std::string>(_msg->iterations())));
+ }
+ else if (this->dataPtr->logPlayWidget->isVisible())
+ {
+ // Set current time
+ this->dataPtr->logPlayWidget->EmitSetCurrentTime(
+ msgs::Convert(_msg->sim_time()));
+ // Set start time in text and in ms
+ this->dataPtr->logPlayWidget->EmitSetStartTime(
+ msgs::Convert(_msg->log_playback_stats().start_time()));
+
+ // Set end time in text and in ms
+ this->dataPtr->logPlayWidget->EmitSetEndTime(
+ msgs::Convert(_msg->log_playback_stats().end_time()));
+ }
+}
/////////////////////////////////////////////////
void TimePanel::Update()
{
- boost::mutex::scoped_lock lock(this->mutex);
+ boost::mutex::scoped_lock lock(this->dataPtr->mutex);
+
+ // Avoid apparent race condition on start, seen on Windows.
+ if (!this->dataPtr->simTimes.size() || !this->dataPtr->realTimes.size())
+ return;
std::ostringstream percent;
common::Time simAvg, realAvg;
std::list<common::Time>::iterator simIter, realIter;
- simIter = ++(this->simTimes.begin());
- realIter = ++(this->realTimes.begin());
- while (simIter != this->simTimes.end() && realIter != this->realTimes.end())
+ simIter = ++(this->dataPtr->simTimes.begin());
+ realIter = ++(this->dataPtr->realTimes.begin());
+ while (simIter != this->dataPtr->simTimes.end() &&
+ realIter != this->dataPtr->realTimes.end())
{
- simAvg += ((*simIter) - this->simTimes.front());
- realAvg += ((*realIter) - this->realTimes.front());
+ simAvg += ((*simIter) - this->dataPtr->simTimes.front());
+ realAvg += ((*realIter) - this->dataPtr->realTimes.front());
++simIter;
++realIter;
}
@@ -288,7 +287,22 @@ void TimePanel::Update()
else
percent << "0";
- this->percentRealTimeEdit->setText(tr(percent.str().c_str()));
+ if (this->dataPtr->timeWidget->isVisible())
+ this->dataPtr->timeWidget->SetPercentRealTimeEdit(percent.str().c_str());
+
+ rendering::UserCameraPtr cam = gui::get_active_camera();
+ if (cam)
+ {
+ std::ostringstream avgFPS;
+ avgFPS << cam->GetAvgFPS();
+
+ if (this->dataPtr->timeWidget->isVisible())
+ {
+ // Set the avg fps
+ this->dataPtr->timeWidget->EmitSetFPS(QString::fromStdString(
+ boost::lexical_cast<std::string>(avgFPS.str().c_str())));
+ }
+ }
}
/////////////////////////////////////////////////
@@ -297,19 +311,11 @@ void TimePanel::OnTimeReset()
msgs::WorldControl msg;
msg.mutable_reset()->set_all(false);
msg.mutable_reset()->set_time_only(true);
- this->worldControlPub->Publish(msg);
+ this->dataPtr->worldControlPub->Publish(msg);
}
/////////////////////////////////////////////////
void TimePanel::OnStepValueChanged(int _value)
{
- // text formating and resizing for better presentation
- std::string numStr = QString::number(_value).toStdString();
- QFont stepFont = this->stepButton->font();
- stepFont.setPointSizeF(11 - numStr.size()/2.0);
- this->stepButton->setFont(stepFont);
- numStr.insert(numStr.end(), 4 - numStr.size(), ' ');
- this->stepButton->setText(tr(numStr.c_str()));
-
emit gui::Events::inputStepSize(_value);
}
diff --git a/gazebo/gui/TimePanel.hh b/gazebo/gui/TimePanel.hh
index 42e8ed8..7cff99b 100644
--- a/gazebo/gui/TimePanel.hh
+++ b/gazebo/gui/TimePanel.hh
@@ -22,6 +22,8 @@
#include <string>
#include "gazebo/gui/qt.h"
+#include "gazebo/gui/TimeWidget.hh"
+#include "gazebo/gui/LogPlayWidget.hh"
#include "gazebo/transport/TransportTypes.hh"
#include "gazebo/msgs/MessageTypes.hh"
#include "gazebo/common/Event.hh"
@@ -35,85 +37,87 @@ namespace gazebo
{
namespace gui
{
+ class TimePanelPrivate;
+ class TimeWidget;
+ class LogPlayWidget;
+
class GAZEBO_VISIBLE TimePanel : public QWidget
{
Q_OBJECT
/// \brief Constructor
+ /// \param[in] _parent Parent widget.
public: TimePanel(QWidget *_parent = 0);
/// \brief Destructor
public: virtual ~TimePanel();
- /// \brief A signal used to set the sim time line edit.
- /// \param[in] _string String representation of sim time.
- signals: void SetSimTime(QString _string);
+ /// \brief Show real time factor.
+ /// \param[in] _show True to display real time factor.
+ public: void ShowRealTimeFactor(bool _show);
- /// \brief A signal used to set the real time line edit.
- /// \param[in] _string String representation of real time.
- signals: void SetRealTime(QString _string);
+ /// \brief Show real time
+ /// \param[in] _show True to display real time.
+ public: void ShowRealTime(bool _show);
- /// \brief A signal used to set the iterations line edit.
- /// \param[in] _string String representation of iterations.
- signals: void SetIterations(QString _string);
+ /// \brief Show sim time
+ /// \param[in] _show True to display sim time.
+ public: void ShowSimTime(bool _show);
- /// \brief Update the data output.
- private slots: void Update();
+ /// \brief Show the iterations.
+ /// \param[in] _show True to show the iterations widget.
+ public: void ShowIterations(bool _show);
- /// \brief Qt call back when the step value in the spinbox changed
- private slots: void OnStepValueChanged(int _value);
+ /// \brief Show the step widget.
+ /// \param[in] _show True to show the step widget.
+ public: void ShowStepWidget(bool _show);
- /// \brief Called when the GUI enters/leaves full-screen mode.
- /// \param[in] _value True when entering full screen, false when
- /// leaving.
- private: void OnFullScreen(bool &_value);
+ /// \brief Show fps.
+ /// \param[in] _show True to show the fps widget.
+ public: void ShowFPS(bool _show);
- /// \brief Called when a world stats message is received.
- /// \param[in] _msg World statistics message.
- private: void OnStats(ConstWorldStatisticsPtr &_msg);
-
- /// \brief Helper function to format time string.
- /// \param[in] _msg Time message.
- private: static std::string FormatTime(const msgs::Time &_msg);
-
- /// \brief QT callback when the reset time button is pressed.
- private slots: void OnTimeReset();
+ /// \brief Returns if the simulation is displayed as paused.
+ /// \return True if paused, false otherwise.
+ public: bool IsPaused() const;
- /// \brief Display the real time percentage.
- private: QLineEdit *percentRealTimeEdit;
+ /// \brief Set whether to display the simulation as paused.
+ /// \param[in] _paused True to display the simulation as paused. False
+ /// indicates the simulation is running
+ public: void SetPaused(bool _paused);
- /// \brief Display the simulation time.
- private: QLineEdit *simTimeEdit;
-
- /// \brief Display the real time.
- private: QLineEdit *realTimeEdit;
-
- /// \brief Display the number of iterations.
- private: QLineEdit *iterationsEdit;
+ /// \brief Qt call back when the step value in the spinbox changed
+ /// \param[in] _value New step value.
+ public slots: void OnStepValueChanged(int _value);
- /// \brief Node used for communication.
- private: transport::NodePtr node;
+ /// \brief QT callback when the reset time button is pressed.
+ public slots: void OnTimeReset();
- /// \brief Subscriber to the statistics topic.
- private: transport::SubscriberPtr statsSub;
+ /// \brief QT signal to set visibility of time widget.
+ /// \param[in] _visible True to make visible.
+ signals: void SetTimeWidgetVisible(bool _visible);
- /// \brief Used to start, stop, and step simulation.
- private: transport::PublisherPtr worldControlPub;
+ /// \brief QT signal to set visibility of log play widget.
+ /// \param[in] _visible True to make visible.
+ signals: void SetLogPlayWidgetVisible(bool _visible);
- /// \brief Event based connections.
- private: std::vector<event::ConnectionPtr> connections;
+ /// \brief Update the data output.
+ private slots: void Update();
- /// \brief List of simulation times used to compute averages.
- private: std::list<common::Time> simTimes;
+ /// \brief Qt call back when the play action state changes
+ private slots: void OnPlayActionChanged();
- /// \brief List of real times used to compute averages.
- private: std::list<common::Time> realTimes;
+ /// \brief Called when the GUI enters/leaves full-screen mode.
+ /// \param[in] _value True when entering full screen, false when
+ /// leaving.
+ private: void OnFullScreen(bool _value);
- /// \brief Mutex to protect the memeber variables.
- private: boost::mutex mutex;
+ /// \brief Called when a world stats message is received.
+ /// \param[in] _msg World statistics message.
+ private: void OnStats(ConstWorldStatisticsPtr &_msg);
- /// \brief Tool button that holds the step widget
- private: QToolButton *stepButton;
+ /// \internal
+ /// \brief Pointer to private data.
+ private: TimePanelPrivate *dataPtr;
};
}
}
diff --git a/gazebo/gui/TimePanelPrivate.hh b/gazebo/gui/TimePanelPrivate.hh
new file mode 100644
index 0000000..238f158
--- /dev/null
+++ b/gazebo/gui/TimePanelPrivate.hh
@@ -0,0 +1,65 @@
+/*
+ * 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_TIME_PANEL_PRIVATE_HH_
+#define _GAZEBO_TIME_PANEL_PRIVATE_HH_
+
+#include <list>
+#include <vector>
+
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \internal
+ /// \brief Private data for the TimePanel class
+ class TimePanelPrivate
+ {
+ /// \brief Node used for communication.
+ public: transport::NodePtr node;
+
+ /// \brief Subscriber to the statistics topic.
+ public: transport::SubscriberPtr statsSub;
+
+ /// \brief Used to start, stop, and step simulation.
+ public: transport::PublisherPtr worldControlPub;
+
+ /// \brief Event based connections.
+ public: std::vector<event::ConnectionPtr> connections;
+
+ /// \brief List of simulation times used to compute averages.
+ public: std::list<common::Time> simTimes;
+
+ /// \brief List of real times used to compute averages.
+ public: std::list<common::Time> realTimes;
+
+ /// \brief Mutex to protect the memeber variables.
+ public: boost::mutex mutex;
+
+ /// \brief Paused state of the simulation.
+ public: bool paused;
+
+ /// \brief Paused state of the simulation.
+ public: TimeWidget *timeWidget;
+
+ /// \brief Paused state of the simulation.
+ public: LogPlayWidget *logPlayWidget;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/TimePanel_TEST.cc b/gazebo/gui/TimePanel_TEST.cc
index 7b028ed..f303ac4 100644
--- a/gazebo/gui/TimePanel_TEST.cc
+++ b/gazebo/gui/TimePanel_TEST.cc
@@ -19,56 +19,24 @@
#include "gazebo/gui/TimePanel_TEST.hh"
/////////////////////////////////////////////////
-void TimePanel_TEST::ValidTimes()
+void TimePanel_TEST::SetPaused()
{
- QBENCHMARK
- {
- this->Load("empty.world");
+ this->Load("empty.world");
- // Create a new data logger widget
- gazebo::gui::TimePanel *timePanel = new gazebo::gui::TimePanel;
+ // Create a new time panel widget
+ gazebo::gui::TimePanel *timePanel = new gazebo::gui::TimePanel;
+ QVERIFY(timePanel != NULL);
- // Get the percent real time line
- QLineEdit *percentEdit = timePanel->findChild<QLineEdit*>(
- "timePanelPercentRealTime");
+ // verify initial state
+ QVERIFY(!timePanel->IsPaused());
- // Get the sim time line
- QLineEdit *simTimeEdit = timePanel->findChild<QLineEdit*>(
- "timePanelSimTime");
+ // set paused state and verify
+ timePanel->SetPaused(true);
+ QVERIFY(timePanel->IsPaused());
- // Get the real time line
- QLineEdit *realTimeEdit = timePanel->findChild<QLineEdit*>(
- "timePanelRealTime");
-
- QVERIFY(percentEdit != NULL);
- QVERIFY(simTimeEdit != NULL);
- QVERIFY(realTimeEdit != NULL);
-
- // Wait a little bit so that time increases.
- for (unsigned int i = 0; i < 10; ++i)
- {
- gazebo::common::Time::MSleep(100);
- QCoreApplication::processEvents();
- }
-
- 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 the percent real time is greater than zero
- txt = percentEdit->text().toStdString();
- value = boost::lexical_cast<double>(txt.substr(0, txt.find(" ")));
- QVERIFY(value > 0.0);
- }
+ timePanel->SetPaused(false);
+ QVERIFY(!timePanel->IsPaused());
+ delete timePanel;
}
// Generate a main function for the test
diff --git a/gazebo/gui/TimePanel_TEST.hh b/gazebo/gui/TimePanel_TEST.hh
index 5be0fb9..af1f8cd 100644
--- a/gazebo/gui/TimePanel_TEST.hh
+++ b/gazebo/gui/TimePanel_TEST.hh
@@ -20,13 +20,13 @@
#include "gazebo/gui/QTestFixture.hh"
-/// \brief A test class for the DataLogger widget.
+/// \brief A test class for the TimePanel widget.
class TimePanel_TEST : public QTestFixture
{
Q_OBJECT
- /// \brief Test that the TimePanel gets valid times from the server.
- private slots: void ValidTimes();
+ /// \brief Test setting the play/pause state.
+ private slots: void SetPaused();
};
#endif
diff --git a/gazebo/gui/TimeWidget.cc b/gazebo/gui/TimeWidget.cc
new file mode 100644
index 0000000..3c12f42
--- /dev/null
+++ b/gazebo/gui/TimeWidget.cc
@@ -0,0 +1,308 @@
+/*
+ * 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/gui/Actions.hh"
+#include "gazebo/gui/TimeWidget.hh"
+#include "gazebo/gui/TimeWidgetPrivate.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+TimeWidget::TimeWidget(QWidget *_parent)
+ : QWidget(_parent), dataPtr(new TimeWidgetPrivate)
+{
+ this->setObjectName("timeWidget");
+
+ this->dataPtr->timePanel = dynamic_cast<TimePanel *>(_parent);
+
+ QHBoxLayout *mainLayout = new QHBoxLayout;
+
+ QScrollArea *scrollArea = new QScrollArea(this);
+ scrollArea->setLineWidth(1);
+ scrollArea->setFrameShape(QFrame::NoFrame);
+ scrollArea->setFrameShadow(QFrame::Plain);
+ scrollArea->setWidgetResizable(true);
+ scrollArea->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum);
+ scrollArea->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
+
+ // Play control (Play/Step/Pause)
+ QSpinBox *stepSpinBox = new QSpinBox;
+ stepSpinBox->setRange(1, 9999);
+ connect(stepSpinBox, SIGNAL(valueChanged(int)), this,
+ SLOT(OnStepValueChanged(int)));
+
+ QWidget *stepWidget = new QWidget;
+ QLabel *stepLabel = new QLabel(tr("Steps:"));
+ QVBoxLayout *stepLayout = new QVBoxLayout;
+ stepLayout->addWidget(stepLabel);
+ stepLayout->addWidget(stepSpinBox);
+ stepWidget->setLayout(stepLayout);
+
+ QLabel *stepToolBarLabel = new QLabel(tr("Steps:"));
+
+ QMenu *stepMenu = new QMenu;
+ this->dataPtr->stepButton = new QToolButton;
+ this->dataPtr->stepButton->setMaximumSize(35,
+ this->dataPtr->stepButton->height());
+ QWidgetAction *stepAction = new QWidgetAction(stepMenu);
+ stepAction->setDefaultWidget(stepWidget);
+ stepMenu->addAction(stepAction);
+ this->dataPtr->stepButton->setMenu(stepMenu);
+ this->dataPtr->stepButton->setPopupMode(QToolButton::InstantPopup);
+ this->dataPtr->stepButton->setToolButtonStyle(Qt::ToolButtonTextOnly);
+ this->dataPtr->stepButton->setContentsMargins(0, 0, 0, 0);
+ this->OnStepValueChanged(1);
+
+ connect(stepSpinBox, SIGNAL(editingFinished()), stepMenu,
+ SLOT(hide()));
+
+ QFrame *frame = new QFrame(scrollArea);
+ frame->setFrameShape(QFrame::NoFrame);
+ scrollArea->setWidget(frame);
+
+ QToolBar *playToolbar = new QToolBar;
+ playToolbar->setObjectName("playToolBar");
+ playToolbar->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);
+ if (g_playAct)
+ playToolbar->addAction(g_playAct);
+ if (g_pauseAct)
+ playToolbar->addAction(g_pauseAct);
+ this->dataPtr->paused = false;
+
+ QLabel *emptyLabel = new QLabel(tr(" "));
+ playToolbar->addWidget(emptyLabel);
+
+ if (g_stepAct)
+ {
+ playToolbar->addAction(g_stepAct);
+ g_stepAct->setEnabled(this->dataPtr->paused);
+ }
+ this->dataPtr->stepToolBarLabelAction =
+ playToolbar->addWidget(stepToolBarLabel);
+ this->dataPtr->stepButtonAction =
+ playToolbar->addWidget(this->dataPtr->stepButton);
+ this->dataPtr->stepButtonAction->setObjectName("timeWidgetStepAction");
+
+ this->dataPtr->percentRealTimeEdit = new QLineEdit;
+ this->dataPtr->percentRealTimeEdit->setObjectName(
+ "timeWidgetPercentRealTime");
+ this->dataPtr->percentRealTimeEdit->setReadOnly(true);
+ this->dataPtr->percentRealTimeEdit->setFixedWidth(90);
+
+ this->dataPtr->simTimeEdit = new QLineEdit;
+ this->dataPtr->simTimeEdit->setObjectName("timeWidgetSimTime");
+ this->dataPtr->simTimeEdit->setReadOnly(true);
+ this->dataPtr->simTimeEdit->setFixedWidth(110);
+
+ this->dataPtr->realTimeEdit = new QLineEdit;
+ this->dataPtr->realTimeEdit->setObjectName("timeWidgetRealTime");
+ this->dataPtr->realTimeEdit->setReadOnly(true);
+ this->dataPtr->realTimeEdit->setFixedWidth(110);
+
+ this->dataPtr->iterationsEdit = new QLineEdit;
+ this->dataPtr->iterationsEdit->setReadOnly(true);
+ this->dataPtr->iterationsEdit->setFixedWidth(110);
+ this->dataPtr->iterationsEdit->setObjectName("timeWidgetIterations");
+
+ this->dataPtr->fpsEdit = new QLineEdit;
+ this->dataPtr->fpsEdit->setReadOnly(true);
+ this->dataPtr->fpsEdit->setFixedWidth(90);
+ this->dataPtr->fpsEdit->setObjectName("timeWidgetFPS");
+
+ QPushButton *timeResetButton = new QPushButton("Reset");
+ timeResetButton->setFocusPolicy(Qt::NoFocus);
+ connect(timeResetButton, SIGNAL(clicked()), this, SLOT(OnTimeReset()));
+
+ QHBoxLayout *frameLayout = new QHBoxLayout;
+ frameLayout->setContentsMargins(0, 0, 0, 0);
+ frameLayout->addItem(new QSpacerItem(5, -1, QSizePolicy::Expanding,
+ QSizePolicy::Minimum));
+ frameLayout->addWidget(playToolbar);
+
+ this->dataPtr->realTimeFactorLabel = new QLabel(tr("Real Time Factor:"));
+ frameLayout->addWidget(this->dataPtr->realTimeFactorLabel);
+ frameLayout->addWidget(this->dataPtr->percentRealTimeEdit);
+
+ this->dataPtr->simTimeLabel = new QLabel(tr("Sim Time:"));
+ frameLayout->addWidget(this->dataPtr->simTimeLabel);
+ frameLayout->addWidget(this->dataPtr->simTimeEdit);
+
+ this->dataPtr->realTimeLabel = new QLabel(tr("Real Time:"));
+ frameLayout->addWidget(this->dataPtr->realTimeLabel);
+ frameLayout->addWidget(this->dataPtr->realTimeEdit);
+
+ this->dataPtr->iterationsLabel = new QLabel(tr("Iterations:"));
+ frameLayout->addWidget(this->dataPtr->iterationsLabel);
+ frameLayout->addWidget(this->dataPtr->iterationsEdit);
+
+ this->dataPtr->fpsLabel = new QLabel(tr("FPS:"));
+ frameLayout->addWidget(this->dataPtr->fpsLabel);
+ frameLayout->addWidget(this->dataPtr->fpsEdit);
+
+ frameLayout->addWidget(timeResetButton);
+
+ frameLayout->addItem(new QSpacerItem(15, -1, QSizePolicy::Expanding,
+ QSizePolicy::Minimum));
+
+ frame->setLayout(frameLayout);
+ frame->layout()->setContentsMargins(0, 0, 0, 0);
+
+ mainLayout->addWidget(scrollArea);
+ this->setLayout(mainLayout);
+
+ this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
+ this->layout()->setContentsMargins(0, 0, 0, 0);
+
+ // Create a QueuedConnection to set avg fps.
+ // This is used for thread safety.
+ connect(this, SIGNAL(SetFPS(QString)), this->dataPtr->fpsEdit,
+ SLOT(setText(QString)), Qt::QueuedConnection);
+
+ // Create a QueuedConnection to set iterations.
+ // This is used for thread safety.
+ connect(this, SIGNAL(SetIterations(QString)), this->dataPtr->iterationsEdit,
+ SLOT(setText(QString)), Qt::QueuedConnection);
+
+ // Create a QueuedConnection to set sim time.
+ // This is used for thread safety.
+ connect(this, SIGNAL(SetSimTime(QString)), this->dataPtr->simTimeEdit,
+ SLOT(setText(QString)), Qt::QueuedConnection);
+
+ // Create a QueuedConnection to set real time.
+ // This is used for thread safety.
+ connect(this, SIGNAL(SetRealTime(QString)), this->dataPtr->realTimeEdit,
+ SLOT(setText(QString)), Qt::QueuedConnection);
+}
+
+/////////////////////////////////////////////////
+TimeWidget::~TimeWidget()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::ShowRealTimeFactor(bool _show)
+{
+ this->dataPtr->realTimeFactorLabel->setVisible(_show);
+ this->dataPtr->percentRealTimeEdit->setVisible(_show);
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::ShowRealTime(bool _show)
+{
+ this->dataPtr->realTimeLabel->setVisible(_show);
+ this->dataPtr->realTimeEdit->setVisible(_show);
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::ShowSimTime(bool _show)
+{
+ this->dataPtr->simTimeLabel->setVisible(_show);
+ this->dataPtr->simTimeEdit->setVisible(_show);
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::ShowIterations(bool _show)
+{
+ this->dataPtr->iterationsLabel->setVisible(_show);
+ this->dataPtr->iterationsEdit->setVisible(_show);
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::ShowFPS(bool _show)
+{
+ this->dataPtr->fpsLabel->setVisible(_show);
+ this->dataPtr->fpsEdit->setVisible(_show);
+}
+
+/////////////////////////////////////////////////
+bool TimeWidget::IsPaused() const
+{
+ return this->dataPtr->paused;
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::SetPaused(bool _paused)
+{
+ this->dataPtr->paused = _paused;
+
+ if (g_pauseAct)
+ g_pauseAct->setVisible(!_paused);
+ if (g_playAct)
+ g_playAct->setVisible(_paused);
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::ShowStepWidget(bool _show)
+{
+ if (g_stepAct)
+ g_stepAct->setVisible(_show);
+ this->dataPtr->stepToolBarLabelAction->setVisible(_show);
+ this->dataPtr->stepButtonAction->setVisible(_show);
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::OnTimeReset()
+{
+ this->dataPtr->timePanel->OnTimeReset();
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::OnStepValueChanged(int _value)
+{
+ // text formating and resizing for better presentation
+ std::string numStr = QString::number(_value).toStdString();
+ QFont stepFont = this->dataPtr->stepButton->font();
+ stepFont.setPointSizeF(11 - numStr.size()/2.0);
+ this->dataPtr->stepButton->setFont(stepFont);
+ numStr.insert(numStr.end(), 4 - numStr.size(), ' ');
+ this->dataPtr->stepButton->setText(tr(numStr.c_str()));
+
+ this->dataPtr->timePanel->OnStepValueChanged(_value);
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::EmitSetSimTime(QString _time)
+{
+ this->SetSimTime(_time);
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::EmitSetRealTime(QString _time)
+{
+ this->SetRealTime(_time);
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::EmitSetIterations(QString _time)
+{
+ this->SetIterations(_time);
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::EmitSetFPS(QString _time)
+{
+ this->SetFPS(_time);
+}
+
+/////////////////////////////////////////////////
+void TimeWidget::SetPercentRealTimeEdit(QString _text)
+{
+ this->dataPtr->percentRealTimeEdit->setText(_text);
+}
diff --git a/gazebo/gui/TimeWidget.hh b/gazebo/gui/TimeWidget.hh
new file mode 100644
index 0000000..a7d26d7
--- /dev/null
+++ b/gazebo/gui/TimeWidget.hh
@@ -0,0 +1,125 @@
+/*
+ * 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_TIME_WIDGET_HH_
+#define _GAZEBO_TIME_WIDGET_HH_
+
+#include "gazebo/gui/qt.h"
+#include "gazebo/gui/TimePanel.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class TimeWidgetPrivate;
+ class TimePanel;
+
+ class GAZEBO_VISIBLE TimeWidget : public QWidget
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ /// \param[in] _parent Parent widget, commonly a TimePanel.
+ public: TimeWidget(QWidget *_parent = 0);
+
+ /// \brief Destructor
+ public: virtual ~TimeWidget();
+
+ /// \brief Show real time factor.
+ /// \param[in] _show True to display real time factor.
+ public: void ShowRealTimeFactor(bool _show);
+
+ /// \brief Show real time.
+ /// \param[in] _show True to display real time.
+ public: void ShowRealTime(bool _show);
+
+ /// \brief Show sim time.
+ /// \param[in] _show True to display sim time.
+ public: void ShowSimTime(bool _show);
+
+ /// \brief Show the iterations.
+ /// \param[in] _show True to show the iterations widget.
+ public: void ShowIterations(bool _show);
+
+ /// \brief Show the step widget.
+ /// \param[in] _show True to show the step widget.
+ public: void ShowStepWidget(bool _show);
+
+ /// \brief Show fps.
+ /// \param[in] _show True to show the fps widget.
+ public: void ShowFPS(bool _show);
+
+ /// \brief Returns if the simulation is displayed as paused.
+ /// \return True if paused, false otherwise.
+ public: bool IsPaused() const;
+
+ /// \brief Set whether to display the simulation as paused.
+ /// \param[in] _paused True to display the simulation as paused. False
+ /// indicates the simulation is running
+ public: void SetPaused(bool _paused);
+
+ /// \brief Emit a signal used to set the sim time line edit.
+ /// \param[in] _string String representation of sim time.
+ public: void EmitSetSimTime(QString _string);
+
+ /// \brief Emit a signal used to set the real time line edit.
+ /// \param[in] _string String representation of real time.
+ public: void EmitSetRealTime(QString _string);
+
+ /// \brief Emit a signal used to set the iterations line edit.
+ /// \param[in] _string String representation of iterations.
+ public: void EmitSetIterations(QString _string);
+
+ /// \brief Emit a signal used to set the FPS line edit.
+ /// \param[in] _string String representation of average FPS.
+ public: void EmitSetFPS(QString _string);
+
+ /// \brief A signal used to set the sim time line edit.
+ /// \param[in] _string String representation of real time factor.
+ public: void SetPercentRealTimeEdit(QString _text);
+
+ /// \brief Qt call back when the step value in the spinbox changed
+ /// \param[in] _value New step value.
+ public slots: void OnStepValueChanged(int _value);
+
+ /// \brief QT callback when the reset time button is pressed.
+ public slots: void OnTimeReset();
+
+ /// \brief A signal used to set the sim time line edit.
+ /// \param[in] _string String representation of sim time.
+ signals: void SetSimTime(QString _string);
+
+ /// \brief A signal used to set the real time line edit.
+ /// \param[in] _string String representation of real time.
+ signals: void SetRealTime(QString _string);
+
+ /// \brief A signal used to set the iterations line edit.
+ /// \param[in] _string String representation of iterations.
+ signals: void SetIterations(QString _string);
+
+ /// \brief A signal used to set the avg fps line edit.
+ /// \param[in] _string String representation of avg fps.
+ signals: void SetFPS(QString _string);
+
+ /// \internal
+ /// \brief Pointer to private data.
+ private: TimeWidgetPrivate *dataPtr;
+ };
+ }
+}
+
+#endif
diff --git a/gazebo/gui/TimeWidgetPrivate.hh b/gazebo/gui/TimeWidgetPrivate.hh
new file mode 100644
index 0000000..f856256
--- /dev/null
+++ b/gazebo/gui/TimeWidgetPrivate.hh
@@ -0,0 +1,82 @@
+/*
+ * 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_TIME_WIDGET_PRIVATE_HH_
+#define _GAZEBO_TIME_WIDGET_PRIVATE_HH_
+
+#include <vector>
+
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \internal
+ /// \brief Private data for the TimeWidget class
+ class TimeWidgetPrivate
+ {
+ /// \brief Sim time label.
+ public: QLabel *simTimeLabel;
+
+ /// \brief Display the simulation time.
+ public: QLineEdit *simTimeEdit;
+
+ /// \brief Real time label.
+ public: QLabel *realTimeLabel;
+
+ /// \brief Display the real time.
+ public: QLineEdit *realTimeEdit;
+
+ /// \brief Real time factor label.
+ public: QLabel *realTimeFactorLabel;
+
+ /// \brief Display the real time percentage.
+ public: QLineEdit *percentRealTimeEdit;
+
+ /// \brief Iterations label.
+ public: QLabel *iterationsLabel;
+
+ /// \brief Display the number of iterations.
+ public: QLineEdit *iterationsEdit;
+
+ /// \brief FPS label.
+ public: QLabel *fpsLabel;
+
+ /// \brief Display the average frames per second.
+ public: QLineEdit *fpsEdit;
+
+ /// \brief Action associated with the step label in the toolbar.
+ public: QAction *stepToolBarLabelAction;
+
+ /// \brief Tool button that holds the step widget
+ public: QToolButton *stepButton;
+
+ /// \brief Action associated with the step button in the toolbar.
+ public: QAction *stepButtonAction;
+
+ /// \brief Paused state of the simulation.
+ public: bool paused;
+
+ /// \brief Event based connections.
+ public: std::vector<event::ConnectionPtr> connections;
+
+ /// \brief Paused state of the simulation.
+ public: TimePanel *timePanel;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/TimeWidget_TEST.cc b/gazebo/gui/TimeWidget_TEST.cc
new file mode 100644
index 0000000..5ff4b9f
--- /dev/null
+++ b/gazebo/gui/TimeWidget_TEST.cc
@@ -0,0 +1,186 @@
+/*
+ * Copyright (C) 2012-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/gui/TimeWidget.hh"
+#include "gazebo/gui/MainWindow.hh"
+#include "gazebo/gui/RenderWidget.hh"
+#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/rendering/UserCamera.hh"
+#include "gazebo/gui/TimeWidget_TEST.hh"
+
+/////////////////////////////////////////////////
+void TimeWidget_TEST::ValidTimes()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ QBENCHMARK
+ {
+ 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();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ // 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 percent real time line
+ QLineEdit *percentEdit = timeWidget->findChild<QLineEdit *>(
+ "timeWidgetPercentRealTime");
+ QVERIFY(percentEdit != NULL);
+
+ // Get the sim time line
+ QLineEdit *simTimeEdit = timeWidget->findChild<QLineEdit *>(
+ "timeWidgetSimTime");
+ QVERIFY(simTimeEdit != NULL);
+
+ // Get the real time line
+ QLineEdit *realTimeEdit = timeWidget->findChild<QLineEdit *>(
+ "timeWidgetRealTime");
+ QVERIFY(realTimeEdit != NULL);
+
+ // Get the fps line
+ QLineEdit *fpsEdit = timeWidget->findChild<QLineEdit *>("timeWidgetFPS");
+ QVERIFY(fpsEdit != NULL);
+
+ // Wait a little bit so that time increases.
+ for (unsigned int i = 0; i < 10000; ++i)
+ {
+ gazebo::common::Time::NSleep(500000);
+ QCoreApplication::processEvents();
+ }
+
+ 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 the percent real time is greater than zero
+ txt = percentEdit->text().toStdString();
+ value = boost::lexical_cast<double>(txt.substr(0, txt.find(" ")));
+ QVERIFY(value > 0.0);
+
+ // Make sure the fps is somewhere close to 60 fps
+ txt = fpsEdit->text().toStdString();
+ value = boost::lexical_cast<double>(txt.substr(0, txt.find(" ")));
+ QVERIFY(value > 45.0);
+ QVERIFY(value < 75.0);
+
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+ }
+}
+
+/////////////////////////////////////////////////
+void TimeWidget_TEST::Visibility()
+{
+ this->Load("empty.world");
+
+ // Create a new time widget
+ gazebo::gui::TimeWidget *timeWidget = new gazebo::gui::TimeWidget;
+ timeWidget->setVisible(true);
+ QVERIFY(timeWidget->isVisible());
+
+ // Get the percent real time line
+ QLineEdit *percentEdit = timeWidget->findChild<QLineEdit *>(
+ "timeWidgetPercentRealTime");
+
+ // Get the sim time line
+ QLineEdit *simTimeEdit = timeWidget->findChild<QLineEdit *>(
+ "timeWidgetSimTime");
+
+ // Get the real time line
+ QLineEdit *realTimeEdit = timeWidget->findChild<QLineEdit *>(
+ "timeWidgetRealTime");
+
+ // Get the step button
+ QAction *stepButton = timeWidget->findChild<QAction *>(
+ "timeWidgetStepAction");
+
+ QLineEdit *iterationsEdit = timeWidget->findChild<QLineEdit *>(
+ "timeWidgetIterations");
+
+ // visible by default
+ QVERIFY(percentEdit->isVisible());
+ QVERIFY(simTimeEdit->isVisible());
+ QVERIFY(realTimeEdit->isVisible());
+ QVERIFY(stepButton->isVisible());
+ QVERIFY(iterationsEdit->isVisible());
+
+ // hide the widgets
+ timeWidget->ShowRealTimeFactor(false);
+ timeWidget->ShowSimTime(false);
+ timeWidget->ShowRealTime(false);
+ timeWidget->ShowStepWidget(false);
+ timeWidget->ShowIterations(false);
+
+ QVERIFY(!percentEdit->isVisible());
+ QVERIFY(!simTimeEdit->isVisible());
+ QVERIFY(!realTimeEdit->isVisible());
+ QVERIFY(!stepButton->isVisible());
+ QVERIFY(!iterationsEdit->isVisible());
+
+ // show the widgets again
+ timeWidget->ShowRealTimeFactor(true);
+ timeWidget->ShowSimTime(true);
+ timeWidget->ShowRealTime(true);
+ timeWidget->ShowStepWidget(true);
+ timeWidget->ShowIterations(true);
+
+ QVERIFY(percentEdit->isVisible());
+ QVERIFY(simTimeEdit->isVisible());
+ QVERIFY(realTimeEdit->isVisible());
+ QVERIFY(stepButton->isVisible());
+ QVERIFY(iterationsEdit->isVisible());
+
+ delete timeWidget;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(TimeWidget_TEST)
diff --git a/gazebo/gui/TimeWidget_TEST.hh b/gazebo/gui/TimeWidget_TEST.hh
new file mode 100644
index 0000000..5daf76b
--- /dev/null
+++ b/gazebo/gui/TimeWidget_TEST.hh
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2012-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 _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();
+};
+
+#endif
diff --git a/gazebo/gui/ToolsWidget.cc b/gazebo/gui/ToolsWidget.cc
index 0382af5..672b222 100644
--- a/gazebo/gui/ToolsWidget.cc
+++ b/gazebo/gui/ToolsWidget.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/common/Events.hh"
#include "gazebo/gui/JointControlWidget.hh"
#include "gazebo/gui/ToolsWidget.hh"
diff --git a/gazebo/gui/TopicSelector.cc b/gazebo/gui/TopicSelector.cc
index b34078e..950557b 100644
--- a/gazebo/gui/TopicSelector.cc
+++ b/gazebo/gui/TopicSelector.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/transport/transport.hh"
#include "gazebo/gui/TopicSelector.hh"
diff --git a/gazebo/gui/ViewAngleWidget.cc b/gazebo/gui/ViewAngleWidget.cc
new file mode 100644
index 0000000..1084c10
--- /dev/null
+++ b/gazebo/gui/ViewAngleWidget.cc
@@ -0,0 +1,262 @@
+/*
+ * 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/rendering/UserCamera.hh"
+#include "gazebo/rendering/Visual.hh"
+
+#include "gazebo/gui/Actions.hh"
+#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/GLWidget.hh"
+#include "gazebo/gui/MainWindow.hh"
+#include "gazebo/gui/ViewAngleWidgetPrivate.hh"
+#include "gazebo/gui/ViewAngleWidget.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+ViewAngleWidget::ViewAngleWidget(QWidget *_parent)
+ : QWidget(_parent), dataPtr(new ViewAngleWidgetPrivate)
+{
+ // Keep pointer to main window
+ this->dataPtr->mainWindow = qobject_cast<MainWindow *>(_parent);
+
+ // Lateral buttons
+ this->dataPtr->topButton = new QToolButton(this);
+ this->dataPtr->bottomButton = new QToolButton(this);
+ this->dataPtr->frontButton = new QToolButton(this);
+ this->dataPtr->backButton = new QToolButton(this);
+ this->dataPtr->leftButton = new QToolButton(this);
+ this->dataPtr->rightButton = new QToolButton(this);
+
+ // Reset / home button
+ this->dataPtr->resetButton = new QToolButton(this);
+
+ // Button size
+ QSize iconSize(30, 30);
+ this->dataPtr->topButton->setIconSize(iconSize);
+ this->dataPtr->bottomButton->setIconSize(iconSize);
+ this->dataPtr->frontButton->setIconSize(iconSize);
+ this->dataPtr->backButton->setIconSize(iconSize);
+ this->dataPtr->leftButton->setIconSize(iconSize);
+ this->dataPtr->rightButton->setIconSize(iconSize);
+ this->dataPtr->resetButton->setIconSize(iconSize);
+
+ // Dropdown
+ this->dataPtr->projectionComboBox = new QComboBox(this);
+ this->dataPtr->projectionComboBox->setMinimumWidth(150);
+ this->dataPtr->projectionComboBox->addItem("Perspective", 0);
+ this->dataPtr->projectionComboBox->addItem("Orthographic", 1);
+ connect(this->dataPtr->projectionComboBox, SIGNAL(currentIndexChanged(int)),
+ this, SLOT(OnProjection(int)));
+
+ // Main layout
+ this->dataPtr->mainLayout = new QGridLayout();
+ this->dataPtr->mainLayout->addWidget(this->dataPtr->projectionComboBox,
+ 3, 0, 1, 4);
+ this->setLayout(this->dataPtr->mainLayout);
+
+ // Connect the ortho action
+ connect(g_cameraOrthoAct, SIGNAL(triggered()), this, SLOT(OnOrtho()));
+
+ // connect the perspective action
+ connect(g_cameraPerspectiveAct, SIGNAL(triggered()), this,
+ SLOT(OnPerspective()));
+}
+
+/////////////////////////////////////////////////
+ViewAngleWidget::~ViewAngleWidget()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::Add(const Mode _mode, QAction *_action)
+{
+ if (_mode == TOP)
+ {
+ this->dataPtr->topButton->setDefaultAction(_action);
+ this->dataPtr->mainLayout->addWidget(this->dataPtr->topButton, 0, 1);
+ connect(_action, SIGNAL(triggered()), this, SLOT(OnTopView()));
+ }
+ else if (_mode == BOTTOM)
+ {
+ this->dataPtr->bottomButton->setDefaultAction(_action);
+ this->dataPtr->mainLayout->addWidget(this->dataPtr->bottomButton, 2, 1);
+ connect(_action, SIGNAL(triggered()), this, SLOT(OnBottomView()));
+ }
+ else if (_mode == FRONT)
+ {
+ this->dataPtr->frontButton->setDefaultAction(_action);
+ this->dataPtr->mainLayout->addWidget(this->dataPtr->frontButton, 1, 1);
+ connect(_action, SIGNAL(triggered()), this, SLOT(OnFrontView()));
+ }
+ else if (_mode == BACK)
+ {
+ this->dataPtr->backButton->setDefaultAction(_action);
+ this->dataPtr->mainLayout->addWidget(this->dataPtr->backButton, 1, 3);
+ connect(_action, SIGNAL(triggered()), this, SLOT(OnBackView()));
+ }
+ else if (_mode == LEFT)
+ {
+ this->dataPtr->leftButton->setDefaultAction(_action);
+ this->dataPtr->mainLayout->addWidget(this->dataPtr->leftButton, 1, 0);
+ connect(_action, SIGNAL(triggered()), this, SLOT(OnLeftView()));
+ }
+ else if (_mode == RIGHT)
+ {
+ this->dataPtr->rightButton->setDefaultAction(_action);
+ this->dataPtr->mainLayout->addWidget(this->dataPtr->rightButton, 1, 2);
+ connect(_action, SIGNAL(triggered()), this, SLOT(OnRightView()));
+ }
+ else if (_mode == RESET)
+ {
+ this->dataPtr->resetButton->setDefaultAction(_action);
+ this->dataPtr->mainLayout->addWidget(this->dataPtr->resetButton, 0, 3);
+ connect(_action, SIGNAL(triggered()), this, SLOT(OnResetView()));
+ }
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::LookDirection(const ignition::math::Vector3d &_dir)
+{
+ rendering::UserCameraPtr cam = gui::get_active_camera();
+ if (!cam)
+ return;
+
+ GLWidget *glWidget =
+ this->dataPtr->mainWindow->findChild<GLWidget *>("GLWidget");
+ if (!glWidget)
+ {
+ gzerr << "GLWidget not found, this should never happen. " <<
+ "Camera pose won't be changed." << std::endl;
+ return;
+ }
+
+ // Look at world origin unless there are visuals selected
+ ignition::math::Vector3d lookAt = ignition::math::Vector3d::Zero;
+
+ // If there are selected visuals, look at their center
+ std::vector<rendering::VisualPtr> selectedVisuals =
+ glWidget->SelectedVisuals();
+
+ if (!selectedVisuals.empty())
+ {
+ for (auto const &vis : selectedVisuals)
+ {
+ ignition::math::Vector3d visPos = vis->GetWorldPose().pos.Ign();
+ lookAt += visPos;
+ }
+ lookAt /= selectedVisuals.size();
+ }
+
+ // Keep current distance to look target
+ ignition::math::Vector3d camPos = cam->GetWorldPose().pos.Ign();
+ double distance = std::fabs((camPos - lookAt).Length());
+
+ // Calculate camera position
+ ignition::math::Vector3d newCamPos = lookAt - _dir * distance;
+
+ // Calculate camera orientation
+ double roll = 0;
+ double pitch = -std::atan2(_dir.Z(),
+ std::sqrt(std::pow(_dir.X(), 2) + std::pow(_dir.Y(), 2)));
+ double yaw = std::atan2(_dir.Y(), _dir.X());
+
+ ignition::math::Quaterniond quat =
+ ignition::math::Quaterniond(roll, pitch, yaw);
+
+ // Move camera to that pose in 1s
+ cam->MoveToPosition(math::Pose(math::Vector3(newCamPos),
+ math::Quaternion(quat)), 1);
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::OnTopView()
+{
+ this->LookDirection(-ignition::math::Vector3d::UnitZ);
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::OnBottomView()
+{
+ this->LookDirection(ignition::math::Vector3d::UnitZ);
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::OnFrontView()
+{
+ this->LookDirection(-ignition::math::Vector3d::UnitX);
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::OnBackView()
+{
+ this->LookDirection(ignition::math::Vector3d::UnitX);
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::OnLeftView()
+{
+ this->LookDirection(-ignition::math::Vector3d::UnitY);
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::OnRightView()
+{
+ this->LookDirection(ignition::math::Vector3d::UnitY);
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::OnResetView()
+{
+ rendering::UserCameraPtr cam = gui::get_active_camera();
+
+ if (!cam)
+ return;
+
+ cam->MoveToPosition(cam->DefaultPose(), 1);
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::OnPerspective()
+{
+ this->dataPtr->projectionComboBox->blockSignals(true);
+ this->dataPtr->projectionComboBox->setCurrentIndex(0);
+ this->dataPtr->projectionComboBox->blockSignals(false);
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::OnOrtho()
+{
+ this->dataPtr->projectionComboBox->blockSignals(true);
+ this->dataPtr->projectionComboBox->setCurrentIndex(1);
+ this->dataPtr->projectionComboBox->blockSignals(false);
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget::OnProjection(int _index)
+{
+ if (_index == 0)
+ g_cameraPerspectiveAct->trigger();
+ else if (_index == 1)
+ g_cameraOrthoAct->trigger();
+ else
+ gzwarn << "Projection index [" << _index << "] not recognized" << std::endl;
+}
+
diff --git a/gazebo/gui/ViewAngleWidget.hh b/gazebo/gui/ViewAngleWidget.hh
new file mode 100644
index 0000000..f71c195
--- /dev/null
+++ b/gazebo/gui/ViewAngleWidget.hh
@@ -0,0 +1,115 @@
+/*
+ * 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_VIEW_ANGLE_WIDGET_HH_
+#define _GAZEBO_VIEW_ANGLE_WIDGET_HH_
+
+#include <string>
+
+#include <ignition/math/Vector3.hh>
+
+#include "gazebo/gui/qt.h"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class ViewAngleWidgetPrivate;
+
+ /// \brief A gui widget for changing the camera view angle.
+ class GAZEBO_VISIBLE ViewAngleWidget : public QWidget
+ {
+ Q_OBJECT
+
+ /// \brief View angle modes
+ public: enum Mode
+ {
+ /// \brief Top
+ TOP,
+ /// \brief Bottom
+ BOTTOM,
+ /// \brief Front
+ FRONT,
+ /// \brief Back
+ BACK,
+ /// \brief Left
+ LEFT,
+ /// \brief Right
+ RIGHT,
+ /// \brief Reset view
+ RESET
+ };
+
+ /// \brief Constructor
+ /// \param[in] _parent Parent Qt widget.
+ public: ViewAngleWidget(QWidget *_parent = 0);
+
+ /// \brief Destructor
+ public: virtual ~ViewAngleWidget();
+
+ /// \brief Add an action to the widget.
+ /// \param[in] _mode View mode
+ /// \param[in] _action The Qt action to add.
+ public: void Add(const Mode _mode, QAction *_action);
+
+ /// \brief Qt callback when the top view has been triggered.
+ private slots: void OnTopView();
+
+ /// \brief Qt callback when the bottom view has been triggered.
+ private slots: void OnBottomView();
+
+ /// \brief Qt callback when the front view has been triggered.
+ private slots: void OnFrontView();
+
+ /// \brief Qt callback when the back view has been triggered.
+ private slots: void OnBackView();
+
+ /// \brief Qt callback when the left view has been triggered.
+ private slots: void OnLeftView();
+
+ /// \brief Qt callback when the right view has been triggered.
+ private slots: void OnRightView();
+
+ /// \brief Qt callback when reset view has been triggered.
+ private slots: void OnResetView();
+
+ /// \brief Qt callback when the index of the QComboBox has been changed.
+ /// \param[in] _index Index: 0 for perpective, 1 for orthographic.
+ private slots: void OnProjection(int _index);
+
+ /// \brief QT Callback that turns on orthographic projection.
+ private slots: void OnOrtho();
+
+ /// \brief QT Callback that turns on perspective projection.
+ private slots: void OnPerspective();
+
+ /// \brief Move the camera to face the given direction. In case there's
+ /// an object immediately in front of the camera, the camera will
+ /// continue facing the object, with about the same distance (zoom). If
+ /// there's no object, the camera will face the world origin. The movement
+ /// takes 1 second.
+ /// \param[in] _dir Direction for the camera to face.
+ private: void LookDirection(const ignition::math::Vector3d &_dir);
+
+ /// \internal
+ /// \brief Pointer to private data.
+ private: ViewAngleWidgetPrivate *dataPtr;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/ViewAngleWidgetPrivate.hh b/gazebo/gui/ViewAngleWidgetPrivate.hh
new file mode 100644
index 0000000..3c6b7a3
--- /dev/null
+++ b/gazebo/gui/ViewAngleWidgetPrivate.hh
@@ -0,0 +1,62 @@
+/*
+ * 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_VIEW_ANGLE_WIDGET_PRIVATE_HH_
+#define _GAZEBO_VIEW_ANGLE_WIDGET_PRIVATE_HH_
+
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \internal
+ /// \brief Private data for the ViewAngleWidget class
+ class ViewAngleWidgetPrivate
+ {
+ /// \brief Button for top view.
+ public: QToolButton *topButton;
+
+ /// \brief Button for bottom view.
+ public: QToolButton *bottomButton;
+
+ /// \brief Button for front view.
+ public: QToolButton *frontButton;
+
+ /// \brief Button for back view.
+ public: QToolButton *backButton;
+
+ /// \brief Button for left view.
+ public: QToolButton *leftButton;
+
+ /// \brief Button for right view.
+ public: QToolButton *rightButton;
+
+ /// \brief Button for reset view.
+ public: QToolButton *resetButton;
+
+ /// \brief Dropdown menu for projection types.
+ public: QComboBox *projectionComboBox;
+
+ /// \brief Main layout.
+ public: QGridLayout *mainLayout;
+
+ /// \brief Pointer to the main window.
+ public: MainWindow *mainWindow;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/ViewAngleWidget_TEST.cc b/gazebo/gui/ViewAngleWidget_TEST.cc
new file mode 100644
index 0000000..008011b
--- /dev/null
+++ b/gazebo/gui/ViewAngleWidget_TEST.cc
@@ -0,0 +1,306 @@
+/*
+ * 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/Helpers.hh"
+
+#include "gazebo/gui/Actions.hh"
+#include "gazebo/gui/MainWindow.hh"
+#include "gazebo/gui/GuiIface.hh"
+
+#include "gazebo/gui/ViewAngleWidget.hh"
+#include "gazebo/gui/ViewAngleWidget_TEST.hh"
+
+/////////////////////////////////////////////////
+void ViewAngleWidget_TEST::EmptyWorld()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/empty.world");
+
+ // Create the main window.
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get camera
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ // Get camera default pose and check that's the current pose
+ gazebo::math::Pose defaultPose = cam->DefaultPose();
+ QVERIFY(defaultPose == cam->GetWorldPose());
+
+ // Get the camera distance from the world origin (zoom level)
+ double dist = defaultPose.pos.GetLength();
+
+ // Get the view angle widget
+ gazebo::gui::ViewAngleWidget *viewAngleWidget =
+ mainWindow->findChild<gazebo::gui::ViewAngleWidget *>("viewAngleWidget");
+ QVERIFY(viewAngleWidget != NULL);
+
+ QApplication::postEvent(viewAngleWidget, new QShowEvent());
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get buttons
+ QList<QToolButton *> buttons =
+ viewAngleWidget->findChildren<QToolButton *>();
+ QVERIFY(buttons.size() == 7u);
+
+ // Trigger the top view button
+ buttons[0]->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 50; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check the camera position
+ double tol = 1;
+ gazebo::math::Pose pose = cam->GetWorldPose();
+ QVERIFY((pose.pos - gazebo::math::Vector3(0, 0, dist)).GetLength() < tol);
+
+ // Trigger the front view button
+ buttons[2]->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 50; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check the camera position
+ pose = cam->GetWorldPose();
+ QVERIFY((pose.pos - gazebo::math::Vector3(dist, 0, 0)).GetLength() < tol);
+
+ // Trigger the left view button
+ buttons[5]->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 50; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check the camera position
+ pose = cam->GetWorldPose();
+ QVERIFY((pose.pos - gazebo::math::Vector3(0, -dist, 0)).GetLength() < tol);
+
+ // Trigger the bottom view button
+ buttons[1]->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 50; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check the camera position
+ pose = cam->GetWorldPose();
+ QVERIFY((pose.pos - gazebo::math::Vector3(0, 0, -dist)).GetLength() < tol);
+
+ // Trigger the back view button
+ buttons[3]->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 50; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check the camera position
+ pose = cam->GetWorldPose();
+ QVERIFY((pose.pos - gazebo::math::Vector3(-dist, 0, 0)).GetLength() < tol);
+
+ // Trigger the right view button
+ buttons[4]->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 50; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check the camera position
+ pose = cam->GetWorldPose();
+ QVERIFY((pose.pos - gazebo::math::Vector3(0, dist, 0)).GetLength() < tol);
+
+ // Trigger the reset view button
+ buttons[6]->click();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 50; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check the camera position
+ pose = cam->GetWorldPose();
+ QVERIFY((pose.pos - defaultPose.pos).GetLength() < tol);
+
+ // Clean up
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+}
+
+/////////////////////////////////////////////////
+void ViewAngleWidget_TEST::Projections()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/empty.world");
+
+ // Create the main window.
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get camera
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+
+ // Get the view angle widget
+ gazebo::gui::ViewAngleWidget *viewAngleWidget =
+ mainWindow->findChild<gazebo::gui::ViewAngleWidget *>("viewAngleWidget");
+ QVERIFY(viewAngleWidget != NULL);
+
+ // Get the combobox
+ QList<QComboBox *> comboBoxes =
+ viewAngleWidget->findChildren<QComboBox *>();
+ QVERIFY(comboBoxes.size() == 1u);
+
+ // Check that it is in perspective projection
+ QVERIFY(gazebo::gui::g_cameraPerspectiveAct->isChecked());
+ QVERIFY(!gazebo::gui::g_cameraOrthoAct->isChecked());
+ QVERIFY(comboBoxes[0]->currentText() == "Perspective");
+
+ // Trigger ortho and see it changed
+ gazebo::gui::g_cameraOrthoAct->trigger();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that it is in orthographic projection
+ QVERIFY(!gazebo::gui::g_cameraPerspectiveAct->isChecked());
+ QVERIFY(gazebo::gui::g_cameraOrthoAct->isChecked());
+ QVERIFY(comboBoxes[0]->currentText() == "Orthographic");
+
+ // Trigger perpective and see it changed
+ gazebo::gui::g_cameraPerspectiveAct->trigger();
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that it is in perspective projection
+ QVERIFY(gazebo::gui::g_cameraPerspectiveAct->isChecked());
+ QVERIFY(!gazebo::gui::g_cameraOrthoAct->isChecked());
+ QVERIFY(comboBoxes[0]->currentText() == "Perspective");
+
+ // Change combobox
+ comboBoxes[0]->setCurrentIndex(1);
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that it is in orthographic projection
+ QVERIFY(!gazebo::gui::g_cameraPerspectiveAct->isChecked());
+ QVERIFY(gazebo::gui::g_cameraOrthoAct->isChecked());
+ QVERIFY(comboBoxes[0]->currentText() == "Orthographic");
+
+ // Change combobox
+ comboBoxes[0]->setCurrentIndex(0);
+
+ // Process some events and draw the screen
+ for (size_t i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Check that it is in perspective projection
+ QVERIFY(gazebo::gui::g_cameraPerspectiveAct->isChecked());
+ QVERIFY(!gazebo::gui::g_cameraOrthoAct->isChecked());
+ QVERIFY(comboBoxes[0]->currentText() == "Perspective");
+
+ // Clean up
+ cam->Fini();
+ mainWindow->close();
+ delete mainWindow;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(ViewAngleWidget_TEST)
diff --git a/gazebo/gui/ViewAngleWidget_TEST.hh b/gazebo/gui/ViewAngleWidget_TEST.hh
new file mode 100644
index 0000000..39f2917
--- /dev/null
+++ b/gazebo/gui/ViewAngleWidget_TEST.hh
@@ -0,0 +1,35 @@
+/*
+ * 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_VIEW_ANGLE_WIDGET_TEST_HH_
+#define _GAZEBO_VIEW_ANGLE_WIDGET_TEST_HH_
+
+#include "gazebo/gui/QTestFixture.hh"
+
+/// \brief A test class for the ViewAngleWidget widget.
+class ViewAngleWidget_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Test the different view angles on an empty world.
+ private slots: void EmptyWorld();
+
+ /// \brief Test triggering the different projections.
+ private slots: void Projections();
+};
+
+#endif
diff --git a/gazebo/gui/building/BaseInspectorDialog.cc b/gazebo/gui/building/BaseInspectorDialog.cc
new file mode 100644
index 0000000..dc15dae
--- /dev/null
+++ b/gazebo/gui/building/BaseInspectorDialog.cc
@@ -0,0 +1,134 @@
+/*
+ * 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/common/Assert.hh"
+#include "gazebo/gui/building/BaseInspectorDialog.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+BaseInspectorDialog::BaseInspectorDialog(QWidget *_parent):QDialog(_parent)
+{
+}
+
+/////////////////////////////////////////////////
+BaseInspectorDialog::~BaseInspectorDialog()
+{
+}
+
+/////////////////////////////////////////////////
+void BaseInspectorDialog::InitColorComboBox()
+{
+ this->colorComboBox = new QComboBox;
+ this->colorComboBox->setIconSize(QSize(15, 15));
+ this->colorComboBox->setMinimumWidth(50);
+ this->colorComboBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
+ QPixmap colorIcon(15, 15);
+ this->colorList.push_back(QColor(255, 255, 255, 255));
+ this->colorList.push_back(QColor(194, 169, 160, 255));
+ this->colorList.push_back(QColor(235, 206, 157, 255));
+ this->colorList.push_back(QColor(254, 121, 5, 255));
+ this->colorList.push_back(QColor(255, 195, 78, 255));
+ this->colorList.push_back(QColor(111, 203, 172, 255));
+ for (unsigned int i = 0; i < this->colorList.size(); ++i)
+ {
+ colorIcon.fill(this->colorList.at(i));
+ this->colorComboBox->addItem(colorIcon, QString(""));
+ }
+}
+
+/////////////////////////////////////////////////
+void BaseInspectorDialog::InitTextureComboBox()
+{
+ this->textureComboBox = new QComboBox;
+ this->textureComboBox->setIconSize(QSize(30, 30));
+ this->textureComboBox->setMinimumWidth(50);
+ this->textureComboBox->setMinimumHeight(50);
+ this->textureComboBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
+ this->textureList.push_back(":wood.jpg");
+ this->textureList.push_back(":tiles.jpg");
+ this->textureList.push_back(":bricks.png");
+ for (unsigned int i = 0; i < this->textureList.size(); ++i)
+ {
+ this->textureComboBox->addItem(QPixmap(this->textureList[i]).scaled(
+ QSize(90, 90), Qt::IgnoreAspectRatio), QString(""));
+ }
+ this->textureComboBox->addItem("X");
+ this->textureComboBox->setCurrentIndex(this->textureComboBox->count()-1);
+}
+
+/////////////////////////////////////////////////
+QColor BaseInspectorDialog::GetColor() const
+{
+ return this->colorList[this->colorComboBox->currentIndex()];
+}
+
+/////////////////////////////////////////////////
+QString BaseInspectorDialog::GetTexture() const
+{
+ QString texture = QString("");
+ if (this->textureComboBox->currentIndex() != -1 &&
+ this->textureComboBox->currentIndex() <
+ this->textureComboBox->count() - 1)
+ {
+ texture = this->textureList[this->textureComboBox->currentIndex()];
+ }
+ return texture;
+}
+
+/////////////////////////////////////////////////
+void BaseInspectorDialog::SetColor(const QColor _color)
+{
+ int index = -1;
+ for (unsigned int i = 0; i < this->colorList.size(); ++i)
+ {
+ if (this->colorList[i] == _color)
+ {
+ index = i;
+ break;
+ }
+ }
+
+ if (index == -1)
+ {
+ // Add a new color
+ this->colorList.push_back(_color);
+ QPixmap colorIcon(15, 15);
+ colorIcon.fill(this->colorList.back());
+ this->colorComboBox->addItem(colorIcon, QString(""));
+ index = this->colorComboBox->count()-1;
+ }
+ GZ_ASSERT(index >= 0, "Color index is broken < 0");
+ this->colorComboBox->setCurrentIndex(index);
+}
+
+/////////////////////////////////////////////////
+void BaseInspectorDialog::SetTexture(QString _texture)
+{
+ // Find index corresponding to texture (only a few textures allowed so far)
+ int index = this->textureComboBox->count()-1;
+ for (unsigned int i = 0; i < this->textureList.size(); ++i)
+ {
+ if (this->textureList[i] == _texture)
+ {
+ index = i;
+ break;
+ }
+ }
+ this->textureComboBox->setCurrentIndex(index);
+}
diff --git a/gazebo/gui/building/BaseInspectorDialog.hh b/gazebo/gui/building/BaseInspectorDialog.hh
new file mode 100644
index 0000000..dea79e6
--- /dev/null
+++ b/gazebo/gui/building/BaseInspectorDialog.hh
@@ -0,0 +1,84 @@
+/*
+ * 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_BASE_INSPECTOR_DIALOG_HH_
+#define _GAZEBO_BASE_INSPECTOR_DIALOG_HH_
+
+#include <string>
+#include <vector>
+#include "gazebo/gui/qt.h"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \addtogroup gazebo_gui
+ /// \{
+
+ /// \class BaseInspectorDialog BaseInspectorDialog.hh
+ /// \brief Base Dialog for a specific inspector dialog.
+ class GZ_GUI_BUILDING_VISIBLE BaseInspectorDialog : public QDialog
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ /// \param[in] _parent Parent QWidget
+ public: BaseInspectorDialog(QWidget *_parent);
+
+ /// \brief Destructor
+ public: ~BaseInspectorDialog();
+
+ /// \brief Initiate color combo box.
+ public: void InitColorComboBox();
+
+ /// \brief Initiate texture combo box.
+ public: void InitTextureComboBox();
+
+ /// \brief Get the color.
+ /// \return Color.
+ public: QColor GetColor() const;
+
+ /// \brief Get the texture.
+ /// \return Texture.
+ public: QString GetTexture() const;
+
+ /// \brief Set the color.
+ /// \param[in] _color Color.
+ public: void SetColor(const QColor _color);
+
+ /// \brief Set the texture.
+ /// \param[in] _texture Texture.
+ public: void SetTexture(const QString _texture);
+
+ /// \brief Combo box for selecting the color.
+ public: QComboBox *colorComboBox;
+
+ /// \brief Vector of color options.
+ public: std::vector<QColor> colorList;
+
+ /// \brief Combo box for selecting the texture.
+ public: QComboBox *textureComboBox;
+
+ /// \brief Vector of texture options.
+ public: std::vector<QString> textureList;
+ };
+ /// \}
+ }
+}
+
+#endif
diff --git a/gazebo/gui/building/BuildingEditor.cc b/gazebo/gui/building/BuildingEditor.cc
index 36c4251..98f77b7 100644
--- a/gazebo/gui/building/BuildingEditor.cc
+++ b/gazebo/gui/building/BuildingEditor.cc
@@ -19,6 +19,7 @@
#include "gazebo/gui/Actions.hh"
#include "gazebo/gui/MainWindow.hh"
#include "gazebo/gui/RenderWidget.hh"
+#include "gazebo/gui/building/BuildingEditorWidget.hh"
#include "gazebo/gui/building/BuildingEditorEvents.hh"
#include "gazebo/gui/building/BuildingEditorPalette.hh"
#include "gazebo/gui/building/BuildingEditor.hh"
@@ -31,19 +32,20 @@ BuildingEditor::BuildingEditor(MainWindow *_mainWindow)
: Editor(_mainWindow)
{
// Tips
- QLabel *tipsLabel = new QLabel(tr(
+ this->tipsLabel = new QLabel(tr(
"<font size=4 color='white'><b>?</b></font>"));
- tipsLabel->setToolTip(tr("<font size=3><p><b> Tips: </b></b>"
+ this->tipsLabel->setToolTip(tr("<font size=3><p><b> Tips: </b></b>"
"<p>Double-click an object to open an Inspector with configuration "
"options.</p>"
"<p>Currently, windows & doors are simple holes in the wall.</p>"
"<p>Because Gazebo only supports simple primitive shapes, all floors "
"will be rectangular.</p>"));
+ this->tipsLabel->installEventFilter(this);
// Create the building editor tab
this->buildingPalette = new BuildingEditorPalette;
this->Init("buildingEditorTab", "Building Editor", this->buildingPalette,
- tipsLabel);
+ this->tipsLabel);
this->newAct = new QAction(tr("&New"), this->mainWindow);
this->newAct->setStatusTip(tr("New"));
@@ -75,6 +77,15 @@ BuildingEditor::BuildingEditor(MainWindow *_mainWindow)
gui::editor::Events::ConnectFinishBuildingModel(
boost::bind(&BuildingEditor::OnFinish, this)));
+ this->buildingEditorWidget = new BuildingEditorWidget(
+ this->mainWindow->GetRenderWidget());
+ this->buildingEditorWidget->setSizePolicy(QSizePolicy::Expanding,
+ QSizePolicy::Expanding);
+ this->buildingEditorWidget->hide();
+
+ this->mainWindow->GetRenderWidget()->InsertWidget(0,
+ this->buildingEditorWidget);
+
this->menuBar = NULL;
}
@@ -86,15 +97,13 @@ BuildingEditor::~BuildingEditor()
////////////////////////////////////////////////
void BuildingEditor::Save()
{
- gui::editor::Events::saveBuildingEditor(
- this->buildingPalette->GetModelName());
+ gui::editor::Events::saveBuildingEditor();
}
////////////////////////////////////////////////
void BuildingEditor::SaveAs()
{
- gui::editor::Events::saveAsBuildingEditor(
- this->buildingPalette->GetModelName());
+ gui::editor::Events::saveAsBuildingEditor();
}
/////////////////////////////////////////////////
@@ -138,17 +147,41 @@ void BuildingEditor::OnEdit(bool _checked)
if (_checked)
{
this->CreateMenus();
+ this->mainWindowPaused = this->mainWindow->IsPaused();
this->mainWindow->Pause();
this->mainWindow->ShowLeftColumnWidget("buildingEditorTab");
this->mainWindow->ShowMenuBar(this->menuBar);
- this->mainWindow->GetRenderWidget()->ShowEditor(true);
+ this->buildingEditorWidget->show();
+ this->mainWindow->GetRenderWidget()->DisplayOverlayMsg(
+ "Building is View Only");
+ this->mainWindow->GetRenderWidget()->ShowTimePanel(false);
+ this->mainWindow->GetRenderWidget()->ShowToolbar(false);
}
else
{
+ this->buildingPalette->CustomColorDialog()->reject();
this->mainWindow->ShowLeftColumnWidget();
- this->mainWindow->GetRenderWidget()->ShowEditor(false);
+ this->buildingEditorWidget->hide();
+ this->mainWindow->GetRenderWidget()->DisplayOverlayMsg("");
+ this->mainWindow->GetRenderWidget()->ShowTimePanel(true);
+ this->mainWindow->GetRenderWidget()->ShowToolbar(true);
this->mainWindow->ShowMenuBar();
- this->mainWindow->Play();
+ if (!this->mainWindowPaused)
+ this->mainWindow->Play();
}
gui::editor::Events::toggleEditMode(_checked);
}
+
+/////////////////////////////////////////////////
+bool BuildingEditor::eventFilter(QObject *_obj, QEvent *_event)
+{
+ QLabel *label = qobject_cast<QLabel *>(_obj);
+ if (label && label == this->tipsLabel &&
+ _event->type() == QEvent::MouseButtonRelease)
+ {
+ QToolTip::showText(this->tipsLabel->mapToGlobal(QPoint()),
+ this->tipsLabel->toolTip());
+ return true;
+ }
+ return false;
+}
diff --git a/gazebo/gui/building/BuildingEditor.hh b/gazebo/gui/building/BuildingEditor.hh
index eea7e57..577e484 100644
--- a/gazebo/gui/building/BuildingEditor.hh
+++ b/gazebo/gui/building/BuildingEditor.hh
@@ -26,10 +26,11 @@ namespace gazebo
namespace gui
{
class BuildingEditorPalette;
+ class BuildingEditorWidget;
/// \class TerrainEditor TerrainEditor.hh gui/gui.hh
/// \brief Interface to the terrain editor.
- class GAZEBO_VISIBLE BuildingEditor : public Editor
+ class GZ_GUI_BUILDING_VISIBLE BuildingEditor : public Editor
{
Q_OBJECT
@@ -67,16 +68,38 @@ namespace gazebo
/// \brief Contains all the building editor tools.
private: BuildingEditorPalette *buildingPalette;
+ /// \brief Building editor widget for creating a building model
+ private: BuildingEditorWidget *buildingEditorWidget;
+
/// \brief Create menus
private: void CreateMenus();
+ /// \brief Qt event filter currently used to filter tips label events.
+ /// \param[in] _obj Object that is watched by the event filter.
+ /// \param[in] _event Qt event.
+ /// \return True if the event is handled.
+ private: bool eventFilter(QObject *_obj, QEvent *_event);
+
/// \brief Our custom menubar
private: QMenuBar *menuBar;
+ /// \brief Action to save model.
private: QAction *saveAct;
+
+ /// \brief Action to save model as.
private: QAction *saveAsAct;
+
+ /// \brief Action to start a new model.
private: QAction *newAct;
+
+ /// \brief Action to exit the editor.
private: QAction *exitAct;
+
+ /// \brief Save the main window paused state to use when returning.
+ private: bool mainWindowPaused;
+
+ /// \brief Label which shows tips when clicked or hovered.
+ private: QLabel *tipsLabel;
};
}
}
diff --git a/gazebo/gui/building/BuildingEditorEvents.cc b/gazebo/gui/building/BuildingEditorEvents.cc
index 15dad39..a74cb4a 100644
--- a/gazebo/gui/building/BuildingEditorEvents.cc
+++ b/gazebo/gui/building/BuildingEditorEvents.cc
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2015 Open Source Robotics Foundation
+ * Copyright (C) 2013-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.
@@ -24,8 +24,7 @@ event::EventT<void (bool)> editor::Events::toggleEditMode;
event::EventT<void (std::string)> editor::Events::createBuildingEditorItem;
event::EventT<void (QColor)> editor::Events::colorSelected;
event::EventT<void (QString)> editor::Events::textureSelected;
-event::EventT<void (std::string, std::string)>
- editor::Events::saveBuildingModel;
+event::EventT<void (std::string)> editor::Events::saveBuildingModel;
event::EventT<void ()> editor::Events::finishBuildingModel;
event::EventT<void ()> editor::Events::newBuildingModel;
event::EventT<void (int)> editor::Events::changeBuildingLevel;
@@ -37,8 +36,8 @@ event::EventT<void ()> editor::Events::showElements;
event::EventT<void ()> editor::Events::triggerShowElements;
event::EventT<void (int, std::string)> editor::Events::updateLevelWidget;
event::EventT<void (double)> editor::Events::changeBuildingEditorZoom;
-event::EventT<bool (std::string)> editor::Events::saveAsBuildingEditor;
-event::EventT<bool (std::string)> editor::Events::saveBuildingEditor;
+event::EventT<bool ()> editor::Events::saveAsBuildingEditor;
+event::EventT<bool ()> editor::Events::saveBuildingEditor;
event::EventT<void ()> editor::Events::newBuildingEditor;
event::EventT<void ()> editor::Events::exitBuildingEditor;
event::EventT<void (std::string)> editor::Events::buildingNameChanged;
diff --git a/gazebo/gui/building/BuildingEditorEvents.hh b/gazebo/gui/building/BuildingEditorEvents.hh
index 0ba2977..5be6675 100644
--- a/gazebo/gui/building/BuildingEditorEvents.hh
+++ b/gazebo/gui/building/BuildingEditorEvents.hh
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2015 Open Source Robotics Foundation
+ * Copyright (C) 2013-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.
@@ -28,9 +28,9 @@ namespace gazebo
{
namespace editor
{
- class GAZEBO_VISIBLE Events
+ class GZ_GUI_BUILDING_VISIBLE Events
{
- /// \brief Connect a Gazebo event to the toggle edit mode signal
+ /// \brief Connect a Gazebo event to the toggle edit mode signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -38,13 +38,13 @@ namespace gazebo
ConnectToggleEditMode(T _subscriber)
{ return toggleEditMode.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the toggle edit mode signal
+ /// \brief Disconnect a Gazebo event from the toggle edit mode signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectToggleEditMode(
event::ConnectionPtr _subscriber)
{ toggleEditMode.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the create editor item signal
+ /// \brief Connect a Gazebo event to the create editor item signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -52,13 +52,13 @@ namespace gazebo
ConnectCreateBuildingEditorItem(T _subscriber)
{ return createBuildingEditorItem.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the create editor item signal
+ /// \brief Disconnect a Gazebo event from the create editor item signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectCreateBuildingEditorItem(
event::ConnectionPtr _subscriber)
{ createBuildingEditorItem.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the color selected signal
+ /// \brief Connect a Gazebo event to the color selected signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -66,13 +66,13 @@ namespace gazebo
ConnectColorSelected(T _subscriber)
{ return colorSelected.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the color selected signal
+ /// \brief Disconnect a Gazebo event from the color selected signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectColorSelected(
event::ConnectionPtr _subscriber)
{ colorSelected.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the texture selected signal
+ /// \brief Connect a Gazebo event to the texture selected signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -80,26 +80,26 @@ namespace gazebo
ConnectTextureSelected(T _subscriber)
{ return textureSelected.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the texture selected signal
+ /// \brief Disconnect a Gazebo event from the texture selected signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectTextureSelected(
event::ConnectionPtr _subscriber)
{ textureSelected.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the save model signal
+ /// \brief Connect a Gazebo event to the save model signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
static event::ConnectionPtr ConnectSaveBuildingModel(T _subscriber)
{ return saveBuildingModel.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the save model signal
+ /// \brief Disconnect a Gazebo event from the save model signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectSaveBuildingModel(
event::ConnectionPtr _subscriber)
{ saveBuildingModel.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the finish model signal
+ /// \brief Connect a Gazebo event to the finish model signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -107,13 +107,13 @@ namespace gazebo
ConnectFinishBuildingModel(T _subscriber)
{ return finishBuildingModel.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the finish model signal
+ /// \brief Disconnect a Gazebo event from the finish model signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectFinishBuildingModel(
event::ConnectionPtr _subscriber)
{ finishBuildingModel.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the new model signal
+ /// \brief Connect a Gazebo event to the new model signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -121,13 +121,13 @@ namespace gazebo
ConnectNewBuildingModel(T _subscriber)
{ return newBuildingModel.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the new model signal
+ /// \brief Disconnect a Gazebo event from the new model signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectNewBuildingModel(
event::ConnectionPtr _subscriber)
{ newBuildingModel.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the change model signal
+ /// \brief Connect a Gazebo event to the change model signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -135,26 +135,26 @@ namespace gazebo
ConnectChangeBuildingLevel(T _subscriber)
{ return changeBuildingLevel.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the change level signal
+ /// \brief Disconnect a Gazebo event from the change level signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectChangeBuildingLevel(
event::ConnectionPtr _subscriber)
{ changeBuildingLevel.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the add level signal
+ /// \brief Connect a Gazebo event to the add level signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
static event::ConnectionPtr ConnectAddBuildingLevel(T _subscriber)
{ return addBuildingLevel.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the add level signal
+ /// \brief Disconnect a Gazebo event from the add level signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectAddBuildingLevel(
event::ConnectionPtr _subscriber)
{ addBuildingLevel.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the delete level signal
+ /// \brief Connect a Gazebo event to the delete level signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -162,27 +162,27 @@ namespace gazebo
ConnectDeleteBuildingLevel(T _subscriber)
{ return deleteBuildingLevel.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the delete level signal
+ /// \brief Disconnect a Gazebo event from the delete level signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectDeleteBuildingLevel(
event::ConnectionPtr _subscriber)
{ deleteBuildingLevel.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the show floorplan signal
+ /// \brief Connect a Gazebo event to the show floorplan signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
static event::ConnectionPtr ConnectShowFloorplan(T _subscriber)
{ return showFloorplan.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the show floorplan signal
+ /// \brief Disconnect a Gazebo event from the show floorplan signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectShowFloorplan(
event::ConnectionPtr _subscriber)
{ showFloorplan.Disconnect(_subscriber); }
/// \brief Connect a Gazebo event to the trigger show floorplan
- /// signal
+ /// signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -191,27 +191,27 @@ namespace gazebo
{ return triggerShowFloorplan.Connect(_subscriber); }
/// \brief Disconnect a Gazebo event from the trigger show floorplan
- /// signal
+ /// signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectTriggerShowFloorplan(
event::ConnectionPtr _subscriber)
{ triggerShowFloorplan.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the show elements signal
+ /// \brief Connect a Gazebo event to the show elements signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
static event::ConnectionPtr ConnectShowElements(T _subscriber)
{ return showElements.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the show elements signal
+ /// \brief Disconnect a Gazebo event from the show elements signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectShowElements(
event::ConnectionPtr _subscriber)
{ showElements.Disconnect(_subscriber); }
/// \brief Connect a Gazebo event to the trigger show elements
- /// signal
+ /// signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -220,13 +220,13 @@ namespace gazebo
{ return triggerShowElements.Connect(_subscriber); }
/// \brief Disconnect a Gazebo event from the trigger show elements
- /// signal
+ /// signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectTriggerShowElements(
event::ConnectionPtr _subscriber)
{ triggerShowElements.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the update level widget signal
+ /// \brief Connect a Gazebo event to the update level widget signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -240,7 +240,7 @@ namespace gazebo
event::ConnectionPtr _subscriber)
{ updateLevelWidget.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the change zoom signal
+ /// \brief Connect a Gazebo event to the change zoom signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -248,20 +248,26 @@ namespace gazebo
ConnectChangeBuildingEditorZoom(T _subscriber)
{ return changeBuildingEditorZoom.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the change zoom level signal
+ /// \brief Disconnect a Gazebo event from the change zoom level signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectChangeBuildingEditorZoom(
event::ConnectionPtr _subscriber)
{ changeBuildingEditorZoom.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the save signal
+ /// \brief Connect a Gazebo event to the save signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
static event::ConnectionPtr ConnectSaveBuildingEditor(T _subscriber)
{ return saveBuildingEditor.Connect(_subscriber); }
- /// \brief Connect a Gazebo event to the save signal
+ /// \brief Disconnect a Gazebo event from the save signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectSaveBuildingEditor(
+ event::ConnectionPtr _subscriber)
+ { saveBuildingEditor.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the save as signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -269,19 +275,13 @@ namespace gazebo
(T _subscriber)
{ return saveAsBuildingEditor.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the save signal
- /// \param[in] _subscriber the subscriber to this event
- public: static void DisconnectSaveBuildingEditor(
- event::ConnectionPtr _subscriber)
- { saveBuildingEditor.Disconnect(_subscriber); }
-
- /// \brief Disconnect a Gazebo event from the save as signal
+ /// \brief Disconnect a Gazebo event from the save as signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectSaveAsBuildingEditor(
event::ConnectionPtr _subscriber)
{ saveAsBuildingEditor.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the new signal
+ /// \brief Connect a Gazebo event to the new signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -289,13 +289,13 @@ namespace gazebo
ConnectNewBuildingEditor(T _subscriber)
{ return newBuildingEditor.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the new signal
+ /// \brief Disconnect a Gazebo event from the new signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectNewBuildingEditor(
event::ConnectionPtr _subscriber)
{ newBuildingEditor.Disconnect(_subscriber); }
- /// \brief Connect a Gazebo event to the exit signal
+ /// \brief Connect a Gazebo event to the exit signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -308,10 +308,7 @@ namespace gazebo
event::ConnectionPtr _subscriber)
{ exitBuildingEditor.Disconnect(_subscriber); }
- /// \brief Toggle if the edit mode was checked or not.
- public: static event::EventT<void (bool)> toggleEditMode;
-
- /// \brief Connect a Gazebo event to the name changed signal
+ /// \brief Connect a Gazebo event to the name changed signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
@@ -319,12 +316,15 @@ namespace gazebo
(T _subscriber)
{ return buildingNameChanged.Connect(_subscriber); }
- /// \brief Disconnect a Gazebo event from the exit signal
+ /// \brief Disconnect a Gazebo event from the name changed signal.
/// \param[in] _subscriber the subscriber to this event
- public: static void ConnectBuildingNameChanged(
+ public: static void DisconnectBuildingNameChanged(
event::ConnectionPtr _subscriber)
{ buildingNameChanged.Disconnect(_subscriber); }
+ /// \brief Toggle if the edit mode was checked or not.
+ public: static event::EventT<void (bool)> toggleEditMode;
+
/// \brief An editor item is to be created
public: static event::EventT<void (std::string)>
createBuildingEditorItem;
@@ -336,8 +336,7 @@ namespace gazebo
public: static event::EventT<void (QString)> textureSelected;
/// \brief A model has been saved with a name and a location
- public: static event::EventT<void (std::string, std::string)>
- saveBuildingModel;
+ public: static event::EventT<void (std::string)> saveBuildingModel;
/// \brief A model has been completed and uploaded onto the server.
public: static event::EventT<void ()> finishBuildingModel;
@@ -374,10 +373,10 @@ namespace gazebo
public: static event::EventT<void (double)> changeBuildingEditorZoom;
/// \brief Save the model
- public: static event::EventT<bool (std::string)> saveBuildingEditor;
+ public: static event::EventT<bool ()> saveBuildingEditor;
/// \brief Save the model as
- public: static event::EventT<bool (std::string)> saveAsBuildingEditor;
+ public: static event::EventT<bool ()> saveAsBuildingEditor;
/// \brief Make a new model
public: static event::EventT<void ()> newBuildingEditor;
diff --git a/gazebo/gui/building/BuildingEditorPalette.cc b/gazebo/gui/building/BuildingEditorPalette.cc
index 9143cb8..438dd59 100644
--- a/gazebo/gui/building/BuildingEditorPalette.cc
+++ b/gazebo/gui/building/BuildingEditorPalette.cc
@@ -70,7 +70,10 @@ BuildingEditorPalette::BuildingEditorPalette(QWidget *_parent)
wallButton->setText("Wall");
wallButton->setIconSize(QSize(iconSize));
wallButton->setToolTip("Hold Shift to override snapping");
- connect(wallButton, SIGNAL(clicked()), this, SLOT(OnDrawWall()));
+ this->dataPtr->brushIdToModeMap["wall"] =
+ this->dataPtr->brushes->buttons().size();
+ this->dataPtr->brushes->addButton(wallButton,
+ this->dataPtr->brushes->buttons().size());
// Features label
QLabel *featuresLabel = new QLabel(tr(
@@ -85,7 +88,10 @@ BuildingEditorPalette::BuildingEditorPalette(QWidget *_parent)
windowButton->setIcon(QPixmap(":/images/window.svg"));
windowButton->setText("Window");
windowButton->setIconSize(QSize(iconSize));
- connect(windowButton, SIGNAL(clicked()), this, SLOT(OnAddWindow()));
+ this->dataPtr->brushIdToModeMap["window"] =
+ this->dataPtr->brushes->buttons().size();
+ this->dataPtr->brushes->addButton(windowButton,
+ this->dataPtr->brushes->buttons().size());
// Door button
QToolButton *doorButton = new QToolButton(this);
@@ -96,7 +102,10 @@ BuildingEditorPalette::BuildingEditorPalette(QWidget *_parent)
doorButton->setIcon(QPixmap(":/images/door.svg"));
doorButton->setText("Door");
doorButton->setIconSize(QSize(iconSize));
- connect(doorButton, SIGNAL(clicked()), this, SLOT(OnAddDoor()));
+ this->dataPtr->brushIdToModeMap["door"] =
+ this->dataPtr->brushes->buttons().size();
+ this->dataPtr->brushes->addButton(doorButton,
+ this->dataPtr->brushes->buttons().size());
// Stairs button
QToolButton *stairsButton = new QToolButton(this);
@@ -107,7 +116,10 @@ BuildingEditorPalette::BuildingEditorPalette(QWidget *_parent)
stairsButton->setIcon(QPixmap(":/images/stairs.svg"));
stairsButton->setText("Stairs");
stairsButton->setIconSize(QSize(iconSize));
- connect(stairsButton, SIGNAL(clicked()), this, SLOT(OnAddStair()));
+ this->dataPtr->brushIdToModeMap["stairs"] =
+ this->dataPtr->brushes->buttons().size();
+ this->dataPtr->brushes->addButton(stairsButton,
+ this->dataPtr->brushes->buttons().size());
// Features layout
QGridLayout *featuresLayout = new QGridLayout;
@@ -136,10 +148,33 @@ BuildingEditorPalette::BuildingEditorPalette(QWidget *_parent)
QPixmap colorIcon(30, 30);
colorIcon.fill(this->dataPtr->colorList.at(i));
colorButton->setIcon(colorIcon);
- this->dataPtr->brushes->addButton(colorButton, i);
+ std::ostringstream colorStr;
+ colorStr << "color_" << i;
+ this->dataPtr->lastDefaultColor = colorStr.str();
+ this->dataPtr->brushIdToModeMap[this->dataPtr->lastDefaultColor] =
+ this->dataPtr->brushes->buttons().size();
+ this->dataPtr->brushes->addButton(colorButton,
+ this->dataPtr->brushes->buttons().size());
colorsLayout->addWidget(colorButton, 0, i);
}
+ this->dataPtr->customColorButton = new QPushButton("More");
+ this->dataPtr->customColorButton->setCheckable(true);
+ this->dataPtr->customColorButton->setChecked(false);
+ colorsLayout->addWidget(this->dataPtr->customColorButton, 1, 4, 1, 2);
+ this->dataPtr->brushIdToModeMap["color_custom"] =
+ this->dataPtr->brushes->buttons().size();
+ this->dataPtr->brushes->addButton(this->dataPtr->customColorButton,
+ this->dataPtr->brushes->buttons().size());
+
+ this->dataPtr->customColorDialog = new QColorDialog(Qt::green, this);
+ this->dataPtr->customColorDialog->setWindowModality(Qt::NonModal);
+ connect(this->dataPtr->customColorDialog,
+ SIGNAL(currentColorChanged(const QColor)),
+ this, SLOT(OnCustomColor(const QColor)));
+ connect(this->dataPtr->customColorDialog, SIGNAL(rejected()), this,
+ SLOT(CancelDrawModes()));
+
// Textures
QLabel *texturesLabel = new QLabel(tr(
"<font size=4 color='white'>Add Texture</font>"));
@@ -165,6 +200,11 @@ BuildingEditorPalette::BuildingEditorPalette(QWidget *_parent)
QSize(90, 90), Qt::IgnoreAspectRatio));
textureButton->setText(textureButtonTextList[i]);
textureButton->setIconSize(QSize(40, 40));
+ std::ostringstream textureStr;
+ textureStr << "texture_" << i;
+ this->dataPtr->lastDefaultTexture = textureStr.str();
+ this->dataPtr->brushIdToModeMap[this->dataPtr->lastDefaultTexture] =
+ this->dataPtr->brushes->buttons().size();
this->dataPtr->brushes->addButton(textureButton,
this->dataPtr->brushes->buttons().size());
texturesLayout->addWidget(textureButton, 0, i);
@@ -177,7 +217,10 @@ BuildingEditorPalette::BuildingEditorPalette(QWidget *_parent)
importImageButton->setChecked(false);
importImageButton->setToolTip(tr(
"Import an existing floor plan to use as a guide"));
- connect(importImageButton, SIGNAL(clicked()), this, SLOT(OnImportImage()));
+ this->dataPtr->brushIdToModeMap["image"] =
+ this->dataPtr->brushes->buttons().size();
+ this->dataPtr->brushes->addButton(importImageButton,
+ this->dataPtr->brushes->buttons().size());
QHBoxLayout *buttonsLayout = new QHBoxLayout;
buttonsLayout->addWidget(importImageButton);
@@ -206,7 +249,7 @@ BuildingEditorPalette::BuildingEditorPalette(QWidget *_parent)
// Connections
this->dataPtr->connections.push_back(
gui::editor::Events::ConnectSaveBuildingModel(
- boost::bind(&BuildingEditorPalette::OnSaveModel, this, _1, _2)));
+ boost::bind(&BuildingEditorPalette::OnSaveModel, this, _1)));
this->dataPtr->connections.push_back(
gui::editor::Events::ConnectNewBuildingModel(
@@ -215,18 +258,6 @@ BuildingEditorPalette::BuildingEditorPalette(QWidget *_parent)
this->dataPtr->connections.push_back(
gui::editor::Events::ConnectCreateBuildingEditorItem(
boost::bind(&BuildingEditorPalette::OnCreateEditorItem, this, _1)));
-
- // All buttons must be added after the color and texture buttons
- this->dataPtr->brushes->addButton(wallButton,
- this->dataPtr->brushes->buttons().size());
- this->dataPtr->brushes->addButton(windowButton,
- this->dataPtr->brushes->buttons().size());
- this->dataPtr->brushes->addButton(doorButton,
- this->dataPtr->brushes->buttons().size());
- this->dataPtr->brushes->addButton(stairsButton,
- this->dataPtr->brushes->buttons().size());
- this->dataPtr->brushes->addButton(importImageButton,
- this->dataPtr->brushes->buttons().size());
}
/////////////////////////////////////////////////
@@ -243,12 +274,58 @@ std::string BuildingEditorPalette::GetModelName() const
}
/////////////////////////////////////////////////
+void BuildingEditorPalette::OnBrush(int _buttonId)
+{
+ if (_buttonId == this->dataPtr->brushIdToModeMap["wall"])
+ {
+ this->OnDrawWall();
+ }
+ else if (_buttonId == this->dataPtr->brushIdToModeMap["window"])
+ {
+ this->OnAddWindow();
+ }
+ else if (_buttonId == this->dataPtr->brushIdToModeMap["door"])
+ {
+ this->OnAddDoor();
+ }
+ else if (_buttonId == this->dataPtr->brushIdToModeMap["stairs"])
+ {
+ this->OnAddStair();
+ }
+ else if (_buttonId >= this->dataPtr->brushIdToModeMap["color_0"] &&
+ _buttonId <= this->dataPtr->brushIdToModeMap[
+ this->dataPtr->lastDefaultColor])
+ {
+ this->OnDefaultColor(_buttonId -
+ this->dataPtr->brushIdToModeMap["color_0"]);
+ }
+ else if (_buttonId == this->dataPtr->brushIdToModeMap["color_custom"])
+ {
+ this->OnCustomColorDialog();
+ }
+ else if (_buttonId >= this->dataPtr->brushIdToModeMap["texture_0"] &&
+ _buttonId <= this->dataPtr->brushIdToModeMap[
+ this->dataPtr->lastDefaultTexture])
+ {
+ this->OnTexture(_buttonId - this->dataPtr->brushIdToModeMap["texture_0"]);
+ }
+ else if (_buttonId == this->dataPtr->brushIdToModeMap["image"])
+ {
+ this->OnImportImage();
+ }
+ else
+ {
+ gzerr << "Requested brush doesn't exist." << std::endl;
+ }
+}
+
+/////////////////////////////////////////////////
void BuildingEditorPalette::OnDrawWall()
{
if (this->dataPtr->currentMode != "wall")
gui::editor::Events::createBuildingEditorItem("wall");
else
- gui::editor::Events::createBuildingEditorItem(std::string());
+ this->CancelDrawModes();
}
/////////////////////////////////////////////////
@@ -257,7 +334,7 @@ void BuildingEditorPalette::OnAddWindow()
if (this->dataPtr->currentMode != "window")
gui::editor::Events::createBuildingEditorItem("window");
else
- gui::editor::Events::createBuildingEditorItem(std::string());
+ this->CancelDrawModes();
}
/////////////////////////////////////////////////
@@ -266,7 +343,7 @@ void BuildingEditorPalette::OnAddDoor()
if (this->dataPtr->currentMode != "door")
gui::editor::Events::createBuildingEditorItem("door");
else
- gui::editor::Events::createBuildingEditorItem(std::string());
+ this->CancelDrawModes();
}
/////////////////////////////////////////////////
@@ -275,7 +352,7 @@ void BuildingEditorPalette::OnImportImage()
if (this->dataPtr->currentMode != "image")
gui::editor::Events::createBuildingEditorItem("image");
else
- gui::editor::Events::createBuildingEditorItem(std::string());
+ this->CancelDrawModes();
}
/////////////////////////////////////////////////
@@ -284,7 +361,7 @@ void BuildingEditorPalette::OnAddStair()
if (this->dataPtr->currentMode != "stairs")
gui::editor::Events::createBuildingEditorItem("stairs");
else
- gui::editor::Events::createBuildingEditorItem(std::string());
+ this->CancelDrawModes();
}
/////////////////////////////////////////////////
@@ -295,8 +372,7 @@ void BuildingEditorPalette::OnNewModel()
}
/////////////////////////////////////////////////
-void BuildingEditorPalette::OnSaveModel(const std::string &_saveName,
- const std::string &/*_saveLocation*/)
+void BuildingEditorPalette::OnSaveModel(const std::string &_saveName)
{
this->dataPtr->modelNameEdit->setText(tr(_saveName.c_str()));
}
@@ -320,7 +396,7 @@ void BuildingEditorPalette::OnCreateEditorItem(const std::string &_mode)
this->dataPtr->brushes->checkedButton()->setChecked(false);
this->dataPtr->brushes->setExclusive(true);
- this->dataPtr->currentMode = std::string();
+ this->dataPtr->currentMode.clear();
}
else
{
@@ -329,71 +405,90 @@ void BuildingEditorPalette::OnCreateEditorItem(const std::string &_mode)
}
/////////////////////////////////////////////////
-void BuildingEditorPalette::OnBrush(int _buttonId)
+void BuildingEditorPalette::OnDefaultColor(int _colorId)
{
- if (_buttonId < static_cast<int>(this->dataPtr->colorList.size()))
- {
- this->OnColor(_buttonId);
- }
- else if (_buttonId < static_cast<int>(this->dataPtr->colorList.size()) +
- static_cast<int>(this->dataPtr->textureList.size()))
+ std::ostringstream colorStr;
+ colorStr << "color_" << _colorId;
+ QColor color = this->dataPtr->colorList[_colorId];
+ if (this->dataPtr->currentMode != colorStr.str())
{
- this->OnTexture(_buttonId -
- static_cast<int>(this->dataPtr->colorList.size()));
+ this->dataPtr->currentMode = colorStr.str();
+ this->OnColor(color);
}
else
{
- gzwarn << "Brushes other than color and texture are handled elsewhere."
- << std::endl;
+ this->CancelDrawModes();
}
}
/////////////////////////////////////////////////
-void BuildingEditorPalette::OnColor(int _buttonId)
+void BuildingEditorPalette::OnCustomColorDialog()
{
- std::ostringstream colorStr;
- colorStr << "color_" << _buttonId;
- QColor color = this->dataPtr->colorList[_buttonId];
- if (this->dataPtr->currentMode != colorStr.str())
- {
- gui::editor::Events::colorSelected(color);
- this->dataPtr->currentMode = colorStr.str();
+ this->CancelDrawModes();
+ this->dataPtr->customColorButton->setChecked(true);
+ this->dataPtr->customColorDialog->show();
+}
- QPixmap colorCursor(30, 30);
- colorCursor.fill(color);
- QApplication::setOverrideCursor(QCursor(colorCursor));
+/////////////////////////////////////////////////
+void BuildingEditorPalette::OnCustomColor(const QColor _color)
+{
+ this->dataPtr->customColorButton->setChecked(true);
+ if (_color.isValid())
+ {
+ this->dataPtr->currentMode = "color_custom";
+ this->OnColor(_color);
}
else
{
- gui::editor::Events::createBuildingEditorItem(std::string());
+ this->CancelDrawModes();
}
}
/////////////////////////////////////////////////
-void BuildingEditorPalette::OnTexture(int _buttonId)
+void BuildingEditorPalette::OnColor(QColor _color)
+{
+ gui::editor::Events::colorSelected(_color);
+ QPixmap colorCursor(30, 30);
+ colorCursor.fill(_color);
+ QApplication::setOverrideCursor(QCursor(colorCursor));
+}
+
+/////////////////////////////////////////////////
+void BuildingEditorPalette::OnTexture(int _textureId)
{
std::ostringstream textureStr;
- textureStr << "texture_" << _buttonId;
- QString texture = this->dataPtr->textureList[_buttonId];
+ textureStr << "texture_" << _textureId;
+ QString texture = this->dataPtr->textureList[_textureId];
if (this->dataPtr->currentMode != textureStr.str())
{
gui::editor::Events::textureSelected(texture);
this->dataPtr->currentMode = textureStr.str();
- QPixmap textureCursor(this->dataPtr->textureList[_buttonId]);
+ QPixmap textureCursor(this->dataPtr->textureList[_textureId]);
textureCursor = textureCursor.scaled(QSize(30, 30), Qt::IgnoreAspectRatio,
Qt::SmoothTransformation);
QApplication::setOverrideCursor(textureCursor);
}
else
{
- gui::editor::Events::createBuildingEditorItem(std::string());
+ this->CancelDrawModes();
}
}
/////////////////////////////////////////////////
void BuildingEditorPalette::mousePressEvent(QMouseEvent * /*_event*/)
{
- // Cancel draw mode
+ this->CancelDrawModes();
+}
+
+/////////////////////////////////////////////////
+void BuildingEditorPalette::CancelDrawModes()
+{
gui::editor::Events::createBuildingEditorItem(std::string());
}
+
+/////////////////////////////////////////////////
+QColorDialog *BuildingEditorPalette::CustomColorDialog() const
+{
+ return this->dataPtr->customColorDialog;
+}
diff --git a/gazebo/gui/building/BuildingEditorPalette.hh b/gazebo/gui/building/BuildingEditorPalette.hh
index 460e88f..45bc278 100644
--- a/gazebo/gui/building/BuildingEditorPalette.hh
+++ b/gazebo/gui/building/BuildingEditorPalette.hh
@@ -35,7 +35,7 @@ namespace gazebo
/// \class BuildingEditorPalette BuildingEditorPalette.hh
/// \brief A palette of building items which can be added to the editor.
- class GAZEBO_VISIBLE BuildingEditorPalette : public QWidget
+ class GZ_GUI_BUILDING_VISIBLE BuildingEditorPalette : public QWidget
{
Q_OBJECT
@@ -50,42 +50,29 @@ namespace gazebo
/// \return Model name
public: std::string GetModelName() const;
- /// \brief Qt callback when the draw wall button is pressed.
- private slots: void OnDrawWall();
-
- /// \brief Qt callback when the draw window button is pressed.
- private slots: void OnAddWindow();
-
- /// \brief Qt callback when the draw door button is pressed.
- private slots: void OnAddDoor();
-
- /// \brief Qt callback when the import image button is pressed.
- private slots: void OnImportImage();
-
- /// \brief Qt callback when the draw stairs button is pressed.
- private slots: void OnAddStair();
+ /// \brief Get a pointer to the custom color dialog.
+ /// \return Pointer to the custom color dialog.
+ public: QColorDialog *CustomColorDialog() const;
/// \brief Qt callback when a brush is pressed.
/// \param[in] _buttonId Id of the button clicked.
private slots: void OnBrush(int _buttonId);
- /// \brief Qt callback when a color brush is pressed.
- /// \param[in] _buttonId Id of the button clicked.
- private slots: void OnColor(int _buttonId);
-
- /// \brief Qt callback when a texture brush is pressed.
- /// \param[in] _buttonId Id of the button clicked.
- private slots: void OnTexture(int _buttonId);
-
/// \brief Qt callback when the Model Name field is changed.
+ /// \param[in] _name New name.
private slots: void OnNameChanged(const QString &_name);
+ /// \brief Qt callback when custom color has been selected on the dialog.
+ /// \param[in] _color Selected color.
+ private slots: void OnCustomColor(const QColor _color);
+
+ /// \brief Cancel whatever is being drawn and uncheck all brushes.
+ private slots: void CancelDrawModes();
+
/// \brief Callback when user has provided information on where to save
/// the model to.
/// \param[in] _saveName Name of model being saved.
- /// \param[in] _saveLocation Location to save the model to.
- private: void OnSaveModel(const std::string &_saveName,
- const std::string &_saveLocation);
+ private: void OnSaveModel(const std::string &_saveName);
/// \brief Event received when an editor item is selected.
/// \param[in] _mode Type of item to add or empty for none.
@@ -98,6 +85,36 @@ namespace gazebo
/// \param[in] _event Event.
private: void mousePressEvent(QMouseEvent *_event);
+ /// \brief When the draw wall button is selected.
+ private: void OnDrawWall();
+
+ /// \brief When the draw window button is selected.
+ private: void OnAddWindow();
+
+ /// \brief When the draw door button is selected.
+ private: void OnAddDoor();
+
+ /// \brief When the draw stairs button is selected.
+ private: void OnAddStair();
+
+ /// \brief When a default color button is selected.
+ /// \param[in] _colorId Id of the color clicked.
+ private: void OnDefaultColor(int _colorId);
+
+ /// \brief Open a color dialog when the custom color button is clicked.
+ private: void OnCustomColorDialog();
+
+ /// \brief When any color is selected.
+ /// \param[in] _color Color selected.
+ private: void OnColor(QColor _color);
+
+ /// \brief When a default texture button is selected.
+ /// \param[in] _textureId Id of the texture clicked.
+ private: void OnTexture(int _textureId);
+
+ /// \brief When the import image button is selected.
+ private: void OnImportImage();
+
/// \internal
/// \brief Private data pointer
private: BuildingEditorPalettePrivate *dataPtr;
diff --git a/gazebo/gui/building/BuildingEditorPalettePrivate.hh b/gazebo/gui/building/BuildingEditorPalettePrivate.hh
index ba1ea02..d3da321 100644
--- a/gazebo/gui/building/BuildingEditorPalettePrivate.hh
+++ b/gazebo/gui/building/BuildingEditorPalettePrivate.hh
@@ -20,6 +20,7 @@
#include <vector>
#include <string>
+#include <map>
#include "gazebo/common/Events.hh"
#include "gazebo/gui/qt.h"
@@ -31,6 +32,21 @@ namespace gazebo
/// \brief Private data for BuildingEditorPalette
class BuildingEditorPalettePrivate
{
+ /// \brief Custom color dialog.
+ public: QColorDialog *customColorDialog;
+
+ /// \brief Custom color button.
+ public: QPushButton *customColorButton;
+
+ /// \brief Link each button ID to a draw mode.
+ public: std::map<std::string, int> brushIdToModeMap;
+
+ /// \brief Name of the last default texture mode.
+ public: std::string lastDefaultTexture;
+
+ /// \brief Name of the last default color mode.
+ public: std::string lastDefaultColor;
+
/// \brief Default name of the building model.
public: std::string buildingDefaultName;
diff --git a/gazebo/gui/building/BuildingEditorWidget.hh b/gazebo/gui/building/BuildingEditorWidget.hh
index c496c98..6da7f6e 100644
--- a/gazebo/gui/building/BuildingEditorWidget.hh
+++ b/gazebo/gui/building/BuildingEditorWidget.hh
@@ -34,7 +34,7 @@ namespace gazebo
/// \class BuildingEditorWidget BuildingEditorWidget.hh
/// \brief The parent widget of the building editor, level widget and scale
/// widget.
- class GAZEBO_VISIBLE BuildingEditorWidget : public QWidget
+ class GZ_GUI_BUILDING_VISIBLE BuildingEditorWidget : public QWidget
{
/// \brief Constructor
/// \param[in] _parent Parent QWidget.
diff --git a/gazebo/gui/building/BuildingItem.hh b/gazebo/gui/building/BuildingItem.hh
index e020618..4351ed8 100644
--- a/gazebo/gui/building/BuildingItem.hh
+++ b/gazebo/gui/building/BuildingItem.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \class BuildingItem BuildingItem.hh
/// \brief Base class of a items that have building level properties such as
/// the level number and level height.
- class GAZEBO_VISIBLE BuildingItem
+ class GZ_GUI_BUILDING_VISIBLE BuildingItem
{
/// \brief Constructor
public: BuildingItem();
diff --git a/gazebo/gui/building/BuildingMaker.cc b/gazebo/gui/building/BuildingMaker.cc
index 853b7c8..0da2f30 100644
--- a/gazebo/gui/building/BuildingMaker.cc
+++ b/gazebo/gui/building/BuildingMaker.cc
@@ -15,9 +15,14 @@
*
*/
+#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 <set>
-#include <boost/filesystem.hpp>
#include "gazebo/common/Exception.hh"
@@ -35,7 +40,6 @@
#include "gazebo/gui/KeyEventHandler.hh"
#include "gazebo/gui/MouseEventHandler.hh"
-#include "gazebo/common/SystemPaths.hh"
#ifdef HAVE_GTS
#include "gazebo/common/Mesh.hh"
#include "gazebo/common/MeshManager.hh"
@@ -43,7 +47,7 @@
#endif
#include "gazebo/gazebo_config.h"
-#include "gazebo/gui/building/FinishBuildingDialog.hh"
+#include "gazebo/gui/SaveDialog.hh"
#include "gazebo/gui/building/BuildingEditorEvents.hh"
#include "gazebo/gui/building/BuildingModelManip.hh"
#include "gazebo/gui/building/EditorItem.hh"
@@ -57,79 +61,8 @@ const std::string BuildingMaker::buildingDefaultName = "Untitled";
const std::string BuildingMaker::previewName = "BuildingPreview";
/////////////////////////////////////////////////
-// Helper function to generate a valid folder name from a human-readable model
-// name.
-std::string GetFolderNameFromModelName(const std::string &_modelName)
-{
- // Auto-generate folder name based on model name
- std::string foldername = _modelName;
-
- std::vector<std::pair<std::string, std::string> > replacePairs;
- replacePairs.push_back(std::pair<std::string, std::string>(" ", "_"));
-
- for (unsigned int i = 0; i < replacePairs.size(); ++i)
- {
- std::string forbiddenChar = replacePairs[i].first;
- std::string replaceChar = replacePairs[i].second;
- size_t index = foldername.find(forbiddenChar);
- while (index != std::string::npos)
- {
- foldername.replace(index, forbiddenChar.size(), replaceChar);
- index = foldername.find(forbiddenChar);
- }
- }
-
- return foldername;
-}
-
-/////////////////////////////////////////////////
-// Add the parent folder of _path to the model path represented by SystemPaths,
-// notify InsertModelWidget to display the model name in the "Insert Models"
-// tab, and write the parent folder filename to gui.ini
-void AddDirToModelPaths(const std::string& _path)
-{
- std::string parentDirectory = boost::filesystem::path(_path)
- .parent_path().string();
-
- std::list<std::string> modelPaths =
- gazebo::common::SystemPaths::Instance()->GetModelPaths();
- std::list<std::string>::iterator iter;
- for (iter = modelPaths.begin();
- iter != modelPaths.end(); ++iter)
- {
- if (iter->compare(parentDirectory) == 0)
- {
- break;
- }
- }
-
- gazebo::common::SystemPaths::Instance()->
- AddModelPathsUpdate(parentDirectory);
-
- std::string additionalProperties =
- gui::getINIProperty<std::string>("model_paths.filenames", "");
- if (additionalProperties.find(parentDirectory) == std::string::npos)
- {
- // Add it to gui.ini
- gui::setINIProperty("model_paths.filenames", parentDirectory);
-
- // Save any changes that were made to the property tree
- // TODO: check gui.ini env variable
- char *home = getenv("HOME");
- if (home)
- {
- boost::filesystem::path guiINIPath = home;
- guiINIPath = guiINIPath / ".gazebo" / "gui.ini";
- saveINI(guiINIPath);
- }
- }
-}
-
-/////////////////////////////////////////////////
BuildingMaker::BuildingMaker() : EntityMaker()
{
- this->modelName = this->buildingDefaultName;
-
this->conversionScale = 0.01;
// Counters are only used for giving visuals unique names.
@@ -147,10 +80,10 @@ BuildingMaker::BuildingMaker() : EntityMaker()
this->connections.push_back(
gui::editor::Events::ConnectSaveBuildingEditor(
- boost::bind(&BuildingMaker::OnSave, this, _1)));
+ boost::bind(&BuildingMaker::OnSave, this)));
this->connections.push_back(
gui::editor::Events::ConnectSaveAsBuildingEditor(
- boost::bind(&BuildingMaker::OnSaveAs, this, _1)));
+ boost::bind(&BuildingMaker::OnSaveAs, this)));
this->connections.push_back(
gui::editor::Events::ConnectNewBuildingEditor(
boost::bind(&BuildingMaker::OnNew, this)));
@@ -173,8 +106,9 @@ BuildingMaker::BuildingMaker() : EntityMaker()
gui::editor::Events::ConnectToggleEditMode(
boost::bind(&BuildingMaker::OnEdit, this, _1)));
- this->saveDialog =
- new FinishBuildingDialog(FinishBuildingDialog::MODEL_SAVE, 0);
+ this->saveDialog = new SaveDialog(SaveDialog::BUILDING);
+
+ this->Reset();
}
/////////////////////////////////////////////////
@@ -331,7 +265,7 @@ void BuildingMaker::DetachAllChildren(const std::string &_manip)
std::string BuildingMaker::CreateModel()
{
this->Reset();
- return this->modelName;
+ return this->folderName;
}
/////////////////////////////////////////////////
@@ -370,7 +304,7 @@ std::string BuildingMaker::AddWall(const QVector3D &_size,
visualName << this->previewName << "::" << linkName << "::Visual";
rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
linkVisual));
- sdf::ElementPtr visualElem = this->modelTemplateSDF->root
+ sdf::ElementPtr visualElem = this->modelTemplateSDF->Root()
->GetElement("model")->GetElement("link")->GetElement("visual");
visualElem->GetElement("material")->ClearElements();
visualElem->GetElement("material")->AddElement("ambient")
@@ -416,7 +350,7 @@ std::string BuildingMaker::AddWindow(const QVector3D &_size,
rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
linkVisual));
- sdf::ElementPtr visualElem = this->modelTemplateSDF->root
+ sdf::ElementPtr visualElem = this->modelTemplateSDF->Root()
->GetElement("model")->GetElement("link")->GetElement("visual");
visualElem->GetElement("material")->GetElement("script")->GetElement("name")
->Set("Gazebo/BuildingFrame");
@@ -462,7 +396,7 @@ std::string BuildingMaker::AddDoor(const QVector3D &_size,
rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
linkVisual));
- sdf::ElementPtr visualElem = this->modelTemplateSDF->root
+ sdf::ElementPtr visualElem = this->modelTemplateSDF->Root()
->GetElement("model")->GetElement("link")->GetElement("visual");
visualElem->GetElement("material")->GetElement("script")->GetElement("name")
->Set("Gazebo/BuildingFrame");
@@ -494,6 +428,13 @@ std::string BuildingMaker::AddStairs(const QVector3D &_size,
this->Reset();
}
+ if (_steps == 0)
+ {
+ gzerr << "Can't make stairs with 0 steps" << std::endl;
+ return "";
+ }
+
+ // Link visual
std::ostringstream linkNameStream;
linkNameStream << "Stairs_" << this->stairsCounter++;
std::string linkName = linkNameStream.str();
@@ -502,57 +443,53 @@ std::string BuildingMaker::AddStairs(const QVector3D &_size,
"::" + linkName, this->previewVisual));
linkVisual->Load();
+ // Size for the whole staircase as one thing
+ math::Vector3 totalSize = BuildingMaker::ConvertSize(_size);
+
+ // Parent visual which will act as a container for the all the steps
std::ostringstream visualName;
visualName << this->previewName << "::" << linkName << "::Visual";
rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
linkVisual));
+ visVisual->Load();
+ visVisual->SetPosition(math::Vector3(0, 0, totalSize.z/2.0));
+ visVisual->SetScale(totalSize);
- sdf::ElementPtr visualElem = this->modelTemplateSDF->root
+ // Visual SDF template (unit box)
+ sdf::ElementPtr visualElem = this->modelTemplateSDF->Root()
->GetElement("model")->GetElement("link")->GetElement("visual");
- visVisual->Load(visualElem);
- visVisual->DetachObjects();
-
- BuildingModelManip *stairsManip = new BuildingModelManip();
- stairsManip->SetMaker(this);
- stairsManip->SetName(linkName);
- stairsManip->SetVisual(visVisual);
- stairsManip->SetLevel(this->currentLevel);
- math::Vector3 scaledSize = BuildingMaker::ConvertSize(_size);
- visVisual->SetScale(scaledSize);
- double dSteps = static_cast<double>(_steps);
- visVisual->SetPosition(math::Vector3(0, 0, scaledSize.z/2.0));
- stairsManip->SetPose(_pos.x(), _pos.y(), _pos.z(), 0, 0, _angle);
- this->allItems[linkName] = stairsManip;
-
- std::stringstream visualStepName;
- visualStepName << visualName.str() << "step" << 0;
- rendering::VisualPtr baseStepVisual(new rendering::Visual(
- visualStepName.str(), visVisual));
visualElem->GetElement("material")->ClearElements();
visualElem->GetElement("material")->AddElement("ambient")
->Set(gazebo::common::Color(1, 1, 1));
visualElem->AddElement("cast_shadows")->Set(false);
- baseStepVisual->Load(visualElem);
+ // Relative size of each step within the parent visual
+ double dSteps = static_cast<double>(_steps);
double rise = 1.0 / dSteps;
double run = 1.0 / dSteps;
- baseStepVisual->SetScale(math::Vector3(1, run, rise));
- math::Vector3 baseOffset(0, 0.5 - run/2.0,
- -0.5 + rise/2.0);
- baseStepVisual->SetPosition(baseOffset);
-
- for (int i = 1; i < _steps; ++i)
+ for (int i = 0; i < _steps; ++i)
{
- visualStepName.str("");
+ std::stringstream visualStepName;
visualStepName << visualName.str() << "step" << i;
- rendering::VisualPtr stepVisual = baseStepVisual->Clone(
- visualStepName.str(), visVisual);
- stepVisual->SetPosition(math::Vector3(0, baseOffset.y-(run*i),
- baseOffset.z + rise*i));
- stepVisual->SetRotation(baseStepVisual->GetRotation());
+ rendering::VisualPtr stepVisual(new rendering::Visual(
+ visualStepName.str(), visVisual));
+ stepVisual->Load(visualElem);
+
+ stepVisual->SetPosition(math::Vector3(0, 0.5-run*(0.5+i),
+ -0.5 + rise*(0.5+i)));
+ stepVisual->SetScale(math::Vector3(1, run, rise));
}
+ // Stairs manip
+ BuildingModelManip *stairsManip = new BuildingModelManip();
+ stairsManip->SetMaker(this);
+ stairsManip->SetName(linkName);
+ stairsManip->SetVisual(visVisual);
+ stairsManip->SetLevel(this->currentLevel);
+ stairsManip->SetPose(_pos.x(), _pos.y(), _pos.z(), 0, 0, _angle);
+ this->allItems[linkName] = stairsManip;
+
linkVisual->SetVisibilityFlags(GZ_VISIBILITY_GUI |
GZ_VISIBILITY_SELECTABLE);
this->BuildingChanged();
@@ -583,7 +520,7 @@ std::string BuildingMaker::AddFloor(const QVector3D &_size,
rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
linkVisual));
- sdf::ElementPtr visualElem = this->modelTemplateSDF->root
+ sdf::ElementPtr visualElem = this->modelTemplateSDF->Root()
->GetElement("model")->GetElement("link")->GetElement("visual");
visualElem->GetElement("material")->ClearElements();
visualElem->GetElement("material")->AddElement("ambient")
@@ -669,10 +606,6 @@ void BuildingMaker::Reset()
this->currentSaveState = NEVER_SAVED;
this->SetModelName(this->buildingDefaultName);
- this->defaultPath = (QDir::homePath() + "/building_editor_models")
- .toStdString();
- this->saveLocation = defaultPath + "/" +
- GetFolderNameFromModelName(this->modelName);
this->previewVisual.reset(new rendering::Visual(this->previewName,
scene->GetWorldVisual()));
@@ -680,7 +613,6 @@ void BuildingMaker::Reset()
this->previewVisual->Load();
this->previewVisual->SetPose(math::Pose::Zero);
this->previewVisual->SetVisibilityFlags(GZ_VISIBILITY_GUI);
- scene->AddVisual(this->previewVisual);
std::map<std::string, BuildingModelManip *>::iterator it;
for (it = this->allItems.begin(); it != this->allItems.end(); ++it)
@@ -699,27 +631,18 @@ void BuildingMaker::SetModelName(const std::string &_modelName)
{
this->modelName = _modelName;
this->saveDialog->SetModelName(_modelName);
- this->BuildingChanged();
- // send to palette?
-}
-/////////////////////////////////////////////////
-void BuildingMaker::SaveToSDF(const std::string &_savePath)
-{
- std::ofstream savefile;
- boost::filesystem::path path(_savePath);
- path = path / "model.sdf";
+ this->folderName = this->saveDialog->
+ GetFolderNameFromModelName(this->modelName);
- // FIXME
- savefile.open(path.string().c_str());
- if (!savefile.is_open())
+ if (this->currentSaveState == NEVER_SAVED)
{
- gzerr << "Couldn't open file for writing: " << path.string() << std::endl;
- return;
+ // Set new saveLocation
+ boost::filesystem::path oldPath(this->saveDialog->GetSaveLocation());
+
+ boost::filesystem::path newPath = oldPath.parent_path() / this->folderName;
+ this->saveDialog->SetSaveLocation(newPath.string());
}
- savefile << this->modelSDF->ToString();
- savefile.close();
- gzdbg << "Saved file to " << path.string() << std::endl;
}
/////////////////////////////////////////////////
@@ -749,7 +672,7 @@ void BuildingMaker::GenerateSDF()
this->modelSDF.reset(new sdf::SDF);
this->modelSDF->SetFromString(this->GetTemplateSDFString());
- modelElem = this->modelSDF->root->GetElement("model");
+ modelElem = this->modelSDF->Root()->GetElement("model");
linkElem = modelElem->GetElement("link");
sdf::ElementPtr templateLinkElem = linkElem->Clone();
@@ -761,11 +684,14 @@ void BuildingMaker::GenerateSDF()
std::stringstream visualNameStream;
std::stringstream collisionNameStream;
- modelElem->GetAttribute("name")->Set(
- GetFolderNameFromModelName(this->modelName));
- math::Pose modelOrigin(
+ modelElem->GetAttribute("name")->Set(this->folderName);
+ math::Pose modelOrigin = math::Pose::Zero;
+ if (this->previewVisual)
+ {
+ modelOrigin = math::Pose(
this->previewVisual->GetBoundingBox().GetCenter().x,
this->previewVisual->GetBoundingBox().GetCenter().y, 0, 0, 0, 0);
+ }
modelElem->GetElement("pose")->Set(modelOrigin);
std::map<std::string, BuildingModelManip *>::iterator itemsIt;
@@ -1071,7 +997,7 @@ void BuildingMaker::GenerateSDFWithCSG()
this->modelSDF.reset(new sdf::SDF);
this->modelSDF->SetFromString(this->GetTemplateSDFString());
- modelElem = this->modelSDF->root->GetElement("model");
+ modelElem = this->modelSDF->Root()->GetElement("model");
linkElem = modelElem->GetElement("link");
sdf::ElementPtr templateLinkElem = linkElem->Clone();
@@ -1083,7 +1009,7 @@ void BuildingMaker::GenerateSDFWithCSG()
std::stringstream visualNameStream;
std::stringstream collisionNameStream;
- modelElem->GetAttribute("name")->Set(this->modelName);
+ modelElem->GetAttribute("name")->Set(this->folderName);
std::map<std::string, BuildingModelManip *>::iterator itemsIt;
for (itemsIt = this->allItems.begin(); itemsIt != this->allItems.end();
@@ -1126,14 +1052,14 @@ void BuildingMaker::GenerateSDFWithCSG()
continue;
for (unsigned int j = 0; j < subMesh->GetVertexCount(); ++j)
{
- m1SubMesh->AddVertex(subMesh->GetVertex(j));
+ m1SubMesh->AddVertex(subMesh->Vertex(j));
}
for (unsigned int j = 0; j < subMesh->GetIndexCount(); ++j)
{
m1SubMesh->AddIndex(subMesh->GetIndex(j));
}
}
- m1->SetScale(math::Vector3(wallVis->GetScale()));
+ m1->SetScale(wallVis->GetScale().Ign());
std::string booleanMeshName = buildingModelManip->GetName() + "_Boolean";
common::Mesh *booleanMesh = NULL;
@@ -1167,18 +1093,18 @@ void BuildingMaker::GenerateSDFWithCSG()
continue;
for (unsigned int j = 0; j < subMesh->GetVertexCount(); ++j)
{
- m2SubMesh->AddVertex(subMesh->GetVertex(j));
+ m2SubMesh->AddVertex(subMesh->Vertex(j));
}
for (unsigned int j = 0; j < subMesh->GetIndexCount(); ++j)
{
m2SubMesh->AddIndex(subMesh->GetIndex(j));
}
}
- m2->SetScale(math::Vector3(attachedVis->GetScale()));
+ m2->SetScale(attachedVis->GetScale().Ign());
// create csg but don't add to mesh manager just yet
common::MeshCSG csg;
booleanMesh = csg.CreateBoolean(m1, m2, common::MeshCSG::DIFFERENCE,
- offset);
+ offset.Ign());
}
}
// add to mesh manager after all boolean operations are done
@@ -1194,7 +1120,7 @@ void BuildingMaker::GenerateSDFWithCSG()
visGeomElem->ClearElements();
sdf::ElementPtr meshElem = visGeomElem->AddElement("mesh");
// TODO create the folder
- std::string uri = "model://" + this->modelName + "/meshes/"
+ std::string uri = "model://" + this->folderName + "/meshes/"
+ booleanMeshName;
meshElem->GetElement("uri")->Set(uri);
visualElem->GetElement("pose")->Set(visual->GetPose());
@@ -1240,15 +1166,15 @@ void BuildingMaker::GenerateSDFWithCSG()
/////////////////////////////////////////////////
void BuildingMaker::CreateTheEntity()
{
- msgs::Factory msg;
- // Create a new name if the model exists
- if (!this->modelSDF->root->HasElement("model"))
+ if (!this->modelSDF->Root()->HasElement("model"))
{
gzerr << "Generated invalid SDF! Cannot create entity." << std::endl;
return;
}
- sdf::ElementPtr modelElem = this->modelSDF->root->GetElement("model");
+ msgs::Factory msg;
+ // Create a new name if the model exists
+ sdf::ElementPtr modelElem = this->modelSDF->Root()->GetElement("model");
std::string modelElemName = modelElem->Get<std::string>("name");
if (has_entity_name(modelElemName))
{
@@ -1310,24 +1236,6 @@ double BuildingMaker::ConvertAngle(double _angle)
}
/////////////////////////////////////////////////
-std::string BuildingMaker::GetTemplateConfigString()
-{
- std::ostringstream newModelStr;
- newModelStr << "<?xml version=\"1.0\"?>"
- << "<model>"
- << "<name>building_template_model</name>"
- << "<version>1.0</version>"
- << "<sdf version=\"1.5\">model.sdf</sdf>"
- << "<author>"
- << "<name>author_name</name>"
- << "<email>author_email</email>"
- << "</author>"
- << "<description>Made with the Gazebo Building Editor</description>"
- << "</model>";
- return newModelStr.str();
-}
-
-/////////////////////////////////////////////////
std::string BuildingMaker::GetTemplateSDFString()
{
std::ostringstream newModelStr;
@@ -1584,8 +1492,10 @@ void BuildingMaker::OnNew()
}
QString msg;
QMessageBox msgBox(QMessageBox::Warning, QString("New"), msg);
- QPushButton *cancelButton = msgBox.addButton("Cancel", QMessageBox::YesRole);
- QPushButton *saveButton = msgBox.addButton("Save", QMessageBox::YesRole);
+ QPushButton *cancelButton = msgBox.addButton("Cancel",
+ QMessageBox::RejectRole);
+ msgBox.setEscapeButton(cancelButton);
+ QPushButton *saveButton = new QPushButton("Save");
switch (this->currentSaveState)
{
@@ -1593,8 +1503,9 @@ void BuildingMaker::OnNew()
{
msg.append("Are you sure you want to close this model and open a new "
"canvas?\n\n");
- msgBox.addButton("New Canvas", QMessageBox::ApplyRole);
- saveButton->hide();
+ QPushButton *newButton =
+ msgBox.addButton("New Canvas", QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(newButton);
break;
}
case UNSAVED_CHANGES:
@@ -1602,7 +1513,9 @@ void BuildingMaker::OnNew()
{
msg.append("You have unsaved changes. Do you want to save this model "
"and open a new canvas?\n\n");
- msgBox.addButton("Don't Save", QMessageBox::ApplyRole);
+ msgBox.addButton("Don't Save", QMessageBox::DestructiveRole);
+ msgBox.addButton(saveButton, QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(saveButton);
break;
}
default:
@@ -1619,7 +1532,7 @@ void BuildingMaker::OnNew()
{
if (msgBox.clickedButton() == saveButton)
{
- if (!this->OnSave(this->modelName))
+ if (!this->OnSave())
{
return;
}
@@ -1632,114 +1545,28 @@ void BuildingMaker::OnNew()
void BuildingMaker::SaveModelFiles()
{
- this->SetModelName(this->modelName);
- this->GenerateConfig();
- this->SaveToConfig(this->saveLocation);
+ this->saveDialog->GenerateConfig();
+ this->saveDialog->SaveToConfig();
this->GenerateSDF();
- this->SaveToSDF(this->saveLocation);
+ this->saveDialog->SaveToSDF(this->modelSDF);
this->currentSaveState = ALL_SAVED;
}
/////////////////////////////////////////////////
-void BuildingMaker::GenerateConfig()
-{
- // Create an xml config file
- this->modelConfig.Clear();
- this->modelConfig.Parse(this->GetTemplateConfigString().c_str());
-
- TiXmlElement *modelXML = this->modelConfig.FirstChildElement("model");
- if (!modelXML)
- {
- gzerr << "No model name in default config file\n";
- return;
- }
- TiXmlElement *modelNameXML = modelXML->FirstChildElement("name");
- modelNameXML->FirstChild()->SetValue(this->modelName);
-
- TiXmlElement *versionXML = modelXML->FirstChildElement("version");
- if (!versionXML)
- {
- gzerr << "Couldn't find model version" << std::endl;
- versionXML->FirstChild()->SetValue("1.0");
- }
- else
- {
- versionXML->FirstChild()->SetValue(this->version);
- }
-
- TiXmlElement *descriptionXML = modelXML->FirstChildElement("description");
- if (!descriptionXML)
- {
- gzerr << "Couldn't find model description" << std::endl;
- descriptionXML->FirstChild()->SetValue("");
- }
- else
- {
- descriptionXML->FirstChild()->SetValue(this->description);
- }
-
- // TODO: Multiple authors
- TiXmlElement *authorXML = modelXML->FirstChildElement("author");
- if (!authorXML)
- {
- gzerr << "Couldn't find model author" << std::endl;
- }
- else
- {
- TiXmlElement *authorChild = authorXML->FirstChildElement("name");
- if (!authorChild)
- {
- gzerr << "Couldn't find author name" << std::endl;
- authorChild->FirstChild()->SetValue("");
- }
- else
- {
- authorChild->FirstChild()->SetValue(this->authorName);
- }
- authorChild = authorXML->FirstChildElement("email");
- if (!authorChild)
- {
- gzerr << "Couldn't find author email" << std::endl;
- authorChild->FirstChild()->SetValue("");
- }
- else
- {
- authorChild->FirstChild()->SetValue(this->authorEmail);
- }
- }
-}
-
-/////////////////////////////////////////////////
-void BuildingMaker::SaveToConfig(const std::string &_savePath)
+bool BuildingMaker::OnSave()
{
- boost::filesystem::path path(_savePath);
- path = path / "model.config";
- const char* modelConfigString = path.string().c_str();
-
- this->modelConfig.SaveFile(modelConfigString);
- gzdbg << "Saved file to " << modelConfigString << std::endl;
-}
-
-/////////////////////////////////////////////////
-bool BuildingMaker::OnSave(const std::string &_saveName)
-{
- if (_saveName != "")
- this->SetModelName(_saveName);
-
switch (this->currentSaveState)
{
case UNSAVED_CHANGES:
{
// TODO: Subtle filesystem race condition
this->SaveModelFiles();
- AddDirToModelPaths(this->saveLocation);
- gui::editor::Events::saveBuildingModel(this->modelName,
- this->saveLocation);
+ gui::editor::Events::saveBuildingModel(this->modelName);
return true;
}
case NEVER_SAVED:
{
- return this->OnSaveAs(_saveName);
+ return this->OnSaveAs();
}
default:
return false;
@@ -1747,89 +1574,18 @@ bool BuildingMaker::OnSave(const std::string &_saveName)
}
/////////////////////////////////////////////////
-bool BuildingMaker::OnSaveAs(const std::string &_saveName)
+bool BuildingMaker::OnSaveAs()
{
- this->saveDialog->SetModelName(_saveName);
-
- if (this->saveLocation.length() > 0)
- {
- this->saveDialog->SetSaveLocation(this->saveLocation);
- }
- if (this->saveDialog->exec() == QDialog::Accepted)
+ if (this->saveDialog->OnSaveAs())
{
- if (this->saveDialog->GetModelName().size() == 0)
- {
- QMessageBox msgBox(QMessageBox::Warning, QString("Empty Name"),
- QString("Please give your model a non-empty name."));
-
- msgBox.exec();
- return this->OnSaveAs(_saveName);
- }
- if (this->saveDialog->GetSaveLocation().size() == 0)
- {
- QMessageBox msgBox(QMessageBox::Warning, QString("Empty Location"),
- QString("Please give a path to where your model will be saved."));
-
- msgBox.exec();
- return this->OnSaveAs(_saveName);
- }
-
- this->modelName = this->saveDialog->GetModelName();
- this->saveLocation = this->saveDialog->GetSaveLocation();
- this->authorName = this->saveDialog->GetAuthorName();
- this->authorEmail = this->saveDialog->GetAuthorEmail();
- this->description = this->saveDialog->GetDescription();
- this->version = this->saveDialog->GetVersion();
-
- if (this->modelName.compare(this->buildingDefaultName) == 0)
- {
- // Parse saveLocation and set model name
- boost::filesystem::path saveLocPath(this->saveLocation);
- this->SetModelName(saveLocPath.filename().string());
- }
-
- boost::filesystem::path path;
- path = path / this->saveLocation;
- if (!boost::filesystem::exists(path))
- {
- if (!boost::filesystem::create_directories(path))
- {
- gzerr << "Couldn't create folder for model files." << std::endl;
- return false;
- }
- gzmsg << "Created folder " << path << " for model files." << std::endl;
- }
-
- boost::filesystem::path modelConfigPath = path / "model.config";
-
- boost::filesystem::path sdfPath = path / "model.sdf";
-
- // Before writing
- if (boost::filesystem::exists(sdfPath) ||
- boost::filesystem::exists(modelConfigPath))
- {
- std::string msg = "A model named " + this->modelName +
- " already exists in folder " + path.string() + ".\n\n"
- "Do you wish to overwrite the existing model files?\n";
-
- QMessageBox msgBox(QMessageBox::Warning, QString("Files Exist"),
- QString(msg.c_str()));
-
- QPushButton *saveButton = msgBox.addButton("Save",
- QMessageBox::ApplyRole);
- msgBox.addButton(QMessageBox::Cancel);
- msgBox.exec();
- if (msgBox.clickedButton() != saveButton)
- {
- return this->OnSaveAs(this->modelName);
- }
- }
-
+ // Prevent changing save location
+ this->currentSaveState = ALL_SAVED;
+ // Get name set by user
+ this->SetModelName(this->saveDialog->GetModelName());
+ // Update name on palette
+ gui::editor::Events::saveBuildingModel(this->modelName);
+ // Generate and save files
this->SaveModelFiles();
-
- AddDirToModelPaths(this->saveLocation);
-
- gui::editor::Events::saveBuildingModel(this->modelName, this->saveLocation);
return true;
}
return false;
@@ -1839,21 +1595,9 @@ bool BuildingMaker::OnSaveAs(const std::string &_saveName)
void BuildingMaker::OnNameChanged(const std::string &_name)
{
if (_name.compare(this->modelName) == 0)
- {
return;
- }
- this->SetModelName(_name);
-
- if (this->currentSaveState == NEVER_SAVED)
- {
- // Set new saveLocation
- boost::filesystem::path oldPath(this->saveLocation);
-
- boost::filesystem::path newPath = oldPath.parent_path() /
- GetFolderNameFromModelName(_name);
- this->saveLocation = newPath.string();
- }
+ this->SetModelName(_name);
this->BuildingChanged();
}
@@ -1876,8 +1620,14 @@ void BuildingMaker::OnExit()
"your building will no longer be editable.\n\n"
"Are you ready to exit?\n\n");
QMessageBox msgBox(QMessageBox::NoIcon, QString("Exit"), msg);
- msgBox.addButton("Exit", QMessageBox::ApplyRole);
- QPushButton *cancelButton = msgBox.addButton(QMessageBox::Cancel);
+
+ QPushButton *cancelButton = msgBox.addButton("Cancel",
+ QMessageBox::RejectRole);
+ QPushButton *exitButton =
+ msgBox.addButton("Exit", QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(exitButton);
+ msgBox.setEscapeButton(cancelButton);
+
msgBox.exec();
if (msgBox.clickedButton() == cancelButton)
{
@@ -1895,17 +1645,20 @@ void BuildingMaker::OnExit()
QMessageBox msgBox(QMessageBox::NoIcon, QString("Exit"), msg);
QPushButton *cancelButton = msgBox.addButton("Cancel",
- QMessageBox::ApplyRole);
+ QMessageBox::RejectRole);
+ msgBox.addButton("Don't Save, Exit", QMessageBox::DestructiveRole);
QPushButton *saveButton = msgBox.addButton("Save and Exit",
- QMessageBox::ApplyRole);
- msgBox.addButton("Don't Save, Exit", QMessageBox::ApplyRole);
+ QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(cancelButton);
+ msgBox.setDefaultButton(saveButton);
+
msgBox.exec();
if (msgBox.clickedButton() == cancelButton)
return;
if (msgBox.clickedButton() == saveButton)
{
- if (!this->OnSave(this->modelName))
+ if (!this->OnSave())
{
return;
}
@@ -1947,7 +1700,7 @@ bool BuildingMaker::On3dMouseMove(const common::MouseEvent &_event)
if (!userCamera)
return false;
- if (_event.dragging)
+ if (_event.Dragging())
{
userCamera->HandleMouseEvent(_event);
return true;
@@ -1960,7 +1713,7 @@ bool BuildingMaker::On3dMouseMove(const common::MouseEvent &_event)
return true;
}
- rendering::VisualPtr vis = userCamera->GetVisual(_event.pos);
+ rendering::VisualPtr vis = userCamera->GetVisual(_event.Pos());
// Highlight visual on hover
if (vis)
{
@@ -2008,6 +1761,7 @@ bool BuildingMaker::On3dMouseMove(const common::MouseEvent &_event)
else if (this->selectedTexture == ":bricks.png")
material = "Gazebo/Bricks";
+ // Must set material before color, otherwise color is overwritten
this->hoverVis->SetMaterial(material);
this->hoverVis->SetAmbient((*it).second->GetColor());
}
@@ -2040,7 +1794,15 @@ bool BuildingMaker::On3dMousePress(const common::MouseEvent &_event)
/////////////////////////////////////////////////
bool BuildingMaker::On3dMouseRelease(const common::MouseEvent &_event)
{
- if (_event.button != common::MouseEvent::LEFT)
+ rendering::UserCameraPtr userCamera = gui::get_active_camera();
+
+ if (_event.Dragging())
+ {
+ userCamera->HandleMouseEvent(_event);
+ return true;
+ }
+
+ if (_event.Button() != common::MouseEvent::LEFT)
{
this->StopMaterialModes();
return true;
@@ -2074,7 +1836,6 @@ bool BuildingMaker::On3dMouseRelease(const common::MouseEvent &_event)
}
else
{
- rendering::UserCameraPtr userCamera = gui::get_active_camera();
userCamera->HandleMouseEvent(_event);
this->StopMaterialModes();
}
@@ -2119,8 +1880,9 @@ void BuildingMaker::ResetHoverVis()
else
{
BuildingModelManip *manip = this->allItems[hoverName];
- this->hoverVis->SetAmbient(manip->GetColor());
+ // Must set material before color, otherwise color is overwritten
this->hoverVis->SetMaterial(manip->GetTexture());
+ this->hoverVis->SetAmbient(manip->GetColor());
this->hoverVis->SetTransparency(manip->GetTransparency());
}
this->hoverVis.reset();
diff --git a/gazebo/gui/building/BuildingMaker.hh b/gazebo/gui/building/BuildingMaker.hh
index 4cd7665..4cc54c3 100644
--- a/gazebo/gui/building/BuildingMaker.hh
+++ b/gazebo/gui/building/BuildingMaker.hh
@@ -42,14 +42,14 @@ namespace gazebo
class EntityMaker;
class EditorItem;
class BuildingModelManip;
- class FinishBuildingDialog;
+ class SaveDialog;
/// \addtogroup gazebo_gui
/// \{
/// \class BuildingMaker BuildingMaker.hh
/// \brief Create and manage 3D visuals of a building.
- class GAZEBO_VISIBLE BuildingMaker : public EntityMaker
+ class GZ_GUI_BUILDING_VISIBLE BuildingMaker : public EntityMaker
{
/// \enum SaveState
/// \brief Save states for the building editor.
@@ -208,14 +208,6 @@ namespace gazebo
/// \return Angle in radians.
public: static double ConvertAngle(double _angle);
- /// \brief Save model to SDF format.
- /// \param[in] _savePath Path to save the SDF to.
- public: void SaveToSDF(const std::string &_savePath);
-
- /// \brief Save config file.
- /// \param[in] _savePath Path to save the file to.
- public: void SaveToConfig(const std::string &_savePath);
-
/// \brief Reset the building maker and the SDF.
public: void Reset();
@@ -228,9 +220,6 @@ namespace gazebo
/// \brief Generate the SDF from building part visuals.
public: void GenerateSDF();
- /// \brief Generate the config file.
- public: void GenerateConfig();
-
// Documentation inherited
public: virtual bool IsActive() const;
@@ -253,9 +242,6 @@ namespace gazebo
/// \brief Get a template SDF string of a simple model.
private: std::string GetTemplateSDFString();
- /// \brief Get a template config file for a simple model.
- private: std::string GetTemplateConfigString();
-
/// \brief Internal helper function for QPointF comparison used by the
/// surface subsivision algorithm.
private: static bool PointCompareY(const QPointF &_a, const QPointF &_b);
@@ -281,14 +267,12 @@ namespace gazebo
private: void SaveModelFiles();
/// \brief Callback for saving the model.
- /// \param[in] _saveName Name to save the model.
/// \return True if the user chose to save, false if the user cancelled.
- private: bool OnSave(const std::string &_saveName = "");
+ private: bool OnSave();
/// \brief Callback for selecting a folder and saving the model.
- /// \param[in] _saveName Name to save the model.
/// \return True if the user chose to save, false if the user cancelled.
- private: bool OnSaveAs(const std::string &_saveName);
+ private: bool OnSaveAs();
/// \brief Callback for when the name is changed through the Palette.
/// \param[in] _modelName The newly entered building name.
@@ -355,12 +339,12 @@ namespace gazebo
/// \brief A template SDF of a simple box model.
private: sdf::SDFPtr modelTemplateSDF;
- /// \brief The building model's config file.
- private: TiXmlDocument modelConfig;
-
/// \brief Name of the building model.
private: std::string modelName;
+ /// \brief Folder name, which is the model name without spaces.
+ private: std::string folderName;
+
/// \brief Name of the building model preview.
private: static const std::string previewName;
@@ -385,24 +369,6 @@ namespace gazebo
/// \brief Store the current save state of the model.
private: enum SaveState currentSaveState;
- /// \brief Default directory to save models: ~/building_editor_models
- private: std::string defaultPath;
-
- /// \brief Path to where the model is saved.
- private: std::string saveLocation;
-
- /// \brief Name of the building model's author.
- private: std::string authorName;
-
- /// \brief Name of the building model's author's email.
- private: std::string authorEmail;
-
- /// \brief Model description.
- private: std::string description;
-
- /// \brief Model version.
- private: std::string version;
-
/// \brief A list of gui editor events connected to the building maker.
private: std::vector<event::ConnectionPtr> connections;
@@ -410,7 +376,7 @@ namespace gazebo
private: static const std::string buildingDefaultName;
/// \brief A dialog for setting building model name and save location.
- private: FinishBuildingDialog *saveDialog;
+ private: SaveDialog *saveDialog;
/// \brief Visual that is currently hovered over by the mouse.
private: rendering::VisualPtr hoverVis;
diff --git a/gazebo/gui/building/BuildingModelManip.cc b/gazebo/gui/building/BuildingModelManip.cc
index 4199451..0e7e6e5 100644
--- a/gazebo/gui/building/BuildingModelManip.cc
+++ b/gazebo/gui/building/BuildingModelManip.cc
@@ -406,6 +406,8 @@ void BuildingModelManip::SetTexture(QString _texture)
// BuildingModelManip and BuildingMaker handle material names,
// Inspectors and palette handle thumbnail uri
this->visual->SetMaterial(this->texture);
+ // Must set color after texture otherwise it gets overwritten
+ this->visual->SetAmbient(this->color);
this->maker->BuildingChanged();
emit TextureChanged(_texture);
}
diff --git a/gazebo/gui/building/BuildingModelManip.hh b/gazebo/gui/building/BuildingModelManip.hh
index 00d7c69..2445e30 100644
--- a/gazebo/gui/building/BuildingModelManip.hh
+++ b/gazebo/gui/building/BuildingModelManip.hh
@@ -38,7 +38,7 @@ namespace gazebo
/// \class BuildingModelManip BuildingModelManip.hh
/// \brief Manipulate a 3D visual associated to a 2D editor item.
- class GAZEBO_VISIBLE BuildingModelManip : public QObject
+ class GZ_GUI_BUILDING_VISIBLE BuildingModelManip : public QObject
{
Q_OBJECT
diff --git a/gazebo/gui/building/CMakeLists.txt b/gazebo/gui/building/CMakeLists.txt
index 1d1a61d..bdcdb00 100644
--- a/gazebo/gui/building/CMakeLists.txt
+++ b/gazebo/gui/building/CMakeLists.txt
@@ -2,6 +2,7 @@ include (${gazebo_cmake_dir}/GazeboUtils.cmake)
include (${QT_USE_FILE})
set (sources
+ BaseInspectorDialog.cc
BuildingEditor.cc
BuildingEditorEvents.cc
BuildingEditorPalette.cc
@@ -12,7 +13,6 @@ set (sources
DoorItem.cc
EditorItem.cc
EditorView.cc
- FinishBuildingDialog.cc
FloorItem.cc
GrabberHandle.cc
GridLines.cc
@@ -34,13 +34,13 @@ set (sources
)
set (qt_headers
+ BaseInspectorDialog.hh
BuildingEditor.hh
BuildingEditorPalette.hh
BuildingModelManip.hh
DoorItem.hh
EditorItem.hh
EditorView.hh
- FinishBuildingDialog.hh
FloorItem.hh
ImportImageDialog.hh
ImportImageView.hh
@@ -74,13 +74,20 @@ QT4_ADD_RESOURCES(resources_RCC ${resources})
include_directories(${CMAKE_CURRENT_BINARY_DIR})
-gz_add_library(gazebo_gui_building ${sources} ${headers_MOC} ${headers} ${resources_RCC})
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_GUI_BUILDING")
+
+if (WIN32)
+ add_library(gazebo_gui_building STATIC ${sources} ${headers_MOC} ${headers} ${resources_RCC})
+ target_link_libraries(gazebo_gui_building ${general_libraries})
+else()
+ gz_add_library(gazebo_gui_building ${sources} ${headers_MOC} ${headers} ${resources_RCC})
+endif()
target_link_libraries(gazebo_gui_building
- gazebo_common
- gazebo_transport
gazebo_rendering
+ gazebo_transport
gazebo_msgs
+ gazebo_common
${QT_LIBRARIES})
gz_install_library(gazebo_gui_building)
diff --git a/gazebo/gui/building/DoorItem.hh b/gazebo/gui/building/DoorItem.hh
index 1fc0a95..d3ab928 100644
--- a/gazebo/gui/building/DoorItem.hh
+++ b/gazebo/gui/building/DoorItem.hh
@@ -36,7 +36,8 @@ namespace gazebo
/// \class DoorItem DoorItem.hh
/// \brief 2D representation of a door
- class GAZEBO_VISIBLE DoorItem : public RectItem, public BuildingItem
+ class GZ_GUI_BUILDING_VISIBLE DoorItem :
+ public RectItem, public BuildingItem
{
Q_OBJECT
diff --git a/gazebo/gui/building/EditorItem.hh b/gazebo/gui/building/EditorItem.hh
index 43ae190..bbec529 100644
--- a/gazebo/gui/building/EditorItem.hh
+++ b/gazebo/gui/building/EditorItem.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class EditorItem EditorItem.hh
/// \brief Base class of an item in the editor.
- class GAZEBO_VISIBLE EditorItem : public QObject
+ class GZ_GUI_BUILDING_VISIBLE EditorItem : public QObject
{
Q_OBJECT
diff --git a/gazebo/gui/building/EditorView.cc b/gazebo/gui/building/EditorView.cc
index 6eb4150..3f05c1b 100644
--- a/gazebo/gui/building/EditorView.cc
+++ b/gazebo/gui/building/EditorView.cc
@@ -203,7 +203,10 @@ void EditorView::contextMenuEvent(QContextMenuEvent *_event)
/////////////////////////////////////////////////
void EditorView::wheelEvent(QWheelEvent *_event)
{
- int numSteps = (_event->delta()/8) / 15;
+ int wheelIncr = 120;
+ int sign = (_event->delta() > 0) ? 1 : -1;
+ int delta = std::max(std::abs(_event->delta()), wheelIncr) * sign;
+ int numSteps = delta / wheelIncr;
QMatrix mat = matrix();
QPointF mousePosition = _event->pos();
@@ -1280,8 +1283,8 @@ void EditorView::OnOpenLevelInspector()
if (floorItem)
{
this->levelInspector->floorWidget->show();
- this->levelInspector->SetFloorColor(floorItem->Get3dColor());
- this->levelInspector->SetFloorTexture(floorItem->Get3dTexture());
+ this->levelInspector->SetColor(floorItem->Get3dColor());
+ this->levelInspector->SetTexture(floorItem->Get3dTexture());
}
else
{
@@ -1302,8 +1305,8 @@ void EditorView::OnLevelApply()
FloorItem *floorItem = this->levels[this->currentLevel]->floorItem;
if (floorItem)
{
- floorItem->Set3dTexture(dialog->GetFloorTexture());
- floorItem->Set3dColor(dialog->GetFloorColor());
+ floorItem->Set3dTexture(dialog->GetTexture());
+ floorItem->Set3dColor(dialog->GetColor());
floorItem->Set3dTransparency(0.4);
floorItem->FloorChanged();
}
diff --git a/gazebo/gui/building/EditorView.hh b/gazebo/gui/building/EditorView.hh
index 0a15c8d..7b9e299 100644
--- a/gazebo/gui/building/EditorView.hh
+++ b/gazebo/gui/building/EditorView.hh
@@ -42,7 +42,7 @@ namespace gazebo
/// \class Level EditorView.hh
/// \brief A convenient structure for storing level information
- class GAZEBO_VISIBLE Level
+ class GZ_GUI_BUILDING_VISIBLE Level
{
/// \brief Constructor
public: Level() : level(0), name("level"), baseHeight(0),
@@ -75,7 +75,7 @@ namespace gazebo
/// \class EditorView EditorView.hh
/// \brief Control the editor view and manage contents in the editor scene.
- class GAZEBO_VISIBLE EditorView : public QGraphicsView
+ class GZ_GUI_BUILDING_VISIBLE EditorView : public QGraphicsView
{
Q_OBJECT
diff --git a/gazebo/gui/building/FinishBuildingDialog.cc b/gazebo/gui/building/FinishBuildingDialog.cc
deleted file mode 100644
index ea620da..0000000
--- a/gazebo/gui/building/FinishBuildingDialog.cc
+++ /dev/null
@@ -1,260 +0,0 @@
-/*
- * Copyright (C) 2012-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/gui/building/FinishBuildingDialog.hh"
-
-using namespace gazebo;
-using namespace gui;
-
-/////////////////////////////////////////////////
-FinishBuildingDialog::FinishBuildingDialog(int _mode, QWidget *_parent)
- : QDialog(_parent)
-{
- this->setObjectName("finishBuildingDialog");
-
- if (_mode == MODEL_FINISH)
- this->setWindowTitle(tr("Finish Model"));
- else if (_mode == MODEL_SAVE)
- this->setWindowTitle(tr("Save Model"));
-
- this->messageLabel = new QLabel;
- if (_mode == MODEL_FINISH)
- {
- this->messageLabel->setText(
- tr("Before we finalize your model, please make sure that\n"
- "the following information is correct:\n"));
- }
- else if (_mode == MODEL_SAVE)
- {
- this->messageLabel->setText(
- tr("Pick a location for your model \"Untitled\":\n"));
- }
-
- QLabel *modelLabel = new QLabel;
- modelLabel->setText(tr("Model Name: "));
- this->modelNameLineEdit = new QLineEdit;
-
- QLabel *modelHeader = new QLabel;
- modelHeader->setText(tr("<b>Model</b>"));
-
- QLabel *modelLocation = new QLabel;
- modelLocation->setText(tr(" Location:"));
- this->modelLocationLineEdit = new QLineEdit;
- // Try to get path to home folder
- this->modelLocationLineEdit->setText(QDir::homePath()+
- "/building_editor_models/Untitled");
-
- QPushButton *browseButton = new QPushButton(tr("Browse"));
- connect(browseButton, SIGNAL(clicked()), this, SLOT(OnBrowse()));
-
- QLabel *authorHeader = new QLabel;
- authorHeader->setText(tr("<b>Author</b>"));
- QLabel *modelAuthorName = new QLabel;
- modelAuthorName->setText(tr(" Name:"));
- this->modelAuthorNameLineEdit = new QLineEdit;
- QLabel *modelAuthorEmail = new QLabel;
- modelAuthorEmail->setText(tr(" Email:"));
- this->modelAuthorEmailLineEdit = new QLineEdit;
-
- QLabel *modelVersion = new QLabel;
- modelVersion->setText(tr(" Version:"));
- this->modelVersionLineEdit = new QLineEdit;
- this->modelVersionLineEdit->setText(tr("1.0"));
-
- QLabel *modelDescription = new QLabel;
- modelDescription->setText(tr(" Description:"));
- this->modelDescriptionLineEdit = new QLineEdit;
-
-
-/* QString contributeText(
- tr("Contribute this model to the Model Database so that\n"
- "the entire Gazebo community can benefit!\n"
- "[This will open up a new tab in your browser]\n"));
- QCheckBox *contributeCheckBox = new QCheckBox(contributeText);*/
-
- QHBoxLayout *buttonsLayout = new QHBoxLayout;
- QPushButton *cancelButton = new QPushButton(tr("&Cancel"));
- connect(cancelButton, SIGNAL(clicked()), this, SLOT(OnCancel()));
-
- std::string finishButtonText = "&Done";
- if (_mode == MODEL_SAVE)
- finishButtonText = "&Save";
-
- QPushButton *finishButton = new QPushButton(tr(finishButtonText.c_str()));
- finishButton->setDefault(true);
- connect(finishButton, SIGNAL(clicked()), this, SLOT(OnFinish()));
- buttonsLayout->addWidget(finishButton);
- buttonsLayout->addWidget(cancelButton);
- buttonsLayout->setAlignment(Qt::AlignRight);
-
- QHBoxLayout *locationLayout = new QHBoxLayout;
-
- locationLayout->addWidget(modelLocation);
- locationLayout->addWidget(this->modelLocationLineEdit);
- locationLayout->addWidget(browseButton);
-
- QRadioButton *advancedOptionsCollapser = new QRadioButton();
- advancedOptionsCollapser->setChecked(false);
- advancedOptionsCollapser->setText("Advanced Options");
- advancedOptionsCollapser->setStyleSheet(
- "QRadioButton {\
- color: #d0d0d0;\
- }\
- QRadioButton::indicator::unchecked {\
- image: url(:/images/right_arrow.png);\
- }\
- QRadioButton::indicator::checked {\
- image: url(:/images/down_arrow.png);\
- }");
- // initialize as "closed" (unchecked)
- // Button behavior: when "open", show advancedOptionsGrid
- connect(advancedOptionsCollapser, SIGNAL(toggled(bool)), this,
- SLOT(ToggleAdvancedOptions(bool)));
-
- QHBoxLayout *advancedOptions = new QHBoxLayout();
- advancedOptions->addWidget(advancedOptionsCollapser);
-
- // Advanced options
- QGridLayout *advancedOptionsGrid = new QGridLayout();
- advancedOptionsGrid->addWidget(modelLabel, 0, 0);
- advancedOptionsGrid->addWidget(modelNameLineEdit, 0, 1);
-
- advancedOptionsGrid->addWidget(modelHeader, 1, 0);
- advancedOptionsGrid->addWidget(modelVersion, 2, 0);
- advancedOptionsGrid->addWidget(this->modelVersionLineEdit, 2, 1);
- advancedOptionsGrid->addWidget(modelDescription, 3, 0);
- advancedOptionsGrid->addWidget(this->modelDescriptionLineEdit, 3, 1);
-
- advancedOptionsGrid->addWidget(authorHeader, 4, 0);
- advancedOptionsGrid->addWidget(modelAuthorName, 5, 0);
- advancedOptionsGrid->addWidget(this->modelAuthorNameLineEdit, 5, 1);
- advancedOptionsGrid->addWidget(modelAuthorEmail, 6, 0);
- advancedOptionsGrid->addWidget(this->modelAuthorEmailLineEdit, 6, 1);
-
- this->advancedOptionsWidget = new QWidget();
- this->advancedOptionsWidget->setLayout(advancedOptionsGrid);
- this->advancedOptionsWidget->hide();
-
- QVBoxLayout *mainLayout = new QVBoxLayout;
- mainLayout->addWidget(this->messageLabel);
- mainLayout->addLayout(locationLayout);
-
- mainLayout->addLayout(advancedOptions);
- mainLayout->addWidget(this->advancedOptionsWidget);
-// if (_mode == MODEL_FINISH)
-// mainLayout->addWidget(contributeCheckBox);
- mainLayout->addLayout(buttonsLayout);
- mainLayout->setAlignment(Qt::AlignTop);
-
- this->setLayout(mainLayout);
- this->setMinimumSize(400, 150);
- this->setMaximumSize(400, 380);
- this->resize(this->minimumSize());
-}
-
-/////////////////////////////////////////////////
-FinishBuildingDialog::~FinishBuildingDialog()
-{
-}
-
-/////////////////////////////////////////////////
-std::string FinishBuildingDialog::GetModelName() const
-{
- return this->modelNameLineEdit->text().toStdString();
-}
-
-/////////////////////////////////////////////////
-std::string FinishBuildingDialog::GetSaveLocation() const
-{
- return this->modelLocationLineEdit->text().toStdString();
-}
-
-/////////////////////////////////////////////////
-std::string FinishBuildingDialog::GetAuthorName() const
-{
- return this->modelAuthorNameLineEdit->text().toStdString();
-}
-
-/////////////////////////////////////////////////
-std::string FinishBuildingDialog::GetAuthorEmail() const
-{
- return this->modelAuthorEmailLineEdit->text().toStdString();
-}
-
-/////////////////////////////////////////////////
-std::string FinishBuildingDialog::GetDescription() const
-{
- return this->modelDescriptionLineEdit->text().toStdString();
-}
-
-/////////////////////////////////////////////////
-std::string FinishBuildingDialog::GetVersion() const
-{
- return this->modelVersionLineEdit->text().toStdString();
-}
-
-/////////////////////////////////////////////////
-void FinishBuildingDialog::SetModelName(const std::string &_name)
-{
- this->modelNameLineEdit->setText(tr(_name.c_str()));
- std::string label = "Pick a location for your model \""
- + _name + "\":\n";
- this->messageLabel->setText(QString(label.c_str()));
-}
-
-/////////////////////////////////////////////////
-void FinishBuildingDialog::SetSaveLocation(const std::string &_location)
-{
- this->modelLocationLineEdit->setText(tr(_location.c_str()));
-}
-
-/////////////////////////////////////////////////
-void FinishBuildingDialog::OnBrowse()
-{
- QString dir = QFileDialog::getExistingDirectory(this, tr("Open Directory"),
- QDir::homePath(), QFileDialog::ShowDirsOnly
- | QFileDialog::DontResolveSymlinks);
- if (!dir.isEmpty())
- this->modelLocationLineEdit->setText(dir);
-}
-
-/////////////////////////////////////////////////
-void FinishBuildingDialog::OnCancel()
-{
- this->close();
-}
-
-/////////////////////////////////////////////////
-void FinishBuildingDialog::OnFinish()
-{
- this->accept();
-}
-
-/////////////////////////////////////////////////
-void FinishBuildingDialog::ToggleAdvancedOptions(bool _checked)
-{
- if (_checked)
- {
- this->advancedOptionsWidget->show();
- this->resize(this->maximumSize());
- }
- else
- {
- this->advancedOptionsWidget->hide();
- this->resize(this->minimumSize());
- }
-}
diff --git a/gazebo/gui/building/FinishBuildingDialog.hh b/gazebo/gui/building/FinishBuildingDialog.hh
deleted file mode 100644
index e76310f..0000000
--- a/gazebo/gui/building/FinishBuildingDialog.hh
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * Copyright (C) 2012-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 _FINISH_BUILDING_DIALOG_HH_
-#define _FINISH_BUILDING_DIALOG_HH_
-
-#include <string>
-#include "gazebo/gui/qt.h"
-#include "gazebo/util/system.hh"
-
-namespace gazebo
-{
- namespace gui
- {
- /// \addtogroup gazebo_gui
- /// \{
-
- /// \class FinishBuildingDialog FinishBuildingDialog.hh
- /// \brief Dialog for saving the building model.
- class GAZEBO_VISIBLE FinishBuildingDialog : public QDialog
- {
- Q_OBJECT
-
- /// \enum FinishMode
- /// \brief Unique identifiers for all dialog modes.
- public: enum FinishMode {
- /// \brief Finish mode
- MODEL_FINISH,
- /// \brief Save mode
- MODEL_SAVE
- };
-
- /// \brief Constructor
- /// \param[in] _mode Mode of the dialog
- /// \param[in] _parent Parent QWidget.
- public: FinishBuildingDialog(int _mode = 0, QWidget *_parent = 0);
-
- /// \brief Destructor
- public: ~FinishBuildingDialog();
-
- /// \brief Get the building model name.
- /// \return The model name.
- public: std::string GetModelName() const;
-
- /// \brief Get the save location.
- /// \return Path of the save location.
- public: std::string GetSaveLocation() const;
-
- /// \brief Set the building model name.
- /// \param[in] _name Name to set the model to.
- public: void SetModelName(const std::string &_name);
-
- /// \brief Set the save location.
- /// \param[in] _location Location to save to.
- public: void SetSaveLocation(const std::string &_location);
-
- /// \brief Get the model's author's name.
- /// \return The author's name.
- public: std::string GetAuthorName() const;
-
- /// \brief Get the model's author's email.
- /// \return The author's email.
- public: std::string GetAuthorEmail() const;
-
- /// \brief Get the model's description.
- /// \return The model's description.
- public: std::string GetDescription() const;
-
- /// \brief Get the model's version.
- /// \return The model's version.
- public: std::string GetVersion() const;
-
- /// \brief Qt callback when the file directory browse button is pressed.
- private slots: void OnBrowse();
-
- /// \brief Qt callback when the Cancel button is pressed.
- private slots: void OnCancel();
-
- /// \brief Qt callback when the Done button is pressed.
- private slots: void OnFinish();
-
- /// \brief Qt callback to show/hide advanced model saving options.
- private slots: void ToggleAdvancedOptions(bool _checked);
-
- /// \brief Widget container to hold advanced model saving options.
- private: QWidget *advancedOptionsWidget;
-
- /// \brief Label appearing at the top of the dialog box.
- private: QLabel *messageLabel;
-
- /// \brief Editable line that holds the model name.
- private: QLineEdit* modelNameLineEdit;
-
- /// \brief Editable line that holds the model's version.
- private: QLineEdit* modelVersionLineEdit;
-
- /// \brief Editable line that holds the model's description.
- private: QLineEdit* modelDescriptionLineEdit;
-
- /// \brief Editable line that holds the model's author's name.
- private: QLineEdit* modelAuthorNameLineEdit;
-
- /// \brief Editable line that holds the model's author's email.
- private: QLineEdit* modelAuthorEmailLineEdit;
-
- /// \brief Editable line that holds the model's save location.
- private: QLineEdit* modelLocationLineEdit;
- };
- /// \}
- }
-}
-
-#endif
diff --git a/gazebo/gui/building/FloorItem.hh b/gazebo/gui/building/FloorItem.hh
index 8c5ee25..2e59237 100644
--- a/gazebo/gui/building/FloorItem.hh
+++ b/gazebo/gui/building/FloorItem.hh
@@ -36,7 +36,8 @@ namespace gazebo
/// \class FloorItem FloorItem.hh
/// \brief 2D representation of a floor.
- class GAZEBO_VISIBLE FloorItem : public RectItem, public BuildingItem
+ class GZ_GUI_BUILDING_VISIBLE FloorItem :
+ public RectItem, public BuildingItem
{
Q_OBJECT
diff --git a/gazebo/gui/building/GrabberHandle.hh b/gazebo/gui/building/GrabberHandle.hh
index f8f7ee3..3b1a363 100644
--- a/gazebo/gui/building/GrabberHandle.hh
+++ b/gazebo/gui/building/GrabberHandle.hh
@@ -26,7 +26,7 @@ namespace gazebo
{
namespace gui
{
- class GAZEBO_VISIBLE GrabberHandle : public QGraphicsItem
+ class GZ_GUI_BUILDING_VISIBLE GrabberHandle : public QGraphicsItem
{
/// \brief Constructor
/// \param[in] _parent Parent graphics item
diff --git a/gazebo/gui/building/GridLines.hh b/gazebo/gui/building/GridLines.hh
index 9415a65..0478e99 100644
--- a/gazebo/gui/building/GridLines.hh
+++ b/gazebo/gui/building/GridLines.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \class GridLines GridLines.hh
/// \brief 2D grid lines.
- class GAZEBO_VISIBLE GridLines : public QGraphicsItem
+ class GZ_GUI_BUILDING_VISIBLE GridLines : public QGraphicsItem
{
/// \brief Constructor
/// \param[in] _width Width of grid lines in pixels.
diff --git a/gazebo/gui/building/ImportImageDialog.cc b/gazebo/gui/building/ImportImageDialog.cc
index cb0933c..fbaab61 100644
--- a/gazebo/gui/building/ImportImageDialog.cc
+++ b/gazebo/gui/building/ImportImageDialog.cc
@@ -217,12 +217,17 @@ void ImportImageDialog::OnBack()
/////////////////////////////////////////////////
void ImportImageDialog::OnSelectFile()
{
- std::string filename = QFileDialog::getOpenFileName(this,
- tr("Open Image"), "",
- tr("Image Files (*.png *.jpg *.jpeg)")).toStdString();
+ QFileDialog fileDialog(this, tr("Open Image"), QDir::homePath(),
+ tr("Image Files (*.png *.jpg *.jpeg)"));
- if (!filename.empty())
+ if (fileDialog.exec() == QDialog::Accepted)
{
+ QStringList selected = fileDialog.selectedFiles();
+ if (selected.empty())
+ return;
+
+ std::string filename = selected[0].toStdString();
+
this->SetFileName(QString::fromStdString(filename));
this->importImageView->SetImage(filename);
diff --git a/gazebo/gui/building/ImportImageDialog.hh b/gazebo/gui/building/ImportImageDialog.hh
index 3c6d3d7..6983921 100644
--- a/gazebo/gui/building/ImportImageDialog.hh
+++ b/gazebo/gui/building/ImportImageDialog.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \addtogroup gazebo_gui
/// \{
- class GAZEBO_VISIBLE ImportImageDialog : public QDialog
+ class GZ_GUI_BUILDING_VISIBLE ImportImageDialog : public QDialog
{
Q_OBJECT
diff --git a/gazebo/gui/building/ImportImageView.hh b/gazebo/gui/building/ImportImageView.hh
index 5332b37..8a77562 100644
--- a/gazebo/gui/building/ImportImageView.hh
+++ b/gazebo/gui/building/ImportImageView.hh
@@ -39,7 +39,7 @@ namespace gazebo
/// \class ImportImageView ImportImageView.hh
/// \brief Control the import image view and manage contents in the scene.
- class GAZEBO_VISIBLE ImportImageView : public QGraphicsView
+ class GZ_GUI_BUILDING_VISIBLE ImportImageView : public QGraphicsView
{
Q_OBJECT
diff --git a/gazebo/gui/building/LevelInspectorDialog.cc b/gazebo/gui/building/LevelInspectorDialog.cc
index 2205703..6c49529 100644
--- a/gazebo/gui/building/LevelInspectorDialog.cc
+++ b/gazebo/gui/building/LevelInspectorDialog.cc
@@ -15,13 +15,15 @@
*
*/
+#include "gazebo/common/Assert.hh"
#include "gazebo/gui/building/LevelInspectorDialog.hh"
using namespace gazebo;
using namespace gui;
/////////////////////////////////////////////////
-LevelInspectorDialog::LevelInspectorDialog(QWidget *_parent) : QDialog(_parent)
+LevelInspectorDialog::LevelInspectorDialog(QWidget *_parent)
+ : BaseInspectorDialog(_parent)
{
this->setObjectName("levelInspectorDialog");
this->setWindowTitle(tr("Level Inspector"));
@@ -31,55 +33,21 @@ LevelInspectorDialog::LevelInspectorDialog(QWidget *_parent) : QDialog(_parent)
this->levelNameLineEdit = new QLineEdit;
this->levelNameLineEdit->setPlaceholderText(tr("Level X"));
- QLabel *floorColorLabel = new QLabel(tr("Floor Color: "));
- this->floorColorComboBox = new QComboBox;
- this->floorColorComboBox->setIconSize(QSize(15, 15));
- this->floorColorComboBox->setMinimumWidth(50);
- this->floorColorComboBox->setSizePolicy(QSizePolicy::Fixed,
- QSizePolicy::Fixed);
- QPixmap floorColorIcon(15, 15);
- this->floorColorList.push_back(QColor(255, 255, 255, 255));
- this->floorColorList.push_back(QColor(194, 169, 160, 255));
- this->floorColorList.push_back(QColor(235, 206, 157, 255));
- this->floorColorList.push_back(QColor(254, 121, 5, 255));
- this->floorColorList.push_back(QColor(255, 195, 78, 255));
- this->floorColorList.push_back(QColor(111, 203, 172, 255));
- for (unsigned int i = 0; i < this->floorColorList.size(); ++i)
- {
- floorColorIcon.fill(this->floorColorList.at(i));
- this->floorColorComboBox->addItem(floorColorIcon, QString(""));
- }
-
- QHBoxLayout *floorColorLayout = new QHBoxLayout;
- floorColorLayout->addWidget(floorColorLabel);
- floorColorLayout->addWidget(floorColorComboBox);
-
- QLabel *floorTextureLabel = new QLabel(tr("Floor Texture: "));
- this->floorTextureComboBox = new QComboBox;
- this->floorTextureComboBox->setIconSize(QSize(30, 30));
- this->floorTextureComboBox->setMinimumWidth(50);
- this->floorTextureComboBox->setMinimumHeight(50);
- this->floorTextureComboBox->setSizePolicy(QSizePolicy::Fixed,
- QSizePolicy::Fixed);
- this->floorTextureList.push_back(":wood.jpg");
- this->floorTextureList.push_back(":tiles.jpg");
- this->floorTextureList.push_back(":bricks.png");
- for (unsigned int i = 0; i < this->floorTextureList.size(); ++i)
- {
- this->floorTextureComboBox->addItem(QPixmap(this->floorTextureList[i])
- .scaled(QSize(90, 90), Qt::IgnoreAspectRatio), QString(""));
- }
- this->floorTextureComboBox->addItem("X");
- this->floorTextureComboBox->setCurrentIndex(
- this->floorTextureComboBox->count()-1);
-
- QHBoxLayout *floorTextureLayout = new QHBoxLayout;
- floorTextureLayout->addWidget(floorTextureLabel);
- floorTextureLayout->addWidget(floorTextureComboBox);
+ this->InitColorComboBox();
+ QHBoxLayout *colorLayout = new QHBoxLayout;
+ QLabel *colorLabel = new QLabel(tr("Floor Color: "));
+ colorLayout->addWidget(colorLabel);
+ colorLayout->addWidget(this->colorComboBox);
+
+ this->InitTextureComboBox();
+ QHBoxLayout *textureLayout = new QHBoxLayout;
+ QLabel *textureLabel = new QLabel(tr("Floor Texture: "));
+ textureLayout->addWidget(textureLabel);
+ textureLayout->addWidget(this->textureComboBox);
QVBoxLayout *floorLayout = new QVBoxLayout;
- floorLayout->addLayout(floorColorLayout);
- floorLayout->addLayout(floorTextureLayout);
+ floorLayout->addLayout(colorLayout);
+ floorLayout->addLayout(textureLayout);
this->floorWidget = new QWidget;
this->floorWidget->setLayout(floorLayout);
@@ -147,26 +115,6 @@ double LevelInspectorDialog::GetHeight() const
}
/////////////////////////////////////////////////
-QColor LevelInspectorDialog::GetFloorColor() const
-{
- return this->floorColorList[this->floorColorComboBox->currentIndex()];
-}
-
-/////////////////////////////////////////////////
-QString LevelInspectorDialog::GetFloorTexture() const
-{
- QString floorTexture = QString("");
- if (this->floorTextureComboBox->currentIndex() != -1 &&
- this->floorTextureComboBox->currentIndex() <
- this->floorTextureComboBox->count() - 1)
- {
- floorTexture = this->floorTextureList[
- this->floorTextureComboBox->currentIndex()];
- }
- return floorTexture;
-}
-
-/////////////////////////////////////////////////
void LevelInspectorDialog::SetLevelName(const std::string &_levelName)
{
this->levelNameLineEdit->setText(QString(_levelName.c_str()));
@@ -180,38 +128,6 @@ void LevelInspectorDialog::SetHeight(double _height)
}
/////////////////////////////////////////////////
-void LevelInspectorDialog::SetFloorColor(const QColor _color)
-{
- // Find index corresponding to color (only a few colors allowed so far)
- int index = 0;
- for (unsigned int i = 0; i < this->floorColorList.size(); ++i)
- {
- if (this->floorColorList[i] == _color)
- {
- index = i;
- break;
- }
- }
- this->floorColorComboBox->setCurrentIndex(index);
-}
-
-/////////////////////////////////////////////////
-void LevelInspectorDialog::SetFloorTexture(QString _floorTexture)
-{
- // Find index corresponding to texture (only a few textures allowed so far)
- int index = this->floorTextureComboBox->count()-1;
- for (unsigned int i = 0; i < this->floorTextureList.size(); ++i)
- {
- if (this->floorTextureList[i] == _floorTexture)
- {
- index = i;
- break;
- }
- }
- this->floorTextureComboBox->setCurrentIndex(index);
-}
-
-/////////////////////////////////////////////////
void LevelInspectorDialog::OnCancel()
{
this->close();
diff --git a/gazebo/gui/building/LevelInspectorDialog.hh b/gazebo/gui/building/LevelInspectorDialog.hh
index 54d0921..f0f38c6 100644
--- a/gazebo/gui/building/LevelInspectorDialog.hh
+++ b/gazebo/gui/building/LevelInspectorDialog.hh
@@ -22,6 +22,7 @@
#include <vector>
#include "gazebo/gui/qt.h"
#include "gazebo/util/system.hh"
+#include "gazebo/gui/building/BaseInspectorDialog.hh"
namespace gazebo
{
@@ -32,7 +33,8 @@ namespace gazebo
/// \class LevelInspectorDialog LevelInspectorDialog.hh
/// \brief Dialog for configuring a building level
- class GAZEBO_VISIBLE LevelInspectorDialog : public QDialog
+ class GZ_GUI_BUILDING_VISIBLE LevelInspectorDialog
+ : public BaseInspectorDialog
{
Q_OBJECT
@@ -51,14 +53,6 @@ namespace gazebo
/// \return The level height in pixels.
public: double GetHeight() const;
- /// \brief Get the color of this level's floor.
- /// \return Floor color.
- public: QColor GetFloorColor() const;
-
- /// \brief Get the texture of this level's floor.
- /// \return Texture.
- public: QString GetFloorTexture() const;
-
/// \brief Set the name of the level.
/// \param[in] _levelName New level name.
public: void SetLevelName(const std::string &_levelName);
@@ -67,14 +61,6 @@ namespace gazebo
/// \param[in] _height Level height in pixels.
public: void SetHeight(double _height);
- /// \brief Set the color of this level's floor.
- /// \param[in] _color Color.
- public: void SetFloorColor(const QColor _color);
-
- /// \brief Set the texture of this level's floor.
- /// \param[in] _texture Texture.
- public: void SetFloorTexture(const QString _texture);
-
/// \brief Qt signal emitted to indicate that changes should be applied.
Q_SIGNALS: void Applied();
@@ -98,18 +84,6 @@ namespace gazebo
/// \brief Spin box for configuring the floor thickness.
private: QDoubleSpinBox *floorThicknessSpinBox;
-
- /// \brief Combo box for selecting the color of the floor.
- private: QComboBox *floorColorComboBox;
-
- /// \brief Vector of color options for the floor.
- private: std::vector<QColor> floorColorList;
-
- /// \brief Combo box for selecting the texture of the floor.
- private: QComboBox *floorTextureComboBox;
-
- /// \brief Vector of texture options.
- private: std::vector<QString> floorTextureList;
};
/// \}
}
diff --git a/gazebo/gui/building/LevelWidget.hh b/gazebo/gui/building/LevelWidget.hh
index d41a2c4..df7b055 100644
--- a/gazebo/gui/building/LevelWidget.hh
+++ b/gazebo/gui/building/LevelWidget.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class GridLines GridLines.hh
/// \brief A widget for adding and changing building levels.
- class GAZEBO_VISIBLE LevelWidget : public QWidget
+ class GZ_GUI_BUILDING_VISIBLE LevelWidget : public QWidget
{
Q_OBJECT
diff --git a/gazebo/gui/building/MeasureItem.hh b/gazebo/gui/building/MeasureItem.hh
index 0893d8a..550dabe 100644
--- a/gazebo/gui/building/MeasureItem.hh
+++ b/gazebo/gui/building/MeasureItem.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class MeasureItem MeasureItem.hh
/// \brief Measurement lines and values.
- class GAZEBO_VISIBLE MeasureItem : public SegmentItem
+ class GZ_GUI_BUILDING_VISIBLE MeasureItem : public SegmentItem
{
Q_OBJECT
diff --git a/gazebo/gui/building/RectItem.hh b/gazebo/gui/building/RectItem.hh
index e932d9e..670d04a 100644
--- a/gazebo/gui/building/RectItem.hh
+++ b/gazebo/gui/building/RectItem.hh
@@ -37,7 +37,8 @@ namespace gazebo
/// \class RectItem RectItem.hh
/// \brief 2D rectangle.
- class GAZEBO_VISIBLE RectItem : public EditorItem, public QGraphicsRectItem
+ class GZ_GUI_BUILDING_VISIBLE RectItem :
+ public EditorItem, public QGraphicsRectItem
{
Q_OBJECT
diff --git a/gazebo/gui/building/RotateHandle.hh b/gazebo/gui/building/RotateHandle.hh
index 7deb20a..cd66704 100644
--- a/gazebo/gui/building/RotateHandle.hh
+++ b/gazebo/gui/building/RotateHandle.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \class RotateHandle RotateHandle.hh
/// \brief Handle for rotating an editor item
- class GAZEBO_VISIBLE RotateHandle : public QGraphicsItem
+ class GZ_GUI_BUILDING_VISIBLE RotateHandle : public QGraphicsItem
{
/// \brief Constructor
/// param[in] _parent Parent graphics item.
diff --git a/gazebo/gui/building/ScaleWidget.hh b/gazebo/gui/building/ScaleWidget.hh
index b44bc02..49de375 100644
--- a/gazebo/gui/building/ScaleWidget.hh
+++ b/gazebo/gui/building/ScaleWidget.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class ScaleWidget ScaleWidget.hh
/// \brief Widget that displays the scale (zoom level) of the editor
- class GAZEBO_VISIBLE ScaleWidget : public QWidget
+ class GZ_GUI_BUILDING_VISIBLE ScaleWidget : public QWidget
{
Q_OBJECT
diff --git a/gazebo/gui/building/SegmentItem.hh b/gazebo/gui/building/SegmentItem.hh
index 9d3ec48..28c7927 100644
--- a/gazebo/gui/building/SegmentItem.hh
+++ b/gazebo/gui/building/SegmentItem.hh
@@ -35,7 +35,7 @@ namespace gazebo
/// \class SegmentItem SegmentItem.hh
/// \brief 2D line segment.
- class GAZEBO_VISIBLE SegmentItem
+ class GZ_GUI_BUILDING_VISIBLE SegmentItem
: public EditorItem, public QGraphicsLineItem
{
/// \brief Constructor
diff --git a/gazebo/gui/building/StairsInspectorDialog.cc b/gazebo/gui/building/StairsInspectorDialog.cc
index cda485f..cf70ab9 100644
--- a/gazebo/gui/building/StairsInspectorDialog.cc
+++ b/gazebo/gui/building/StairsInspectorDialog.cc
@@ -15,6 +15,7 @@
*
*/
+#include "gazebo/common/Assert.hh"
#include "gazebo/gui/building/StairsInspectorDialog.hh"
using namespace gazebo;
@@ -22,7 +23,7 @@ using namespace gui;
/////////////////////////////////////////////////
StairsInspectorDialog::StairsInspectorDialog(QWidget *_parent)
- : QDialog(_parent)
+ : BaseInspectorDialog(_parent)
{
this->setObjectName("stairsInspectorDialog");
@@ -34,9 +35,7 @@ StairsInspectorDialog::StairsInspectorDialog(QWidget *_parent)
QHBoxLayout *nameLayout = new QHBoxLayout;
nameLayout->addWidget(stairsLabel);
- nameLayout->addWidget(stairsNameLabel);
-
- QLabel *startPointLabel = new QLabel(tr("Start Point: "));
+ nameLayout->addWidget(this->stairsNameLabel);
QLabel *startXLabel = new QLabel(tr("x: "));
QLabel *startYLabel = new QLabel(tr("y: "));
@@ -46,118 +45,97 @@ StairsInspectorDialog::StairsInspectorDialog(QWidget *_parent)
this->startXSpinBox->setSingleStep(0.001);
this->startXSpinBox->setDecimals(3);
this->startXSpinBox->setValue(0.000);
+ this->startXSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *startXUnitLabel = new QLabel(tr("m"));
+ startXUnitLabel->setMaximumWidth(40);
this->startYSpinBox = new QDoubleSpinBox;
this->startYSpinBox->setRange(-1000, 1000);
this->startYSpinBox->setSingleStep(0.001);
this->startYSpinBox->setDecimals(3);
this->startYSpinBox->setValue(0.000);
+ this->startYSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *startYUnitLabel = new QLabel(tr("m"));
+ startYUnitLabel->setMaximumWidth(40);
QGridLayout *startXYLayout = new QGridLayout;
startXYLayout->addWidget(startXLabel, 0, 0);
- startXYLayout->addWidget(startXSpinBox, 0, 1);
+ startXYLayout->addWidget(this->startXSpinBox, 0, 1);
+ startXYLayout->addWidget(startXUnitLabel, 0, 2);
startXYLayout->addWidget(startYLabel, 1, 0);
- startXYLayout->addWidget(startYSpinBox, 1, 1);
- startXYLayout->setColumnStretch(1, 1);
- startXYLayout->setAlignment(startXSpinBox, Qt::AlignLeft);
- startXYLayout->setAlignment(startYSpinBox, Qt::AlignLeft);
-
- QVBoxLayout *xyLayout = new QVBoxLayout;
- xyLayout->addWidget(startPointLabel);
- xyLayout->addLayout(startXYLayout);
+ startXYLayout->addWidget(this->startYSpinBox, 1, 1);
+ startXYLayout->addWidget(startYUnitLabel, 1, 2);
QGroupBox *positionGroupBox = new QGroupBox(tr("Position"));
- positionGroupBox->setLayout(xyLayout);
+ positionGroupBox->setLayout(startXYLayout);
QLabel *widthLabel = new QLabel(tr("Width: "));
QLabel *depthLabel = new QLabel(tr("Depth: "));
QLabel *heightLabel = new QLabel(tr("Height: "));
this->widthSpinBox = new QDoubleSpinBox;
- this->widthSpinBox->setRange(-1000, 1000);
+ this->widthSpinBox->setRange(0, 1000);
this->widthSpinBox->setSingleStep(0.001);
this->widthSpinBox->setDecimals(3);
this->widthSpinBox->setValue(0.000);
+ this->widthSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *widthUnitLabel = new QLabel(tr("m"));
+ widthUnitLabel->setMaximumWidth(40);
this->depthSpinBox = new QDoubleSpinBox;
- this->depthSpinBox->setRange(-1000, 1000);
+ this->depthSpinBox->setRange(0, 1000);
this->depthSpinBox->setSingleStep(0.001);
this->depthSpinBox->setDecimals(3);
this->depthSpinBox->setValue(0.000);
+ this->depthSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *depthUnitLabel = new QLabel(tr("m"));
+ depthUnitLabel->setMaximumWidth(40);
this->heightSpinBox = new QDoubleSpinBox;
- this->heightSpinBox->setRange(-1000, 1000);
+ this->heightSpinBox->setRange(0, 1000);
this->heightSpinBox->setSingleStep(0.001);
this->heightSpinBox->setDecimals(3);
this->heightSpinBox->setValue(0.000);
-
- QGridLayout *sizeLayout = new QGridLayout;
- sizeLayout->addWidget(widthLabel, 0, 0);
- sizeLayout->addWidget(widthSpinBox, 0, 1);
- sizeLayout->addWidget(depthLabel, 1, 0);
- sizeLayout->addWidget(depthSpinBox, 1, 1);
- sizeLayout->addWidget(heightLabel, 2, 0);
- sizeLayout->addWidget(heightSpinBox, 2, 1);
+ this->heightSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *heightUnitLabel = new QLabel(tr("m"));
+ heightUnitLabel->setMaximumWidth(40);
QLabel *stepsLabel = new QLabel(tr("# Steps: "));
this->stepsSpinBox = new QSpinBox;
this->stepsSpinBox->setRange(1, 1000);
this->stepsSpinBox->setSingleStep(1);
this->stepsSpinBox->setValue(1);
+ this->stepsSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *stepsDummyLabel = new QLabel(tr(" "));
- QGridLayout *stepsLayout = new QGridLayout;
- stepsLayout->addWidget(stepsLabel, 0, 0);
- stepsLayout->addWidget(stepsSpinBox, 0, 1);
-
- QVBoxLayout *sizeStepsLayout = new QVBoxLayout;
- sizeStepsLayout->addLayout(sizeLayout);
- sizeStepsLayout->addLayout(stepsLayout);
+ QGridLayout *sizeLayout = new QGridLayout;
+ sizeLayout->addWidget(widthLabel, 0, 0);
+ sizeLayout->addWidget(this->widthSpinBox, 0, 1);
+ sizeLayout->addWidget(widthUnitLabel, 0, 2);
+ sizeLayout->addWidget(depthLabel, 1, 0);
+ sizeLayout->addWidget(this->depthSpinBox, 1, 1);
+ sizeLayout->addWidget(depthUnitLabel, 1, 2);
+ sizeLayout->addWidget(heightLabel, 2, 0);
+ sizeLayout->addWidget(this->heightSpinBox, 2, 1);
+ sizeLayout->addWidget(heightUnitLabel, 2, 2);
+ sizeLayout->addWidget(stepsLabel, 3, 0);
+ sizeLayout->addWidget(this->stepsSpinBox, 3, 1);
+ sizeLayout->addWidget(stepsDummyLabel, 3, 2);
QGroupBox *sizeGroupBox = new QGroupBox(tr("Size"));
- sizeGroupBox->setLayout(sizeStepsLayout);
-
- QLabel *colorLabel = new QLabel(tr("Color: "));
- this->colorComboBox = new QComboBox;
- this->colorComboBox->setIconSize(QSize(15, 15));
- this->colorComboBox->setMinimumWidth(50);
- this->colorComboBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
- QPixmap colorIcon(15, 15);
- this->colorList.push_back(QColor(255, 255, 255, 255));
- this->colorList.push_back(QColor(194, 169, 160, 255));
- this->colorList.push_back(QColor(235, 206, 157, 255));
- this->colorList.push_back(QColor(254, 121, 5, 255));
- this->colorList.push_back(QColor(255, 195, 78, 255));
- this->colorList.push_back(QColor(111, 203, 172, 255));
- for (unsigned int i = 0; i < this->colorList.size(); ++i)
- {
- colorIcon.fill(this->colorList.at(i));
- this->colorComboBox->addItem(colorIcon, QString(""));
- }
+ sizeGroupBox->setLayout(sizeLayout);
+ this->InitColorComboBox();
QHBoxLayout *colorLayout = new QHBoxLayout;
+ QLabel *colorLabel = new QLabel(tr("Color: "));
colorLayout->addWidget(colorLabel);
- colorLayout->addWidget(colorComboBox);
-
- QLabel *textureLabel = new QLabel(tr("Texture: "));
- this->textureComboBox = new QComboBox;
- this->textureComboBox->setIconSize(QSize(30, 30));
- this->textureComboBox->setMinimumWidth(50);
- this->textureComboBox->setMinimumHeight(50);
- this->textureComboBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
- this->textureList.push_back(":wood.jpg");
- this->textureList.push_back(":tiles.jpg");
- this->textureList.push_back(":bricks.png");
- for (unsigned int i = 0; i < this->textureList.size(); ++i)
- {
- this->textureComboBox->addItem(QPixmap(this->textureList[i]).scaled(
- QSize(90, 90), Qt::IgnoreAspectRatio), QString(""));
- }
- this->textureComboBox->addItem("X");
- this->textureComboBox->setCurrentIndex(this->textureComboBox->count()-1);
+ colorLayout->addWidget(this->colorComboBox);
+ this->InitTextureComboBox();
QHBoxLayout *textureLayout = new QHBoxLayout;
+ QLabel *textureLabel = new QLabel(tr("Texture: "));
textureLayout->addWidget(textureLabel);
- textureLayout->addWidget(textureComboBox);
+ textureLayout->addWidget(this->textureComboBox);
QHBoxLayout *buttonsLayout = new QHBoxLayout;
QPushButton *cancelButton = new QPushButton(tr("&Cancel"));
@@ -221,25 +199,6 @@ int StairsInspectorDialog::GetSteps() const
}
/////////////////////////////////////////////////
-QColor StairsInspectorDialog::GetColor() const
-{
- return this->colorList[this->colorComboBox->currentIndex()];
-}
-
-/////////////////////////////////////////////////
-QString StairsInspectorDialog::GetTexture() const
-{
- QString texture = QString("");
- if (this->textureComboBox->currentIndex() != -1 &&
- this->textureComboBox->currentIndex() <
- this->textureComboBox->count() - 1)
- {
- texture = this->textureList[this->textureComboBox->currentIndex()];
- }
- return texture;
-}
-
-/////////////////////////////////////////////////
void StairsInspectorDialog::SetName(const std::string &_name)
{
this->stairsNameLabel->setText(tr(_name.c_str()));
@@ -278,38 +237,6 @@ void StairsInspectorDialog::SetSteps(int _steps)
}
/////////////////////////////////////////////////
-void StairsInspectorDialog::SetColor(const QColor _color)
-{
- // Find index corresponding to color (only a few colors allowed so far)
- int index = 0;
- for (unsigned int i = 0; i < this->colorList.size(); ++i)
- {
- if (this->colorList[i] == _color)
- {
- index = i;
- break;
- }
- }
- this->colorComboBox->setCurrentIndex(index);
-}
-
-/////////////////////////////////////////////////
-void StairsInspectorDialog::SetTexture(QString _texture)
-{
- // Find index corresponding to texture (only a few textures allowed so far)
- int index = this->textureComboBox->count()-1;
- for (unsigned int i = 0; i < this->textureList.size(); ++i)
- {
- if (this->textureList[i] == _texture)
- {
- index = i;
- break;
- }
- }
- this->textureComboBox->setCurrentIndex(index);
-}
-
-/////////////////////////////////////////////////
void StairsInspectorDialog::OnCancel()
{
this->close();
diff --git a/gazebo/gui/building/StairsInspectorDialog.hh b/gazebo/gui/building/StairsInspectorDialog.hh
index a57f54b..e961567 100644
--- a/gazebo/gui/building/StairsInspectorDialog.hh
+++ b/gazebo/gui/building/StairsInspectorDialog.hh
@@ -22,6 +22,7 @@
#include <vector>
#include "gazebo/gui/qt.h"
#include "gazebo/util/system.hh"
+#include "gazebo/gui/building/BaseInspectorDialog.hh"
namespace gazebo
{
@@ -32,7 +33,8 @@ namespace gazebo
/// \class StairsInspectorDialog StairsInspectorDialog.hh
/// \brief Dialog for configuring a staircase item.
- class GAZEBO_VISIBLE StairsInspectorDialog : public QDialog
+ class GZ_GUI_BUILDING_VISIBLE StairsInspectorDialog
+ : public BaseInspectorDialog
{
Q_OBJECT
@@ -63,14 +65,6 @@ namespace gazebo
/// \return Number of steps in the staircase.
public: int GetSteps() const;
- /// \brief Get the color of the staircase.
- /// \return Stairs color.
- public: QColor GetColor() const;
-
- /// \brief Get the texture of the staircase.
- /// \return Texture.
- public: QString GetTexture() const;
-
/// \brief Set the name of the staircase.
/// \param[in] _name Name to set to.
public: void SetName(const std::string &_name);
@@ -95,14 +89,6 @@ namespace gazebo
/// \param[in] _steps Number of steps.
public: void SetSteps(int _steps);
- /// \brief Set the color of the staircase.
- /// \param[in] _color Color.
- public: void SetColor(const QColor _color);
-
- /// \brief Set the texture of the staircase.
- /// \param[in] _texture Texture.
- public: void SetTexture(const QString _texture);
-
/// \brief Qt signal emitted to indicate that changes should be applied.
Q_SIGNALS: void Applied();
@@ -137,18 +123,6 @@ namespace gazebo
/// \brief Label that holds the name of the staircase.
private: QLabel* stairsNameLabel;
-
- /// \brief Combo box for selecting the color of the staircase.
- private: QComboBox *colorComboBox;
-
- /// \brief Vector of color options.
- private: std::vector<QColor> colorList;
-
- /// \brief Combo box for selecting the texture of the staircase.
- private: QComboBox *textureComboBox;
-
- /// \brief Vector of texture options.
- private: std::vector<QString> textureList;
};
/// \}
}
diff --git a/gazebo/gui/building/StairsItem.hh b/gazebo/gui/building/StairsItem.hh
index 8ae4ff9..f038e46 100644
--- a/gazebo/gui/building/StairsItem.hh
+++ b/gazebo/gui/building/StairsItem.hh
@@ -36,7 +36,8 @@ namespace gazebo
/// \class StairsItem StairsItem.hh
/// \brief 2D representation of a staircase.
- class GAZEBO_VISIBLE StairsItem : public RectItem, public BuildingItem
+ class GZ_GUI_BUILDING_VISIBLE StairsItem :
+ public RectItem, public BuildingItem
{
Q_OBJECT
diff --git a/gazebo/gui/building/WallInspectorDialog.cc b/gazebo/gui/building/WallInspectorDialog.cc
index d56e1cc..4545b66 100644
--- a/gazebo/gui/building/WallInspectorDialog.cc
+++ b/gazebo/gui/building/WallInspectorDialog.cc
@@ -15,6 +15,7 @@
*
*/
+#include "gazebo/common/Assert.hh"
#include "gazebo/gui/building/WallInspectorDialog.hh"
using namespace gazebo;
@@ -22,7 +23,7 @@ using namespace gui;
/////////////////////////////////////////////////
WallInspectorDialog::WallInspectorDialog(QWidget *_parent)
- : QDialog(_parent)
+ : BaseInspectorDialog(_parent)
{
this->setObjectName("wallInspectorDialog");
@@ -34,21 +35,7 @@ WallInspectorDialog::WallInspectorDialog(QWidget *_parent)
QHBoxLayout *nameLayout = new QHBoxLayout;
nameLayout->addWidget(wallLabel);
- nameLayout->addWidget(wallNameLabel);
-
- QLabel *lengthLabel = new QLabel(tr("Length: "));
- this->lengthSpinBox = new QDoubleSpinBox;
- this->lengthSpinBox->setRange(-1000, 1000);
- this->lengthSpinBox->setSingleStep(0.001);
- this->lengthSpinBox->setDecimals(3);
- this->lengthSpinBox->setValue(0.000);
-
- QHBoxLayout *lengthLayout = new QHBoxLayout;
- lengthLayout->addWidget(lengthLabel);
- lengthLayout->addWidget(lengthSpinBox);
-
- QLabel *lengthCaptionLabel = new QLabel(
- tr("(Distance from Start to End point)\n"));
+ nameLayout->addWidget(this->wallNameLabel);
QLabel *startLabel = new QLabel(tr("Start Point"));
QLabel *endLabel = new QLabel(tr("End Point"));
@@ -64,12 +51,18 @@ WallInspectorDialog::WallInspectorDialog(QWidget *_parent)
this->startXSpinBox->setSingleStep(0.001);
this->startXSpinBox->setDecimals(3);
this->startXSpinBox->setValue(0.000);
+ this->startXSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *startXUnitLabel = new QLabel(tr("m "));
+ startXUnitLabel->setMaximumWidth(40);
this->startYSpinBox = new QDoubleSpinBox;
this->startYSpinBox->setRange(-1000, 1000);
this->startYSpinBox->setSingleStep(0.001);
this->startYSpinBox->setDecimals(3);
this->startYSpinBox->setValue(0.000);
+ this->startYSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *startYUnitLabel = new QLabel(tr("m "));
+ startYUnitLabel->setMaximumWidth(40);
QLabel *endXLabel = new QLabel(tr("x: "));
QLabel *endYLabel = new QLabel(tr("y: "));
@@ -79,38 +72,45 @@ WallInspectorDialog::WallInspectorDialog(QWidget *_parent)
this->endXSpinBox->setSingleStep(0.001);
this->endXSpinBox->setDecimals(3);
this->endXSpinBox->setValue(0.000);
+ this->endXSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *endXUnitLabel = new QLabel(tr("m"));
+ endXUnitLabel->setMaximumWidth(40);
this->endYSpinBox = new QDoubleSpinBox;
this->endYSpinBox->setRange(-1000, 1000);
this->endYSpinBox->setSingleStep(0.001);
this->endYSpinBox->setDecimals(3);
this->endYSpinBox->setValue(0.000);
+ this->endYSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *endYUnitLabel = new QLabel(tr("m"));
+ endYUnitLabel->setMaximumWidth(40);
QGridLayout *startXYLayout = new QGridLayout;
startXYLayout->addWidget(startXLabel, 0, 0);
- startXYLayout->addWidget(startXSpinBox, 0, 1);
+ startXYLayout->addWidget(this->startXSpinBox, 0, 1);
+ startXYLayout->addWidget(startXUnitLabel, 0, 2);
startXYLayout->addWidget(startYLabel, 1, 0);
- startXYLayout->addWidget(startYSpinBox, 1, 1);
+ startXYLayout->addWidget(this->startYSpinBox, 1, 1);
+ startXYLayout->addWidget(startYUnitLabel, 1, 2);
startXYLayout->setColumnStretch(1, 1);
- startXYLayout->setAlignment(startXSpinBox, Qt::AlignLeft);
- startXYLayout->setAlignment(startYSpinBox, Qt::AlignLeft);
+ startXYLayout->setAlignment(this->startXSpinBox, Qt::AlignLeft);
+ startXYLayout->setAlignment(this->startYSpinBox, Qt::AlignLeft);
QGridLayout *endXYLayout = new QGridLayout;
endXYLayout->addWidget(endXLabel, 0, 0);
- endXYLayout->addWidget(endXSpinBox, 0, 1);
+ endXYLayout->addWidget(this->endXSpinBox, 0, 1);
+ endXYLayout->addWidget(endXUnitLabel, 0, 2);
endXYLayout->addWidget(endYLabel, 1, 0);
- endXYLayout->addWidget(endYSpinBox, 1, 1);
+ endXYLayout->addWidget(this->endYSpinBox, 1, 1);
+ endXYLayout->addWidget(endYUnitLabel, 1, 2);
endXYLayout->setColumnStretch(1, 1);
- endXYLayout->setAlignment(endXSpinBox, Qt::AlignLeft);
- endXYLayout->setAlignment(endYSpinBox, Qt::AlignLeft);
+ endXYLayout->setAlignment(this->endXSpinBox, Qt::AlignLeft);
+ endXYLayout->setAlignment(this->endYSpinBox, Qt::AlignLeft);
QHBoxLayout *xyLayout = new QHBoxLayout;
xyLayout->addLayout(startXYLayout);
xyLayout->addLayout(endXYLayout);
- QVBoxLayout *lengthGroupLayout = new QVBoxLayout;
- lengthGroupLayout->addLayout(lengthLayout);
- lengthGroupLayout->addWidget(lengthCaptionLabel);
QVBoxLayout *positionGroupLayout = new QVBoxLayout;
positionGroupLayout->addLayout(startEndLayout);
@@ -119,73 +119,71 @@ WallInspectorDialog::WallInspectorDialog(QWidget *_parent)
QGroupBox *positionGroupBox = new QGroupBox(tr("Position"));
positionGroupBox->setLayout(positionGroupLayout);
- QGroupBox *lengthGroupBox = new QGroupBox(tr("Length"));
- lengthGroupBox->setLayout(lengthGroupLayout);
+ QLabel *lengthLabel = new QLabel(tr("Length: "));
+ this->lengthSpinBox = new QDoubleSpinBox;
+ this->lengthSpinBox->setRange(0, 1000);
+ this->lengthSpinBox->setSingleStep(0.001);
+ this->lengthSpinBox->setDecimals(3);
+ this->lengthSpinBox->setValue(0.000);
+ this->lengthSpinBox->setAlignment(Qt::AlignRight);
+
+ QLabel *lengthUnitLabel = new QLabel(tr("m"));
+ lengthUnitLabel->setMaximumWidth(40);
+
+ QHBoxLayout *lengthLayout = new QHBoxLayout;
+ lengthLayout->addWidget(lengthLabel);
+ lengthLayout->addWidget(this->lengthSpinBox);
+ lengthLayout->addWidget(lengthUnitLabel);
QLabel *heightLabel = new QLabel(tr("Height: "));
this->heightSpinBox = new QDoubleSpinBox;
- this->heightSpinBox->setRange(-1000, 1000);
+ this->heightSpinBox->setRange(0, 1000);
this->heightSpinBox->setSingleStep(0.001);
this->heightSpinBox->setDecimals(3);
this->heightSpinBox->setValue(0.000);
+ this->heightSpinBox->setAlignment(Qt::AlignRight);
+
+ QLabel *heightUnitLabel = new QLabel(tr("m"));
+ heightUnitLabel->setMaximumWidth(40);
QLabel *thicknessLabel = new QLabel(tr("Thickness "));
this->thicknessSpinBox = new QDoubleSpinBox;
- this->thicknessSpinBox->setRange(-1000, 1000);
+ this->thicknessSpinBox->setRange(0, 1000);
this->thicknessSpinBox->setSingleStep(0.001);
this->thicknessSpinBox->setDecimals(3);
this->thicknessSpinBox->setValue(0.000);
+ this->thicknessSpinBox->setAlignment(Qt::AlignRight);
+
+ QLabel *thicknessUnitLabel = new QLabel(tr("m"));
+ thicknessUnitLabel->setMaximumWidth(40);
QGridLayout *heightThicknessLayout = new QGridLayout;
heightThicknessLayout->addWidget(heightLabel, 0, 0);
- heightThicknessLayout->addWidget(heightSpinBox, 0, 1);
+ heightThicknessLayout->addWidget(this->heightSpinBox, 0, 1);
+ heightThicknessLayout->addWidget(heightUnitLabel, 0, 2);
heightThicknessLayout->addWidget(thicknessLabel, 1, 0);
- heightThicknessLayout->addWidget(thicknessSpinBox, 1, 1);
+ heightThicknessLayout->addWidget(this->thicknessSpinBox, 1, 1);
+ heightThicknessLayout->addWidget(thicknessUnitLabel, 1, 2);
// TODO Color and texture code is repeated on all dialogs.
// Make a generalized widget
- QLabel *colorLabel = new QLabel(tr("Color: "));
- this->colorComboBox = new QComboBox;
- this->colorComboBox->setIconSize(QSize(15, 15));
- this->colorComboBox->setMinimumWidth(50);
- this->colorComboBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
- QPixmap colorIcon(15, 15);
- this->colorList.push_back(QColor(255, 255, 255, 255));
- this->colorList.push_back(QColor(194, 169, 160, 255));
- this->colorList.push_back(QColor(235, 206, 157, 255));
- this->colorList.push_back(QColor(254, 121, 5, 255));
- this->colorList.push_back(QColor(255, 195, 78, 255));
- this->colorList.push_back(QColor(111, 203, 172, 255));
- for (unsigned int i = 0; i < this->colorList.size(); ++i)
- {
- colorIcon.fill(this->colorList.at(i));
- this->colorComboBox->addItem(colorIcon, QString(""));
- }
-
+ this->InitColorComboBox();
QHBoxLayout *colorLayout = new QHBoxLayout;
+ QLabel *colorLabel = new QLabel(tr("Color: "));
+ QLabel *colorDummyLabel = new QLabel(tr(""));
+ colorDummyLabel->setMaximumWidth(40);
colorLayout->addWidget(colorLabel);
- colorLayout->addWidget(colorComboBox);
-
- QLabel *textureLabel = new QLabel(tr("Texture: "));
- this->textureComboBox = new QComboBox;
- this->textureComboBox->setIconSize(QSize(30, 30));
- this->textureComboBox->setMinimumWidth(50);
- this->textureComboBox->setMinimumHeight(50);
- this->textureComboBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
- this->textureList.push_back(":wood.jpg");
- this->textureList.push_back(":tiles.jpg");
- this->textureList.push_back(":bricks.png");
- for (unsigned int i = 0; i < this->textureList.size(); ++i)
- {
- this->textureComboBox->addItem(QPixmap(this->textureList[i]).scaled(
- QSize(90, 90), Qt::IgnoreAspectRatio), QString(""));
- }
- this->textureComboBox->addItem("X");
- this->textureComboBox->setCurrentIndex(this->textureComboBox->count()-1);
+ colorLayout->addWidget(this->colorComboBox);
+ colorLayout->addWidget(colorDummyLabel);
+ this->InitTextureComboBox();
QHBoxLayout *textureLayout = new QHBoxLayout;
+ QLabel *textureLabel = new QLabel(tr("Texture: "));
+ QLabel *textureDummyLabel = new QLabel(tr(""));
+ textureDummyLabel->setMaximumWidth(40);
textureLayout->addWidget(textureLabel);
- textureLayout->addWidget(textureComboBox);
+ textureLayout->addWidget(this->textureComboBox);
+ textureLayout->addWidget(textureDummyLabel);
QHBoxLayout *buttonsLayout = new QHBoxLayout;
QPushButton *cancelButton = new QPushButton(tr("&Cancel"));
@@ -203,7 +201,7 @@ WallInspectorDialog::WallInspectorDialog(QWidget *_parent)
QVBoxLayout *mainLayout = new QVBoxLayout;
mainLayout->addLayout(nameLayout);
mainLayout->addWidget(positionGroupBox);
- mainLayout->addWidget(lengthGroupBox);
+ mainLayout->addLayout(lengthLayout);
mainLayout->addLayout(heightThicknessLayout);
mainLayout->addLayout(colorLayout);
mainLayout->addLayout(textureLayout);
@@ -251,25 +249,6 @@ double WallInspectorDialog::GetThickness() const
}
/////////////////////////////////////////////////
-QColor WallInspectorDialog::GetColor() const
-{
- return this->colorList[this->colorComboBox->currentIndex()];
-}
-
-/////////////////////////////////////////////////
-QString WallInspectorDialog::GetTexture() const
-{
- QString texture = QString("");
- if (this->textureComboBox->currentIndex() != -1 &&
- this->textureComboBox->currentIndex() <
- this->textureComboBox->count() - 1)
- {
- texture = this->textureList[this->textureComboBox->currentIndex()];
- }
- return texture;
-}
-
-/////////////////////////////////////////////////
void WallInspectorDialog::SetName(const std::string &_name)
{
this->wallNameLabel->setText(tr(_name.c_str()));
@@ -308,38 +287,6 @@ void WallInspectorDialog::SetThickness(double _thickness)
}
/////////////////////////////////////////////////
-void WallInspectorDialog::SetColor(const QColor _color)
-{
- // Find index corresponding to color (only a few colors allowed so far)
- int index = 0;
- for (unsigned int i = 0; i < this->colorList.size(); ++i)
- {
- if (this->colorList[i] == _color)
- {
- index = i;
- break;
- }
- }
- this->colorComboBox->setCurrentIndex(index);
-}
-
-/////////////////////////////////////////////////
-void WallInspectorDialog::SetTexture(QString _texture)
-{
- // Find index corresponding to texture (only a few textures allowed so far)
- int index = this->textureComboBox->count()-1;
- for (unsigned int i = 0; i < this->textureList.size(); ++i)
- {
- if (this->textureList[i] == _texture)
- {
- index = i;
- break;
- }
- }
- this->textureComboBox->setCurrentIndex(index);
-}
-
-/////////////////////////////////////////////////
void WallInspectorDialog::OnCancel()
{
this->close();
diff --git a/gazebo/gui/building/WallInspectorDialog.hh b/gazebo/gui/building/WallInspectorDialog.hh
index 1f21982..271dfc5 100644
--- a/gazebo/gui/building/WallInspectorDialog.hh
+++ b/gazebo/gui/building/WallInspectorDialog.hh
@@ -22,6 +22,7 @@
#include <vector>
#include "gazebo/gui/qt.h"
#include "gazebo/util/system.hh"
+#include "gazebo/gui/building/BaseInspectorDialog.hh"
namespace gazebo
{
@@ -32,7 +33,8 @@ namespace gazebo
/// \class WallInspectorDialog WallInspectorDialog.hh
/// \brief Dialog for configuring a wall item.
- class GAZEBO_VISIBLE WallInspectorDialog : public QDialog
+ class GZ_GUI_BUILDING_VISIBLE WallInspectorDialog
+ : public BaseInspectorDialog
{
Q_OBJECT
@@ -63,14 +65,6 @@ namespace gazebo
/// \return Wall thickness in pixels.
public: double GetThickness() const;
- /// \brief Get the color of the wall.
- /// \return Wall color.
- public: QColor GetColor() const;
-
- /// \brief Get the texture of the wall.
- /// \return Texture.
- public: QString GetTexture() const;
-
/// \brief Set the name of the wall.
/// \param[in] _name Name to set the wall to.
public: void SetName(const std::string &_name);
@@ -96,14 +90,6 @@ namespace gazebo
/// \param[in] _thickness Thickness of wall in pixels.
public: void SetThickness(double _thickness);
- /// \brief Set the color of the wall.
- /// \param[in] _color Color.
- public: void SetColor(const QColor _color);
-
- /// \brief Set the texture of the wall.
- /// \param[in] _texture Texture.
- public: void SetTexture(const QString _texture);
-
/// \brief Qt signal emitted to indicate that changes should be applied.
Q_SIGNALS: void Applied();
@@ -141,18 +127,6 @@ namespace gazebo
/// \brief Spin box for configuring the length of the wall segment.
private: QDoubleSpinBox *lengthSpinBox;
- /// \brief Combo box for selecting the color of the wall.
- private: QComboBox *colorComboBox;
-
- /// \brief Vector of color options.
- private: std::vector<QColor> colorList;
-
- /// \brief Combo box for selecting the texture of the wall.
- private: QComboBox *textureComboBox;
-
- /// \brief Vector of texture options.
- private: std::vector<QString> textureList;
-
/// \brief Label that holds the name of the wall.
private: QLabel* wallNameLabel;
};
diff --git a/gazebo/gui/building/WallSegmentItem.hh b/gazebo/gui/building/WallSegmentItem.hh
index 4b24267..1e205c9 100644
--- a/gazebo/gui/building/WallSegmentItem.hh
+++ b/gazebo/gui/building/WallSegmentItem.hh
@@ -37,7 +37,7 @@ namespace gazebo
/// \class WallSegmentItem WallSegmentItem.hh
/// \brief 2D representation of a wall.
- class GAZEBO_VISIBLE WallSegmentItem : public SegmentItem, public
+ class GZ_GUI_BUILDING_VISIBLE WallSegmentItem : public SegmentItem, public
BuildingItem
{
Q_OBJECT
diff --git a/gazebo/gui/building/WindowDoorInspectorDialog.cc b/gazebo/gui/building/WindowDoorInspectorDialog.cc
index 0b80836..b8c2cef 100644
--- a/gazebo/gui/building/WindowDoorInspectorDialog.cc
+++ b/gazebo/gui/building/WindowDoorInspectorDialog.cc
@@ -22,7 +22,7 @@ using namespace gui;
/////////////////////////////////////////////////
WindowDoorInspectorDialog::WindowDoorInspectorDialog(int _mode,
- QWidget *_parent) : QDialog(_parent)
+ QWidget *_parent) : BaseInspectorDialog(_parent)
{
this->setObjectName("windowDoorInspectorDialog");
@@ -44,37 +44,45 @@ WindowDoorInspectorDialog::WindowDoorInspectorDialog(int _mode,
QHBoxLayout *nameLayout = new QHBoxLayout;
nameLayout->addWidget(itemlLabel);
- nameLayout->addWidget(itemNameLabel);
+ nameLayout->addWidget(this->itemNameLabel);
QLabel *widthLabel = new QLabel(tr("Width: "));
// QLabel *depthLabel = new QLabel(tr("Depth: "));
QLabel *heightLabel = new QLabel(tr("Height: "));
this->widthSpinBox = new QDoubleSpinBox;
- this->widthSpinBox->setRange(-1000, 1000);
+ this->widthSpinBox->setRange(0, 1000);
this->widthSpinBox->setSingleStep(0.001);
this->widthSpinBox->setDecimals(3);
this->widthSpinBox->setValue(0.000);
+ this->widthSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *widthUnitLabel = new QLabel(tr("m"));
+ widthUnitLabel->setMaximumWidth(40);
this->depthSpinBox = new QDoubleSpinBox;
- this->depthSpinBox->setRange(-1000, 1000);
+ this->depthSpinBox->setRange(0, 1000);
this->depthSpinBox->setSingleStep(0.001);
this->depthSpinBox->setDecimals(3);
this->depthSpinBox->setValue(0.000);
this->heightSpinBox = new QDoubleSpinBox;
- this->heightSpinBox->setRange(-1000, 1000);
+ this->heightSpinBox->setRange(0, 1000);
this->heightSpinBox->setSingleStep(0.001);
this->heightSpinBox->setDecimals(3);
this->heightSpinBox->setValue(0.000);
+ this->heightSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *heightUnitLabel = new QLabel(tr("m"));
+ heightUnitLabel->setMaximumWidth(40);
QGridLayout *sizeLayout = new QGridLayout;
sizeLayout->addWidget(widthLabel, 0, 0);
- sizeLayout->addWidget(widthSpinBox, 0, 1);
+ sizeLayout->addWidget(this->widthSpinBox, 0, 1);
+ sizeLayout->addWidget(widthUnitLabel, 0, 2);
// sizeLayout->addWidget(depthLabel, 1, 0);
-// sizeLayout->addWidget(depthSpinBox, 1, 1);
+// sizeLayout->addWidget(this->depthSpinBox, 1, 1);
sizeLayout->addWidget(heightLabel, 2, 0);
- sizeLayout->addWidget(heightSpinBox, 2, 1);
+ sizeLayout->addWidget(this->heightSpinBox, 2, 1);
+ sizeLayout->addWidget(heightUnitLabel, 2, 2);
QGroupBox *sizeGroupBox = new QGroupBox(tr("Size"));
sizeGroupBox->setLayout(sizeLayout);
@@ -88,26 +96,38 @@ WindowDoorInspectorDialog::WindowDoorInspectorDialog(int _mode,
this->positionXSpinBox->setSingleStep(0.001);
this->positionXSpinBox->setDecimals(3);
this->positionXSpinBox->setValue(0.000);
+ this->positionXSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *positionXUnitLabel = new QLabel(tr("m"));
+ positionXUnitLabel->setMaximumWidth(40);
this->positionYSpinBox = new QDoubleSpinBox;
this->positionYSpinBox->setRange(-1000, 1000);
this->positionYSpinBox->setSingleStep(0.001);
this->positionYSpinBox->setDecimals(3);
this->positionYSpinBox->setValue(0.000);
+ this->positionYSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *positionYUnitLabel = new QLabel(tr("m"));
+ positionYUnitLabel->setMaximumWidth(40);
this->elevationSpinBox = new QDoubleSpinBox;
- this->elevationSpinBox->setRange(-1000, 1000);
+ this->elevationSpinBox->setRange(0, 1000);
this->elevationSpinBox->setSingleStep(0.001);
this->elevationSpinBox->setDecimals(3);
this->elevationSpinBox->setValue(0.000);
+ this->elevationSpinBox->setAlignment(Qt::AlignRight);
+ QLabel *elevationUnitLabel = new QLabel(tr("m"));
+ elevationUnitLabel->setMaximumWidth(40);
QGridLayout *positionLayout = new QGridLayout;
positionLayout->addWidget(positionXLabel, 0, 0);
- positionLayout->addWidget(positionXSpinBox, 0, 1);
+ positionLayout->addWidget(this->positionXSpinBox, 0, 1);
+ positionLayout->addWidget(positionXUnitLabel, 0, 2);
positionLayout->addWidget(positionYLabel), 1, 0;
- positionLayout->addWidget(positionYSpinBox, 1, 1);
- positionLayout->addWidget(elevationLabel, 0, 2);
- positionLayout->addWidget(elevationSpinBox, 0, 3);
+ positionLayout->addWidget(this->positionYSpinBox, 1, 1);
+ positionLayout->addWidget(positionYUnitLabel, 1, 2);
+ positionLayout->addWidget(elevationLabel, 0, 3);
+ positionLayout->addWidget(this->elevationSpinBox, 0, 4);
+ positionLayout->addWidget(elevationUnitLabel, 0, 5);
QGroupBox *positionGroupBox = new QGroupBox(tr("Position"));
positionGroupBox->setLayout(positionLayout);
@@ -118,7 +138,7 @@ WindowDoorInspectorDialog::WindowDoorInspectorDialog(int _mode,
QHBoxLayout *typeLayout = new QHBoxLayout;
typeLayout->addWidget(typeLabel);
- typeLayout->addWidget(typeComboBox);
+ typeLayout->addWidget(this->typeComboBox);
QHBoxLayout *buttonsLayout = new QHBoxLayout;
QPushButton *cancelButton = new QPushButton(tr("&Cancel"));
diff --git a/gazebo/gui/building/WindowDoorInspectorDialog.hh b/gazebo/gui/building/WindowDoorInspectorDialog.hh
index 1ec67c4..3aca82f 100644
--- a/gazebo/gui/building/WindowDoorInspectorDialog.hh
+++ b/gazebo/gui/building/WindowDoorInspectorDialog.hh
@@ -21,12 +21,14 @@
#include <string>
#include "gazebo/gui/qt.h"
#include "gazebo/util/system.hh"
+#include "gazebo/gui/building/BaseInspectorDialog.hh"
namespace gazebo
{
namespace gui
{
- class GAZEBO_VISIBLE WindowDoorInspectorDialog : public QDialog
+ class GZ_GUI_BUILDING_VISIBLE WindowDoorInspectorDialog
+ : public BaseInspectorDialog
{
Q_OBJECT
diff --git a/gazebo/gui/building/WindowItem.hh b/gazebo/gui/building/WindowItem.hh
index c29ab30..590570e 100644
--- a/gazebo/gui/building/WindowItem.hh
+++ b/gazebo/gui/building/WindowItem.hh
@@ -36,7 +36,8 @@ namespace gazebo
/// \class WindowItem WindowItem.hh
/// \brief 2D representation of a window.
- class GAZEBO_VISIBLE WindowItem : public RectItem, public BuildingItem
+ class GZ_GUI_BUILDING_VISIBLE WindowItem :
+ public RectItem, public BuildingItem
{
Q_OBJECT
diff --git a/gazebo/gui/images/com.png b/gazebo/gui/images/com.png
new file mode 100644
index 0000000..b91dc94
Binary files /dev/null and b/gazebo/gui/images/com.png differ
diff --git a/gazebo/gui/images/down_spin_arrow.png b/gazebo/gui/images/down_spin_arrow.png
new file mode 100644
index 0000000..e271f7f
Binary files /dev/null and b/gazebo/gui/images/down_spin_arrow.png differ
diff --git a/gazebo/gui/images/down_spin_arrow_disabled.png b/gazebo/gui/images/down_spin_arrow_disabled.png
new file mode 100644
index 0000000..5805d98
Binary files /dev/null and b/gazebo/gui/images/down_spin_arrow_disabled.png differ
diff --git a/gazebo/gui/images/log_pause.png b/gazebo/gui/images/log_pause.png
new file mode 100644
index 0000000..112fa8f
Binary files /dev/null and b/gazebo/gui/images/log_pause.png differ
diff --git a/gazebo/gui/images/log_play.png b/gazebo/gui/images/log_play.png
new file mode 100644
index 0000000..d250e17
Binary files /dev/null and b/gazebo/gui/images/log_play.png differ
diff --git a/gazebo/gui/images/log_record.png b/gazebo/gui/images/log_record.png
new file mode 100644
index 0000000..203fccb
Binary files /dev/null and b/gazebo/gui/images/log_record.png differ
diff --git a/gazebo/gui/images/log_record.svg b/gazebo/gui/images/log_record.svg
new file mode 100644
index 0000000..cb199b4
--- /dev/null
+++ b/gazebo/gui/images/log_record.svg
@@ -0,0 +1,114 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Generator: Adobe Illustrator 16.0.4, SVG Export Plug-In . SVG Version: 6.00 Build 0) -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg version="1.1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px" width="64px"
+ height="64px" viewBox="0 0 64 64" enable-background="new 0 0 64 64" xml:space="preserve">
+<g id="original" display="none">
+ <path display="inline" fill="none" stroke="#999999" stroke-width="2.6096" d="M43,15.367"/>
+ <path display="inline" fill="none" stroke="#999999" stroke-width="2.6096" d="M29.281,2.981"/>
+ <path display="inline" fill="none" stroke="#999999" stroke-width="2" d="M43,49"/>
+ <line display="inline" fill="none" stroke="#999999" stroke-width="2" x1="43" y1="11" x2="43" y2="15"/>
+ <line display="inline" fill="none" stroke="#999999" stroke-width="2" x1="8" y1="3" x2="34" y2="3"/>
+ <line display="inline" fill="none" stroke="#999999" stroke-width="1.9976" x1="33.25" y1="2.75" x2="43.25" y2="11.688"/>
+ <line display="inline" fill="none" stroke="#999999" stroke-width="2" x1="22" y1="49" x2="8" y2="49"/>
+ <path display="inline" fill="none" stroke="#999999" stroke-width="2" d="M44,49"/>
+ <line display="inline" fill="none" stroke="#999999" stroke-width="2" x1="9" y1="49" x2="9" y2="3"/>
+ <line display="inline" fill="none" stroke="#999999" stroke-width="2" x1="33" y1="3" x2="33" y2="12"/>
+ <line display="inline" fill="none" stroke="#999999" stroke-width="2" x1="32" y1="12" x2="43" y2="12"/>
+ <path display="inline" fill="none" stroke="#999999" stroke-width="2" d="M39,21"/>
+ <line display="inline" fill="none" stroke="#999999" stroke-width="2" x1="12" y1="21" x2="22" y2="21"/>
+ <path display="inline" fill="none" stroke="#999999" stroke-width="2" d="M39,28"/>
+ <line display="inline" fill="none" stroke="#999999" stroke-width="2" x1="12" y1="28" x2="22" y2="28"/>
+ <path display="inline" fill="none" stroke="#999999" stroke-width="2" d="M39,35"/>
+ <line display="inline" fill="none" stroke="#999999" stroke-width="2" x1="12" y1="35" x2="22" y2="35"/>
+ <path display="inline" fill="none" stroke="#999999" stroke-width="2" d="M39,43"/>
+ <line display="inline" fill="none" stroke="#999999" stroke-width="2" x1="12" y1="43" x2="22" y2="43"/>
+ <g display="inline">
+ <g>
+ <path fill="#FFFFFF" stroke="#999999" stroke-width="2.6096" d="M56,27.378"/>
+ <path fill="#FFFFFF" stroke="#999999" stroke-width="2.6096" d="M42.281,14.992"/>
+ <line fill="#FFFFFF" stroke="#999999" stroke-width="2" x1="21" y1="15" x2="47" y2="15"/>
+ <line fill="#FFFFFF" stroke="#999999" stroke-width="2" x1="56" y1="23" x2="56" y2="61"/>
+ <line fill="#FFFFFF" stroke="#999999" stroke-width="2" x1="57" y1="61" x2="21" y2="61"/>
+ <line fill="#FFFFFF" stroke="#999999" stroke-width="2" x1="22" y1="61" x2="22" y2="15"/>
+ <line fill="#FFFFFF" stroke="#999999" stroke-width="2" x1="46" y1="15" x2="46" y2="24"/>
+ <line fill="#FFFFFF" stroke="#999999" stroke-width="2" x1="45" y1="24" x2="56" y2="24"/>
+ </g>
+ <line fill="#FFFFFF" stroke="#999999" stroke-width="2" x1="25" y1="33" x2="52" y2="33"/>
+ <line fill="#FFFFFF" stroke="#999999" stroke-width="2" x1="25" y1="40" x2="52" y2="40"/>
+ <line fill="#FFFFFF" stroke="#999999" stroke-width="2" x1="25" y1="47" x2="52" y2="47"/>
+ <line fill="#FFFFFF" stroke="#999999" stroke-width="2" x1="25" y1="55" x2="52" y2="55"/>
+ <line fill="none" stroke="#999999" stroke-width="1.9976" x1="46.25" y1="14.75" x2="56.25" y2="23.688"/>
+ </g>
+</g>
+<g id="Layer_3">
+ <polyline fill="none" stroke="#E6E6E6" stroke-width="2" points="48,45 55,45 44.707,55.713 38.525,49.6 "/>
+ <line fill="none" stroke="#E6E6E6" x1="48.989" y1="54.021" x2="48.989" y2="54.021"/>
+ <path fill="none" stroke="#E6E6E6" d="M44.938,50"/>
+ <path fill="none" stroke="#E6E6E6" d="M47.707,53.412"/>
+</g>
+<g id="original_copy">
+ <path fill="none" stroke="#999999" stroke-width="2" d="M43.5,41.265"/>
+ <path fill="none" stroke="#999999" stroke-width="2" d="M44.5,41.265"/>
+ <path fill="none" stroke="#999999" stroke-width="2" d="M39.5,27.265"/>
+ <path fill="none" stroke="#999999" stroke-width="2" d="M39.5,35.265"/>
+ <line fill="#D1D1D1" stroke="#FFFFFF" stroke-width="2" x1="26" y1="40" x2="36" y2="40"/>
+ <path fill="none" stroke="#999999" stroke-width="2" d="M39.5,20.265"/>
+ <polygon fill="#D1D1D1" stroke="#E6E6E6" stroke-width="2" points="49,50 14,50 14,8 39.361,8 40.059,8 49,14.319 "/>
+ <g>
+ <path fill="#666666" d="M18.36,26.061v-10.83h2.07v9.09h4.71v1.74H18.36z"/>
+ <path fill="#666666" d="M30.525,26.211c-2.955,0-4.544-2.025-4.544-5.564c0-3.54,1.59-5.565,4.544-5.565s4.545,2.025,4.545,5.565
+ C35.07,24.186,33.479,26.211,30.525,26.211z M30.525,16.821c-1.695,0-2.475,1.29-2.475,3.825c-0.03,2.535,0.765,3.825,2.475,3.825
+ c1.695,0,2.475-1.29,2.475-3.825C33,18.111,32.22,16.821,30.525,16.821z"/>
+ <path fill="#666666" d="M42.51,18.352c-0.495-1.08-1.095-1.53-2.175-1.53c-1.53,0-2.28,1.53-2.28,3.93
+ c0,2.28,0.855,3.72,2.295,3.72c1.351,0,2.221-0.9,2.266-2.145H40.56v-1.74h4.05v5.475h-1.5v-1.11
+ c-0.869,0.96-1.68,1.26-2.819,1.26c-2.94,0-4.305-2.34-4.305-5.4c0-2.76,1.05-5.729,4.515-5.729c2.16,0,3.42,1.17,3.93,2.76
+ L42.51,18.352z"/>
+ </g>
+ <path fill="none" stroke="#F2F2F2" stroke-width="2" stroke-miterlimit="10" d="M50,41"/>
+ <path fill="none" stroke="#F2F2F2" stroke-width="2" stroke-miterlimit="10" d="M50,29"/>
+ <path fill="none" stroke="#F2F2F2" stroke-width="2" stroke-miterlimit="10" d="M56,40"/>
+ <path fill="none" stroke="#F2F2F2" stroke-width="2" stroke-miterlimit="10" d="M51,40"/>
+ <path fill="none" stroke="#F2F2F2" stroke-width="2.347" stroke-miterlimit="10" d="M47.875,50.125"/>
+ <g>
+ <g>
+ <line fill="none" stroke="#666666" stroke-width="6" stroke-miterlimit="10" x1="45" y1="32" x2="45" y2="47"/>
+ <g>
+ <polygon fill="#666666" points="53.09,46 36.324,46 44.707,54.381 "/>
+ </g>
+ </g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+ <g>
+ </g>
+</g>
+</svg>
diff --git a/gazebo/gui/images/log_step_forward.png b/gazebo/gui/images/log_step_forward.png
new file mode 100644
index 0000000..3a22e40
Binary files /dev/null and b/gazebo/gui/images/log_step_forward.png differ
diff --git a/gazebo/gui/images/splash.svg b/gazebo/gui/images/splash.svg
new file mode 100644
index 0000000..e428763
--- /dev/null
+++ b/gazebo/gui/images/splash.svg
@@ -0,0 +1,116 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ version="1.1"
+ width="576"
+ height="320"
+ viewBox="0 0 576 320"
+ id="v1"
+ xml:space="preserve"
+ inkscape:version="0.48.4 r9939"
+ sodipodi:docname="splash.svg"><sodipodi:namedview
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1"
+ objecttolerance="10"
+ gridtolerance="10"
+ guidetolerance="10"
+ inkscape:pageopacity="0"
+ inkscape:pageshadow="2"
+ inkscape:window-width="2239"
+ inkscape:window-height="1268"
+ id="namedview59"
+ showgrid="false"
+ inkscape:zoom="2.8284271"
+ inkscape:cx="317.19478"
+ inkscape:cy="182.10726"
+ inkscape:window-x="0"
+ inkscape:window-y="24"
+ inkscape:window-maximized="0"
+ inkscape:current-layer="v1" /><metadata
+ id="metadata117"><rdf:RDF><cc:Work
+ rdf:about=""><dc:format>image/svg+xml</dc:format><dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" /><dc:title /></cc:Work></rdf:RDF></metadata><rect
+ width="576"
+ height="246"
+ x="0"
+ y="0"
+ id="rect3"
+ style="fill:#f58113" /><defs
+ id="defs6" /><rect
+ width="576"
+ height="74"
+ x="0"
+ y="246"
+ id="rect21"
+ style="fill:#ffffff" /><g
+ id="g105"
+ transform="translate(0,-0.5)"><g
+ transform="translate(142,-104)"
+ id="g5041_4_"><path
+ d="m 145.994,143.617 c -0.564,0.001 -1.125,0.149 -1.623,0.459 l -43.006,26.697 c -0.907,0.562 -1.458,1.566 -1.458,2.636 v 49.641 c 0,1.07 0.551,2.07 1.458,2.636 l 43.006,26.696 c 0.02,0.012 0.038,0.022 0.059,0.036 0.022,0.013 0.047,0.032 0.069,0.047 0.051,0.026 0.104,0.035 0.153,0.06 0.052,0.025 0.103,0.062 0.153,0.082 0.08,0.034 0.165,0.057 0.247,0.082 0.049,0.016 0.093,0.034 0.142,0.048 0.093,0.024 0.188,0.041 0.282,0.058 0.041,0.006 0.077,0.019 0.117,0.024 0.136,0.018 0.266,0 [...]
+ id="path3226_4_"
+ style="fill:#ffffff"
+ inkscape:connector-curvature="0" /><path
+ d="m 108.855,173.403 37.142,23.062 37.145,-23.13 -37.14,-22.998 -37.147,23.066"
+ id="path3230_4_"
+ style="fill:#ffffff"
+ inkscape:connector-curvature="0" /></g><g
+ transform="matrix(0,-1,1,0,571.09237,47.558036)"
+ id="g4514_4_"><path
+ d="m -127.884,-314.602 c -4.116,-0.317 -6.606,-3.91 -6.606,-7.316 0,-4.135 3.35,-7.429 7.168,-7.429 3.593,0 7.168,2.976 7.168,7.41 0,3.669 -2.527,6.008 -3.932,6.55 v -1.61 c 1.366,-0.785 2.677,-2.619 2.677,-4.921 0,-3.499 -2.846,-6.044 -5.933,-6.044 -3.088,0 -5.894,2.545 -5.894,6.101 0,2.938 2.169,5.107 4.097,5.595 v -7.729 h 1.255 v 9.393"
+ id="path3232_4_"
+ style="fill:#ffffff"
+ inkscape:connector-curvature="0" /><path
+ d="m -128.334,-306.709 v 5.371 l 6.362,-2.657 -6.362,-2.714 z m 7.934,1.927 v 1.554 l -13.847,5.95 v -1.516 l 4.658,-2.002 v -6.437 l -4.658,-2.021 v -1.441 l 13.847,5.913"
+ id="path3234_4_"
+ style="fill:#ffffff"
+ inkscape:connector-curvature="0" /><path
+ d="m -132.993,-292.578 11.339,6.812 v -6.606 h 1.254 v 8.159 h -1.254 l -11.339,-6.792 v 6.997 h -1.254 v -8.569 h 1.254"
+ id="path3236_4_"
+ style="fill:#ffffff"
+ inkscape:connector-curvature="0" /><path
+ d="m -120.4,-278.802 v 7.579 h -1.254 v -6.193 h -4.976 v 5.987 h -1.255 v -5.987 h -5.108 v 6.193 h -1.254 v -7.579 h 13.847"
+ id="path3238_4_"
+ style="fill:#ffffff"
+ inkscape:connector-curvature="0" /><path
+ d="m -132.993,-263.923 v 1.945 c 0,1.163 0.057,1.872 0.355,2.619 0.354,0.861 1.309,1.46 2.358,1.46 1.066,0 1.983,-0.711 2.338,-1.625 0.263,-0.658 0.317,-1.219 0.317,-2.695 v -1.704 h -5.368 z m 6.625,0 v 1.741 c 0,1.084 0.057,1.7 0.393,2.376 0.336,0.654 1.121,1.122 1.946,1.122 0.637,0 1.083,-0.224 1.515,-0.58 0.617,-0.542 0.86,-1.386 0.86,-2.807 v -1.853 h -4.714 l 0,10e-4 z m 5.968,-1.386 v 3.481 c 0,4.359 -3.124,4.528 -3.687,4.528 -1.628,0 -2.395,-1.03 -2.788,-1.515 -0.561,1.3 [...]
+ id="path3240_4_"
+ style="fill:#ffffff"
+ inkscape:connector-curvature="0" /><path
+ d="m -133.236,-244.172 c 0,3.2 2.601,5.952 5.894,5.952 3.237,0 5.933,-2.677 5.933,-5.952 0,-3.124 -2.546,-5.932 -5.988,-5.932 -3.161,0 -5.839,2.713 -5.839,5.932 z m 13.081,0 c 0,3.892 -3.088,7.335 -7.168,7.335 -4.08,0 -7.168,-3.462 -7.168,-7.335 0,-3.931 3.201,-7.317 7.093,-7.317 4.361,-0.001 7.243,3.63 7.243,7.317"
+ id="path3242_4_"
+ style="fill:#ffffff"
+ inkscape:connector-curvature="0" /></g></g><text
+ xml:space="preserve"
+ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#f58113;fill-opacity:1;stroke:none;font-family:Sans"
+ x="217.78418"
+ y="286.31055"
+ id="text3035"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan3037"
+ x="217.78418"
+ y="286.31055"
+ style="font-size:12px">Preparing your world ...</tspan></text>
+<text
+ xml:space="preserve"
+ style="font-size:40px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#ffffff;fill-opacity:1;stroke:none;font-family:Sans"
+ x="220.91638"
+ y="204.47205"
+ id="text3035-9"
+ sodipodi:linespacing="125%"><tspan
+ sodipodi:role="line"
+ id="tspan3037-2"
+ x="220.91638"
+ y="204.47205"
+ style="font-size:12px;fill:#ffffff">Simulation made easy</tspan></text>
+</svg>
\ No newline at end of file
diff --git a/gazebo/gui/images/trashcan.png b/gazebo/gui/images/trashcan.png
new file mode 100644
index 0000000..6ea9ed4
Binary files /dev/null and b/gazebo/gui/images/trashcan.png differ
diff --git a/gazebo/gui/images/up_spin_arrow.png b/gazebo/gui/images/up_spin_arrow.png
new file mode 100644
index 0000000..abcc724
Binary files /dev/null and b/gazebo/gui/images/up_spin_arrow.png differ
diff --git a/gazebo/gui/images/up_spin_arrow_disabled.png b/gazebo/gui/images/up_spin_arrow_disabled.png
new file mode 100644
index 0000000..b9c8e3b
Binary files /dev/null and b/gazebo/gui/images/up_spin_arrow_disabled.png differ
diff --git a/gazebo/gui/images/view_angle_back.png b/gazebo/gui/images/view_angle_back.png
new file mode 100644
index 0000000..71ea99f
Binary files /dev/null and b/gazebo/gui/images/view_angle_back.png differ
diff --git a/gazebo/gui/images/view_angle_back.svg b/gazebo/gui/images/view_angle_back.svg
new file mode 100644
index 0000000..e6eeae8
--- /dev/null
+++ b/gazebo/gui/images/view_angle_back.svg
@@ -0,0 +1,99 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Generator: Adobe Illustrator 16.0.4, SVG Export Plug-In . SVG Version: 6.00 Build 0) -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg version="1.1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px" width="107.5px"
+ height="107.5px" viewBox="18 18.5 107.5 107.5" enable-background="new 18 18.5 107.5 107.5" xml:space="preserve">
+<g id="cube">
+ <g id="horz_left" display="none">
+ <g display="inline">
+ <rect x="15.641" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <g>
+ <rect x="31.255" y="27.25" fill="#D1D1D1" width="67.657" height="36.627"/>
+ <g>
+ <path fill="#D1D1D1" d="M124.18,84.302v28.269H35.436V84.302H124.18 M128.359,80.121H31.255v36.629h97.104V80.121
+ L128.359,80.121z"/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="horz_center" display="none">
+ <g display="inline">
+ <rect x="65.672" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <rect x="38.172" y="27.25" fill="#D1D1D1" width="67.656" height="36.627"/>
+ <g>
+ <rect x="25.539" y="82.211" fill="#FFFFFF" width="92.923" height="32.449"/>
+ <path fill="#D1D1D1" d="M116.371,84.302v28.269H27.629V84.302H116.371 M120.552,80.121H23.448v36.629h97.104V80.121
+ L120.552,80.121z"/>
+ </g>
+ </g>
+ </g>
+ <g id="vert_top" display="none">
+ <g display="inline">
+ <rect x="13.931" y="14.81" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="32.086" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,36.267v88.742H31.431V36.267H59.698 M63.879,32.086H27.25v97.104h36.629V32.086L63.879,32.086z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="vert_center" display="none">
+ <g display="inline">
+ <rect x="13.931" y="65.671" fill="#D1D1D1" width="116.139" height="12.657"/>
+ <rect x="80.122" y="38.171" fill="#D1D1D1" width="36.628" height="67.657"/>
+ <g>
+ <rect x="29.34" y="25.539" fill="#FFFFFF" width="32.448" height="92.923"/>
+ <path fill="#D1D1D1" d="M59.698,27.629v88.742H31.431V27.629H59.698 M63.879,23.448H27.25v97.104h36.629V23.448L63.879,23.448z"
+ />
+ </g>
+ </g>
+ </g>
+ <g id="vert_bottom" display="none">
+ <g display="inline">
+ <rect x="13.931" y="116.533" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="44.258" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,18.991v88.741H31.431V18.991H59.698 M63.879,14.811H27.25v97.104h36.629V14.811L63.879,14.811z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <path fill="#D1D1D1" d="M123.12,21.984c-0.004-0.134-0.025-0.269-0.079-0.399c0-0.001-0.001-0.002-0.001-0.003
+ c-0.005-0.011-0.013-0.019-0.019-0.029c-0.052-0.114-0.122-0.214-0.204-0.304c-0.027-0.029-0.06-0.054-0.091-0.081
+ c-0.073-0.063-0.151-0.116-0.237-0.16c-0.037-0.02-0.071-0.038-0.111-0.053c-0.127-0.048-0.261-0.081-0.402-0.081H55.308
+ c-0.152,0-0.304,0.031-0.443,0.088c-0.141,0.059-0.266,0.144-0.372,0.25L21.211,54.494c-0.001,0.002-0.002,0.004-0.003,0.006
+ c-0.096,0.096-0.165,0.208-0.221,0.326c-0.018,0.035-0.025,0.072-0.039,0.109c-0.034,0.099-0.055,0.199-0.061,0.305
+ c-0.002,0.024-0.016,0.046-0.016,0.071v66.661c0,0.638,0.517,1.152,1.152,1.152h66.665c0.442,0,0.814-0.258,1.009-0.623
+ l32.789-32.791c0.332-0.169,0.561-0.49,0.611-0.875c0.004-0.014,0.006-0.027,0.008-0.041c0.004-0.036,0.021-0.068,0.021-0.105
+ V22.027C123.128,22.012,123.12,21.998,123.12,21.984z M54.156,24.809v29.35h-29.35L54.156,24.809z M23.178,56.463l30.979,0.021
+ v31.629l-30.979,30.984V56.463z M87.538,120.775l-62.734-0.047L55.687,89.84h31.852V120.775L87.538,120.775z M87.538,87.438H56.46
+ V56.486l31.078,0.022V87.438z M88.212,54.157H56.46V23.179h62.736L88.212,54.157z M89.843,119.098V89.84h29.255L89.843,119.098z
+ M120.825,87.438H89.843v-31.65l30.982-30.979V87.438z"/>
+</g>
+<g id="front" display="none">
+ <rect x="23.178" y="56.463" display="inline" fill="#F58113" width="64.36" height="64.36"/>
+</g>
+<g id="right" display="none">
+ <polygon display="inline" fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,88.5 89.843,119.098 "/>
+</g>
+<g id="left" display="none">
+ <polygon display="inline" fill="#F58113" points="24.807,54.158 54.156,24.809 54.156,54.158 "/>
+ <polygon display="inline" fill="#F58113" points="23.178,56.463 54.157,56.484 54.157,88.113 23.178,119.098 "/>
+</g>
+<g id="back">
+ <polygon fill="#F58113" points="56.46,23.179 119.196,23.179 88.212,54.157 56.46,54.157 "/>
+ <polygon fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,87.438 89.843,87.438 "/>
+ <rect x="56.46" y="56.486" fill="#F58113" width="31.078" height="31.078"/>
+</g>
+<g id="botton" display="none">
+ <polygon display="inline" fill="#F58113" points="55.687,89.84 87.538,89.84 87.538,120.775 24.804,120.729 "/>
+ <polygon display="inline" fill="#F58113" points="89.843,89.84 89.843,119.098 119.098,89.84 "/>
+</g>
+<g id="top" display="none">
+ <polygon display="inline" fill="#F58113" points="24.807,54.158 55.5,23 119.196,23.179 88.212,54.157 "/>
+</g>
+</svg>
diff --git a/gazebo/gui/images/view_angle_bottom.png b/gazebo/gui/images/view_angle_bottom.png
new file mode 100644
index 0000000..29e6d83
Binary files /dev/null and b/gazebo/gui/images/view_angle_bottom.png differ
diff --git a/gazebo/gui/images/view_angle_bottom.svg b/gazebo/gui/images/view_angle_bottom.svg
new file mode 100644
index 0000000..b1a37ab
--- /dev/null
+++ b/gazebo/gui/images/view_angle_bottom.svg
@@ -0,0 +1,99 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Generator: Adobe Illustrator 16.0.4, SVG Export Plug-In . SVG Version: 6.00 Build 0) -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg version="1.1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px" width="107.5px"
+ height="107.5px" viewBox="18 18.5 107.5 107.5" enable-background="new 18 18.5 107.5 107.5" xml:space="preserve">
+<g id="cube">
+ <g id="horz_left" display="none">
+ <g display="inline">
+ <rect x="15.641" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <g>
+ <rect x="31.255" y="27.25" fill="#D1D1D1" width="67.657" height="36.627"/>
+ <g>
+ <path fill="#D1D1D1" d="M124.18,84.302v28.269H35.436V84.302H124.18 M128.359,80.121H31.255v36.629h97.104V80.121
+ L128.359,80.121z"/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="horz_center" display="none">
+ <g display="inline">
+ <rect x="65.672" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <rect x="38.172" y="27.25" fill="#D1D1D1" width="67.656" height="36.627"/>
+ <g>
+ <rect x="25.539" y="82.211" fill="#FFFFFF" width="92.923" height="32.449"/>
+ <path fill="#D1D1D1" d="M116.371,84.302v28.269H27.629V84.302H116.371 M120.552,80.121H23.448v36.629h97.104V80.121
+ L120.552,80.121z"/>
+ </g>
+ </g>
+ </g>
+ <g id="vert_top" display="none">
+ <g display="inline">
+ <rect x="13.931" y="14.81" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="32.086" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,36.267v88.742H31.431V36.267H59.698 M63.879,32.086H27.25v97.104h36.629V32.086L63.879,32.086z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="vert_center" display="none">
+ <g display="inline">
+ <rect x="13.931" y="65.671" fill="#D1D1D1" width="116.139" height="12.657"/>
+ <rect x="80.122" y="38.171" fill="#D1D1D1" width="36.628" height="67.657"/>
+ <g>
+ <rect x="29.34" y="25.539" fill="#FFFFFF" width="32.448" height="92.923"/>
+ <path fill="#D1D1D1" d="M59.698,27.629v88.742H31.431V27.629H59.698 M63.879,23.448H27.25v97.104h36.629V23.448L63.879,23.448z"
+ />
+ </g>
+ </g>
+ </g>
+ <g id="vert_bottom" display="none">
+ <g display="inline">
+ <rect x="13.931" y="116.533" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="44.258" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,18.991v88.741H31.431V18.991H59.698 M63.879,14.811H27.25v97.104h36.629V14.811L63.879,14.811z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <path fill="#D1D1D1" d="M123.12,21.984c-0.004-0.134-0.025-0.269-0.079-0.399c0-0.001-0.001-0.002-0.001-0.003
+ c-0.005-0.011-0.013-0.019-0.019-0.029c-0.052-0.114-0.122-0.214-0.204-0.304c-0.027-0.029-0.06-0.054-0.091-0.081
+ c-0.073-0.063-0.151-0.116-0.237-0.16c-0.037-0.02-0.071-0.038-0.111-0.053c-0.127-0.048-0.261-0.081-0.402-0.081H55.308
+ c-0.152,0-0.304,0.031-0.443,0.088c-0.141,0.059-0.266,0.144-0.372,0.25L21.211,54.494c-0.001,0.002-0.002,0.004-0.003,0.006
+ c-0.096,0.096-0.165,0.208-0.221,0.326c-0.018,0.035-0.025,0.072-0.039,0.109c-0.034,0.099-0.055,0.199-0.061,0.305
+ c-0.002,0.024-0.016,0.046-0.016,0.071v66.661c0,0.638,0.517,1.152,1.152,1.152h66.665c0.442,0,0.814-0.258,1.009-0.623
+ l32.789-32.791c0.332-0.169,0.561-0.49,0.611-0.875c0.004-0.014,0.006-0.027,0.008-0.041c0.004-0.036,0.021-0.068,0.021-0.105
+ V22.027C123.128,22.012,123.12,21.998,123.12,21.984z M54.156,24.809v29.35h-29.35L54.156,24.809z M23.178,56.463l30.979,0.021
+ v31.629l-30.979,30.984V56.463z M87.538,120.775l-62.734-0.047L55.687,89.84h31.852V120.775L87.538,120.775z M87.538,87.438H56.46
+ V56.486l31.078,0.022V87.438z M88.212,54.157H56.46V23.179h62.736L88.212,54.157z M89.843,119.098V89.84h29.255L89.843,119.098z
+ M120.825,87.438H89.843v-31.65l30.982-30.979V87.438z"/>
+</g>
+<g id="front" display="none">
+ <rect x="23.178" y="56.463" display="inline" fill="#F58113" width="64.36" height="64.36"/>
+</g>
+<g id="right" display="none">
+ <polygon display="inline" fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,88.5 89.843,119.098 "/>
+</g>
+<g id="left" display="none">
+ <polygon display="inline" fill="#F58113" points="24.807,54.158 54.156,24.809 54.156,54.158 "/>
+ <polygon display="inline" fill="#F58113" points="23.178,56.463 54.157,56.484 54.157,88.113 23.178,119.098 "/>
+</g>
+<g id="back" display="none">
+ <polygon display="inline" fill="#F58113" points="56.46,23.179 119.196,23.179 88.212,54.157 56.46,54.157 "/>
+ <polygon display="inline" fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,87.438 89.843,87.438 "/>
+ <rect x="56.46" y="56.486" display="inline" fill="#F58113" width="31.078" height="31.078"/>
+</g>
+<g id="botton">
+ <polygon fill="#F58113" points="55.687,89.84 87.538,89.84 87.538,120.775 24.804,120.729 "/>
+ <polygon fill="#F58113" points="89.843,89.84 89.843,119.098 119.098,89.84 "/>
+</g>
+<g id="top" display="none">
+ <polygon display="inline" fill="#F58113" points="24.807,54.158 55.5,23 119.196,23.179 88.212,54.157 "/>
+</g>
+</svg>
diff --git a/gazebo/gui/images/view_angle_front.png b/gazebo/gui/images/view_angle_front.png
new file mode 100644
index 0000000..05ec1e2
Binary files /dev/null and b/gazebo/gui/images/view_angle_front.png differ
diff --git a/gazebo/gui/images/view_angle_front.svg b/gazebo/gui/images/view_angle_front.svg
new file mode 100644
index 0000000..7e3b1cf
--- /dev/null
+++ b/gazebo/gui/images/view_angle_front.svg
@@ -0,0 +1,99 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Generator: Adobe Illustrator 16.0.4, SVG Export Plug-In . SVG Version: 6.00 Build 0) -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg version="1.1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px" width="107.5px"
+ height="107.5px" viewBox="18 18.5 107.5 107.5" enable-background="new 18 18.5 107.5 107.5" xml:space="preserve">
+<g id="cube">
+ <g id="horz_left" display="none">
+ <g display="inline">
+ <rect x="15.641" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <g>
+ <rect x="31.255" y="27.25" fill="#D1D1D1" width="67.657" height="36.627"/>
+ <g>
+ <path fill="#D1D1D1" d="M124.18,84.302v28.269H35.436V84.302H124.18 M128.359,80.121H31.255v36.629h97.104V80.121
+ L128.359,80.121z"/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="horz_center" display="none">
+ <g display="inline">
+ <rect x="65.672" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <rect x="38.172" y="27.25" fill="#D1D1D1" width="67.656" height="36.627"/>
+ <g>
+ <rect x="25.539" y="82.211" fill="#FFFFFF" width="92.923" height="32.449"/>
+ <path fill="#D1D1D1" d="M116.371,84.302v28.269H27.629V84.302H116.371 M120.552,80.121H23.448v36.629h97.104V80.121
+ L120.552,80.121z"/>
+ </g>
+ </g>
+ </g>
+ <g id="vert_top" display="none">
+ <g display="inline">
+ <rect x="13.931" y="14.81" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="32.086" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,36.267v88.742H31.431V36.267H59.698 M63.879,32.086H27.25v97.104h36.629V32.086L63.879,32.086z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="vert_center" display="none">
+ <g display="inline">
+ <rect x="13.931" y="65.671" fill="#D1D1D1" width="116.139" height="12.657"/>
+ <rect x="80.122" y="38.171" fill="#D1D1D1" width="36.628" height="67.657"/>
+ <g>
+ <rect x="29.34" y="25.539" fill="#FFFFFF" width="32.448" height="92.923"/>
+ <path fill="#D1D1D1" d="M59.698,27.629v88.742H31.431V27.629H59.698 M63.879,23.448H27.25v97.104h36.629V23.448L63.879,23.448z"
+ />
+ </g>
+ </g>
+ </g>
+ <g id="vert_bottom" display="none">
+ <g display="inline">
+ <rect x="13.931" y="116.533" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="44.258" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,18.991v88.741H31.431V18.991H59.698 M63.879,14.811H27.25v97.104h36.629V14.811L63.879,14.811z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <path fill="#D1D1D1" d="M123.12,21.984c-0.004-0.134-0.025-0.269-0.079-0.399c0-0.001-0.001-0.002-0.001-0.003
+ c-0.005-0.011-0.013-0.019-0.019-0.029c-0.052-0.114-0.122-0.214-0.204-0.304c-0.027-0.029-0.06-0.054-0.091-0.081
+ c-0.073-0.063-0.151-0.116-0.237-0.16c-0.037-0.02-0.071-0.038-0.111-0.053c-0.127-0.048-0.261-0.081-0.402-0.081H55.308
+ c-0.152,0-0.304,0.031-0.443,0.088c-0.141,0.059-0.266,0.144-0.372,0.25L21.211,54.494c-0.001,0.002-0.002,0.004-0.003,0.006
+ c-0.096,0.096-0.165,0.208-0.221,0.326c-0.018,0.035-0.025,0.072-0.039,0.109c-0.034,0.099-0.055,0.199-0.061,0.305
+ c-0.002,0.024-0.016,0.046-0.016,0.071v66.661c0,0.638,0.517,1.152,1.152,1.152h66.665c0.442,0,0.814-0.258,1.009-0.623
+ l32.789-32.791c0.332-0.169,0.561-0.49,0.611-0.875c0.004-0.014,0.006-0.027,0.008-0.041c0.004-0.036,0.021-0.068,0.021-0.105
+ V22.027C123.128,22.012,123.12,21.998,123.12,21.984z M54.156,24.809v29.35h-29.35L54.156,24.809z M23.178,56.463l30.979,0.021
+ v31.629l-30.979,30.984V56.463z M87.538,120.775l-62.734-0.047L55.687,89.84h31.852V120.775L87.538,120.775z M87.538,87.438H56.46
+ V56.486l31.078,0.022V87.438z M88.212,54.157H56.46V23.179h62.736L88.212,54.157z M89.843,119.098V89.84h29.255L89.843,119.098z
+ M120.825,87.438H89.843v-31.65l30.982-30.979V87.438z"/>
+</g>
+<g id="front">
+ <rect x="23.178" y="56.463" fill="#F58113" width="64.36" height="64.36"/>
+</g>
+<g id="right" display="none">
+ <polygon display="inline" fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,88.5 89.843,119.098 "/>
+</g>
+<g id="left" display="none">
+ <polygon display="inline" fill="#F58113" points="24.807,54.158 54.156,24.809 54.156,54.158 "/>
+ <polygon display="inline" fill="#F58113" points="23.178,56.463 54.157,56.484 54.157,88.113 23.178,119.098 "/>
+</g>
+<g id="back" display="none">
+ <polygon display="inline" fill="#F58113" points="56.46,23.179 119.196,23.179 88.212,54.157 56.46,54.157 "/>
+ <polygon display="inline" fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,87.438 89.843,87.438 "/>
+ <rect x="56.46" y="56.486" display="inline" fill="#F58113" width="31.078" height="31.078"/>
+</g>
+<g id="botton" display="none">
+ <polygon display="inline" fill="#F58113" points="55.687,89.84 87.538,89.84 87.538,120.775 24.804,120.729 "/>
+ <polygon display="inline" fill="#F58113" points="89.843,89.84 89.843,119.098 119.098,89.84 "/>
+</g>
+<g id="top" display="none">
+ <polygon display="inline" fill="#F58113" points="24.807,54.158 55.5,23 119.196,23.179 88.212,54.157 "/>
+</g>
+</svg>
diff --git a/gazebo/gui/images/view_angle_home.png b/gazebo/gui/images/view_angle_home.png
new file mode 100644
index 0000000..80feee4
Binary files /dev/null and b/gazebo/gui/images/view_angle_home.png differ
diff --git a/gazebo/gui/images/view_angle_home.svg b/gazebo/gui/images/view_angle_home.svg
new file mode 100644
index 0000000..8909edb
--- /dev/null
+++ b/gazebo/gui/images/view_angle_home.svg
@@ -0,0 +1,87 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Generator: Adobe Illustrator 16.0.4, SVG Export Plug-In . SVG Version: 6.00 Build 0) -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg version="1.1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px" width="107.5px"
+ height="107.5px" viewBox="18 18.5 107.5 107.5" enable-background="new 18 18.5 107.5 107.5" xml:space="preserve">
+<g id="template" display="none">
+ <g id="horz_left">
+ <g display="inline">
+ <rect x="15.641" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <g>
+ <rect x="31.255" y="27.25" fill="#D1D1D1" width="67.657" height="36.627"/>
+ <g>
+ <path fill="#D1D1D1" d="M124.18,84.302v28.269H35.436V84.302H124.18 M128.359,80.121H31.255v36.629h97.104V80.121
+ L128.359,80.121z"/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="horz_center">
+ <g display="inline">
+ <rect x="65.672" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <rect x="38.172" y="27.25" fill="#D1D1D1" width="67.656" height="36.627"/>
+ <g>
+ <rect x="25.539" y="82.211" fill="#FFFFFF" width="92.923" height="32.449"/>
+ <path fill="#D1D1D1" d="M116.371,84.302v28.269H27.629V84.302H116.371 M120.552,80.121H23.448v36.629h97.104V80.121
+ L120.552,80.121z"/>
+ </g>
+ </g>
+ </g>
+ <g id="vert_top">
+ <g display="inline">
+ <rect x="13.931" y="14.81" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="32.086" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,36.267v88.742H31.431V36.267H59.698 M63.879,32.086H27.25v97.104h36.629V32.086L63.879,32.086z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="vert_center">
+ <g display="inline">
+ <rect x="13.931" y="65.671" fill="#D1D1D1" width="116.139" height="12.657"/>
+ <rect x="80.122" y="38.171" fill="#D1D1D1" width="36.628" height="67.657"/>
+ <g>
+ <rect x="29.34" y="25.539" fill="#FFFFFF" width="32.448" height="92.923"/>
+ <path fill="#D1D1D1" d="M59.698,27.629v88.742H31.431V27.629H59.698 M63.879,23.448H27.25v97.104h36.629V23.448L63.879,23.448z"
+ />
+ </g>
+ </g>
+ </g>
+ <g id="vert_bottom">
+ <g display="inline">
+ <rect x="13.931" y="116.533" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="44.258" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,18.991v88.741H31.431V18.991H59.698 M63.879,14.811H27.25v97.104h36.629V14.811L63.879,14.811z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <polygon display="inline" fill="#D1D1D1" points="213.762,62.668 213.762,38.464 199.429,38.464 199.429,48.046 181,29.25
+ 129.81,81.465 142.095,81.465 142.095,115.25 173.833,115.25 173.833,87.607 188.167,87.607 188.167,115.25 219.905,115.25
+ 219.905,81.465 232.19,81.465 "/>
+
+ <image display="inline" overflow="visible" width="94" height="76" xlink:href="../../../Desktop/Screen Shot 2015-06-26 at 2.59.15 PM.png" transform="matrix(0.9707 0 0 0.9707 26.125 35.3618)">
+ </image>
+</g>
+<g id="home" display="none">
+ <polygon display="inline" fill="#D1D1D1" points="99.125,16.625 99.125,44.125 141.375,44.125 141.375,16.625 145.875,18.125
+ 150.625,10.875 121.375,-9.875 109.625,-2.625 108.875,-9.875 99.125,-9.875 99.125,5.375 88.875,10.625 94.375,19.625 "/>
+ <rect x="49.75" y="45.625" display="inline" fill="#D1D1D1" width="10" height="11.625"/>
+
+ <rect x="39.216" y="54.42" transform="matrix(0.8282 -0.5604 0.5604 0.8282 -23.7792 41.9269)" display="inline" fill="#D1D1D1" width="34.576" height="10.667"/>
+
+ <rect x="66.966" y="54.42" transform="matrix(-0.8282 -0.5604 0.5604 -0.8282 120.5506 156.4568)" display="inline" fill="#D1D1D1" width="34.576" height="10.667"/>
+ <polygon display="inline" fill="#D1D1D1" points="49.75,71 49.75,98 77.5,98 77.5,86.5 85.875,86.5 85.875,98 92,98 92,71 "/>
+</g>
+<g id="Layer_3">
+ <polygon fill="#D1D1D1" points="88.764,113.925 88.764,93.813 75.807,93.813 75.807,113.925 39.062,113.925 39.062,72.153
+ 31.982,76.575 22.734,62.907 39.062,51.86 39.062,32.896 54.533,32.896 54.533,41.599 70.971,30.575 120.766,62.907
+ 111.002,77.349 105.201,72.927 104.428,113.925 "/>
+</g>
+</svg>
diff --git a/gazebo/gui/images/view_angle_left.png b/gazebo/gui/images/view_angle_left.png
new file mode 100644
index 0000000..b25c7b1
Binary files /dev/null and b/gazebo/gui/images/view_angle_left.png differ
diff --git a/gazebo/gui/images/view_angle_left.svg b/gazebo/gui/images/view_angle_left.svg
new file mode 100644
index 0000000..67746f6
--- /dev/null
+++ b/gazebo/gui/images/view_angle_left.svg
@@ -0,0 +1,99 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Generator: Adobe Illustrator 16.0.4, SVG Export Plug-In . SVG Version: 6.00 Build 0) -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg version="1.1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px" width="107.5px"
+ height="107.5px" viewBox="18 18.5 107.5 107.5" enable-background="new 18 18.5 107.5 107.5" xml:space="preserve">
+<g id="cube">
+ <g id="horz_left" display="none">
+ <g display="inline">
+ <rect x="15.641" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <g>
+ <rect x="31.255" y="27.25" fill="#D1D1D1" width="67.657" height="36.627"/>
+ <g>
+ <path fill="#D1D1D1" d="M124.18,84.302v28.269H35.436V84.302H124.18 M128.359,80.121H31.255v36.629h97.104V80.121
+ L128.359,80.121z"/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="horz_center" display="none">
+ <g display="inline">
+ <rect x="65.672" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <rect x="38.172" y="27.25" fill="#D1D1D1" width="67.656" height="36.627"/>
+ <g>
+ <rect x="25.539" y="82.211" fill="#FFFFFF" width="92.923" height="32.449"/>
+ <path fill="#D1D1D1" d="M116.371,84.302v28.269H27.629V84.302H116.371 M120.552,80.121H23.448v36.629h97.104V80.121
+ L120.552,80.121z"/>
+ </g>
+ </g>
+ </g>
+ <g id="vert_top" display="none">
+ <g display="inline">
+ <rect x="13.931" y="14.81" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="32.086" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,36.267v88.742H31.431V36.267H59.698 M63.879,32.086H27.25v97.104h36.629V32.086L63.879,32.086z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="vert_center" display="none">
+ <g display="inline">
+ <rect x="13.931" y="65.671" fill="#D1D1D1" width="116.139" height="12.657"/>
+ <rect x="80.122" y="38.171" fill="#D1D1D1" width="36.628" height="67.657"/>
+ <g>
+ <rect x="29.34" y="25.539" fill="#FFFFFF" width="32.448" height="92.923"/>
+ <path fill="#D1D1D1" d="M59.698,27.629v88.742H31.431V27.629H59.698 M63.879,23.448H27.25v97.104h36.629V23.448L63.879,23.448z"
+ />
+ </g>
+ </g>
+ </g>
+ <g id="vert_bottom" display="none">
+ <g display="inline">
+ <rect x="13.931" y="116.533" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="44.258" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,18.991v88.741H31.431V18.991H59.698 M63.879,14.811H27.25v97.104h36.629V14.811L63.879,14.811z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <path fill="#D1D1D1" d="M123.12,21.984c-0.004-0.134-0.025-0.269-0.079-0.399c0-0.001-0.001-0.002-0.001-0.003
+ c-0.005-0.011-0.013-0.019-0.019-0.029c-0.052-0.114-0.122-0.214-0.204-0.304c-0.027-0.029-0.06-0.054-0.091-0.081
+ c-0.073-0.063-0.151-0.116-0.237-0.16c-0.037-0.02-0.071-0.038-0.111-0.053c-0.127-0.048-0.261-0.081-0.402-0.081H55.308
+ c-0.152,0-0.304,0.031-0.443,0.088c-0.141,0.059-0.266,0.144-0.372,0.25L21.211,54.494c-0.001,0.002-0.002,0.004-0.003,0.006
+ c-0.096,0.096-0.165,0.208-0.221,0.326c-0.018,0.035-0.025,0.072-0.039,0.109c-0.034,0.099-0.055,0.199-0.061,0.305
+ c-0.002,0.024-0.016,0.046-0.016,0.071v66.661c0,0.638,0.517,1.152,1.152,1.152h66.665c0.442,0,0.814-0.258,1.009-0.623
+ l32.789-32.791c0.332-0.169,0.561-0.49,0.611-0.875c0.004-0.014,0.006-0.027,0.008-0.041c0.004-0.036,0.021-0.068,0.021-0.105
+ V22.027C123.128,22.012,123.12,21.998,123.12,21.984z M54.156,24.809v29.35h-29.35L54.156,24.809z M23.178,56.463l30.979,0.021
+ v31.629l-30.979,30.984V56.463z M87.538,120.775l-62.734-0.047L55.687,89.84h31.852V120.775L87.538,120.775z M87.538,87.438H56.46
+ V56.486l31.078,0.022V87.438z M88.212,54.157H56.46V23.179h62.736L88.212,54.157z M89.843,119.098V89.84h29.255L89.843,119.098z
+ M120.825,87.438H89.843v-31.65l30.982-30.979V87.438z"/>
+</g>
+<g id="front" display="none">
+ <rect x="23.178" y="56.463" display="inline" fill="#F58113" width="64.36" height="64.36"/>
+</g>
+<g id="right" display="none">
+ <polygon display="inline" fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,88.5 89.843,119.098 "/>
+</g>
+<g id="left">
+ <polygon fill="#F58113" points="24.807,54.158 54.156,24.809 54.156,54.158 "/>
+ <polygon fill="#F58113" points="23.178,56.463 54.157,56.484 54.157,88.113 23.178,119.098 "/>
+</g>
+<g id="back" display="none">
+ <polygon display="inline" fill="#F58113" points="56.46,23.179 119.196,23.179 88.212,54.157 56.46,54.157 "/>
+ <polygon display="inline" fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,87.438 89.843,87.438 "/>
+ <rect x="56.46" y="56.486" display="inline" fill="#F58113" width="31.078" height="31.078"/>
+</g>
+<g id="botton" display="none">
+ <polygon display="inline" fill="#F58113" points="55.687,89.84 87.538,89.84 87.538,120.775 24.804,120.729 "/>
+ <polygon display="inline" fill="#F58113" points="89.843,89.84 89.843,119.098 119.098,89.84 "/>
+</g>
+<g id="top" display="none">
+ <polygon display="inline" fill="#F58113" points="24.807,54.158 55.5,23 119.196,23.179 88.212,54.157 "/>
+</g>
+</svg>
diff --git a/gazebo/gui/images/view_angle_right.png b/gazebo/gui/images/view_angle_right.png
new file mode 100644
index 0000000..31b0dc0
Binary files /dev/null and b/gazebo/gui/images/view_angle_right.png differ
diff --git a/gazebo/gui/images/view_angle_right.svg b/gazebo/gui/images/view_angle_right.svg
new file mode 100644
index 0000000..2f4e07c
--- /dev/null
+++ b/gazebo/gui/images/view_angle_right.svg
@@ -0,0 +1,99 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Generator: Adobe Illustrator 16.0.4, SVG Export Plug-In . SVG Version: 6.00 Build 0) -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg version="1.1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px" width="107.5px"
+ height="107.5px" viewBox="18 18.5 107.5 107.5" enable-background="new 18 18.5 107.5 107.5" xml:space="preserve">
+<g id="cube">
+ <g id="horz_left" display="none">
+ <g display="inline">
+ <rect x="15.641" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <g>
+ <rect x="31.255" y="27.25" fill="#D1D1D1" width="67.657" height="36.627"/>
+ <g>
+ <path fill="#D1D1D1" d="M124.18,84.302v28.269H35.436V84.302H124.18 M128.359,80.121H31.255v36.629h97.104V80.121
+ L128.359,80.121z"/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="horz_center" display="none">
+ <g display="inline">
+ <rect x="65.672" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <rect x="38.172" y="27.25" fill="#D1D1D1" width="67.656" height="36.627"/>
+ <g>
+ <rect x="25.539" y="82.211" fill="#FFFFFF" width="92.923" height="32.449"/>
+ <path fill="#D1D1D1" d="M116.371,84.302v28.269H27.629V84.302H116.371 M120.552,80.121H23.448v36.629h97.104V80.121
+ L120.552,80.121z"/>
+ </g>
+ </g>
+ </g>
+ <g id="vert_top" display="none">
+ <g display="inline">
+ <rect x="13.931" y="14.81" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="32.086" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,36.267v88.742H31.431V36.267H59.698 M63.879,32.086H27.25v97.104h36.629V32.086L63.879,32.086z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="vert_center" display="none">
+ <g display="inline">
+ <rect x="13.931" y="65.671" fill="#D1D1D1" width="116.139" height="12.657"/>
+ <rect x="80.122" y="38.171" fill="#D1D1D1" width="36.628" height="67.657"/>
+ <g>
+ <rect x="29.34" y="25.539" fill="#FFFFFF" width="32.448" height="92.923"/>
+ <path fill="#D1D1D1" d="M59.698,27.629v88.742H31.431V27.629H59.698 M63.879,23.448H27.25v97.104h36.629V23.448L63.879,23.448z"
+ />
+ </g>
+ </g>
+ </g>
+ <g id="vert_bottom" display="none">
+ <g display="inline">
+ <rect x="13.931" y="116.533" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="44.258" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,18.991v88.741H31.431V18.991H59.698 M63.879,14.811H27.25v97.104h36.629V14.811L63.879,14.811z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <path fill="#D1D1D1" d="M123.12,21.984c-0.004-0.134-0.025-0.269-0.079-0.399c0-0.001-0.001-0.002-0.001-0.003
+ c-0.005-0.011-0.013-0.019-0.019-0.029c-0.052-0.114-0.122-0.214-0.204-0.304c-0.027-0.029-0.06-0.054-0.091-0.081
+ c-0.073-0.063-0.151-0.116-0.237-0.16c-0.037-0.02-0.071-0.038-0.111-0.053c-0.127-0.048-0.261-0.081-0.402-0.081H55.308
+ c-0.152,0-0.304,0.031-0.443,0.088c-0.141,0.059-0.266,0.144-0.372,0.25L21.211,54.494c-0.001,0.002-0.002,0.004-0.003,0.006
+ c-0.096,0.096-0.165,0.208-0.221,0.326c-0.018,0.035-0.025,0.072-0.039,0.109c-0.034,0.099-0.055,0.199-0.061,0.305
+ c-0.002,0.024-0.016,0.046-0.016,0.071v66.661c0,0.638,0.517,1.152,1.152,1.152h66.665c0.442,0,0.814-0.258,1.009-0.623
+ l32.789-32.791c0.332-0.169,0.561-0.49,0.611-0.875c0.004-0.014,0.006-0.027,0.008-0.041c0.004-0.036,0.021-0.068,0.021-0.105
+ V22.027C123.128,22.012,123.12,21.998,123.12,21.984z M54.156,24.809v29.35h-29.35L54.156,24.809z M23.178,56.463l30.979,0.021
+ v31.629l-30.979,30.984V56.463z M87.538,120.775l-62.734-0.047L55.687,89.84h31.852V120.775L87.538,120.775z M87.538,87.438H56.46
+ V56.486l31.078,0.022V87.438z M88.212,54.157H56.46V23.179h62.736L88.212,54.157z M89.843,119.098V89.84h29.255L89.843,119.098z
+ M120.825,87.438H89.843v-31.65l30.982-30.979V87.438z"/>
+</g>
+<g id="front" display="none">
+ <rect x="23.178" y="56.463" display="inline" fill="#F58113" width="64.36" height="64.36"/>
+</g>
+<g id="right">
+ <polygon fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,88.5 89.843,119.098 "/>
+</g>
+<g id="left" display="none">
+ <polygon display="inline" fill="#F58113" points="24.807,54.158 54.156,24.809 54.156,54.158 "/>
+ <polygon display="inline" fill="#F58113" points="23.178,56.463 54.157,56.484 54.157,88.113 23.178,119.098 "/>
+</g>
+<g id="back" display="none">
+ <polygon display="inline" fill="#F58113" points="56.46,23.179 119.196,23.179 88.212,54.157 56.46,54.157 "/>
+ <polygon display="inline" fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,87.438 89.843,87.438 "/>
+ <rect x="56.46" y="56.486" display="inline" fill="#F58113" width="31.078" height="31.078"/>
+</g>
+<g id="botton" display="none">
+ <polygon display="inline" fill="#F58113" points="55.687,89.84 87.538,89.84 87.538,120.775 24.804,120.729 "/>
+ <polygon display="inline" fill="#F58113" points="89.843,89.84 89.843,119.098 119.098,89.84 "/>
+</g>
+<g id="top" display="none">
+ <polygon display="inline" fill="#F58113" points="24.807,54.158 55.5,23 119.196,23.179 88.212,54.157 "/>
+</g>
+</svg>
diff --git a/gazebo/gui/images/view_angle_top.png b/gazebo/gui/images/view_angle_top.png
new file mode 100644
index 0000000..ad30618
Binary files /dev/null and b/gazebo/gui/images/view_angle_top.png differ
diff --git a/gazebo/gui/images/view_angle_top.svg b/gazebo/gui/images/view_angle_top.svg
new file mode 100644
index 0000000..c468545
--- /dev/null
+++ b/gazebo/gui/images/view_angle_top.svg
@@ -0,0 +1,99 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Generator: Adobe Illustrator 16.0.4, SVG Export Plug-In . SVG Version: 6.00 Build 0) -->
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<svg version="1.1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px" width="107.5px"
+ height="107.5px" viewBox="18 18.5 107.5 107.5" enable-background="new 18 18.5 107.5 107.5" xml:space="preserve">
+<g id="cube">
+ <g id="horz_left" display="none">
+ <g display="inline">
+ <rect x="15.641" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <g>
+ <rect x="31.255" y="27.25" fill="#D1D1D1" width="67.657" height="36.627"/>
+ <g>
+ <path fill="#D1D1D1" d="M124.18,84.302v28.269H35.436V84.302H124.18 M128.359,80.121H31.255v36.629h97.104V80.121
+ L128.359,80.121z"/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="horz_center" display="none">
+ <g display="inline">
+ <rect x="65.672" y="13.931" fill="#D1D1D1" width="12.656" height="116.139"/>
+ <rect x="38.172" y="27.25" fill="#D1D1D1" width="67.656" height="36.627"/>
+ <g>
+ <rect x="25.539" y="82.211" fill="#FFFFFF" width="92.923" height="32.449"/>
+ <path fill="#D1D1D1" d="M116.371,84.302v28.269H27.629V84.302H116.371 M120.552,80.121H23.448v36.629h97.104V80.121
+ L120.552,80.121z"/>
+ </g>
+ </g>
+ </g>
+ <g id="vert_top" display="none">
+ <g display="inline">
+ <rect x="13.931" y="14.81" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="32.086" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,36.267v88.742H31.431V36.267H59.698 M63.879,32.086H27.25v97.104h36.629V32.086L63.879,32.086z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <g id="vert_center" display="none">
+ <g display="inline">
+ <rect x="13.931" y="65.671" fill="#D1D1D1" width="116.139" height="12.657"/>
+ <rect x="80.122" y="38.171" fill="#D1D1D1" width="36.628" height="67.657"/>
+ <g>
+ <rect x="29.34" y="25.539" fill="#FFFFFF" width="32.448" height="92.923"/>
+ <path fill="#D1D1D1" d="M59.698,27.629v88.742H31.431V27.629H59.698 M63.879,23.448H27.25v97.104h36.629V23.448L63.879,23.448z"
+ />
+ </g>
+ </g>
+ </g>
+ <g id="vert_bottom" display="none">
+ <g display="inline">
+ <rect x="13.931" y="116.533" fill="#D1D1D1" width="116.139" height="12.656"/>
+ <g>
+ <rect x="80.122" y="44.258" fill="#D1D1D1" width="36.628" height="67.656"/>
+ <g>
+ <path fill="#D1D1D1" d="M59.698,18.991v88.741H31.431V18.991H59.698 M63.879,14.811H27.25v97.104h36.629V14.811L63.879,14.811z
+ "/>
+ </g>
+ </g>
+ </g>
+ </g>
+ <path fill="#D1D1D1" d="M123.12,21.984c-0.004-0.134-0.025-0.269-0.079-0.399c0-0.001-0.001-0.002-0.001-0.003
+ c-0.005-0.011-0.013-0.019-0.019-0.029c-0.052-0.114-0.122-0.214-0.204-0.304c-0.027-0.029-0.06-0.054-0.091-0.081
+ c-0.073-0.063-0.151-0.116-0.237-0.16c-0.037-0.02-0.071-0.038-0.111-0.053c-0.127-0.048-0.261-0.081-0.402-0.081H55.308
+ c-0.152,0-0.304,0.031-0.443,0.088c-0.141,0.059-0.266,0.144-0.372,0.25L21.211,54.494c-0.001,0.002-0.002,0.004-0.003,0.006
+ c-0.096,0.096-0.165,0.208-0.221,0.326c-0.018,0.035-0.025,0.072-0.039,0.109c-0.034,0.099-0.055,0.199-0.061,0.305
+ c-0.002,0.024-0.016,0.046-0.016,0.071v66.661c0,0.638,0.517,1.152,1.152,1.152h66.665c0.442,0,0.814-0.258,1.009-0.623
+ l32.789-32.791c0.332-0.169,0.561-0.49,0.611-0.875c0.004-0.014,0.006-0.027,0.008-0.041c0.004-0.036,0.021-0.068,0.021-0.105
+ V22.027C123.128,22.012,123.12,21.998,123.12,21.984z M54.156,24.809v29.35h-29.35L54.156,24.809z M23.178,56.463l30.979,0.021
+ v31.629l-30.979,30.984V56.463z M87.538,120.775l-62.734-0.047L55.687,89.84h31.852V120.775L87.538,120.775z M87.538,87.438H56.46
+ V56.486l31.078,0.022V87.438z M88.212,54.157H56.46V23.179h62.736L88.212,54.157z M89.843,119.098V89.84h29.255L89.843,119.098z
+ M120.825,87.438H89.843v-31.65l30.982-30.979V87.438z"/>
+</g>
+<g id="front" display="none">
+ <rect x="23.178" y="56.463" display="inline" fill="#F58113" width="64.36" height="64.36"/>
+</g>
+<g id="right" display="none">
+ <polygon display="inline" fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,88.5 89.843,119.098 "/>
+</g>
+<g id="left" display="none">
+ <polygon display="inline" fill="#F58113" points="24.807,54.158 54.156,24.809 54.156,54.158 "/>
+ <polygon display="inline" fill="#F58113" points="23.178,56.463 54.157,56.484 54.157,88.113 23.178,119.098 "/>
+</g>
+<g id="back" display="none">
+ <polygon display="inline" fill="#F58113" points="56.46,23.179 119.196,23.179 88.212,54.157 56.46,54.157 "/>
+ <polygon display="inline" fill="#F58113" points="89.843,55.787 120.825,24.809 120.825,87.438 89.843,87.438 "/>
+ <rect x="56.46" y="56.486" display="inline" fill="#F58113" width="31.078" height="31.078"/>
+</g>
+<g id="botton" display="none">
+ <polygon display="inline" fill="#F58113" points="55.687,89.84 87.538,89.84 87.538,120.775 24.804,120.729 "/>
+ <polygon display="inline" fill="#F58113" points="89.843,89.84 89.843,119.098 119.098,89.84 "/>
+</g>
+<g id="top">
+ <polygon fill="#F58113" points="24.807,54.158 55.5,23 119.196,23.179 88.212,54.157 "/>
+</g>
+</svg>
diff --git a/gazebo/gui/main.cc b/gazebo/gui/main.cc
index f1d23d2..56f7029 100644
--- a/gazebo/gui/main.cc
+++ b/gazebo/gui/main.cc
@@ -14,11 +14,13 @@
* limitations under the License.
*
*/
+#include "gazebo/gui/qt.h"
#include "gazebo/gui/GuiIface.hh"
//////////////////////////////////////////////////
int main(int _argc, char **_argv)
{
+ Q_INIT_RESOURCE(resources);
int result = 0;
if (!gazebo::gui::run(_argc, _argv))
result = -1;
diff --git a/gazebo/gui/model/CMakeLists.txt b/gazebo/gui/model/CMakeLists.txt
index 134ee81..ed00ec8 100644
--- a/gazebo/gui/model/CMakeLists.txt
+++ b/gazebo/gui/model/CMakeLists.txt
@@ -1,34 +1,94 @@
+if (HAVE_GRAPHVIZ)
+ add_subdirectory(qgv)
+endif()
+
include (${gazebo_cmake_dir}/GazeboUtils.cmake)
include (${QT_USE_FILE})
set (sources
+ CollisionConfig.cc
+ ExtrudeDialog.cc
ImportDialog.cc
JointInspector.cc
JointMaker.cc
+ LinkConfig.cc
+ LinkInspector.cc
ModelCreator.cc
+ ModelData.cc
ModelEditor.cc
ModelEditorEvents.cc
ModelEditorPalette.cc
+ VisualConfig.cc
)
set (qt_headers
+ CollisionConfig.hh
+ ExtrudeDialog.hh
ImportDialog.hh
JointInspector.hh
JointMaker.hh
+ LinkConfig.hh
+ LinkInspector.hh
ModelCreator.hh
ModelData.hh
ModelEditor.hh
ModelEditorPalette.hh
+ VisualConfig.hh
)
set (headers
ModelEditorEvents.hh
)
-# MainWindow is using screen rendering
+set (install_headers
+ ModelEditor.hh
+)
+
if (ENABLE_SCREEN_TESTS)
- set (qt_tests ${qt_tests} JointMaker_TEST.cc)
+ set (qt_tests ${qt_tests}
+ JointMaker_TEST.cc
+ ExtrudeDialog_TEST.cc
+ ModelCreator_TEST.cc
+ ModelEditor_TEST.cc
+ ModelEditorPalette_TEST.cc
+ )
+endif()
+
+set (libraries_to_link
+ gazebo_rendering
+ gazebo_msgs
+ gazebo_common
+)
+
+if (HAVE_GRAPHVIZ)
+ set (qt_headers
+ ${qt_headers}
+ GraphScene.hh
+ GraphView.hh
+ )
+
+ set (sources
+ ${sources}
+ GraphScene.cc
+ GraphView.cc
+ SchematicViewWidget.cc
+ )
+
+ set (headers
+ ${headers}
+ SchematicViewWidget.hh
+ )
+
+ set (qt_tests
+ ${qt_tests}
+ SchematicViewWidget_TEST.cc
+ )
+
+ set (libraries_to_link
+ ${libraries_to_link}
+ gazebo_gui_model_qgv
+ )
endif()
# Generate executables for each of the QT unit tests
@@ -40,13 +100,18 @@ QT4_ADD_RESOURCES(resources_RCC ${resources})
include_directories(${CMAKE_CURRENT_BINARY_DIR})
-gz_add_library(gazebo_gui_model ${sources} ${headers_MOC} ${headers} ${resources_RCC})
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_GUI_MODEL")
+
+if (WIN32)
+ add_library(gazebo_gui_model STATIC ${sources} ${headers_MOC} ${headers} ${resources_RCC})
+ target_link_libraries(gazebo_gui_model ${general_libraries})
+else()
+ gz_add_library(gazebo_gui_model ${sources} ${headers_MOC} ${headers} ${resources_RCC})
+endif()
target_link_libraries(gazebo_gui_model
- gazebo_common
-# gazebo_transport
-# gazebo_sdf_interface
- gazebo_rendering
- gazebo_msgs)
+ ${libraries_to_link}
+)
gz_install_library(gazebo_gui_model)
+gz_install_includes("gui/model" ${install_headers})
diff --git a/gazebo/gui/model/CollisionConfig.cc b/gazebo/gui/model/CollisionConfig.cc
new file mode 100644
index 0000000..d77b5da
--- /dev/null
+++ b/gazebo/gui/model/CollisionConfig.cc
@@ -0,0 +1,312 @@
+/*
+ * 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/common/Console.hh"
+#include "gazebo/gui/ConfigWidget.hh"
+#include "gazebo/gui/model/CollisionConfig.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+CollisionConfig::CollisionConfig()
+{
+ this->setObjectName("CollisionConfig");
+
+ // Layout for list
+ this->listLayout = new QVBoxLayout();
+ this->listLayout->setContentsMargins(0, 0, 0, 0);
+ this->listLayout->setAlignment(Qt::AlignTop);
+
+ // Widget for list, which will be scrollable
+ QWidget *listWidget = new QWidget();
+ listWidget->setLayout(this->listLayout);
+ listWidget->setStyleSheet("QWidget{background-color: #808080}");
+
+ // Scroll area for list
+ QScrollArea *scrollArea = new QScrollArea;
+ scrollArea->setWidget(listWidget);
+ scrollArea->setWidgetResizable(true);
+
+ // Add Collision button
+ QPushButton *addCollisionButton = new QPushButton(tr("+ &Another Collision"));
+ addCollisionButton->setMaximumWidth(200);
+ connect(addCollisionButton, SIGNAL(clicked()), this, SLOT(OnAddCollision()));
+
+ // Main layout
+ QVBoxLayout *mainLayout = new QVBoxLayout;
+ mainLayout->addWidget(scrollArea);
+ mainLayout->addWidget(addCollisionButton);
+ mainLayout->setContentsMargins(0, 0, 0, 0);
+ this->setLayout(mainLayout);
+
+ this->counter = 0;
+ this->signalMapper = new QSignalMapper(this);
+
+ connect(this->signalMapper, SIGNAL(mapped(int)),
+ this, SLOT(OnRemoveCollision(int)));
+}
+
+/////////////////////////////////////////////////
+CollisionConfig::~CollisionConfig()
+{
+ while (!this->configs.empty())
+ {
+ auto config = this->configs.begin();
+ this->configs.erase(config);
+ }
+}
+
+/////////////////////////////////////////////////
+void CollisionConfig::OnAddCollision()
+{
+ std::stringstream collisionIndex;
+ collisionIndex << "collision_" << this->counter;
+ this->AddCollision(collisionIndex.str());
+ emit CollisionAdded(collisionIndex.str());
+}
+
+/////////////////////////////////////////////////
+unsigned int CollisionConfig::GetCollisionCount() const
+{
+ return this->configs.size();
+}
+
+/////////////////////////////////////////////////
+void CollisionConfig::Reset()
+{
+ for (auto &it : this->configs)
+ {
+ this->listLayout->removeWidget(it.second->widget);
+ delete it.second;
+ }
+
+ this->configs.clear();
+}
+
+/////////////////////////////////////////////////
+void CollisionConfig::UpdateCollision(const std::string &_name,
+ ConstCollisionPtr _collisionMsg)
+{
+ for (auto &it : this->configs)
+ {
+ if (it.second->name == _name)
+ {
+ CollisionConfigData *configData = it.second;
+ configData->configWidget->UpdateFromMsg(_collisionMsg.get());
+ break;
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void CollisionConfig::AddCollision(const std::string &_name,
+ const msgs::Collision *_collisionMsg)
+{
+ // Header button
+ QRadioButton *headerButton = new QRadioButton();
+ headerButton->setChecked(false);
+ headerButton->setFocusPolicy(Qt::NoFocus);
+ headerButton->setText(QString(_name.c_str()));
+ headerButton->setStyleSheet(
+ "QRadioButton {\
+ color: #d0d0d0;\
+ }\
+ QRadioButton::indicator::unchecked {\
+ image: url(:/images/right_arrow.png);\
+ }\
+ QRadioButton::indicator::checked {\
+ image: url(:/images/down_arrow.png);\
+ }");
+
+ // Remove button
+ QToolButton *removeCollisionButton = new QToolButton(this);
+ removeCollisionButton->setFixedSize(QSize(30, 30));
+ removeCollisionButton->setToolTip("Remove " + QString(_name.c_str()));
+ removeCollisionButton->setIcon(QPixmap(":/images/trashcan.png"));
+ removeCollisionButton->setToolButtonStyle(Qt::ToolButtonIconOnly);
+ removeCollisionButton->setIconSize(QSize(16, 16));
+ removeCollisionButton->setCheckable(false);
+ connect(removeCollisionButton, SIGNAL(clicked()), this->signalMapper,
+ SLOT(map()));
+ this->signalMapper->setMapping(removeCollisionButton, this->counter);
+
+ // Header Layout
+ QHBoxLayout *headerLayout = new QHBoxLayout;
+ headerLayout->setContentsMargins(0, 0, 0, 0);
+ headerLayout->addWidget(headerButton);
+ headerLayout->addWidget(removeCollisionButton);
+
+ // Header widget
+ QWidget *headerWidget = new QWidget;
+ headerWidget->setLayout(headerLayout);
+
+ // ConfigWidget
+ msgs::Collision msgToLoad;
+ if (_collisionMsg)
+ msgToLoad = *_collisionMsg;
+
+ // set default values
+ // TODO: auto-fill them with SDF defaults
+ if (!msgToLoad.has_max_contacts())
+ msgToLoad.set_max_contacts(10);
+ msgs::Surface *surfaceMsg = msgToLoad.mutable_surface();
+ if (!surfaceMsg->has_bounce_threshold())
+ surfaceMsg->set_bounce_threshold(10e5);
+ if (!surfaceMsg->has_soft_erp())
+ surfaceMsg->set_soft_erp(0.2);
+ if (!surfaceMsg->has_kp())
+ surfaceMsg->set_kp(10e12);
+ if (!surfaceMsg->has_kd())
+ surfaceMsg->set_kd(1.0);
+ if (!surfaceMsg->has_max_vel())
+ surfaceMsg->set_max_vel(0.01);
+ if (!surfaceMsg->has_collide_without_contact_bitmask())
+ surfaceMsg->set_collide_without_contact_bitmask(1);
+ if (!surfaceMsg->has_collide_bitmask())
+ surfaceMsg->set_collide_bitmask(1);
+ msgs::Friction *frictionMsg = surfaceMsg->mutable_friction();
+ if (!frictionMsg->has_mu())
+ frictionMsg->set_mu(1.0);
+ if (!frictionMsg->has_mu2())
+ frictionMsg->set_mu2(1.0);
+
+ ConfigWidget *configWidget = new ConfigWidget;
+ configWidget->Load(&msgToLoad);
+ configWidget->hide();
+
+ configWidget->SetWidgetVisible("id", false);
+ configWidget->SetWidgetVisible("name", false);
+ configWidget->SetWidgetReadOnly("id", true);
+ configWidget->SetWidgetReadOnly("name", true);
+
+ // Item layout
+ QVBoxLayout *itemLayout = new QVBoxLayout();
+ itemLayout->addWidget(headerWidget);
+ itemLayout->addWidget(configWidget);
+
+ // Put the layout in a widget which can be added/deleted
+ QWidget *item = new QWidget();
+ item->setLayout(itemLayout);
+
+ // Add to the list
+ this->listLayout->addWidget(item);
+
+ // Fill ConfigData
+ CollisionConfigData *configData = new CollisionConfigData;
+ configData->configWidget = configWidget;
+ configData->id = this->counter;
+ configData->widget = item;
+ configData->name = _name;
+ connect(headerButton, SIGNAL(toggled(bool)), configData,
+ SLOT(OnToggleItem(bool)));
+ this->configs[this->counter] = configData;
+
+ this->counter++;
+}
+
+/////////////////////////////////////////////////
+void CollisionConfig::OnRemoveCollision(int _id)
+{
+ auto it = this->configs.find(_id);
+ if (it == this->configs.end())
+ {
+ gzerr << "Collision not found " << std::endl;
+ return;
+ }
+
+ CollisionConfigData *configData = this->configs[_id];
+
+ // Ask for confirmation
+ std::string msg;
+
+ if (this->configs.size() == 1)
+ {
+ msg = "Are you sure you want to remove " +
+ configData->name + "?\n\n" +
+ "This is the only collision. \n" +
+ "Without collisions, this link won't collide with anything.\n";
+ }
+ else
+ {
+ msg = "Are you sure you want to remove " +
+ configData->name + "?\n";
+ }
+
+ QMessageBox msgBox(QMessageBox::Warning, QString("Remove collision?"),
+ QString(msg.c_str()));
+ msgBox.setWindowFlags(Qt::WindowStaysOnTopHint);
+
+ QPushButton *cancelButton =
+ msgBox.addButton("Cancel", QMessageBox::RejectRole);
+ QPushButton *removeButton = msgBox.addButton("Remove",
+ QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(removeButton);
+ msgBox.setEscapeButton(cancelButton);
+ msgBox.exec();
+ if (msgBox.clickedButton() != removeButton)
+ return;
+
+ // Remove
+ this->listLayout->removeWidget(configData->widget);
+ delete configData->widget;
+
+ emit CollisionRemoved(configData->name);
+ this->configs.erase(it);
+}
+
+/////////////////////////////////////////////////
+msgs::Collision *CollisionConfig::GetData(const std::string &_name) const
+{
+ for (auto const &it : this->configs)
+ {
+ std::string name = it.second->name;
+ if (name == _name)
+ {
+ return dynamic_cast<msgs::Collision *>(it.second->configWidget->GetMsg());
+ }
+ }
+ return NULL;
+}
+
+/////////////////////////////////////////////////
+void CollisionConfig::SetGeometry(const std::string &_name,
+ const math::Vector3 &_size, const std::string &_uri)
+{
+ for (auto &it : this->configs)
+ {
+ if (it.second->name == _name)
+ {
+ math::Vector3 dimensions;
+ std::string uri;
+ std::string type = it.second->configWidget->GetGeometryWidgetValue(
+ "geometry", dimensions, uri);
+ it.second->configWidget->SetGeometryWidgetValue("geometry", type,
+ _size, _uri);
+ break;
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void CollisionConfigData::OnToggleItem(bool _checked)
+{
+ if (_checked)
+ this->configWidget->show();
+ else
+ this->configWidget->hide();
+}
diff --git a/gazebo/gui/model/CollisionConfig.hh b/gazebo/gui/model/CollisionConfig.hh
new file mode 100644
index 0000000..4b110b1
--- /dev/null
+++ b/gazebo/gui/model/CollisionConfig.hh
@@ -0,0 +1,139 @@
+/*
+ * 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 _COLLISION_CONFIG_HH_
+#define _COLLISION_CONFIG_HH_
+
+#include <map>
+#include <string>
+
+#include "gazebo/math/Pose.hh"
+
+#include "gazebo/gui/qt.h"
+#include "gazebo/gui/model/ModelData.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class ConfigWidget;
+
+ /// \addtogroup gazebo_gui
+ /// \{
+
+ /// \class CollisionConfigData CollisionConfig.hh
+ /// \brief A class of widgets used for configuring collision properties.
+ class CollisionConfigData : public QWidget
+ {
+ Q_OBJECT
+
+ /// \brief Qt callback when this item's button has been pressed.
+ /// \param[in] _checked Whether it was checked or unchecked.
+ private slots: void OnToggleItem(bool _checked);
+
+ /// \brief Unique ID of this collision config.
+ public: int id;
+
+ /// \brief Name of the collision.
+ public: std::string name;
+
+ /// \brief Config widget for configuring collision properties.
+ public: ConfigWidget *configWidget;
+
+ /// \brief Widget associated with this data.
+ public: QWidget *widget;
+ };
+
+ /// \class CollisionConfig CollisionConfig.hh
+ /// \brief A tab for configuring collision properties of a link.
+ class CollisionConfig : public QWidget
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ public: CollisionConfig();
+
+ /// \brief Destructor
+ public: ~CollisionConfig();
+
+ /// \brief Add a collision widget to the tab.
+ /// \param[in] _name Name of collision added.
+ /// \param[in] _collisionMsg Msg containing information of the collision
+ /// to be added.
+ public: void AddCollision(const std::string &_name,
+ const msgs::Collision *_collisionMsg = NULL);
+
+ /// \brief Update a collision widget from a collision msg.
+ /// \param[in] _name Name of collision to be updated.
+ /// \param[in] _collisionMsg Msg used to update the collision widget
+ /// values.
+ public: void UpdateCollision(const std::string &_name,
+ ConstCollisionPtr _collisionMsg);
+
+ /// \brief Reset the collision tab.
+ public: void Reset();
+
+ /// \brief Get the number of collisions.
+ /// \return Number of collisions.
+ public: unsigned int GetCollisionCount() const;
+
+ /// \brief Get the msg containing all collision data.
+ /// \param[in] _name Name of collision.
+ /// \return Collision msg.
+ public: msgs::Collision *GetData(const std::string &_name) const;
+
+ /// \brief Set the geometry data of a collision
+ /// \param[in] _name Name of collision.
+ /// \param[in] _size Size to set the geometry to.
+ /// \param[in] _uri URI of the geometry.
+ public: void SetGeometry(const std::string &_name,
+ const math::Vector3 &_size, const std::string &_uri = "");
+
+ /// \brief Qt signal emitted when a collision is removed.
+ /// \param[in] _name Name of collision removed.
+ Q_SIGNALS: void CollisionRemoved(const std::string &_name);
+
+ /// \brief Qt signal emitted when a collision is added.
+ /// \param[in] _name Name of collision added.
+ Q_SIGNALS: void CollisionAdded(const std::string &_name);
+
+ /// \brief Qt callback when a collision is to be added.
+ private slots: void OnAddCollision();
+
+ /// \brief Qt callback when a collision is to be removed.
+ /// \param[in] _id Id of item to be removed.
+ private slots: void OnRemoveCollision(int _id);
+
+ /// \brief Map of id to collision config widget.
+ private: std::map<int, CollisionConfigData *> configs;
+
+ /// \brief Counter for the number of collisions.
+ private: int counter;
+
+ /// \brief Qt signal mapper for mapping remove button signals.
+ private: QSignalMapper *signalMapper;
+
+ /// \brief A map of collision items to their id.
+ private: std::map<int, QTreeWidgetItem *> collisionItems;
+
+ /// \brief Layout which holds all collision items.
+ private: QVBoxLayout *listLayout;
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/gui/model/ExtrudeDialog.cc b/gazebo/gui/model/ExtrudeDialog.cc
new file mode 100644
index 0000000..d57e551
--- /dev/null
+++ b/gazebo/gui/model/ExtrudeDialog.cc
@@ -0,0 +1,311 @@
+/*
+ * 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/common/SVGLoader.hh"
+
+#include "gazebo/gui/model/ExtrudeDialogPrivate.hh"
+#include "gazebo/gui/model/ExtrudeDialog.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+ExtrudeDialog::ExtrudeDialog(const std::string &_filename, QWidget *_parent)
+ : QDialog(_parent), dataPtr(new ExtrudeDialogPrivate)
+{
+ this->setObjectName("ExtrudeDialog");
+ this->setWindowTitle(tr("Extrude Link"));
+
+ // Title
+ QLabel *titleLabel = new QLabel(tr(
+ "Extrude a 2D polyline into a 3D mesh.<br>"));
+
+ // Thickness
+ this->dataPtr->thicknessSpin = new QDoubleSpinBox();
+ this->dataPtr->thicknessSpin->setObjectName("thicknessSpin");
+ this->dataPtr->thicknessSpin->setRange(0.001, 1000);
+ this->dataPtr->thicknessSpin->setSingleStep(0.1);
+ this->dataPtr->thicknessSpin->setDecimals(3);
+ this->dataPtr->thicknessSpin->setValue(0.1);
+
+ // Resolution
+ this->dataPtr->resolutionSpin = new QDoubleSpinBox();
+ this->dataPtr->resolutionSpin->setObjectName("resolutionSpin");
+ this->dataPtr->resolutionSpin->setRange(1, 100000);
+ this->dataPtr->resolutionSpin->setSingleStep(100);
+ this->dataPtr->resolutionSpin->setDecimals(3);
+ // 3543.3 px/m == 90 dpi
+ this->dataPtr->resolutionSpin->setValue(3543.3);
+ connect(this->dataPtr->resolutionSpin, SIGNAL(valueChanged(double)),
+ this, SLOT(OnUpdateView(double)));
+
+ // Samples
+ this->dataPtr->samplesSpin = new QSpinBox();
+ this->dataPtr->samplesSpin->setObjectName("samplesSpin");
+ this->dataPtr->samplesSpin->setRange(2, 100);
+ this->dataPtr->samplesSpin->setSingleStep(1);
+ this->dataPtr->samplesSpin->setValue(5);
+ QLabel *samplesTips = new QLabel(tr("<b><font size=4>?</font></b>"));
+ samplesTips->setToolTip(
+ "Number of points to divide each curve segment into.");
+ connect(this->dataPtr->samplesSpin, SIGNAL(valueChanged(int)),
+ this, SLOT(OnUpdateView(int)));
+
+ QGridLayout *inputsLayout = new QGridLayout();
+ inputsLayout->addWidget(new QLabel("Thickness:"), 0, 0);
+ inputsLayout->addWidget(this->dataPtr->thicknessSpin, 0, 1);
+ inputsLayout->addWidget(new QLabel("m"), 0, 2);
+ inputsLayout->addWidget(new QLabel("Resolution:"), 1, 0);
+ inputsLayout->addWidget(this->dataPtr->resolutionSpin, 1, 1);
+ inputsLayout->addWidget(new QLabel("px/m"), 1, 2);
+ inputsLayout->addWidget(new QLabel("Samples per segment:"), 2, 0);
+ inputsLayout->addWidget(this->dataPtr->samplesSpin, 2, 1);
+ inputsLayout->addWidget(samplesTips, 2, 2);
+
+ // Buttons
+ QPushButton *backButton = new QPushButton(tr("Back"));
+ connect(backButton, SIGNAL(clicked()), this, SLOT(OnReject()));
+
+ QPushButton *okButton = new QPushButton("Ok");
+ okButton->setDefault(true);
+ connect(okButton, SIGNAL(clicked()), this, SLOT(OnAccept()));
+
+ QHBoxLayout *buttonsLayout = new QHBoxLayout();
+ buttonsLayout->addWidget(backButton);
+ buttonsLayout->addWidget(okButton);
+ buttonsLayout->setAlignment(Qt::AlignRight);
+
+ // Left column
+ QWidget *leftColumn = new QWidget();
+ leftColumn->setSizePolicy(QSizePolicy::Fixed,
+ QSizePolicy::Fixed);
+ QVBoxLayout *leftColumnLayout = new QVBoxLayout();
+ leftColumn->setLayout(leftColumnLayout);
+ leftColumnLayout->addWidget(titleLabel);
+ leftColumnLayout->addLayout(inputsLayout);
+ leftColumnLayout->addSpacing(30);
+ leftColumnLayout->addLayout(buttonsLayout);
+
+ // Image view
+ this->dataPtr->filename = _filename;
+
+ this->dataPtr->view = new QGraphicsView(this);
+ QGraphicsScene *scene = new QGraphicsScene();
+ scene->setBackgroundBrush(Qt::white);
+ this->dataPtr->view->setSizePolicy(QSizePolicy::Expanding,
+ QSizePolicy::Expanding);
+ this->dataPtr->view->setScene(scene);
+ this->dataPtr->view->setViewportUpdateMode(
+ QGraphicsView::FullViewportUpdate);
+ this->dataPtr->view->setDragMode(QGraphicsView::ScrollHandDrag);
+ this->dataPtr->view->setMinimumWidth(200);
+ this->dataPtr->viewWidth = 500;
+ this->dataPtr->view->installEventFilter(this);
+ this->UpdateView();
+
+ // Main layout
+ QHBoxLayout *mainLayout = new QHBoxLayout;
+ mainLayout->addWidget(leftColumn, 0, Qt::AlignTop);
+ mainLayout->addWidget(this->dataPtr->view);
+
+ this->setLayout(mainLayout);
+}
+
+/////////////////////////////////////////////////
+ExtrudeDialog::~ExtrudeDialog()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+void ExtrudeDialog::OnAccept()
+{
+ this->accept();
+}
+
+/////////////////////////////////////////////////
+void ExtrudeDialog::OnReject()
+{
+ // back to import
+ this->reject();
+}
+
+/////////////////////////////////////////////////
+double ExtrudeDialog::GetThickness() const
+{
+ return this->dataPtr->thicknessSpin->value();
+}
+
+/////////////////////////////////////////////////
+unsigned int ExtrudeDialog::GetSamples() const
+{
+ return this->dataPtr->samplesSpin->value();
+}
+
+/////////////////////////////////////////////////
+double ExtrudeDialog::GetResolution() const
+{
+ return this->dataPtr->resolutionSpin->value();
+}
+
+/////////////////////////////////////////////////
+void ExtrudeDialog::OnUpdateView(int /*_value*/)
+{
+ this->UpdateView();
+}
+
+/////////////////////////////////////////////////
+void ExtrudeDialog::OnUpdateView(double /*_value*/)
+{
+ this->UpdateView();
+}
+
+/////////////////////////////////////////////////
+void ExtrudeDialog::UpdateView()
+{
+ QGraphicsScene *scene = this->dataPtr->view->scene();
+ scene->clear();
+
+ common::SVGLoader svgLoader(this->GetSamples());
+ std::vector<common::SVGPath> paths;
+ svgLoader.Parse(this->dataPtr->filename, paths);
+
+ if (paths.empty())
+ {
+ gzerr << "An empty path should never get here." << std::endl;
+ return;
+ }
+
+ // Find extreme values to center and scale
+ math::Vector2d min(paths[0].polylines[0][0]);
+ math::Vector2d max(min);
+ for (const auto &p : paths)
+ {
+ for (const auto &poly : p.polylines)
+ {
+ for (const auto &pt : poly)
+ {
+ if (pt.X() < min.x)
+ min.x = pt.X();
+ if (pt.Y() < min.y)
+ min.y = pt.Y();
+ if (pt.X() > max.x)
+ max.x = pt.X();
+ if (pt.Y() > max.y)
+ max.y = pt.Y();
+ }
+ }
+ }
+
+ int margin = 50;
+ double svgWidth = this->dataPtr->viewWidth - margin * 2;
+ double resolutionView = svgWidth / (max.x-min.x);
+ double svgHeight = (max.y - min.y) * resolutionView;
+ double viewHeight = svgHeight + 2 * margin;
+ scene->setSceneRect(0, 0, this->dataPtr->viewWidth, viewHeight);
+
+ // Draw grid lines
+ double sceneMeter = 1 * this->GetResolution() * resolutionView;
+ for (double r = 0; r <= viewHeight; r += sceneMeter/10.0)
+ {
+ scene->addLine(- margin, r, this->dataPtr->viewWidth + margin, r,
+ QPen(QColor(230, 230, 255)));
+ }
+ for (double c = 0; c <= this->dataPtr->viewWidth; c += sceneMeter/10.0)
+ {
+ scene->addLine(c, - margin, c, viewHeight + margin,
+ QPen(QColor(230, 230, 255)));
+ }
+ for (double r = 0; r <= viewHeight; r += sceneMeter)
+ {
+ scene->addLine(- margin, r, this->dataPtr->viewWidth + margin, r,
+ QPen(QColor(108, 108, 255)));
+ }
+ for (double c = 0; c <= this->dataPtr->viewWidth; c += sceneMeter)
+ {
+ scene->addLine(c, - margin, c, viewHeight + margin,
+ QPen(QColor(108, 108, 255)));
+ }
+
+ // Draw origin
+ scene->addLine(this->dataPtr->viewWidth/2.0 - this->dataPtr->viewWidth/30.0,
+ viewHeight/2.0,
+ this->dataPtr->viewWidth/2.0 + this->dataPtr->viewWidth/30.0,
+ viewHeight/2.0,
+ QPen(QColor(50, 50, 255), 2));
+ scene->addLine(this->dataPtr->viewWidth/2.0,
+ viewHeight/2.0 - this->dataPtr->viewWidth/30.0,
+ this->dataPtr->viewWidth/2.0,
+ viewHeight/2.0 + this->dataPtr->viewWidth/30.0,
+ QPen(QColor(50, 50, 255), 2));
+
+ // Draw polygons
+ for (common::SVGPath path : paths)
+ {
+ for (std::vector<ignition::math::Vector2d> poly : path.polylines)
+ {
+ QPainterPath painterPath;
+ bool firstPoint = true;
+ for (ignition::math::Vector2d pt : poly)
+ {
+ // Centroid at SVG 0,0
+ pt = pt - min.Ign() - (max-min).Ign()*0.5;
+ // Scale to view while keeping aspect ratio
+ pt = ignition::math::Vector2d(
+ pt.X() * resolutionView, pt.Y() * resolutionView);
+ // Translate to view center
+ pt.X() += this->dataPtr->viewWidth/2.0;
+ pt.Y() += viewHeight/2.0;
+
+ // Draw point
+ double pointSize = 5;
+ QGraphicsEllipseItem *ptItem = new QGraphicsEllipseItem(
+ pt.X() - pointSize/2.0, pt.Y() - pointSize/2.0,
+ pointSize, pointSize);
+ ptItem->setBrush(Qt::red);
+ ptItem->setZValue(5);
+ scene->addItem(ptItem);
+ if (firstPoint)
+ {
+ firstPoint = false;
+ painterPath.moveTo(pt.X(), pt.Y());
+ }
+ else
+ painterPath.lineTo(pt.X(), pt.Y());
+ }
+
+ QGraphicsPathItem *pathItem = new QGraphicsPathItem(painterPath);
+ // Draw polygon
+ pathItem->setPen(QPen(Qt::black, 3, Qt::SolidLine));
+ scene->addItem(pathItem);
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+bool ExtrudeDialog::eventFilter(QObject *_obj, QEvent *_event)
+{
+ QGraphicsView *graphicsView = qobject_cast<QGraphicsView *>(_obj);
+ if (graphicsView && _event->type() == QEvent::Resize)
+ {
+ QResizeEvent *resizeEv = dynamic_cast<QResizeEvent *>(_event);
+ this->dataPtr->viewWidth = resizeEv->size().width() - 20;
+ this->UpdateView();
+ }
+ return QObject::eventFilter(_obj, _event);
+}
diff --git a/gazebo/gui/model/ExtrudeDialog.hh b/gazebo/gui/model/ExtrudeDialog.hh
new file mode 100644
index 0000000..1394d5d
--- /dev/null
+++ b/gazebo/gui/model/ExtrudeDialog.hh
@@ -0,0 +1,95 @@
+/*
+ * 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_EXTRUDE_DIALOG_HH_
+#define _GAZEBO_EXTRUDE_DIALOG_HH_
+
+#include <string>
+
+#include "gazebo/gui/qt.h"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class ExtrudeDialogPrivate;
+
+ /// \addtogroup gazebo_gui
+ /// \{
+
+ /// \class ExtrudeDialog ExtrudeDialog.hh gui/gui.hh
+ /// \brief Dialog for saving to file.
+ class GZ_GUI_MODEL_VISIBLE ExtrudeDialog : public QDialog
+ {
+ Q_OBJECT
+
+ /// \brief Constructor.
+ /// \param[in] _filename Full path to the SVG file.
+ /// \param[in] _parent Parent QWidget.
+ public: ExtrudeDialog(const std::string &_filename, QWidget *_parent = 0);
+
+ /// \brief Destructor.
+ public: ~ExtrudeDialog();
+
+ /// \brief Reload SVG file to update the view with the current data in
+ /// the dialog.
+ public: void UpdateView();
+
+ /// \brief Get the thickness value.
+ /// \return Thickness value.
+ public: double GetThickness() const;
+
+ /// \brief Get the number of samples per segment.
+ /// \return Number of samples.
+ public: unsigned int GetSamples() const;
+
+ /// \brief Get the resolution in px/m.
+ /// \return Resolution value.
+ public: double GetResolution() const;
+
+ /// \brief Qt callback when the dialog is accepted.
+ private slots: void OnAccept();
+
+ /// \brief Qt callback when the dialog is rejected.
+ private slots: void OnReject();
+
+ /// \brief Qt callback when an int value was changed and the view.
+ /// should be updated.
+ /// \param[in] _value New value, not used but needed for the slot.
+ private slots: void OnUpdateView(int _value);
+
+ /// \brief Qt callback when a double value was changed and the view.
+ /// should be updated.
+ /// \param[in] _value New value, not used but needed for the slot.
+ private slots: void OnUpdateView(double _value);
+
+ /// \brief Qt event filter used to filter child widget events.
+ /// \param[in] _obj Object that is watched by the event filter.
+ /// \param[in] _event Qt event.
+ /// \return True if the event is handled.
+ private: bool eventFilter(QObject *_obj, QEvent *_event);
+
+ /// \internal
+ /// \brief Pointer to private data.
+ private: ExtrudeDialogPrivate *dataPtr;
+ };
+ /// \}
+ }
+}
+
+#endif
diff --git a/gazebo/gui/model/ExtrudeDialogPrivate.hh b/gazebo/gui/model/ExtrudeDialogPrivate.hh
new file mode 100644
index 0000000..4c48cc2
--- /dev/null
+++ b/gazebo/gui/model/ExtrudeDialogPrivate.hh
@@ -0,0 +1,52 @@
+/*
+ * 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_EXTRUDE_DIALOG_PRIVATE_HH_
+#define _GAZEBO_EXTRUDE_DIALOG_PRIVATE_HH_
+
+#include <string>
+
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \class ExtrudeDialogPrivate ExtrudeDialogPrivate.hh
+ /// \brief Private data for the ExtrudeDialog class
+ class ExtrudeDialogPrivate
+ {
+ /// \brief Full file path of the SVG file.
+ public: std::string filename;
+
+ /// \brief Thickness spin box.
+ public: QDoubleSpinBox *thicknessSpin;
+
+ /// \brief Resolution spin box.
+ public: QDoubleSpinBox *resolutionSpin;
+
+ /// \brief Samples spin box.
+ public: QSpinBox *samplesSpin;
+
+ /// \brief Width of the graphics view in px.
+ public: int viewWidth;
+
+ /// \brief Display the SVG paths.
+ public: QGraphicsView *view;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/model/ExtrudeDialog_TEST.cc b/gazebo/gui/model/ExtrudeDialog_TEST.cc
new file mode 100644
index 0000000..4cea9f8
--- /dev/null
+++ b/gazebo/gui/model/ExtrudeDialog_TEST.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 "gazebo/gui/model/ExtrudeDialog.hh"
+#include "gazebo/gui/model/ExtrudeDialog_TEST.hh"
+
+#include "test_config.h"
+
+/////////////////////////////////////////////////
+void ExtrudeDialog_TEST::BadFilename()
+{
+ // Check that bad filenames don't break anything
+ std::string bad("/not/a/file.svg");
+
+ gazebo::gui::ExtrudeDialog *extrudeDialog =
+ new gazebo::gui::ExtrudeDialog(bad);
+ QVERIFY(extrudeDialog != NULL);
+
+ delete extrudeDialog;
+}
+
+/////////////////////////////////////////////////
+void ExtrudeDialog_TEST::GetSpinValues()
+{
+ // Check a good file
+ std::string filePath = std::string(PROJECT_SOURCE_PATH);
+ filePath += "/test/data/loader.svg";
+ gazebo::gui::ExtrudeDialog *extrudeDialog =
+ new gazebo::gui::ExtrudeDialog(filePath);
+
+ // Get default thickness, change value and check new value
+ double thickness = extrudeDialog->GetThickness();
+
+ QCOMPARE(thickness, 0.1);
+ QDoubleSpinBox *thicknessSpin =
+ extrudeDialog->findChild<QDoubleSpinBox *>("thicknessSpin");
+ QVERIFY(thicknessSpin != NULL);
+ thicknessSpin->setValue(1.0);
+ thickness = extrudeDialog->GetThickness();
+ QCOMPARE(thickness, 1.0);
+
+ // Get default resolution, change value and check new value
+ double resolution = extrudeDialog->GetResolution();
+ QCOMPARE(resolution, 3543.3);
+ QDoubleSpinBox *resolutionSpin =
+ extrudeDialog->findChild<QDoubleSpinBox *>("resolutionSpin");
+ QVERIFY(resolutionSpin != NULL);
+ resolutionSpin->setValue(1010.1);
+ resolution = extrudeDialog->GetResolution();
+ QCOMPARE(resolution, 1010.1);
+
+ // Get default number of samples, change value and check new value
+ unsigned int samples = extrudeDialog->GetSamples();
+ QCOMPARE(samples, (unsigned int)5);
+ QSpinBox *samplesSpin =
+ extrudeDialog->findChild<QSpinBox *>("samplesSpin");
+ QVERIFY(samplesSpin != NULL);
+ samplesSpin->setValue(20);
+ samples = extrudeDialog->GetSamples();
+ QCOMPARE(samples, (unsigned int)20);
+}
+
+// Generate a main function for the test
+QTEST_MAIN(ExtrudeDialog_TEST)
diff --git a/gazebo/gui/model/ExtrudeDialog_TEST.hh b/gazebo/gui/model/ExtrudeDialog_TEST.hh
new file mode 100644
index 0000000..e966898
--- /dev/null
+++ b/gazebo/gui/model/ExtrudeDialog_TEST.hh
@@ -0,0 +1,35 @@
+/*
+ * 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_EXTRUDEDIALOG_TEST_HH_
+#define _GAZEBO_EXTRUDEDIALOG_TEST_HH_
+
+#include "gazebo/gui/QTestFixture.hh"
+
+/// \brief A test class for the polyline extrude dialog.
+class ExtrudeDialog_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Test a bad input.
+ private slots: void BadFilename();
+
+ /// \brief Test getting spin values.
+ private slots: void GetSpinValues();
+};
+
+#endif
diff --git a/gazebo/gui/model/GraphScene.cc b/gazebo/gui/model/GraphScene.cc
new file mode 100644
index 0000000..4e82934
--- /dev/null
+++ b/gazebo/gui/model/GraphScene.cc
@@ -0,0 +1,110 @@
+/*
+ * 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 <iostream>
+#include "gazebo/gui/model/GraphScene.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+GraphScene::GraphScene(QWidget *_parent)
+ : QGVScene("schematic_view_scene", _parent)
+{
+ this->setObjectName("GraphScene");
+
+ // Configure scene attributes
+ this->setGraphAttribute("splines", "ortho");
+ this->setGraphAttribute("rankdir", "LR");
+ this->setGraphAttribute("nodesep", "0.4");
+ this->setNodeAttribute("margin", "0.3");
+ this->setNodeAttribute("shape", "box");
+ this->setNodeAttribute("style", "filled");
+ this->setNodeAttribute("fillcolor", "white");
+ this->setNodeAttribute("height", "1.0");
+ this->setEdgeAttribute("minlen", "3");
+}
+
+/////////////////////////////////////////////////
+QGVNode *GraphScene::AddNode(const std::string &_name)
+{
+ QGVNode *node = this->addNode(tr(_name.c_str()));
+
+ return node;
+}
+
+/////////////////////////////////////////////////
+void GraphScene::RemoveNode(const std::string &_name)
+{
+ this->removeNode(tr(_name.c_str()));
+}
+
+/////////////////////////////////////////////////
+bool GraphScene::HasNode(const std::string &_name)
+{
+ return this->hasNode(tr(_name.c_str()));
+}
+
+/////////////////////////////////////////////////
+QGVNode *GraphScene::GetNode(const std::string &_name)
+{
+ return this->getNode(tr(_name.c_str()));
+}
+
+/////////////////////////////////////////////////
+QGVEdge *GraphScene::AddEdge(const std::string &_id,
+ const std::string &_node1, const std::string &_node2)
+{
+ QGVEdge *edge = this->addEdge(tr(_node1.c_str()), tr(_node2.c_str()),
+ tr(_id.c_str()));
+ return edge;
+}
+
+/////////////////////////////////////////////////
+void GraphScene::RemoveEdge(const std::string &_id)
+{
+ this->removeEdge(tr(_id.c_str()));
+}
+
+/////////////////////////////////////////////////
+void GraphScene::drawBackground(QPainter *_painter, const QRectF & _rect)
+{
+ QColor c(250, 250, 250);
+ QBrush brush(c, Qt::SolidPattern);
+ _painter->setBrush(brush);
+ _painter->drawRect(_rect);
+
+ const int gridSize = 25;
+
+ const qreal left = static_cast<int>(_rect.left()) -
+ (static_cast<int>(_rect.left()) % gridSize);
+ const qreal top = static_cast<int>(_rect.top()) -
+ (static_cast<int>(_rect.top()) % gridSize);
+
+ QVarLengthArray<QLineF, 100> lines;
+
+ for (qreal x = left; x < _rect.right(); x += gridSize)
+ lines.append(QLineF(x, _rect.top(), x, _rect.bottom()));
+ for (qreal y = top; y < _rect.bottom(); y += gridSize)
+ lines.append(QLineF(_rect.left(), y, _rect.right(), y));
+
+ _painter->setRenderHint(QPainter::Antialiasing, false);
+
+ _painter->setPen(QColor(200, 200, 200, 125));
+ _painter->drawLines(lines.data(), lines.size());
+ _painter->setPen(Qt::black);
+}
diff --git a/gazebo/gui/model/GraphScene.hh b/gazebo/gui/model/GraphScene.hh
new file mode 100644
index 0000000..1127250
--- /dev/null
+++ b/gazebo/gui/model/GraphScene.hh
@@ -0,0 +1,82 @@
+/*
+ * 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_GRAPH_SCENE_HH_
+#define _GAZEBO_GRAPH_SCENE_HH_
+
+#include <string>
+
+#include "gazebo/gui/qt.h"
+#include "gazebo/gui/model/qgv/QGVScene.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \class GraphScene GraphScene.hh
+ /// \brief A scene of 2D graph nodes and edges
+ class GraphScene : public QGVScene
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ /// \param[in] _parent Parent Widget.
+ public: GraphScene(QWidget *_parent = 0);
+
+ /// \brief Destructor
+ public: virtual ~GraphScene() = default;
+
+ /// \brief Add a node to the scene.
+ /// \param[in] _name Name of the node.
+ /// \return the Node created.
+ public: QGVNode *AddNode(const std::string &_name);
+
+ /// \brief Remove a node from the scene.
+ /// \param[in] _name Name of the name.
+ public: void RemoveNode(const std::string &_name);
+
+ /// \brief Check if a node exists in the scene.
+ /// \param[in] _name Name of the node.
+ /// \return True if the node exists.
+ public: bool HasNode(const std::string &_name);
+
+ /// \brief Get a node from the scene.
+ /// \param[in] _name Name of the name.
+ /// \return Pointer to the node, NULL if it does not exist.
+ public: QGVNode *GetNode(const std::string &_name);
+
+ /// \brief Add an edge to connect two nodes.
+ /// \param[in] _id Edge ID.
+ /// \param[in] _node1 Name of the first node.
+ /// \param[in] _node2 Name of the second node.
+ /// \return the Edge created.
+ public: QGVEdge *AddEdge(const std::string &_id,
+ const std::string &_node1, const std::string &_node2);
+
+ /// \brief Remove an edge between two nodes.
+ /// \param[in] _id Edge ID.
+ public: void RemoveEdge(const std::string &_id);
+
+ /// \brief Overrides the default background with grid lines.
+ /// \param[in] _painter Qt painter object.
+ /// \param[in] _rect Qt scene background rectangle
+ private: void drawBackground(QPainter *_painter, const QRectF &_rect);
+ };
+ }
+}
+
+#endif
diff --git a/gazebo/gui/model/GraphView.cc b/gazebo/gui/model/GraphView.cc
new file mode 100644
index 0000000..e361820
--- /dev/null
+++ b/gazebo/gui/model/GraphView.cc
@@ -0,0 +1,128 @@
+/*
+ * 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 <cstdlib>
+#include "gazebo/gui/model/GraphView.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+GraphView::GraphView(QWidget *_parent)
+ : QGraphicsView(_parent)
+{
+ this->setObjectName("GraphView");
+ this->setTransformationAnchor(QGraphicsView::AnchorUnderMouse);
+
+ this->viewScale = 1.0;
+}
+
+/////////////////////////////////////////////////
+void GraphView::scrollContentsBy(int _dx, int _dy)
+{
+ QGraphicsView::scrollContentsBy(_dx, _dy);
+}
+
+/////////////////////////////////////////////////
+void GraphView::resizeEvent(QResizeEvent */*_event*/)
+{
+}
+
+/////////////////////////////////////////////////
+void GraphView::contextMenuEvent(QContextMenuEvent *_event)
+{
+ QGraphicsItem *item = this->scene()->itemAt(this->mapToScene(_event->pos()));
+ if (item)
+ {
+ _event->ignore();
+ QGraphicsView::contextMenuEvent(_event);
+ return;
+ }
+}
+
+/////////////////////////////////////////////////
+void GraphView::wheelEvent(QWheelEvent *_event)
+{
+ int wheelIncr = 120;
+ int sign = (_event->delta() > 0) ? 1 : -1;
+ int delta = std::max(std::abs(_event->delta()), wheelIncr) * sign;
+ int numSteps = delta / wheelIncr;
+
+ QMatrix mat = matrix();
+ QPointF mousePosition = _event->pos();
+
+ mat.translate((width()/2) - mousePosition.x(), (height()/2) -
+ mousePosition.y());
+
+ double scaleFactor = 1.15;
+
+ if (numSteps > 0)
+ {
+ mat.scale(numSteps*scaleFactor, numSteps*scaleFactor);
+ this->viewScale *= numSteps*scaleFactor;
+ }
+ else
+ {
+ mat.scale(-1/(numSteps*scaleFactor), -1/(numSteps*scaleFactor));
+ this->viewScale *= -1/(numSteps*scaleFactor);
+ }
+ mat.translate(mousePosition.x() - (this->width()/2),
+ mousePosition.y() -(this->height()/2));
+ this->setMatrix(mat);
+
+ _event->accept();
+}
+
+/////////////////////////////////////////////////
+void GraphView::mousePressEvent(QMouseEvent *_event)
+{
+ if (_event->button() != Qt::RightButton)
+ {
+ QGraphicsItem *mouseItem =
+ this->scene()->itemAt(this->mapToScene(_event->pos()));
+ if (mouseItem && !mouseItem->isSelected())
+ {
+ this->scene()->clearSelection();
+ mouseItem->setSelected(true);
+ }
+ QGraphicsView::mousePressEvent(_event);
+ }
+}
+
+/////////////////////////////////////////////////
+void GraphView::mouseReleaseEvent(QMouseEvent *_event)
+{
+ QGraphicsView::mouseReleaseEvent(_event);
+}
+
+/////////////////////////////////////////////////
+void GraphView::mouseMoveEvent(QMouseEvent *_event)
+{
+ QGraphicsView::mouseMoveEvent(_event);
+}
+
+/////////////////////////////////////////////////
+void GraphView::keyPressEvent(QKeyEvent *_event)
+{
+ QGraphicsView::keyPressEvent(_event);
+}
+
+/////////////////////////////////////////////////
+void GraphView::mouseDoubleClickEvent(QMouseEvent *_event)
+{
+ QGraphicsView::mouseDoubleClickEvent(_event);
+}
diff --git a/gazebo/gui/model/GraphView.hh b/gazebo/gui/model/GraphView.hh
new file mode 100644
index 0000000..6192d77
--- /dev/null
+++ b/gazebo/gui/model/GraphView.hh
@@ -0,0 +1,83 @@
+/*
+ * 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_GRAPH_VIEW_HH_
+#define _GAZEBO_GRAPH_VIEW_HH_
+
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \class GraphView GraphView.hh
+ /// \brief A view for the graph scene
+ class GraphView : public QGraphicsView
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ /// \param[in] _parent Parent Widget.
+ public: GraphView(QWidget *_parent = 0);
+
+ /// \brief Destructor
+ public: virtual ~GraphView() = default;
+
+ /// \brief Qt resize event received when the parent widget changes size.
+ /// \param[in] _event Qt resize event
+ private: void resizeEvent(QResizeEvent *_event);
+
+ /// \brief Qt event received when the editor view is being scrolled.
+ /// \param[in] _dx Change in X position of the scrollbar.
+ /// \param[in] _dx Change in Y position of the scrollbar.
+ private: void scrollContentsBy(int _dx, int _dy);
+
+ /// \brief Qt context menu received on a mouse right click.
+ /// \param[in] _event Qt context menu event.
+ private: void contextMenuEvent(QContextMenuEvent *_event);
+
+ /// \brief Qt wheel event received when the mouse wheel is being scrolled.
+ /// \param[in] _event Qt wheel event.
+ private: void wheelEvent(QWheelEvent *_event);
+
+ /// \brief Qt mouse move event.
+ /// \param[in] _event Qt mouse event.
+ private: void mouseMoveEvent(QMouseEvent *_event);
+
+ /// \brief Qt mouse press event.
+ /// \param[in] _event Qt mouse event.
+ private: void mousePressEvent(QMouseEvent *_event);
+
+ /// \brief Qt mouse release event.
+ /// \param[in] _event Qt mouse event.
+ private: void mouseReleaseEvent(QMouseEvent *_event);
+
+ /// \brief Qt mouse double click event.
+ /// \param[in] _event Qt mouse event.
+ private: void mouseDoubleClickEvent(QMouseEvent *_event);
+
+ /// \brief Qt key press event.
+ /// \param[in] _event Qt key event.
+ private: void keyPressEvent(QKeyEvent *_event);
+
+ /// \brief Scale (zoom level) of the editor view.
+ private: double viewScale;
+ };
+ }
+}
+
+#endif
diff --git a/gazebo/gui/model/ImportDialog.cc b/gazebo/gui/model/ImportDialog.cc
index 4e48900..4744c24 100644
--- a/gazebo/gui/model/ImportDialog.cc
+++ b/gazebo/gui/model/ImportDialog.cc
@@ -15,6 +15,8 @@
*
*/
+#include "gazebo/common/SVGLoader.hh"
+
#include "gazebo/gui/model/ImportDialog.hh"
using namespace gazebo;
@@ -24,14 +26,15 @@ using namespace gui;
ImportDialog::ImportDialog(QWidget *_parent) : QDialog(_parent)
{
this->setObjectName("ImportDialog");
- this->setWindowTitle("Custom Part");
+ this->setWindowTitle("Import Link");
this->messageLabel = new QLabel;
this->messageLabel->setText(
- tr("You can import a 3D mesh that you have \n"
- "made with a modelling tool such as Blender \n"
- "Maya, or SolidWorks. It will apear as a \n"
- "part in the 3D View."));
+ tr("You can import a 3D mesh (.dae, .stl) \n"
+ "that you have made with a modelling tool \n"
+ "such as Blender, Maya or SolidWorks.\n\n"
+ "You can also extrude a 2D image (.svg) to \n"
+ "create a 3D mesh."));
this->pathLineEdit = new QLineEdit;
this->pathLineEdit->setText(QDir::homePath());
@@ -39,7 +42,7 @@ ImportDialog::ImportDialog(QWidget *_parent) : QDialog(_parent)
connect(browseButton, SIGNAL(clicked()), this, SLOT(OnBrowse()));
QLabel *nameLabel = new QLabel;
- nameLabel->setText(tr("Part Name:"));
+ nameLabel->setText(tr("Link Name:"));
this->nameLineEdit = new QLineEdit;
this->nameLineEdit->setText(tr("DefaultName"));
@@ -74,7 +77,7 @@ ImportDialog::~ImportDialog()
}
/////////////////////////////////////////////////
-std::string ImportDialog::GetPartName() const
+std::string ImportDialog::GetLinkName() const
{
return this->nameLineEdit->text().toStdString();
}
@@ -86,7 +89,7 @@ std::string ImportDialog::GetImportPath() const
}
/////////////////////////////////////////////////
-void ImportDialog::SetPartName(const std::string &_name)
+void ImportDialog::SetLinkName(const std::string &_name)
{
this->nameLineEdit->setText(tr(_name.c_str()));
}
@@ -106,8 +109,8 @@ void ImportDialog::SetTitle(const std::string &_title)
/////////////////////////////////////////////////
void ImportDialog::OnBrowse()
{
- QFileDialog fd(this, tr("Import Part"), QDir::homePath(),
- tr("Mesh files (*.dae *.stl)"));
+ QFileDialog fd(this, tr("Import Link"), QDir::homePath(),
+ tr("Files (*.svg *.dae *.stl)"));
fd.setFilter(QDir::AllDirs | QDir::Hidden);
fd.setFileMode(QFileDialog::ExistingFile);
if (fd.exec())
@@ -133,15 +136,37 @@ void ImportDialog::OnCancel()
void ImportDialog::OnImport()
{
QFileInfo info(this->pathLineEdit->text());
- if (info.isFile())
+ if (info.isFile() && (info.completeSuffix().toLower() == "dae" ||
+ info.completeSuffix().toLower() == "stl"))
{
this->accept();
}
+ else if (info.completeSuffix().toLower() == "svg")
+ {
+ // Check if the SVG has any paths before accepting
+ std::string filename = this->pathLineEdit->text().toStdString();
+ common::SVGLoader svgLoader(2);
+ std::vector<common::SVGPath> paths;
+ svgLoader.Parse(filename, paths);
+
+ if (paths.empty())
+ {
+ std::string msg = "No paths found on file \"" + filename +
+ "\"\n\nPlease select another file.";
+ QMessageBox::warning(this, QString("Invalid File"),
+ QString(msg.c_str()), QMessageBox::Ok,
+ QMessageBox::Ok);
+ }
+ else
+ {
+ this->accept();
+ }
+ }
else
{
- std::string msg = this->pathLineEdit->text().toStdString() +
- " is not a valid mesh file.\nPlease select another file.";
- QMessageBox::warning(this, QString("Invalid Mesh File"),
+ std::string msg = "\"" + this->pathLineEdit->text().toStdString() +
+ "\" is not a valid mesh or image file.\nPlease select another file.";
+ QMessageBox::warning(this, QString("Invalid File"),
QString(msg.c_str()), QMessageBox::Ok,
QMessageBox::Ok);
}
diff --git a/gazebo/gui/model/ImportDialog.hh b/gazebo/gui/model/ImportDialog.hh
index 234fcc4..779db83 100644
--- a/gazebo/gui/model/ImportDialog.hh
+++ b/gazebo/gui/model/ImportDialog.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class ImportDialog gui/ImportDialog.hh
/// \brief Dialog for saving to file.
- class GAZEBO_VISIBLE ImportDialog : public QDialog
+ class GZ_GUI_MODEL_VISIBLE ImportDialog : public QDialog
{
Q_OBJECT
@@ -44,18 +44,18 @@ namespace gazebo
/// \brief Get name of file.
/// \return The name of file.
- public: std::string GetPartName() const;
+ public: std::string GetLinkName() const;
- /// \brief Get the import path of the custom part.
+ /// \brief Get the import path of the custom link.
/// \return Path of the custom location.
public: std::string GetImportPath() const;
- /// \brief Set the name of the custom part.
+ /// \brief Set the name of the custom link.
/// \param[in] _name Name of file.
- public: void SetPartName(const std::string &_name);
+ public: void SetLinkName(const std::string &_name);
- /// \brief Set the import path of the custom part.
- /// \param[in] _path Path to import the custom part.
+ /// \brief Set the import path of the custom link.
+ /// \param[in] _path Path to import the custom link.
public: void SetImportPath(const std::string &_path);
/// \brief Set the message to be displayed.
diff --git a/gazebo/gui/model/JointInspector.cc b/gazebo/gui/model/JointInspector.cc
index f2fb4fa..672e0ca 100644
--- a/gazebo/gui/model/JointInspector.cc
+++ b/gazebo/gui/model/JointInspector.cc
@@ -18,200 +18,100 @@
#include "gazebo/common/Console.hh"
#include "gazebo/common/Assert.hh"
+#include "gazebo/gui/ConfigWidget.hh"
#include "gazebo/gui/model/JointInspector.hh"
using namespace gazebo;
using namespace gui;
/////////////////////////////////////////////////
-JointInspector::JointInspector(JointMaker::JointType _jointType,
- QWidget *_parent) : QDialog(_parent)
+JointInspector::JointInspector(QWidget *_parent) : QDialog(_parent)
{
this->setObjectName("JointInspectorDialog");
this->setWindowTitle(tr("Joint Inspector"));
this->setWindowFlags(Qt::WindowStaysOnTopHint);
- this->jointType = _jointType;
+ QVBoxLayout *generalLayout = new QVBoxLayout;
- QLabel *jointLabel = new QLabel(tr("Name:"));
- this->jointNameLineEdit = new QLineEdit(tr(""));
+ this->configWidget = new ConfigWidget;
+ msgs::Joint jointMsg;
+ configWidget->Load(&jointMsg);
- QHBoxLayout *nameLayout = new QHBoxLayout;
- nameLayout->addWidget(jointLabel);
- nameLayout->addWidget(this->jointNameLineEdit);
+ QScrollArea *scrollArea = new QScrollArea;
+ scrollArea->setWidget(configWidget);
+ scrollArea->setWidgetResizable(true);
- QLabel *typeLabel = new QLabel(tr("Type: "));
- this->jointTypeComboBox = new QComboBox;
- this->jointTypeComboBox->addItem("revolute",
- QVariant(JointMaker::JOINT_HINGE));
- this->jointTypeComboBox->addItem("revolute2",
- QVariant(JointMaker::JOINT_HINGE2));
- this->jointTypeComboBox->addItem("prismatic",
- QVariant(JointMaker::JOINT_SLIDER));
- this->jointTypeComboBox->addItem("ball",
- QVariant(JointMaker::JOINT_BALL));
- this->jointTypeComboBox->addItem("universal",
- QVariant(JointMaker::JOINT_UNIVERSAL));
- this->jointTypeComboBox->addItem("screw",
- QVariant(JointMaker::JOINT_SCREW));
- connect(this->jointTypeComboBox, SIGNAL(currentIndexChanged(int)),
- this, SLOT(OnJointTypeChanged(int)));
+ generalLayout->setContentsMargins(0, 0, 0, 0);
+ generalLayout->addWidget(scrollArea);
- QHBoxLayout *typeLayout = new QHBoxLayout;
- typeLayout->addWidget(typeLabel);
- typeLayout->addWidget(this->jointTypeComboBox);
-
- QLabel *parentLabel = new QLabel(tr("Parent: "));
- this->jointParentLabel = new QLabel(tr(""));
-
- QHBoxLayout *parentLayout = new QHBoxLayout;
- parentLayout->addWidget(parentLabel);
- parentLayout->addWidget(jointParentLabel);
-
- QLabel *childLabel = new QLabel(tr("Child: "));
- this->jointChildLabel = new QLabel(tr(""));
-
- QHBoxLayout *childLayout = new QHBoxLayout;
- childLayout->addWidget(childLabel);
- childLayout->addWidget(jointChildLabel);
-
- QLabel *poseXLabel = new QLabel(tr("x: "));
- QLabel *poseYLabel = new QLabel(tr("y: "));
- QLabel *poseZLabel = new QLabel(tr("z: "));
- QLabel *poseRollLabel = new QLabel(tr("roll: "));
- QLabel *posePitchLabel = new QLabel(tr("pitch: "));
- QLabel *poseYawLabel = new QLabel(tr("yaw: "));
-
- this->poseXSpinBox = new QDoubleSpinBox;
- this->poseXSpinBox->setRange(-1000, 1000);
- this->poseXSpinBox->setSingleStep(0.01);
- this->poseXSpinBox->setDecimals(3);
- this->poseXSpinBox->setValue(0.000);
-
- this->poseYSpinBox = new QDoubleSpinBox;
- this->poseYSpinBox->setRange(-1000, 1000);
- this->poseYSpinBox->setSingleStep(0.01);
- this->poseYSpinBox->setDecimals(3);
- this->poseYSpinBox->setValue(0.000);
-
- this->poseZSpinBox = new QDoubleSpinBox;
- this->poseZSpinBox->setRange(-1000, 1000);
- this->poseZSpinBox->setSingleStep(0.01);
- this->poseZSpinBox->setDecimals(3);
- this->poseZSpinBox->setValue(0.000);
-
- this->poseRollSpinBox = new QDoubleSpinBox;
- this->poseRollSpinBox->setRange(-1000, 1000);
- this->poseRollSpinBox->setSingleStep(0.01);
- this->poseRollSpinBox->setDecimals(3);
- this->poseRollSpinBox->setValue(0.000);
-
- this->posePitchSpinBox = new QDoubleSpinBox;
- this->posePitchSpinBox->setRange(-1000, 1000);
- this->posePitchSpinBox->setSingleStep(0.01);
- this->posePitchSpinBox->setDecimals(3);
- this->posePitchSpinBox->setValue(0.000);
-
- this->poseYawSpinBox = new QDoubleSpinBox;
- this->poseYawSpinBox->setRange(-1000, 1000);
- this->poseYawSpinBox->setSingleStep(0.01);
- this->poseYawSpinBox->setDecimals(3);
- this->poseYawSpinBox->setValue(0.000);
-
- QGridLayout *poseLayout = new QGridLayout;
- poseLayout->addWidget(poseXLabel, 0, 0);
- poseLayout->addWidget(poseXSpinBox, 0, 1);
- poseLayout->addWidget(poseYLabel, 1, 0);
- poseLayout->addWidget(poseYSpinBox, 1, 1);
- poseLayout->addWidget(poseZLabel, 2, 0);
- poseLayout->addWidget(poseZSpinBox, 2, 1);
- poseLayout->addWidget(poseRollLabel, 0, 2);
- poseLayout->addWidget(poseRollSpinBox, 0, 3);
- poseLayout->addWidget(posePitchLabel, 1, 2);
- poseLayout->addWidget(posePitchSpinBox, 1, 3);
- poseLayout->addWidget(poseYawLabel, 2, 2);
- poseLayout->addWidget(poseYawSpinBox, 2, 3);
-
- QGroupBox *poseGroupBox = new QGroupBox(tr("Pose"));
- poseGroupBox->setLayout(poseLayout);
-
- int axisCount = 2;
- for (int i = 0; i < axisCount; ++i)
+ // fill them with SDF default values
+ sdf::ElementPtr jointElem = msgs::JointToSDF(jointMsg);
+ sdf::ElementPtr axisElem = jointElem->GetElement("axis");
+ sdf::ElementPtr axisLimitElem = axisElem->GetElement("limit");
+ sdf::ElementPtr odeElem = jointElem->GetElement("physics")->GetElement("ode");
+ for (unsigned int i = 0; i < 2u; ++i)
{
- QLabel *axisXLabel = new QLabel(tr("x: "));
- QLabel *axisYLabel = new QLabel(tr("y: "));
- QLabel *axisZLabel = new QLabel(tr("z: "));
-
- QDoubleSpinBox *axisXSpinBox = new QDoubleSpinBox;
- axisXSpinBox->setRange(-1000, 1000);
- axisXSpinBox->setSingleStep(0.01);
- axisXSpinBox->setDecimals(3);
- axisXSpinBox->setValue(0.000);
- this->axisXSpinBoxes.push_back(axisXSpinBox);
-
- QDoubleSpinBox *axisYSpinBox = new QDoubleSpinBox;
- axisYSpinBox->setRange(-1000, 1000);
- axisYSpinBox->setSingleStep(0.01);
- axisYSpinBox->setDecimals(3);
- axisYSpinBox->setValue(0.000);
- this->axisYSpinBoxes.push_back(axisYSpinBox);
-
- QDoubleSpinBox *axisZSpinBox = new QDoubleSpinBox;
- axisZSpinBox->setRange(-1000, 1000);
- axisZSpinBox->setSingleStep(0.01);
- axisZSpinBox->setDecimals(3);
- axisZSpinBox->setValue(0.000);
- this->axisZSpinBoxes.push_back(axisZSpinBox);
-
- QGridLayout *axisLayout = new QGridLayout;
- axisLayout->addWidget(axisXLabel, 0, 0);
- axisLayout->addWidget(axisXSpinBox, 0, 1);
- axisLayout->addWidget(axisYLabel), 1, 0;
- axisLayout->addWidget(axisYSpinBox, 1, 1);
- axisLayout->addWidget(axisZLabel), 2, 0;
- axisLayout->addWidget(axisZSpinBox, 2, 1);
-
- QLabel *lowerLimitLabel = new QLabel(tr("Lower: "));
- QLabel *upperLimitLabel = new QLabel(tr("Upper: "));
-
- QDoubleSpinBox *lowerLimitSpinBox = new QDoubleSpinBox;
- lowerLimitSpinBox->setRange(-1000, 1000);
- lowerLimitSpinBox->setSingleStep(0.01);
- lowerLimitSpinBox->setDecimals(3);
- this->lowerLimitSpinBoxes.push_back(lowerLimitSpinBox);
-
- QDoubleSpinBox *upperLimitSpinBox = new QDoubleSpinBox;
- upperLimitSpinBox->setRange(-1000, 1000);
- upperLimitSpinBox->setSingleStep(0.01);
- upperLimitSpinBox->setDecimals(3);
- this->upperLimitSpinBoxes.push_back(upperLimitSpinBox);
-
- QGridLayout *limitLayout = new QGridLayout;
- limitLayout->addWidget(lowerLimitLabel, 0, 0);
- limitLayout->addWidget(lowerLimitSpinBox, 0, 1);
- limitLayout->addWidget(upperLimitLabel), 1, 0;
- limitLayout->addWidget(upperLimitSpinBox, 1, 1);
-
- QGroupBox *limitGroupBox = new QGroupBox(tr("Limit"));
- limitGroupBox->setLayout(limitLayout);
-
- QVBoxLayout *axisAllLayout = new QVBoxLayout;
- axisAllLayout->addLayout(axisLayout);
- axisAllLayout->addWidget(limitGroupBox);
-
- std::stringstream ss;
- ss << "Axis" << (i+1);
- QGroupBox *axisGroupBox = new QGroupBox(tr(ss.str().c_str()));
- axisGroupBox->setLayout(axisAllLayout);
- this->axisGroupBoxes.push_back(axisGroupBox);
+ std::stringstream axis;
+ axis << "axis" << i+1;
+ std::string axisStr = axis.str();
+ this->configWidget->SetVector3WidgetValue(axisStr + "::xyz",
+ axisElem->Get<math::Vector3>("xyz"));
+ this->configWidget->SetDoubleWidgetValue(axisStr + "::limit_lower",
+ axisLimitElem->Get<double>("lower"));
+ this->configWidget->SetDoubleWidgetValue(axisStr + "::limit_upper",
+ axisLimitElem->Get<double>("upper"));
+ this->configWidget->SetDoubleWidgetValue(axisStr + "::limit_effort",
+ axisLimitElem->Get<double>("effort"));
+ this->configWidget->SetDoubleWidgetValue(axisStr + "::limit_velocity",
+ axisLimitElem->Get<double>("velocity"));
+ this->configWidget->SetDoubleWidgetValue(axisStr + "::damping",
+ axisElem->GetElement("dynamics")->Get<double>("damping"));
+ this->configWidget->SetDoubleWidgetValue(axisStr + "::friction",
+ axisElem->GetElement("dynamics")->Get<double>("friction"));
+ this->configWidget->SetBoolWidgetValue(axisStr + "::parent_model_frame",
+ axisElem->Get<bool>("use_parent_model_frame"));
}
+ this->configWidget->SetDoubleWidgetValue("cfm",
+ odeElem->Get<double>("cfm"));
+ this->configWidget->SetDoubleWidgetValue("bounce",
+ odeElem->Get<double>("bounce"));
+ this->configWidget->SetDoubleWidgetValue("velocity",
+ odeElem->Get<double>("velocity"));
+ this->configWidget->SetDoubleWidgetValue("fudge_factor",
+ odeElem->Get<double>("fudge_factor"));
+ this->configWidget->SetDoubleWidgetValue("limit_cfm",
+ odeElem->GetElement("limit")->Get<double>("cfm"));
+ this->configWidget->SetDoubleWidgetValue("limit_erp",
+ odeElem->GetElement("limit")->Get<double>("erp"));
+ this->configWidget->SetDoubleWidgetValue("suspension_cfm",
+ odeElem->GetElement("suspension")->Get<double>("cfm"));
+ this->configWidget->SetDoubleWidgetValue("suspension_erp",
+ odeElem->GetElement("suspension")->Get<double>("erp"));
+
+ this->configWidget->SetWidgetVisible("id", false);
+ this->configWidget->SetWidgetVisible("parent_id", false);
+ this->configWidget->SetWidgetVisible("child_id", false);
+
+ this->configWidget->SetWidgetReadOnly("id", true);
+ this->configWidget->SetWidgetReadOnly("parent_id", true);
+ this->configWidget->SetWidgetReadOnly("child_id", true);
+ this->configWidget->SetWidgetReadOnly("parent", true);
+ this->configWidget->SetWidgetReadOnly("child", true);
+
+ QObject::connect(this->configWidget,
+ SIGNAL(EnumValueChanged(const QString &, const QString &)), this,
+ SLOT(OnJointTypeChanged(const QString &, const QString &)));
+
+ this->OnJointTypeChanged("type",
+ tr(msgs::Joint_Type_Name(jointMsg.type()).c_str()));
+
QHBoxLayout *buttonsLayout = new QHBoxLayout;
- QPushButton *cancelButton = new QPushButton(tr("&Cancel"));
+ QPushButton *cancelButton = new QPushButton(tr("Cancel"));
connect(cancelButton, SIGNAL(clicked()), this, SLOT(OnCancel()));
- QPushButton *applyButton = new QPushButton(tr("&Apply"));
+ QPushButton *applyButton = new QPushButton(tr("Apply"));
connect(applyButton, SIGNAL(clicked()), this, SLOT(OnApply()));
- QPushButton *OKButton = new QPushButton(tr("&OK"));
+ QPushButton *OKButton = new QPushButton(tr("OK"));
OKButton->setDefault(true);
connect(OKButton, SIGNAL(clicked()), this, SLOT(OnOK()));
buttonsLayout->addWidget(cancelButton);
@@ -220,200 +120,64 @@ JointInspector::JointInspector(JointMaker::JointType _jointType,
buttonsLayout->setAlignment(Qt::AlignRight);
QVBoxLayout *mainLayout = new QVBoxLayout;
- mainLayout->addLayout(nameLayout);
- mainLayout->addLayout(typeLayout);
- mainLayout->addLayout(parentLayout);
- mainLayout->addLayout(childLayout);
- mainLayout->addWidget(poseGroupBox);
- for (unsigned int i = 0; i < axisGroupBoxes.size(); ++i)
- {
- mainLayout->addWidget(this->axisGroupBoxes[i]);
- this->axisGroupBoxes[i]->setVisible(false);
- }
+ mainLayout->addLayout(generalLayout);
mainLayout->addLayout(buttonsLayout);
- this->setLayout(mainLayout);
- this->layout()->setSizeConstraint(QLayout::SetFixedSize);
-
- if (this->jointType)
- this->SetType(this->jointType);
-}
-
-/////////////////////////////////////////////////
-JointInspector::~JointInspector()
-{
-}
-
-/////////////////////////////////////////////////
-math::Pose JointInspector::GetPose() const
-{
- return math::Pose(this->poseXSpinBox->value(),
- this->poseYSpinBox->value(), this->poseZSpinBox->value(),
- this->poseRollSpinBox->value(), this->posePitchSpinBox->value(),
- this->poseYawSpinBox->value());
-}
-/////////////////////////////////////////////////
-math::Vector3 JointInspector::GetAxis(unsigned int _index) const
-{
- if (_index > this->axisXSpinBoxes.size())
- {
- gzerr << "Axis index is out of range" << std::endl;
- return math::Vector3::Zero;
- }
-
- return math::Vector3(this->axisXSpinBoxes[_index]->value(),
- this->axisYSpinBoxes[_index]->value(),
- this->axisZSpinBoxes[_index]->value());
-}
-
-/////////////////////////////////////////////////
-double JointInspector::GetLowerLimit(unsigned int _index) const
-{
- if (_index > this->lowerLimitSpinBoxes.size())
- {
- gzerr << "Axis index is out of range" << std::endl;
- return 0;
- }
-
- return this->lowerLimitSpinBoxes[_index]->value();
-}
+ this->setMinimumWidth(500);
+ this->setMinimumHeight(300);
-/////////////////////////////////////////////////
-double JointInspector::GetUpperLimit(unsigned int _index) const
-{
- if (_index > this->upperLimitSpinBoxes.size())
- {
- gzerr << "Axis index is out of range" << std::endl;
- return 0;
- }
-
- return this->upperLimitSpinBoxes[_index]->value();
-}
-
-/////////////////////////////////////////////////
-JointMaker::JointType JointInspector::GetType() const
-{
- return this->jointType;
-}
-/////////////////////////////////////////////////
-std::string JointInspector::GetName() const
-{
- return this->jointNameLineEdit->text().toStdString();
-}
-
-/////////////////////////////////////////////////
-void JointInspector::SetType(JointMaker::JointType _type)
-{
- this->jointType = _type;
-
- std::string jointTypeStr = JointMaker::GetTypeAsString(_type);
- int axisCount = JointMaker::GetJointAxisCount(_type);
- GZ_ASSERT(axisCount >= 0, "Invalid axis count");
-
-
- int index = this->jointTypeComboBox->findText(tr(jointTypeStr.c_str()));
-
- if (index >= 0)
- this->jointTypeComboBox->setCurrentIndex(index);
- else
- {
- gzerr << "Joint type not found in inspector" << std::endl;
- return;
- }
-
- for (int i = 0; i < axisCount; ++i)
- this->axisGroupBoxes[i]->setVisible(true);
-
- for (int i = axisCount;
- i < static_cast<int>(this->axisGroupBoxes.size()); ++i)
- {
- this->axisGroupBoxes[i]->setVisible(false);
- }
-}
-
-/////////////////////////////////////////////////
-void JointInspector::SetName(const std::string &_name)
-{
- this->jointNameLineEdit->setText(tr(_name.c_str()));
+ this->setLayout(mainLayout);
}
/////////////////////////////////////////////////
-void JointInspector::SetParent(const std::string &_parent)
+JointInspector::~JointInspector()
{
- this->jointParentLabel->setText(tr(_parent.c_str()));
}
/////////////////////////////////////////////////
-void JointInspector::SetChild(const std::string &_child)
+void JointInspector::Update(ConstJointPtr _jointMsg)
{
- this->jointChildLabel->setText(tr(_child.c_str()));
+ this->configWidget->UpdateFromMsg(_jointMsg.get());
}
/////////////////////////////////////////////////
void JointInspector::SetPose(const math::Pose &_pose)
{
- this->poseXSpinBox->setValue(_pose.pos.x);
- this->poseYSpinBox->setValue(_pose.pos.y);
- this->poseZSpinBox->setValue(_pose.pos.z);
-
- math::Vector3 rot = _pose.rot.GetAsEuler();
- this->poseRollSpinBox->setValue(rot.x);
- this->posePitchSpinBox->setValue(rot.y);
- this->poseYawSpinBox->setValue(rot.z);
+ this->configWidget->SetPoseWidgetValue("pose", _pose);
}
/////////////////////////////////////////////////
-void JointInspector::SetAxis(unsigned int _index, const math::Vector3 &_axis)
+msgs::Joint *JointInspector::GetData() const
{
- if (_index > this->axisXSpinBoxes.size())
- {
- gzerr << "Axis index is out of range" << std::endl;
- return;
- }
-
- this->axisXSpinBoxes[_index]->setValue(_axis.x);
- this->axisYSpinBoxes[_index]->setValue(_axis.y);
- this->axisZSpinBoxes[_index]->setValue(_axis.z);
+ return dynamic_cast<msgs::Joint *>(this->configWidget->GetMsg());
}
/////////////////////////////////////////////////
-void JointInspector::SetLowerLimit(unsigned int _index, double _lower)
+void JointInspector::OnJointTypeChanged(const QString &/*_name*/,
+ const QString &_value)
{
- if (_index > this->lowerLimitSpinBoxes.size())
- {
- gzerr << "Axis index is out of range" << std::endl;
- return;
- }
-
- this->lowerLimitSpinBoxes[_index]->setValue(_lower);
-}
+ std::string valueStr = _value.toLower().toStdString();
+ unsigned int axisCount = JointMaker::GetJointAxisCount(
+ JointMaker::ConvertJointType(valueStr));
-/////////////////////////////////////////////////
-void JointInspector::SetUpperLimit(unsigned int _index, double _upper)
-{
- if (_index > this->upperLimitSpinBoxes.size())
+ for (unsigned int i = 0; i < axisCount; ++i)
{
- gzerr << "Axis index is out of range" << std::endl;
- return;
+ std::stringstream axis;
+ axis << "axis" << i+1;
+ std::string axisStr = axis.str();
+ this->configWidget->SetWidgetVisible(axisStr, true);
+ this->configWidget->SetWidgetReadOnly(axisStr, false);
+ this->configWidget->UpdateFromMsg(this->configWidget->GetMsg());
}
- this->upperLimitSpinBoxes[_index]->setValue(_upper);
-}
-
-/////////////////////////////////////////////////
-void JointInspector::OnJointTypeChanged(int _index)
-{
- QVariant jointTypeData = this->jointTypeComboBox->itemData(_index);
- this->jointType = static_cast<JointMaker::JointType>(jointTypeData.toInt());
-
- int axisCount = JointMaker::GetJointAxisCount(this->jointType);
- for (int i = 0; i < axisCount; ++i)
- this->axisGroupBoxes[i]->setVisible(true);
-
- for (int i = axisCount;
- i < static_cast<int>(this->axisGroupBoxes.size()); ++i)
+ for (unsigned int i = axisCount; i < 2u; ++i)
{
- this->axisGroupBoxes[i]->setVisible(false);
+ std::stringstream axis;
+ axis << "axis" << i+1;
+ std::string axisStr = axis.str();
+ this->configWidget->SetWidgetVisible(axisStr, false);
+ this->configWidget->SetWidgetReadOnly(axisStr, true);
+ this->configWidget->UpdateFromMsg(this->configWidget->GetMsg());
}
}
@@ -435,3 +199,9 @@ void JointInspector::OnOK()
emit Applied();
this->accept();
}
+
+/////////////////////////////////////////////////
+void JointInspector::enterEvent(QEvent */*_event*/)
+{
+ QApplication::setOverrideCursor(Qt::ArrowCursor);
+}
diff --git a/gazebo/gui/model/JointInspector.hh b/gazebo/gui/model/JointInspector.hh
index 7607fe1..f689688 100644
--- a/gazebo/gui/model/JointInspector.hh
+++ b/gazebo/gui/model/JointInspector.hh
@@ -31,90 +31,47 @@ namespace gazebo
namespace gui
{
class JointMaker;
+ class ConfigWidget;
/// \class JointInspector gui/JointInspector.hh
/// \brief A class to inspect and modify joints.
- class GAZEBO_VISIBLE JointInspector : public QDialog
+ class GZ_GUI_MODEL_VISIBLE JointInspector : public QDialog
{
Q_OBJECT
/// \brief Constructor
- /// \param[in] _mode Dialog mode
/// \param[in] _parent Parent QWidget.
- public: JointInspector(JointMaker::JointType _jointType,
- QWidget *_parent = 0);
+ public: JointInspector(QWidget *_parent = 0);
/// \brief Destructor
public: ~JointInspector();
- /// \brief Get joint pose.
- /// \return Joint pose.
- public: math::Pose GetPose() const;
+ /// \brief Update the joint config widget with a joint msg.
+ /// \param[in] _jointMsg Joint message.
+ public: void Update(ConstJointPtr _jointMsg);
- /// \brief Get axis.
- /// \param[in] _index Index of axis
- /// \return Axis direction.
- public: math::Vector3 GetAxis(unsigned int _index) const;
+ /// \brief Get the msg containing all joint data.
+ /// \return Joint msg.
+ public: msgs::Joint *GetData() const;
- /// \brief Get axis lower limit.
- /// \param[in] _index Index of axis
- /// \return Lower limit.
- public: double GetLowerLimit(unsigned int _index) const;
-
- /// \brief Get axis upper limit.
- /// \param[in] _index Index of axis
- /// \return Upper limit.
- public: double GetUpperLimit(unsigned int _index) const;
-
- /// \brief Get joint type.
- /// \return Joint type.
- public: JointMaker::JointType GetType() const;
-
- /// \brief Get joint name.
- /// \return Joint name.
- public: std::string GetName() const;
-
- /// \brief Set the joint name.
- /// \param[in] _name Name to set to.
- public: void SetName(const std::string &_name);
-
- /// \brief Set the joint parent name.
- /// \param[in] _name Name of joint parent.
- public: void SetParent(const std::string &_parent);
-
- /// \brief Set the joint child name.
- /// \param[in] _name Name of joint child.
- public: void SetChild(const std::string &_child);
-
- /// \brief Set joint pose.
- /// \param[in] _pose Joint pose.
+ /// \brief Set the pose of the joint.
+ /// \param[in] _pose Pose to set the joint to.
public: void SetPose(const math::Pose &_pose);
- /// \brief Set axis.
- /// \param[in] _index Index of axis.
- /// \param[in] _axis Axis direction.
- public: void SetAxis(unsigned int _index, const math::Vector3 &_axis);
+ /// \brief Qt event emiited when the mouse enters this widget.
+ /// \param[in] _event Qt event.
+ protected: virtual void enterEvent(QEvent *_event);
- /// \brief Set axis lower limit.
- /// \param[in] _index Index of axis.
- /// \param[in] _lower Lower limit.
- public: void SetLowerLimit(unsigned int _index, double _lower);
-
- /// \brief Set axis upper limit.
- /// \param[in] _index Index of axis.
- /// \param[in] _upper Upper limit.
- public: void SetUpperLimit(unsigned int _index, double _upper);
-
- /// \brief Set joint type.
- /// \param[in] _type joint type.
- public: void SetType(JointMaker::JointType _type);
+ /// \brief Qt callback when the joint type has changed.
+ /// \param[in] _name of widget in the config widget that emitted the
+ /// signal.
+ /// \param[in] _value New joint type value in string.
+ private slots: void OnJointTypeChanged(const QString &_name,
+ const QString &_value);
/// \brief Qt signal emitted to indicate that changes should be applied.
Q_SIGNALS: void Applied();
- /// \brief Qt callback when the joint type combox index has changed.
- private slots: void OnJointTypeChanged(int _index);
-
/// \brief Qt callback when the Cancel button is pressed.
private slots: void OnCancel();
@@ -124,56 +81,8 @@ namespace gazebo
/// \brief Qt callback when the Ok button is pressed.
private slots: void OnOK();
- /// \brief Line edit that displays the name of the joint.
- private: QLineEdit* jointNameLineEdit;
-
- /// \brief Label that displays the parent name of the joint.
- private: QLabel* jointParentLabel;
-
- /// \brief Label that displays the child name of the joint.
- private: QLabel* jointChildLabel;
-
- /// \brief ComboBox that displays the type of the joint.
- private: QComboBox *jointTypeComboBox;
-
- /// \brief Spin box for configuring the X position of the joint.
- private: QDoubleSpinBox *poseXSpinBox;
-
- /// \brief Spin box for configuring the Y position of the joint.
- private: QDoubleSpinBox *poseYSpinBox;
-
- /// \brief Spin box for configuring the Z position of joint.
- private: QDoubleSpinBox *poseZSpinBox;
-
- /// \brief Spin box for configuring the roll of the joint.
- private: QDoubleSpinBox *poseRollSpinBox;
-
- /// \brief Spin box for configuring the pitch of the joint.
- private: QDoubleSpinBox *posePitchSpinBox;
-
- /// \brief Spin box for configuring the yaw of the joint.
- private: QDoubleSpinBox *poseYawSpinBox;
-
- /// \brief Spin box for configuring the X direction of the axis.
- private: std::vector<QDoubleSpinBox *> axisXSpinBoxes;
-
- /// \brief Spin box for configuring the Y direction of the axis.
- private: std::vector<QDoubleSpinBox *> axisYSpinBoxes;
-
- /// \brief Spin box for configuring the Z direction of the axis.
- private: std::vector<QDoubleSpinBox *> axisZSpinBoxes;
-
- /// \brief Spin box for configuring the lower limit of the axis.
- private: std::vector<QDoubleSpinBox *> lowerLimitSpinBoxes;
-
- /// \brief Spin box for configuring the upper limit of the axis.
- private: std::vector<QDoubleSpinBox *> upperLimitSpinBoxes;
-
- /// \brief Type of joint.
- private: JointMaker::JointType jointType;
-
- /// \brief A list of group boxes for configuring joint axis properties.
- private: std::vector<QGroupBox *> axisGroupBoxes;
+ /// \brief Config widget for configuring joint properties.
+ private: ConfigWidget *configWidget;
};
/// \}
}
diff --git a/gazebo/gui/model/JointMaker.cc b/gazebo/gui/model/JointMaker.cc
index 1d13d6b..1f27103 100644
--- a/gazebo/gui/model/JointMaker.cc
+++ b/gazebo/gui/model/JointMaker.cc
@@ -24,26 +24,32 @@
#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/DynamicLines.hh"
#include "gazebo/rendering/Visual.hh"
+#include "gazebo/rendering/JointVisual.hh"
#include "gazebo/rendering/UserCamera.hh"
#include "gazebo/rendering/Scene.hh"
+#include "gazebo/rendering/RenderTypes.hh"
#include "gazebo/gui/GuiIface.hh"
#include "gazebo/gui/KeyEventHandler.hh"
#include "gazebo/gui/MouseEventHandler.hh"
#include "gazebo/gui/GuiEvents.hh"
+#include "gazebo/gui/MainWindow.hh"
#include "gazebo/gui/model/JointInspector.hh"
+#include "gazebo/gui/model/ModelEditorEvents.hh"
#include "gazebo/gui/model/JointMaker.hh"
using namespace gazebo;
using namespace gui;
+std::map<JointMaker::JointType, std::string> JointMaker::jointTypes;
+
/////////////////////////////////////////////////
JointMaker::JointMaker()
{
- this->UnitVectors.push_back(math::Vector3::UnitX);
- this->UnitVectors.push_back(math::Vector3::UnitY);
- this->UnitVectors.push_back(math::Vector3::UnitZ);
+ this->unitVectors.push_back(ignition::math::Vector3d::UnitX);
+ this->unitVectors.push_back(ignition::math::Vector3d::UnitY);
+ this->unitVectors.push_back(ignition::math::Vector3d::UnitZ);
this->newJointCreated = false;
this->mouseJoint = NULL;
@@ -59,9 +65,35 @@ JointMaker::JointMaker()
this->jointMaterials[JOINT_UNIVERSAL] = "Gazebo/Blue";
this->jointMaterials[JOINT_BALL] = "Gazebo/Purple";
+
+ jointTypes[JOINT_FIXED] = "fixed";
+ jointTypes[JOINT_HINGE] = "revolute";
+ jointTypes[JOINT_HINGE2] = "revolute2";
+ jointTypes[JOINT_SLIDER] = "prismatic";
+ jointTypes[JOINT_SCREW] = "screw";
+ jointTypes[JOINT_UNIVERSAL] = "universal";
+ jointTypes[JOINT_BALL] = "ball";
+ jointTypes[JOINT_NONE] = "none";
+
this->connections.push_back(
event::Events::ConnectPreRender(
- boost::bind(&JointMaker::Update, this)));
+ boost::bind(&JointMaker::Update, this)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectOpenJointInspector(
+ boost::bind(&JointMaker::OpenInspector, this, _1)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectShowJointContextMenu(
+ boost::bind(&JointMaker::ShowContextMenu, this, _1)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectSetSelectedJoint(
+ boost::bind(&JointMaker::OnSetSelectedJoint, this, _1, _2)));
+
+ this->connections.push_back(
+ event::Events::ConnectSetSelectedEntity(
+ boost::bind(&JointMaker::OnSetSelectedEntity, this, _1, _2)));
this->inspectAct = new QAction(tr("Open Joint Inspector"), this);
connect(this->inspectAct, SIGNAL(triggered()), this, SLOT(OnOpenInspector()));
@@ -72,7 +104,23 @@ JointMaker::JointMaker()
/////////////////////////////////////////////////
JointMaker::~JointMaker()
{
- this->Reset();
+ if (this->mouseJoint)
+ {
+ delete this->mouseJoint;
+ this->mouseJoint = NULL;
+ }
+
+ {
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+ while (this->joints.size() > 0)
+ {
+ std::string jointName = this->joints.begin()->first;
+ this->RemoveJoint(jointName);
+ }
+ this->joints.clear();
+ }
+
+ delete this->updateMutex;
}
/////////////////////////////////////////////////
@@ -90,11 +138,16 @@ void JointMaker::Reset()
this->selectedVis.reset();
this->hoverVis.reset();
this->prevHoverVis.reset();
- this->inspectVis.reset();
- this->selectedJoint.reset();
+ this->inspectName = "";
+ this->selectedJoints.clear();
+
+ this->scopedLinkedNames.clear();
- while (this->joints.size() > 0)
- this->RemoveJoint(this->joints.begin()->first);
+ while (!this->joints.empty())
+ {
+ std::string jointId = this->joints.begin()->first;
+ this->RemoveJoint(jointId);
+ }
this->joints.clear();
}
@@ -125,40 +178,58 @@ void JointMaker::DisableEventHandlers()
}
/////////////////////////////////////////////////
-void JointMaker::RemoveJoint(const std::string &_jointName)
+void JointMaker::RemoveJoint(const std::string &_jointId)
{
boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
- if (this->joints.find(_jointName) != this->joints.end())
+ auto jointIt = this->joints.find(_jointId);
+ if (jointIt != this->joints.end())
{
- JointData *joint = this->joints[_jointName];
+ JointData *joint = jointIt->second;
rendering::ScenePtr scene = joint->hotspot->GetScene();
scene->GetManager()->destroyBillboardSet(joint->handles);
scene->RemoveVisual(joint->hotspot);
scene->RemoveVisual(joint->visual);
joint->visual->Fini();
+ if (joint->jointVisual)
+ {
+ rendering::JointVisualPtr parentAxisVis = joint->jointVisual
+ ->GetParentAxisVisual();
+ if (parentAxisVis)
+ {
+ parentAxisVis->GetParent()->DetachVisual(
+ parentAxisVis->GetName());
+ scene->RemoveVisual(parentAxisVis);
+ }
+ joint->jointVisual->GetParent()->DetachVisual(
+ joint->jointVisual->GetName());
+ scene->RemoveVisual(joint->jointVisual);
+ }
joint->hotspot.reset();
joint->visual.reset();
+ joint->jointVisual.reset();
joint->parent.reset();
joint->child.reset();
joint->inspector->hide();
delete joint->inspector;
delete joint;
- this->joints.erase(_jointName);
+ this->joints.erase(jointIt);
+ gui::model::Events::modelChanged();
+ gui::model::Events::jointRemoved(_jointId);
}
}
/////////////////////////////////////////////////
-void JointMaker::RemoveJointsByPart(const std::string &_partName)
+void JointMaker::RemoveJointsByLink(const std::string &_linkName)
{
std::vector<std::string> toDelete;
- boost::unordered_map<std::string, JointData *>::iterator it;
- for (it = this->joints.begin(); it != this->joints.end(); ++it)
+ for (auto it : this->joints)
{
- JointData *joint = it->second;
- if (joint->child->GetName() == _partName ||
- joint->parent->GetName() == _partName)
+ JointData *joint = it.second;
+
+ if (joint->child->GetName() == _linkName ||
+ joint->parent->GetName() == _linkName)
{
- toDelete.push_back(it->first);
+ toDelete.push_back(it.first);
}
}
@@ -169,24 +240,41 @@ void JointMaker::RemoveJointsByPart(const std::string &_partName)
}
/////////////////////////////////////////////////
+std::vector<JointData *> JointMaker::GetJointDataByLink(
+ const std::string &_linkName) const
+{
+ std::vector<JointData *> linkJoints;
+ for (auto jointIt : this->joints)
+ {
+ JointData *jointData = jointIt.second;
+
+ if (jointData->child->GetName() == _linkName ||
+ jointData->parent->GetName() == _linkName)
+ {
+ linkJoints.push_back(jointData);
+ }
+ }
+ return linkJoints;
+}
+
+/////////////////////////////////////////////////
bool JointMaker::OnMousePress(const common::MouseEvent &_event)
{
rendering::UserCameraPtr camera = gui::get_active_camera();
- if (_event.button == common::MouseEvent::MIDDLE)
+ if (_event.Button() == common::MouseEvent::MIDDLE)
{
QApplication::setOverrideCursor(QCursor(Qt::ArrowCursor));
camera->HandleMouseEvent(_event);
return true;
}
- else if (_event.button != common::MouseEvent::LEFT)
+ else if (_event.Button() != common::MouseEvent::LEFT)
return false;
if (this->jointType != JointMaker::JOINT_NONE)
return false;
// intercept mouse press events when user clicks on the joint hotspot visual
- rendering::ScenePtr scene = camera->GetScene();
- rendering::VisualPtr vis = camera->GetVisual(_event.pos);
+ rendering::VisualPtr vis = camera->GetVisual(_event.Pos());
if (vis)
{
if (this->joints.find(vis->GetName()) != this->joints.end())
@@ -205,34 +293,45 @@ bool JointMaker::OnMouseRelease(const common::MouseEvent &_event)
rendering::UserCameraPtr camera = gui::get_active_camera();
if (this->jointType == JointMaker::JOINT_NONE)
{
- rendering::VisualPtr vis = camera->GetVisual(_event.pos);
+ rendering::VisualPtr vis = camera->GetVisual(_event.Pos());
if (vis)
{
- if (this->selectedJoint)
- this->selectedJoint->SetHighlighted(false);
- this->selectedJoint.reset();
if (this->joints.find(vis->GetName()) != this->joints.end())
{
// trigger joint inspector on right click
- if (_event.button == common::MouseEvent::RIGHT)
+ if (_event.Button() == common::MouseEvent::RIGHT)
{
- this->inspectVis = vis;
- QMenu menu;
- menu.addAction(this->inspectAct);
- menu.exec(QCursor::pos());
+ this->inspectName = vis->GetName();
+ this->ShowContextMenu(this->inspectName);
+ return true;
}
- else if (_event.button == common::MouseEvent::LEFT)
+ else if (_event.Button() == common::MouseEvent::LEFT)
{
- this->selectedJoint = vis;
- this->selectedJoint->SetHighlighted(true);
+ // Not in multi-selection mode.
+ if (!(QApplication::keyboardModifiers() & Qt::ControlModifier))
+ {
+ this->DeselectAll();
+ this->SetSelected(vis, true);
+ }
+ // Multi-selection mode
+ else
+ {
+ auto it = std::find(this->selectedJoints.begin(),
+ this->selectedJoints.end(), vis);
+ // Highlight and select clicked joint if not already selected
+ // Otherwise deselect if already selected
+ this->SetSelected(vis, it == this->selectedJoints.end());
+ }
}
}
+ else
+ this->DeselectAll();
return false;
}
}
else
{
- if (_event.button == common::MouseEvent::LEFT)
+ if (_event.Button() == common::MouseEvent::LEFT)
{
if (this->hoverVis)
{
@@ -243,7 +342,7 @@ bool JointMaker::OnMouseRelease(const common::MouseEvent &_event)
return true;
}
- // Pressed parent part
+ // Pressed parent link
if (!this->selectedVis)
{
if (this->mouseJoint)
@@ -252,26 +351,27 @@ bool JointMaker::OnMouseRelease(const common::MouseEvent &_event)
this->hoverVis->SetEmissive(common::Color(0, 0, 0));
this->selectedVis = this->hoverVis;
this->hoverVis.reset();
+
// Create joint data with selected visual as parent
// the child will be set on the second mouse release.
- this->mouseJoint = this->CreateJoint(this->selectedVis,
- rendering::VisualPtr());
+ this->mouseJoint = this->CreateJointLine("JOINT_LINE",
+ this->selectedVis);
}
- // Pressed child part
+ // Pressed child link
else if (this->selectedVis != this->hoverVis)
{
if (this->hoverVis)
this->hoverVis->SetEmissive(common::Color(0, 0, 0));
if (this->selectedVis)
this->selectedVis->SetEmissive(common::Color(0, 0, 0));
- this->mouseJoint->child = this->hoverVis;
-
- // reset variables.
- this->selectedVis.reset();
- this->hoverVis.reset();
- this->AddJoint(JointMaker::JOINT_NONE);
+ this->mouseJoint->child = this->hoverVis;
+ JointData *newJoint = this->CreateJoint(this->mouseJoint->parent,
+ this->mouseJoint->child);
+ this->Stop();
+ this->mouseJoint = newJoint;
this->newJointCreated = true;
+ gui::model::Events::modelChanged();
}
}
}
@@ -284,84 +384,145 @@ bool JointMaker::OnMouseRelease(const common::MouseEvent &_event)
}
/////////////////////////////////////////////////
-JointData *JointMaker::CreateJoint(rendering::VisualPtr _parent,
- rendering::VisualPtr _child)
+JointData *JointMaker::CreateJointLine(const std::string &_name,
+ rendering::VisualPtr _parent)
{
- std::stringstream ss;
- ss << _parent->GetName() << "_JOINT_" << this->jointCounter++;
rendering::VisualPtr jointVis(
- new rendering::Visual(ss.str(), _parent->GetParent()));
+ new rendering::Visual(_name, _parent->GetParent()));
jointVis->Load();
rendering::DynamicLines *jointLine =
jointVis->CreateDynamicLine(rendering::RENDERING_LINE_LIST);
- math::Vector3 origin = this->GetPartWorldCentroid(_parent)
+ math::Vector3 origin = _parent->GetWorldPose().pos
- _parent->GetParent()->GetWorldPose().pos;
jointLine->AddPoint(origin);
jointLine->AddPoint(origin + math::Vector3(0, 0, 0.1));
jointVis->GetSceneNode()->setInheritScale(false);
jointVis->GetSceneNode()->setInheritOrientation(false);
- JointData *jointData = new JointData;
- jointData->name = jointVis->GetName();
+ std::string jointVisName = jointVis->GetName();
+ std::string leafName = jointVisName;
+ size_t pIdx = jointVisName.find_last_of("::");
+ if (pIdx != std::string::npos)
+ leafName = jointVisName.substr(pIdx+1);
+
+ JointData *jointData = new JointData();
jointData->dirty = false;
+ jointData->name = leafName;
jointData->visual = jointVis;
jointData->parent = _parent;
- jointData->child = _child;
jointData->line = jointLine;
jointData->type = this->jointType;
- jointData->inspector = new JointInspector(JointMaker::JOINT_NONE);
- jointData->inspector->setModal(false);
- connect(jointData->inspector, SIGNAL(Applied()),
- jointData, SLOT(OnApply()));
-
- int axisCount = JointMaker::GetJointAxisCount(jointData->type);
- for (int i = 0; i < axisCount; ++i)
- {
- if (i < static_cast<int>(this->UnitVectors.size()))
- jointData->axis[i] = this->UnitVectors[i];
- else
- jointData->axis[i] = this->UnitVectors[0];
-
- jointData->lowerLimit[i] = -3.14;
- jointData->upperLimit[i] = 3.14;
- }
- jointData->pose = math::Pose::Zero;
jointData->line->setMaterial(this->jointMaterials[jointData->type]);
return jointData;
}
/////////////////////////////////////////////////
-void JointMaker::AddJoint(const std::string &_type)
+JointData *JointMaker::CreateJoint(rendering::VisualPtr _parent,
+ rendering::VisualPtr _child)
{
- if (_type == "revolute")
- {
- this->AddJoint(JointMaker::JOINT_HINGE);
- }
- else if (_type == "revolute2")
- {
- this->AddJoint(JointMaker::JOINT_HINGE2);
- }
- else if (_type == "prismatic")
- {
- this->AddJoint(JointMaker::JOINT_SLIDER);
- }
- else if (_type == "ball")
+ std::stringstream ss;
+ ss << _parent->GetName() << "_JOINT_" << this->jointCounter++;
+
+ JointData *jointData = this->CreateJointLine(ss.str(), _parent);
+ jointData->child = _child;
+
+ jointData->inspector = new JointInspector();
+ jointData->inspector->setModal(false);
+ connect(jointData->inspector, SIGNAL(Applied()),
+ jointData, SLOT(OnApply()));
+
+ MainWindow *mainWindow = gui::get_main_window();
+ if (mainWindow)
{
- this->AddJoint(JointMaker::JOINT_BALL);
+ connect(gui::get_main_window(), SIGNAL(Close()), jointData->inspector,
+ SLOT(close()));
}
- else if (_type == "universal")
+
+ // setup the joint msg
+ jointData->jointMsg.reset(new msgs::Joint);
+ jointData->jointMsg->set_name(jointData->name);
+ if (jointData->parent)
{
- this->AddJoint(JointMaker::JOINT_UNIVERSAL);
+ std::string jointParentName = jointData->parent->GetName();
+ std::string leafName = jointParentName;
+ size_t pIdx = jointParentName.find_last_of("::");
+ if (pIdx != std::string::npos)
+ leafName = jointParentName.substr(pIdx+1);
+
+ jointData->jointMsg->set_parent(leafName);
+ jointData->jointMsg->set_parent_id(jointData->parent->GetId());
}
- else if (_type == "screw")
+ if (jointData->child)
{
- this->AddJoint(JointMaker::JOINT_SCREW);
+ std::string jointChildName = jointData->child->GetName();
+ std::string leafName = jointChildName;
+ size_t pIdx = jointChildName.find_last_of("::");
+ if (pIdx != std::string::npos)
+ leafName = jointChildName.substr(pIdx+1);
+
+ jointData->jointMsg->set_child(leafName);
+ jointData->jointMsg->set_child_id(jointData->child->GetId());
}
- else if (_type == "none")
+ msgs::Set(jointData->jointMsg->mutable_pose(),
+ ignition::math::Pose3d::Zero);
+
+ jointData->jointMsg->set_type(
+ msgs::ConvertJointType(this->GetTypeAsString(jointData->type)));
+
+ unsigned int axisCount = JointMaker::GetJointAxisCount(jointData->type);
+ for (unsigned int i = 0; i < axisCount; ++i)
{
- this->AddJoint(JointMaker::JOINT_NONE);
+ msgs::Axis *axisMsg;
+ if (i == 0u)
+ {
+ axisMsg = jointData->jointMsg->mutable_axis1();
+ }
+ else if (i == 1u)
+ {
+ axisMsg = jointData->jointMsg->mutable_axis2();
+ }
+ else
+ {
+ gzerr << "Invalid axis index["
+ << i
+ << "]"
+ << std::endl;
+ continue;
+ }
+ msgs::Set(axisMsg->mutable_xyz(),
+ this->unitVectors[i%this->unitVectors.size()]);
+ axisMsg->set_use_parent_model_frame(false);
+ axisMsg->set_limit_lower(-GZ_DBL_MAX);
+ axisMsg->set_limit_upper(GZ_DBL_MAX);
+ axisMsg->set_limit_effort(-1);
+ axisMsg->set_limit_velocity(-1);
+ axisMsg->set_damping(0);
+
+ // Add angle field after we've checked that index i is valid
+ jointData->jointMsg->add_angle(0);
}
+ jointData->jointMsg->set_limit_erp(0.2);
+ jointData->jointMsg->set_suspension_erp(0.2);
+
+ jointData->inspector->Update(jointData->jointMsg);
+ return jointData;
+}
+
+/////////////////////////////////////////////////
+JointMaker::JointType JointMaker::ConvertJointType(const std::string &_type)
+{
+ for (auto iter : jointTypes)
+ if (iter.second == _type)
+ return iter.first;
+
+ return JOINT_NONE;
+}
+
+/////////////////////////////////////////////////
+void JointMaker::AddJoint(const std::string &_type)
+{
+ this->AddJoint(this->ConvertJointType(_type));
}
/////////////////////////////////////////////////
@@ -396,7 +557,6 @@ void JointMaker::Stop()
rendering::ScenePtr scene = this->mouseJoint->visual->GetScene();
scene->RemoveVisual(this->mouseJoint->visual);
this->mouseJoint->visual.reset();
- delete this->mouseJoint->inspector;
delete this->mouseJoint;
this->mouseJoint = NULL;
}
@@ -416,7 +576,7 @@ bool JointMaker::OnMouseMove(const common::MouseEvent &_event)
// Get the active camera and scene.
rendering::UserCameraPtr camera = gui::get_active_camera();
- if (_event.dragging)
+ if (_event.Dragging())
{
// this enables the joint maker to pan while connecting joints
QApplication::setOverrideCursor(QCursor(Qt::ArrowCursor));
@@ -424,7 +584,7 @@ bool JointMaker::OnMouseMove(const common::MouseEvent &_event)
return true;
}
- rendering::VisualPtr vis = camera->GetVisual(_event.pos);
+ rendering::VisualPtr vis = camera->GetVisual(_event.Pos());
// Highlight visual on hover
if (vis)
@@ -432,13 +592,13 @@ bool JointMaker::OnMouseMove(const common::MouseEvent &_event)
if (this->hoverVis && this->hoverVis != this->selectedVis)
this->hoverVis->SetEmissive(common::Color(0.0, 0.0, 0.0));
- // only highlight editor parts by making sure it's not an item in the
+ // only highlight editor links by making sure it's not an item in the
// gui tree widget or a joint hotspot.
rendering::VisualPtr rootVis = vis->GetRootVisual();
if (rootVis->IsPlane())
this->hoverVis = vis->GetParent();
else if (!gui::get_entity_id(rootVis->GetName()) &&
- vis->GetName().find("_HOTSPOT_") == std::string::npos)
+ vis->GetName().find("_UNIQUE_ID_") == std::string::npos)
{
this->hoverVis = vis->GetParent();
if (!this->selectedVis ||
@@ -447,35 +607,35 @@ bool JointMaker::OnMouseMove(const common::MouseEvent &_event)
}
}
- // Case when a parent part is already selected and currently
- // extending the joint line to a child part
+ // Case when a parent link is already selected and currently
+ // extending the joint line to a child link
if (this->selectedVis && this->hoverVis
&& this->mouseJoint && this->mouseJoint->line)
{
math::Vector3 parentPos;
- // Set end point to center of child part
+ // Set end point to center of child link
if (!this->hoverVis->IsPlane())
{
if (this->mouseJoint->parent)
{
- parentPos = this->GetPartWorldCentroid(this->mouseJoint->parent)
+ parentPos = this->GetLinkWorldCentroid(this->mouseJoint->parent)
- this->mouseJoint->line->GetPoint(0);
this->mouseJoint->line->SetPoint(1,
- this->GetPartWorldCentroid(this->hoverVis) - parentPos);
+ this->GetLinkWorldCentroid(this->hoverVis) - parentPos);
}
}
else
{
// Set end point to mouse plane intersection
math::Vector3 pt;
- camera->GetWorldPointOnPlane(_event.pos.x, _event.pos.y,
+ camera->GetWorldPointOnPlane(_event.Pos().X(), _event.Pos().Y(),
math::Plane(math::Vector3(0, 0, 1)), pt);
if (this->mouseJoint->parent)
{
- parentPos = this->GetPartWorldCentroid(this->mouseJoint->parent)
+ parentPos = this->GetLinkWorldCentroid(this->mouseJoint->parent)
- this->mouseJoint->line->GetPoint(0);
this->mouseJoint->line->SetPoint(1,
- this->GetPartWorldCentroid(this->hoverVis) - parentPos + pt);
+ this->GetLinkWorldCentroid(this->hoverVis) - parentPos + pt);
}
}
}
@@ -485,36 +645,30 @@ bool JointMaker::OnMouseMove(const common::MouseEvent &_event)
/////////////////////////////////////////////////
void JointMaker::OnOpenInspector()
{
- this->OpenInspector(this->inspectVis->GetName());
- this->inspectVis.reset();
+ if (this->inspectName.empty())
+ return;
+
+ this->OpenInspector(this->inspectName);
+ this->inspectName = "";
}
/////////////////////////////////////////////////
-void JointMaker::OpenInspector(const std::string &_name)
+void JointMaker::OpenInspector(const std::string &_jointId)
{
- JointData *joint = this->joints[_name];
- joint->inspector->SetType(joint->type);
- joint->inspector->SetName(joint->name);
- joint->inspector->SetParent(joint->parent->GetName());
- joint->inspector->SetChild(joint->child->GetName());
- joint->inspector->SetPose(joint->pose);
- int axisCount = JointMaker::GetJointAxisCount(joint->type);
- for (int i = 0; i < axisCount; ++i)
+ JointData *joint = this->joints[_jointId];
+ if (!joint)
{
- joint->inspector->SetAxis(i, joint->axis[i]);
- joint->inspector->SetAxis(i, joint->axis[i]);
- joint->inspector->SetLowerLimit(i, joint->lowerLimit[i]);
- joint->inspector->SetUpperLimit(i, joint->upperLimit[i]);
+ gzerr << "Joint [" << _jointId << "] not found." << std::endl;
+ return;
}
- joint->inspector->move(QCursor::pos());
- joint->inspector->show();
+ joint->OpenInspector();
}
/////////////////////////////////////////////////
bool JointMaker::OnMouseDoubleClick(const common::MouseEvent &_event)
{
rendering::UserCameraPtr camera = gui::get_active_camera();
- rendering::VisualPtr vis = camera->GetVisual(_event.pos);
+ rendering::VisualPtr vis = camera->GetVisual(_event.Pos());
if (vis)
{
@@ -533,16 +687,31 @@ bool JointMaker::OnKeyPress(const common::KeyEvent &_event)
{
if (_event.key == Qt::Key_Delete)
{
- if (this->selectedJoint)
+ if (!this->selectedJoints.empty())
{
- this->RemoveJoint(this->selectedJoint->GetName());
- this->selectedJoint.reset();
+ for (auto jointVis : this->selectedJoints)
+ {
+ this->RemoveJoint(jointVis->GetName());
+ }
+ this->DeselectAll();
+ return true;
}
}
+
return false;
}
/////////////////////////////////////////////////
+void JointMaker::OnDelete()
+{
+ if (this->inspectName.empty())
+ return;
+
+ this->RemoveJoint(this->inspectName);
+ this->inspectName = "";
+}
+
+/////////////////////////////////////////////////
void JointMaker::CreateHotSpot(JointData *_joint)
{
if (!_joint)
@@ -550,21 +719,20 @@ void JointMaker::CreateHotSpot(JointData *_joint)
rendering::UserCameraPtr camera = gui::get_active_camera();
- std::string hotSpotName = _joint->visual->GetName() + "_HOTSPOT_";
+ // Joint hotspot visual name is the JointId for easy access when clicking
+ std::string jointId = _joint->visual->GetName() + "_UNIQUE_ID_";
rendering::VisualPtr hotspotVisual(
- new rendering::Visual(hotSpotName, _joint->visual, false));
-
- _joint->hotspot = hotspotVisual;
+ new rendering::Visual(jointId, _joint->visual, false));
// create a cylinder to represent the joint
hotspotVisual->InsertMesh("unit_cylinder");
Ogre::MovableObject *hotspotObj =
(Ogre::MovableObject*)(camera->GetScene()->GetManager()->createEntity(
- "__HOTSPOT__" + _joint->visual->GetName(), "unit_cylinder"));
- hotspotObj->getUserObjectBindings().setUserAny(Ogre::Any(hotSpotName));
+ _joint->visual->GetName(), "unit_cylinder"));
+ hotspotObj->getUserObjectBindings().setUserAny(Ogre::Any(jointId));
hotspotVisual->GetSceneNode()->attachObject(hotspotObj);
hotspotVisual->SetMaterial(this->jointMaterials[_joint->type]);
- hotspotVisual->SetTransparency(0.5);
+ hotspotVisual->SetTransparency(0.7);
// create a handle at the parent end
Ogre::BillboardSet *handleSet =
@@ -576,7 +744,12 @@ void JointMaker::CreateHotSpot(JointData *_joint)
this->jointMaterials[_joint->type]);
Ogre::ColourValue color = mat->getTechnique(0)->getPass(0)->getDiffuse();
color.a = 0.5;
- double dimension = 0.1;
+
+ double linkSize = std::min(0.1,
+ _joint->parent->GetBoundingBox().GetSize().GetLength()*0.05);
+ linkSize = std::max(linkSize, 0.01);
+
+ double dimension = linkSize;
handleSet->setDefaultDimensions(dimension, dimension);
Ogre::Billboard *parentHandle = handleSet->createBillboard(0, 0, 0);
parentHandle->setColour(color);
@@ -591,14 +764,12 @@ void JointMaker::CreateHotSpot(JointData *_joint)
GZ_VISIBILITY_SELECTABLE);
hotspotVisual->GetSceneNode()->setInheritScale(false);
- this->joints[hotSpotName] = _joint;
+ this->joints[jointId] = _joint;
camera->GetScene()->AddVisual(hotspotVisual);
- // remove line as we are using a cylinder hotspot visual to
- // represent the joint
- _joint->visual->DeleteDynamicLine(_joint->line);
-
- _joint->dirty = true;
+ _joint->hotspot = hotspotVisual;
+ gui::model::Events::jointInserted(jointId, _joint->name,
+ _joint->parent->GetName(), _joint->child->GetName());
}
/////////////////////////////////////////////////
@@ -613,101 +784,182 @@ void JointMaker::Update()
}
// update joint line and hotspot position.
- boost::unordered_map<std::string, JointData *>::iterator it;
- for (it = this->joints.begin(); it != this->joints.end(); ++it)
+ for (auto it : this->joints)
{
- JointData *joint = it->second;
- if (joint->dirty)
+ JointData *joint = it.second;
+ if (joint->hotspot)
{
if (joint->child && joint->parent)
{
- // get centroid of parent part visuals
- math::Vector3 parentCentroid =
- this->GetPartWorldCentroid(joint->parent);
-
- // get centroid of child part visuals
- math::Vector3 childCentroid =
- this->GetPartWorldCentroid(joint->child);
-
- // set orientation of joint hotspot
- math::Vector3 dPos = (childCentroid - parentCentroid);
- math::Vector3 center = dPos/2.0;
- joint->hotspot->SetScale(math::Vector3(0.02, 0.02, dPos.GetLength()));
- joint->hotspot->SetWorldPosition(parentCentroid + center);
- math::Vector3 u = dPos.Normalize();
- math::Vector3 v = math::Vector3::UnitZ;
- double cosTheta = v.Dot(u);
- double angle = acos(cosTheta);
- math::Vector3 w = (v.Cross(u)).Normalize();
- math::Quaternion q;
- q.SetFromAxis(w, angle);
- joint->hotspot->SetWorldRotation(q);
-
- // set new material if joint type has changed
- std::string material = this->jointMaterials[joint->type];
- if (joint->hotspot->GetMaterialName() != material)
+ bool poseUpdate = false;
+ if (joint->parentPose != joint->parent->GetWorldPose() ||
+ joint->childPose != joint->child->GetWorldPose() ||
+ joint->childScale != joint->child->GetScale())
+ {
+ joint->parentPose = joint->parent->GetWorldPose();
+ joint->childPose = joint->child->GetWorldPose();
+ joint->childScale = joint->child->GetScale();
+ poseUpdate = true;
+ }
+
+ if (joint->dirty || poseUpdate)
{
- // Note: issue setting material when there is a billboard child,
- // seems to hang so detach before setting and re-attach later.
- Ogre::SceneNode *handleNode = joint->handles->getParentSceneNode();
- joint->handles->detachFromParent();
- joint->hotspot->SetMaterial(material);
- handleNode->attachObject(joint->handles);
- Ogre::MaterialPtr mat =
- Ogre::MaterialManager::getSingleton().getByName(material);
- Ogre::ColourValue color =
- mat->getTechnique(0)->getPass(0)->getDiffuse();
- color.a = 0.5;
- joint->handles->getBillboard(0)->setColour(color);
+ // get origin of parent link visuals
+ math::Vector3 parentOrigin = joint->parent->GetWorldPose().pos;
+
+ // get origin of child link visuals
+ math::Vector3 childOrigin = joint->child->GetWorldPose().pos;
+
+ // set orientation of joint hotspot
+ math::Vector3 dPos = (childOrigin - parentOrigin);
+ math::Vector3 center = dPos * 0.5;
+ double length = std::max(dPos.GetLength(), 0.001);
+ joint->hotspot->SetScale(
+ math::Vector3(0.008, 0.008, length));
+ joint->hotspot->SetWorldPosition(parentOrigin + center);
+ math::Vector3 u = dPos.Normalize();
+ math::Vector3 v = math::Vector3::UnitZ;
+ double cosTheta = v.Dot(u);
+ double angle = acos(cosTheta);
+ math::Vector3 w = (v.Cross(u)).Normalize();
+ math::Quaternion q;
+ q.SetFromAxis(w, angle);
+ joint->hotspot->SetWorldRotation(q);
+
+ // set new material if joint type has changed
+ std::string material = this->jointMaterials[joint->type];
+ if (joint->hotspot->GetMaterialName() != material)
+ {
+ // Note: issue setting material when there is a billboard child,
+ // seems to hang so detach before setting and re-attach later.
+ Ogre::SceneNode *handleNode = joint->handles->getParentSceneNode();
+ joint->handles->detachFromParent();
+ joint->hotspot->SetMaterial(material);
+ joint->hotspot->SetTransparency(0.7);
+ handleNode->attachObject(joint->handles);
+ Ogre::MaterialPtr mat =
+ Ogre::MaterialManager::getSingleton().getByName(material);
+ Ogre::ColourValue color =
+ mat->getTechnique(0)->getPass(0)->getDiffuse();
+ color.a = 0.5;
+ joint->handles->getBillboard(0)->setColour(color);
+ }
+
+ // set pos of joint handle
+ joint->handles->getBillboard(0)->setPosition(
+ rendering::Conversions::Convert(parentOrigin -
+ joint->hotspot->GetWorldPose().pos));
+ joint->handles->_updateBounds();
}
- // set pos of joint handle
- joint->handles->getBillboard(0)->setPosition(
- rendering::Conversions::Convert(parentCentroid -
- joint->hotspot->GetWorldPose().pos));
- joint->handles->_updateBounds();
+ // Create / update joint visual
+ if (joint->dirty || poseUpdate)
+ {
+ msgs::JointPtr jointUpdateMsg = joint->jointMsg;
+ unsigned int axisCount = JointMaker::GetJointAxisCount(joint->type);
+ for (unsigned int i = axisCount; i < 2u; ++i)
+ {
+ if (i == 0u)
+ jointUpdateMsg->clear_axis1();
+ else if (i == 1u)
+ jointUpdateMsg->clear_axis2();
+ }
+
+ if (joint->jointVisual)
+ {
+ joint->jointVisual->UpdateFromMsg(jointUpdateMsg);
+ }
+ else
+ {
+ std::string childName = joint->child->GetName();
+ std::string jointVisName = childName;
+ size_t idx = childName.find("::");
+ if (idx != std::string::npos)
+ jointVisName = childName.substr(0, idx+2);
+ jointVisName += "_JOINT_VISUAL_";
+ gazebo::rendering::JointVisualPtr jointVis(
+ new gazebo::rendering::JointVisual(jointVisName, joint->child));
+
+ jointVis->Load(jointUpdateMsg);
+
+ joint->jointVisual = jointVis;
+ }
+
+ // Line now connects the child link to the joint frame
+ joint->line->SetPoint(0, joint->child->GetWorldPose().pos
+ - joint->child->GetParent()->GetWorldPose().pos);
+ joint->line->SetPoint(1,
+ joint->jointVisual->GetWorldPose().pos
+ - joint->child->GetParent()->GetWorldPose().pos);
+ joint->line->setMaterial(this->jointMaterials[joint->type]);
+ joint->dirty = false;
+ }
}
}
}
}
/////////////////////////////////////////////////
+void JointMaker::AddScopedLinkName(const std::string &_name)
+{
+ this->scopedLinkedNames.push_back(_name);
+}
+
+/////////////////////////////////////////////////
+std::string JointMaker::GetScopedLinkName(const std::string &_name)
+{
+ for (unsigned int i = 0; i < this->scopedLinkedNames.size(); ++i)
+ {
+ std::string scopedName = this->scopedLinkedNames[i];
+ size_t idx = scopedName.find("::" + _name);
+ if (idx != std::string::npos)
+ return scopedName;
+ }
+ return _name;
+}
+
+/////////////////////////////////////////////////
void JointMaker::GenerateSDF()
{
this->modelSDF.reset(new sdf::Element);
sdf::initFile("model.sdf", this->modelSDF);
this->modelSDF->ClearElements();
- boost::unordered_map<std::string, JointData *>::iterator jointsIt;
- // loop through all parts
- for (jointsIt = this->joints.begin(); jointsIt != this->joints.end();
- ++jointsIt)
+ // loop through all joints
+ for (auto jointsIt : this->joints)
{
- JointData *joint = jointsIt->second;
+ JointData *joint = jointsIt.second;
sdf::ElementPtr jointElem = this->modelSDF->AddElement("joint");
- jointElem->GetAttribute("name")->Set(joint->name);
- jointElem->GetAttribute("type")->Set(GetTypeAsString(joint->type));
- sdf::ElementPtr parentElem = jointElem->GetElement("parent");
- parentElem->Set(joint->parent->GetName());
- sdf::ElementPtr childElem = jointElem->GetElement("child");
- childElem->Set(joint->child->GetName());
- sdf::ElementPtr poseElem = jointElem->GetElement("pose");
- poseElem->Set(joint->pose);
- int axisCount = GetJointAxisCount(joint->type);
- for (int i = 0; i < axisCount; ++i)
+ msgs::JointPtr jointMsg = joint->jointMsg;
+ unsigned int axisCount = GetJointAxisCount(joint->type);
+ for (unsigned int i = axisCount; i < 2u; ++i)
{
- std::stringstream ss;
- ss << "axis";
- if (i > 0)
- ss << (i+1);
- sdf::ElementPtr axisElem = jointElem->GetElement(ss.str());
- axisElem->GetElement("xyz")->Set(joint->axis[i]);
-
- sdf::ElementPtr limitElem = axisElem->GetElement("limit");
- limitElem->GetElement("lower")->Set(joint->lowerLimit[i]);
- limitElem->GetElement("upper")->Set(joint->upperLimit[i]);
+ if (i == 0u)
+ jointMsg->clear_axis1();
+ else if (i == 1u)
+ jointMsg->clear_axis2();
}
+ jointElem = msgs::JointToSDF(*jointMsg.get(), jointElem);
+
+ sdf::ElementPtr parentElem = jointElem->GetElement("parent");
+ std::string parentName = joint->parent->GetName();
+ std::string parentLeafName = parentName;
+ size_t pIdx = parentName.find_last_of("::");
+ if (pIdx != std::string::npos)
+ parentLeafName = parentName.substr(pIdx+1);
+
+ parentLeafName = this->GetScopedLinkName(parentLeafName);
+ parentElem->Set(parentLeafName);
+
+ sdf::ElementPtr childElem = jointElem->GetElement("child");
+ std::string childName = joint->child->GetName();
+ std::string childLeafName = childName;
+ size_t cIdx = childName.find_last_of("::");
+ if (cIdx != std::string::npos)
+ childLeafName = childName.substr(cIdx+1);
+ childLeafName = this->GetScopedLinkName(childLeafName);
+ childElem->Set(childLeafName);
}
}
@@ -722,44 +974,15 @@ std::string JointMaker::GetTypeAsString(JointMaker::JointType _type)
{
std::string jointTypeStr = "";
- if (_type == JointMaker::JOINT_FIXED)
- {
- jointTypeStr = "fixed";
- }
- else if (_type == JointMaker::JOINT_SLIDER)
- {
- jointTypeStr = "prismatic";
- }
- else if (_type == JointMaker::JOINT_HINGE)
- {
- jointTypeStr = "revolute";
- }
- else if (_type == JointMaker::JOINT_HINGE2)
- {
- jointTypeStr = "revolute2";
- }
- else if (_type == JointMaker::JOINT_SCREW)
- {
- jointTypeStr = "screw";
- }
- else if (_type == JointMaker::JOINT_UNIVERSAL)
- {
- jointTypeStr = "universal";
- }
- else if (_type == JointMaker::JOINT_BALL)
- {
- jointTypeStr = "ball";
- }
- else if (_type == JointMaker::JOINT_NONE)
- {
- jointTypeStr = "none";
- }
+ auto iter = jointTypes.find(_type);
+ if (iter != jointTypes.end())
+ jointTypeStr = iter->second;
return jointTypeStr;
}
/////////////////////////////////////////////////
-int JointMaker::GetJointAxisCount(JointMaker::JointType _type)
+unsigned int JointMaker::GetJointAxisCount(JointMaker::JointType _type)
{
if (_type == JOINT_FIXED)
{
@@ -800,13 +1023,21 @@ JointMaker::JointType JointMaker::GetState() const
}
/////////////////////////////////////////////////
-math::Vector3 JointMaker::GetPartWorldCentroid(
+math::Vector3 JointMaker::GetLinkWorldCentroid(
const rendering::VisualPtr _visual)
{
math::Vector3 centroid;
+ int count = 0;
for (unsigned int i = 0; i < _visual->GetChildCount(); ++i)
- centroid += _visual->GetChild(i)->GetWorldPose().pos;
- centroid /= _visual->GetChildCount();
+ {
+ if (_visual->GetChild(i)->GetName().find("_JOINT_VISUAL_") ==
+ std::string::npos)
+ {
+ centroid += _visual->GetChild(i)->GetWorldPose().pos;
+ count++;
+ }
+ }
+ centroid /= count;
return centroid;
}
@@ -819,15 +1050,192 @@ unsigned int JointMaker::GetJointCount()
/////////////////////////////////////////////////
void JointData::OnApply()
{
- this->pose = this->inspector->GetPose();
- this->type = this->inspector->GetType();
- this->name = this->inspector->GetName();
+ this->jointMsg->CopyFrom(*this->inspector->GetData());
+ this->type = JointMaker::ConvertJointType(
+ msgs::ConvertJointType(this->jointMsg->type()));
+
+ if (this->name != this->jointMsg->name())
+ gui::model::Events::jointNameChanged(this->hotspot->GetName(),
+ this->jointMsg->name());
+ this->name = this->jointMsg->name();
+
+ this->dirty = true;
+ gui::model::Events::modelChanged();
+}
+
+/////////////////////////////////////////////////
+void JointData::OnOpenInspector()
+{
+ this->OpenInspector();
+}
+
+/////////////////////////////////////////////////
+void JointData::OpenInspector()
+{
+ this->inspector->Update(this->jointMsg);
+ this->inspector->move(QCursor::pos());
+ this->inspector->show();
+}
+
+/////////////////////////////////////////////////
+void JointMaker::ShowContextMenu(const std::string &_name)
+{
+ auto it = this->joints.find(_name);
+ if (it == this->joints.end())
+ return;
- int axisCount = JointMaker::GetJointAxisCount(this->type);
- for (int i = 0; i < axisCount; ++i)
+ QMenu menu;
+ if (this->inspectAct)
+ menu.addAction(this->inspectAct);
+
+ this->inspectName = _name;
+ QAction *deleteAct = new QAction(tr("Delete"), this);
+ connect(deleteAct, SIGNAL(triggered()), this, SLOT(OnDelete()));
+ menu.addAction(deleteAct);
+
+ menu.exec(QCursor::pos());
+}
+
+/////////////////////////////////////////////////
+void JointMaker::OnSetSelectedEntity(const std::string &/*_name*/,
+ const std::string &/*_mode*/)
+{
+ this->DeselectAll();
+}
+
+/////////////////////////////////////////////////
+void JointMaker::OnSetSelectedJoint(const std::string &_name,
+ const bool _selected)
+{
+ this->SetSelected(_name, _selected);
+}
+
+/////////////////////////////////////////////////
+void JointMaker::SetSelected(const std::string &_name,
+ const bool _selected)
+{
+ auto it = this->joints.find(_name);
+ if (it == this->joints.end())
+ return;
+
+ this->SetSelected((*it).second->hotspot, _selected);
+}
+
+/////////////////////////////////////////////////
+void JointMaker::SetSelected(rendering::VisualPtr _jointVis,
+ const bool _selected)
+{
+ if (!_jointVis)
+ return;
+
+ _jointVis->SetHighlighted(_selected);
+ auto it = std::find(this->selectedJoints.begin(),
+ this->selectedJoints.end(), _jointVis);
+ if (_selected)
{
- this->axis[i] = this->inspector->GetAxis(i);
- this->lowerLimit[i] = this->inspector->GetLowerLimit(i);
- this->upperLimit[i] = this->inspector->GetUpperLimit(i);
+ if (it == this->selectedJoints.end())
+ {
+ this->selectedJoints.push_back(_jointVis);
+ model::Events::setSelectedJoint(_jointVis->GetName(), _selected);
+ }
+ }
+ else
+ {
+ if (it != this->selectedJoints.end())
+ {
+ this->selectedJoints.erase(it);
+ model::Events::setSelectedJoint(_jointVis->GetName(), _selected);
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void JointMaker::DeselectAll()
+{
+ while (!this->selectedJoints.empty())
+ {
+ rendering::VisualPtr vis = this->selectedJoints[0];
+ vis->SetHighlighted(false);
+ this->selectedJoints.erase(this->selectedJoints.begin());
+ model::Events::setSelectedJoint(vis->GetName(), false);
+ }
+}
+
+/////////////////////////////////////////////////
+void JointMaker::CreateJointFromSDF(sdf::ElementPtr _jointElem,
+ const std::string &_modelName)
+{
+ msgs::Joint jointMsg = msgs::JointFromSDF(_jointElem);
+
+ // Parent
+ std::string parentName = _modelName + "::" + jointMsg.parent();
+ rendering::VisualPtr parentVis =
+ gui::get_active_camera()->GetScene()->GetVisual(parentName);
+
+ // Child
+ std::string childName = _modelName + "::" + jointMsg.child();
+ rendering::VisualPtr childVis =
+ gui::get_active_camera()->GetScene()->GetVisual(childName);
+
+ if (!parentVis || !childVis)
+ {
+ gzerr << "Unable to load joint. Joint child / parent not found"
+ << std::endl;
+ return;
+ }
+
+ JointData *joint = new JointData();
+ joint->name = jointMsg.name();
+ joint->parent = parentVis;
+ joint->child = childVis;
+ joint->type = this->ConvertJointType(msgs::ConvertJointType(jointMsg.type()));
+ std::string jointVisName = _modelName + "::" + joint->name;
+
+ joint->jointMsg.reset(new msgs::Joint);
+ joint->jointMsg->CopyFrom(jointMsg);
+ joint->jointMsg->set_parent_id(joint->parent->GetId());
+ joint->jointMsg->set_child_id(joint->child->GetId());
+
+ // Inspector
+ joint->inspector = new JointInspector();
+ joint->inspector->Update(joint->jointMsg);
+ joint->inspector->setModal(false);
+ connect(joint->inspector, SIGNAL(Applied()), joint, SLOT(OnApply()));
+
+ // Visuals
+ rendering::VisualPtr jointVis(
+ new rendering::Visual(jointVisName, parentVis->GetParent()));
+ jointVis->Load();
+ rendering::DynamicLines *jointLine =
+ jointVis->CreateDynamicLine(rendering::RENDERING_LINE_LIST);
+
+ math::Vector3 origin = parentVis->GetWorldPose().pos
+ - parentVis->GetParent()->GetWorldPose().pos;
+ jointLine->AddPoint(origin);
+ jointLine->AddPoint(origin + math::Vector3(0, 0, 0.1));
+
+ jointVis->GetSceneNode()->setInheritScale(false);
+ jointVis->GetSceneNode()->setInheritOrientation(false);
+ joint->visual = jointVis;
+ joint->line = jointLine;
+ joint->dirty = true;
+
+ this->CreateHotSpot(joint);
+}
+
+/////////////////////////////////////////////////
+void JointMaker::ShowJoints(bool _show)
+{
+ for (auto iter : this->joints)
+ {
+ rendering::VisualPtr vis = iter.second->hotspot;
+ if (vis)
+ {
+ vis->SetVisible(_show);
+ vis->SetHighlighted(false);
+ }
+ if (iter.second->jointVisual)
+ iter.second->jointVisual->SetVisible(_show);
}
+ this->DeselectAll();
}
diff --git a/gazebo/gui/model/JointMaker.hh b/gazebo/gui/model/JointMaker.hh
index 08a50c3..cc4f2be 100644
--- a/gazebo/gui/model/JointMaker.hh
+++ b/gazebo/gui/model/JointMaker.hh
@@ -15,18 +15,19 @@
*
*/
-#ifndef _JOINTMAKER_HH_
-#define _JOINTMAKER_HH_
+#ifndef _GAZEBO_JOINTMAKER_HH_
+#define _GAZEBO_JOINTMAKER_HH_
+#include <map>
#include <string>
#include <vector>
-#include <boost/unordered/unordered_map.hpp>
#include <sdf/sdf.hh>
#include "gazebo/common/MouseEvent.hh"
#include "gazebo/common/KeyEvent.hh"
#include "gazebo/common/CommonTypes.hh"
+#include "gazebo/msgs/msgs.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/rendering/RenderTypes.hh"
#include "gazebo/gui/qt.h"
@@ -54,7 +55,7 @@ namespace gazebo
/// \class JointMaker JointMaker.hh
/// \brief Joint visualization
- class GAZEBO_VISIBLE JointMaker : public QObject
+ class GZ_GUI_MODEL_VISIBLE JointMaker : public QObject
{
Q_OBJECT
@@ -121,9 +122,16 @@ namespace gazebo
/// \param[in] _jointName Name of joint to be removed.
public: void RemoveJoint(const std::string &_jointName);
- /// \brief Remove all joints connected to part
- /// \param[in] _partName Name of joint to be removed.
- public: void RemoveJointsByPart(const std::string &_partName);
+ /// \brief Remove all joints connected to link.
+ /// \param[in] _linkName Name of the link.
+ public: void RemoveJointsByLink(const std::string &_linkName);
+
+ /// \brief Get a vector containing data for all joints connected to
+ /// the given link.
+ /// \param[in] _linkName Name of the link.
+ /// \return Vector with joint data.
+ public: std::vector<JointData *> GetJointDataByLink(
+ const std::string &_linkName) const;
/// \brief Generate SDF for all joints.
public: void GenerateSDF();
@@ -133,13 +141,19 @@ namespace gazebo
/// \brief Get the axis count for joint type.
/// \param[in] _type Type of joint.
- public: static int GetJointAxisCount(JointMaker::JointType _type);
+ public: static unsigned int GetJointAxisCount(
+ JointMaker::JointType _type);
/// \brief Get the joint type in string.
/// \param[in] _type Type of joint.
/// \return Joint type in string.
public: static std::string GetTypeAsString(JointMaker::JointType _type);
+ /// \brief Convert a joint type string to enum.
+ /// \param[in] _type Joint type in string.
+ /// \return Joint type enum.
+ public: static JointType ConvertJointType(const std::string &_type);
+
/// \brief Get state
/// \return State of JointType if joint creation is in process, otherwise
/// JOINT_NONE
@@ -152,6 +166,35 @@ namespace gazebo
/// return Number of joints.
public: unsigned int GetJointCount();
+ /// \brief Create a joint from SDF. This is mainly used when editing
+ /// existing models.
+ /// \param[in] _jointElement SDF element to load.
+ /// \param[in] _modelName Name of the model that contains this joint.
+ public: void CreateJointFromSDF(sdf::ElementPtr _jointElem,
+ const std::string &_modelName = "");
+
+ /// \brief Add a scoped link name. Nested model's link names are scoped
+ /// but the parent and child field in the joint SDF element may not be.
+ /// So keep track of scoped link names in order to generate the correct
+ /// SDF before spawning the model.
+ /// \param[in] _name Scoped link name.
+ public: void AddScopedLinkName(const std::string &_name);
+
+ /// \brief Qt Callback to show / hide joint visuals.
+ /// \param[in] _show True to show joints, false to hide them.
+ public slots: void ShowJoints(bool _show);
+
+ /// \brief Set the select state of a joint.
+ /// \param[in] _name Name of the joint.
+ /// \param[in] _selected True to select the joint.
+ public: void SetSelected(const std::string &_name, const bool selected);
+
+ /// \brief Set the select state of a joint visual.
+ /// \param[in] _jointVis Pointer to the joint visual.
+ /// \param[in] _selected True to select the joint.
+ public: void SetSelected(rendering::VisualPtr _jointVis,
+ const bool selected);
+
/// \brief Mouse event filter callback when mouse button is pressed.
/// \param[in] _event The mouse event.
/// \return True if the event was handled
@@ -177,24 +220,60 @@ namespace gazebo
/// \return True if the event was handled
private: bool OnKeyPress(const common::KeyEvent &_event);
- /// \brief Get the centroid of the part visual in world coordinates.
- /// \param[in] _visual Visual of the part.
+ /// \brief Get the centroid of the link visual in world coordinates.
+ /// \param[in] _visual Visual of the link.
/// \return Centroid in world coordinates;
- private: math::Vector3 GetPartWorldCentroid(
+ private: math::Vector3 GetLinkWorldCentroid(
const rendering::VisualPtr _visual);
/// \brief Open joint inspector.
/// \param[in] _name Name of joint.
private: void OpenInspector(const std::string &_name);
+ /// \brief Get the scoped name of a link.
+ /// \param[in] _name Unscoped link name.
+ /// \return Scoped link name.
+ private: std::string GetScopedLinkName(const std::string &_name);
+
+ /// \brief Show a joint's context menu
+ /// \param[in] _joint Name of joint the context menu is associated with.
+ private: void ShowContextMenu(const std::string &_joint);
+
+ /// \brief Deselect all currently selected joint visuals.
+ private: void DeselectAll();
+
+ /// \brief Callback when an entity is selected.
+ /// \param[in] _name Name of entity.
+ /// \param[in] _mode Select mode
+ private: void OnSetSelectedEntity(const std::string &_name,
+ const std::string &_mode);
+
+ /// \brief Callback when a joint is selected.
+ /// \param[in] _name Name of joint.
+ /// \param[in] _selected True if the joint is selected, false if
+ /// deselected.
+ private: void OnSetSelectedJoint(const std::string &_name,
+ const bool _selected);
+
+ /// \brief Create a joint line.
+ /// \param[in] _name Name to give the visual that contains the joint line.
+ /// \param[in] _parent Parent of the joint.
+ /// \return joint data.
+ private: JointData *CreateJointLine(const std::string &_name,
+ rendering::VisualPtr _parent);
+
/// \brief Qt signal when the joint creation process has ended.
Q_SIGNALS: void JointAdded();
/// \brief Qt Callback to open joint inspector
private slots: void OnOpenInspector();
+ /// \brief Qt callback when a delete signal has been emitted. This is
+ /// currently triggered by the context menu via right click.
+ private slots: void OnDelete();
+
/// \brief Constant vector containing [UnitX, UnitY, UnitZ].
- private: std::vector<math::Vector3> UnitVectors;
+ private: std::vector<ignition::math::Vector3d> unitVectors;
/// \brief Type of joint to create
private: JointMaker::JointType jointType;
@@ -208,11 +287,11 @@ namespace gazebo
/// \brief Currently selected visual
private: rendering::VisualPtr selectedVis;
- /// \brief Joint visual that is currently being inspected.
- private: rendering::VisualPtr inspectVis;
+ /// \brief Name of joint that is currently being inspected.
+ private: std::string inspectName;
/// \brief All joints created by joint maker.
- private: boost::unordered_map<std::string, JointData *> joints;
+ private: std::map<std::string, JointData *> joints;
/// \brief Joint currently being created.
private: JointData *mouseJoint;
@@ -224,7 +303,7 @@ namespace gazebo
private: bool newJointCreated;
/// \brief A map of joint type to its corresponding material.
- private: boost::unordered_map<JointMaker::JointType, std::string>
+ private: std::map<JointMaker::JointType, std::string>
jointMaterials;
/// \brief The SDF element pointer to the model that contains the joints.
@@ -239,15 +318,21 @@ namespace gazebo
/// \brief Mutex to protect the list of joints
private: boost::recursive_mutex *updateMutex;
- /// \brief Selected joint.
- private: rendering::VisualPtr selectedJoint;
+ /// \brief A list of selected link visuals.
+ private: std::vector<rendering::VisualPtr> selectedJoints;
+
+ /// \brief A list of scoped link names.
+ private: std::vector<std::string> scopedLinkedNames;
+
+ /// \brief A map of joint type to its string value.
+ private: static std::map<JointMaker::JointType, std::string> jointTypes;
};
/// \}
/// \class JointData JointData.hh
/// \brief Helper class to store joint data
- class GAZEBO_VISIBLE JointData : public QObject
+ class GZ_GUI_MODEL_VISIBLE JointData : public QObject
{
Q_OBJECT
@@ -257,6 +342,9 @@ namespace gazebo
/// \brief Visual of the dynamic line
public: rendering::VisualPtr visual;
+ /// \brief Joint visual.
+ public: rendering::JointVisualPtr jointVisual;
+
/// \brieft Visual of the hotspot
public: rendering::VisualPtr hotspot;
@@ -266,6 +354,18 @@ namespace gazebo
/// \brief Child visual the joint is connected to.
public: rendering::VisualPtr child;
+ /// \internal
+ /// \brief Parent visual pose used to determine if updates are needed.
+ public: math::Pose parentPose;
+
+ /// \internal
+ /// \brief Child visual pose used to determine if updates are needed.
+ public: math::Pose childPose;
+
+ /// \internal
+ /// \brief Child visual scale used to determine if updates are needed.
+ public: math::Vector3 childScale;
+
/// \brief Visual line used to represent joint connecting parent and child
public: rendering::DynamicLines *line;
@@ -275,24 +375,21 @@ namespace gazebo
/// \brief Type of joint.
public: JointMaker::JointType type;
- /// \brief Joint axis direction.
- public: math::Vector3 axis[2];
-
- /// \brief Joint lower limit.
- public: double lowerLimit[2];
-
- /// \brief Joint upper limit.
- public: double upperLimit[2];
-
- /// \brief Joint pose.
- public: math::Pose pose;
-
/// \brief True if the joint visual needs update.
public: bool dirty;
+ /// \brief Msg containing joint data.
+ public: msgs::JointPtr jointMsg;
+
/// \brief Inspector for configuring joint properties.
public: JointInspector *inspector;
+ /// \brief Open the joint inspector.
+ public: void OpenInspector();
+
+ /// \brief Qt Callback when joint inspector is to be opened.
+ private slots: void OnOpenInspector();
+
/// \brief Qt Callback when joint inspector configurations are to be
/// applied.
private slots: void OnApply();
diff --git a/gazebo/gui/model/JointMaker_TEST.cc b/gazebo/gui/model/JointMaker_TEST.cc
index 87375ce..bcb69d0 100644
--- a/gazebo/gui/model/JointMaker_TEST.cc
+++ b/gazebo/gui/model/JointMaker_TEST.cc
@@ -19,6 +19,7 @@
#include "gazebo/gui/MainWindow.hh"
#include "gazebo/gui/MouseEventHandler.hh"
#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/model/JointInspector.hh"
#include "gazebo/gui/model/JointMaker.hh"
#include "gazebo/gui/model/JointMaker_TEST.hh"
@@ -50,15 +51,10 @@ void JointMaker_TEST::JointState()
/////////////////////////////////////////////////
void JointMaker_TEST::CreateRemoveJoint()
{
- // FIXME Test passes but segfaults when QTestFixture clean up
- // Problem: JointMaker's destructor resets visual shared_ptrs
- // but this later causes a segfault in Visual's destructor when exiting the
- // program.
-
this->resMaxPercentChange = 5.0;
this->shareMaxPercentChange = 2.0;
- this->Load("worlds/shapes.world", false, false, true);
+ this->Load("worlds/shapes.world", false, false, false);
gui::JointMaker *jointMaker = new gui::JointMaker();
QCOMPARE(jointMaker->GetState(), gui::JointMaker::JOINT_NONE);
@@ -124,14 +120,336 @@ void JointMaker_TEST::CreateRemoveJoint()
QCOMPARE(jointMaker->GetJointCount(), 3u);
// Remove the two joints connected to the sphere
- jointMaker->RemoveJointsByPart(sphereLink->GetName());
+ jointMaker->RemoveJointsByLink(sphereLink->GetName());
QCOMPARE(jointMaker->GetJointCount(), 1u);
// Remove the last joint
jointMaker->RemoveJoint(ballJointData->hotspot->GetName());
QCOMPARE(jointMaker->GetJointCount(), 0u);
- // delete jointMaker;
+ delete jointMaker;
+ mainWindow->close();
+ delete mainWindow;
+}
+
+/////////////////////////////////////////////////
+void JointMaker_TEST::JointDefaultProperties()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/shapes.world", false, false, false);
+
+ gui::JointMaker *jointMaker = new gui::JointMaker();
+ QCOMPARE(jointMaker->GetState(), gui::JointMaker::JOINT_NONE);
+ QCOMPARE(jointMaker->GetJointCount(), 0u);
+
+ gui::MainWindow *mainWindow = new gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ rendering::UserCameraPtr cam = gui::get_active_camera();
+ Q_ASSERT(cam);
+ rendering::ScenePtr scene = cam->GetScene();
+ Q_ASSERT(scene);
+
+ rendering::VisualPtr boxLink = scene->GetVisual("box::link");
+ rendering::VisualPtr sphereLink = scene->GetVisual("sphere::link");
+ rendering::VisualPtr cylinderLink = scene->GetVisual("cylinder::link");
+
+ Q_ASSERT(boxLink.get());
+ Q_ASSERT(sphereLink.get());
+ Q_ASSERT(cylinderLink.get());
+
+ // Add a revolute2 joint
+ jointMaker->AddJoint(gui::JointMaker::JOINT_HINGE2);
+ gui::JointData *revoluteJointData =
+ jointMaker->CreateJoint(boxLink, sphereLink);
+ jointMaker->CreateHotSpot(revoluteJointData);
+ QCOMPARE(jointMaker->GetJointCount(), 1u);
+
+ // verify connected joints
+ std::vector<gui::JointData *> boxJointData =
+ jointMaker->GetJointDataByLink("box::link");
+ QCOMPARE(static_cast<unsigned int>(boxJointData.size()), 1u);
+
+ gui::JointData *rev2joint = boxJointData[0];
+ QVERIFY(rev2joint != NULL);
+ QVERIFY(rev2joint->inspector != NULL);
+
+ // verify default values
+ QVERIFY(msgs::ConvertJointType(rev2joint->jointMsg->type()) == "revolute2");
+ QCOMPARE(msgs::ConvertIgn(rev2joint->jointMsg->pose()),
+ ignition::math::Pose3d::Zero);
+ qFuzzyCompare(rev2joint->jointMsg->cfm(), 0.0);
+ qFuzzyCompare(rev2joint->jointMsg->bounce(), 0.0);
+ qFuzzyCompare(rev2joint->jointMsg->fudge_factor(), 0.0);
+ qFuzzyCompare(rev2joint->jointMsg->limit_cfm(), 0.0);
+ qFuzzyCompare(rev2joint->jointMsg->limit_erp(), 0.2);
+ qFuzzyCompare(rev2joint->jointMsg->suspension_cfm(), 0.0);
+ qFuzzyCompare(rev2joint->jointMsg->suspension_erp(), 0.2);
+
+ msgs::Axis rev2Axis1Msg = rev2joint->jointMsg->axis1();
+ QCOMPARE(msgs::ConvertIgn(rev2Axis1Msg.xyz()),
+ ignition::math::Vector3d(1, 0, 0));
+ qFuzzyCompare(rev2Axis1Msg.limit_lower(), -GZ_DBL_MAX);
+ qFuzzyCompare(rev2Axis1Msg.limit_upper(), GZ_DBL_MAX);
+ qFuzzyCompare(rev2Axis1Msg.limit_effort(), -1);
+ qFuzzyCompare(rev2Axis1Msg.limit_velocity(), -1);
+ qFuzzyCompare(rev2Axis1Msg.damping(), 0.0);
+ qFuzzyCompare(rev2Axis1Msg.friction(), 0.0);
+ QCOMPARE(rev2Axis1Msg.use_parent_model_frame(), false);
+
+ msgs::Axis rev2Axis2Msg = rev2joint->jointMsg->axis2();
+ QCOMPARE(msgs::ConvertIgn(rev2Axis2Msg.xyz()),
+ ignition::math::Vector3d(0, 1, 0));
+ qFuzzyCompare(rev2Axis2Msg.limit_lower(), -GZ_DBL_MAX);
+ qFuzzyCompare(rev2Axis2Msg.limit_upper(), GZ_DBL_MAX);
+ qFuzzyCompare(rev2Axis2Msg.limit_effort(), -1);
+ qFuzzyCompare(rev2Axis2Msg.limit_velocity(), -1);
+ qFuzzyCompare(rev2Axis2Msg.damping(), 0.0);
+ qFuzzyCompare(rev2Axis2Msg.friction(), 0.0);
+ QCOMPARE(rev2Axis2Msg.use_parent_model_frame(), false);
+
+ // Add a prismatic joint
+ jointMaker->AddJoint(gui::JointMaker::JOINT_SLIDER);
+ gui::JointData *prismaticJointData =
+ jointMaker->CreateJoint(sphereLink, cylinderLink);
+ jointMaker->CreateHotSpot(prismaticJointData);
+ QCOMPARE(jointMaker->GetJointCount(), 2u);
+
+ // verify connected joints
+ std::vector<gui::JointData *> sphereJointData =
+ jointMaker->GetJointDataByLink("sphere::link");
+ QCOMPARE(static_cast<unsigned int>(sphereJointData.size()), 2u);
+
+ std::vector<gui::JointData *> cylinderJointData =
+ jointMaker->GetJointDataByLink("cylinder::link");
+ QCOMPARE(static_cast<unsigned int>(cylinderJointData.size()), 1u);
+
+ gui::JointData *prisJoint = cylinderJointData[0];
+ QVERIFY(prisJoint != NULL);
+ QVERIFY(prisJoint->inspector != NULL);
+
+ // verify default values
+ QVERIFY(msgs::ConvertJointType(prisJoint->jointMsg->type()) == "prismatic");
+ QCOMPARE(msgs::ConvertIgn(prisJoint->jointMsg->pose()),
+ ignition::math::Pose3d::Zero);
+ qFuzzyCompare(prisJoint->jointMsg->cfm(), 0.0);
+ qFuzzyCompare(prisJoint->jointMsg->bounce(), 0.0);
+ qFuzzyCompare(prisJoint->jointMsg->fudge_factor(), 0.0);
+ qFuzzyCompare(prisJoint->jointMsg->limit_cfm(), 0.0);
+ qFuzzyCompare(prisJoint->jointMsg->limit_erp(), 0.2);
+ qFuzzyCompare(prisJoint->jointMsg->suspension_cfm(), 0.0);
+ qFuzzyCompare(prisJoint->jointMsg->suspension_erp(), 0.2);
+
+ msgs::Axis prisAxis1Msg = prisJoint->jointMsg->axis1();
+ QCOMPARE(msgs::ConvertIgn(prisAxis1Msg.xyz()),
+ ignition::math::Vector3d(1, 0, 0));
+ qFuzzyCompare(prisAxis1Msg.limit_lower(), -GZ_DBL_MAX);
+ qFuzzyCompare(prisAxis1Msg.limit_upper(), GZ_DBL_MAX);
+ qFuzzyCompare(prisAxis1Msg.limit_effort(), -1);
+ qFuzzyCompare(prisAxis1Msg.limit_velocity(), -1);
+ qFuzzyCompare(prisAxis1Msg.damping(), 0.0);
+ qFuzzyCompare(prisAxis1Msg.friction(), 0.0);
+ QCOMPARE(prisAxis1Msg.use_parent_model_frame(), false);
+
+ delete jointMaker;
+ mainWindow->close();
+ delete mainWindow;
+}
+
+/////////////////////////////////////////////////
+void JointMaker_TEST::ShowJoints()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/shapes.world", false, false, false);
+
+ gui::JointMaker *jointMaker = new gui::JointMaker();
+
+ gui::MainWindow *mainWindow = new gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ rendering::UserCameraPtr cam = gui::get_active_camera();
+ Q_ASSERT(cam);
+ rendering::ScenePtr scene = cam->GetScene();
+ Q_ASSERT(scene);
+
+ rendering::VisualPtr boxLink = scene->GetVisual("box::link");
+ rendering::VisualPtr sphereLink = scene->GetVisual("sphere::link");
+ rendering::VisualPtr cylinderLink = scene->GetVisual("cylinder::link");
+
+ Q_ASSERT(boxLink.get());
+ Q_ASSERT(sphereLink.get());
+ Q_ASSERT(cylinderLink.get());
+
+ // Add a revolute joint
+ jointMaker->AddJoint(gui::JointMaker::JOINT_HINGE);
+ gui::JointData *revoluteJointData =
+ jointMaker->CreateJoint(boxLink, sphereLink);
+ jointMaker->CreateHotSpot(revoluteJointData);
+ QCOMPARE(jointMaker->GetJointCount(), 1u);
+
+ // Add a prismatic joint
+ jointMaker->AddJoint(gui::JointMaker::JOINT_SLIDER);
+ gui::JointData *prismaticJointData =
+ jointMaker->CreateJoint(sphereLink, cylinderLink);
+ jointMaker->CreateHotSpot(prismaticJointData);
+ QCOMPARE(jointMaker->GetJointCount(), 2u);
+
+ // 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(revoluteJointData->hotspot != NULL);
+ QVERIFY(prismaticJointData->hotspot != NULL);
+ QVERIFY(revoluteJointData->jointVisual != NULL);
+ QVERIFY(prismaticJointData->jointVisual != NULL);
+
+ // toggle joint visualization and verify
+ jointMaker->ShowJoints(false);
+ QVERIFY(!revoluteJointData->hotspot->GetVisible());
+ QVERIFY(!prismaticJointData->hotspot->GetVisible());
+ QVERIFY(!revoluteJointData->jointVisual->GetVisible());
+ QVERIFY(!prismaticJointData->jointVisual->GetVisible());
+
+ jointMaker->ShowJoints(true);
+ QVERIFY(revoluteJointData->hotspot->GetVisible());
+ QVERIFY(prismaticJointData->hotspot->GetVisible());
+ QVERIFY(revoluteJointData->jointVisual->GetVisible());
+ QVERIFY(prismaticJointData->jointVisual->GetVisible());
+
+ jointMaker->ShowJoints(false);
+ QVERIFY(!revoluteJointData->hotspot->GetVisible());
+ QVERIFY(!prismaticJointData->hotspot->GetVisible());
+ QVERIFY(!revoluteJointData->jointVisual->GetVisible());
+ QVERIFY(!prismaticJointData->jointVisual->GetVisible());
+
+ delete jointMaker;
+ mainWindow->close();
+ delete mainWindow;
+}
+
+/////////////////////////////////////////////////
+void JointMaker_TEST::Selection()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/shapes.world", false, false, false);
+
+ gui::JointMaker *jointMaker = new gui::JointMaker();
+
+ QCOMPARE(jointMaker->GetState(), gui::JointMaker::JOINT_NONE);
+ QCOMPARE(jointMaker->GetJointCount(), 0u);
+
+ gui::MainWindow *mainWindow = new gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ rendering::UserCameraPtr cam = gui::get_active_camera();
+ Q_ASSERT(cam);
+ rendering::ScenePtr scene = cam->GetScene();
+ Q_ASSERT(scene);
+
+ rendering::VisualPtr boxLink = scene->GetVisual("box::link");
+ rendering::VisualPtr sphereLink = scene->GetVisual("sphere::link");
+ rendering::VisualPtr cylinderLink = scene->GetVisual("cylinder::link");
+
+ Q_ASSERT(boxLink.get());
+ Q_ASSERT(sphereLink.get());
+ Q_ASSERT(cylinderLink.get());
+
+ // Add a revolute joint
+ jointMaker->AddJoint(gui::JointMaker::JOINT_HINGE);
+ gui::JointData *revoluteJointData =
+ jointMaker->CreateJoint(boxLink, sphereLink);
+ jointMaker->CreateHotSpot(revoluteJointData);
+ QCOMPARE(jointMaker->GetJointCount(), 1u);
+
+ // Add a prismatic joint
+ jointMaker->AddJoint(gui::JointMaker::JOINT_SLIDER);
+ gui::JointData *prismaticJointData =
+ jointMaker->CreateJoint(sphereLink, cylinderLink);
+ jointMaker->CreateHotSpot(prismaticJointData);
+ QCOMPARE(jointMaker->GetJointCount(), 2u);
+
+ // Add a screw joint
+ jointMaker->AddJoint(gui::JointMaker::JOINT_SCREW);
+ gui::JointData *screwJointData =
+ jointMaker->CreateJoint(cylinderLink, boxLink);
+ jointMaker->CreateHotSpot(screwJointData);
+ QCOMPARE(jointMaker->GetJointCount(), 3u);
+
+ // verify initial selected state
+ QVERIFY(!revoluteJointData->hotspot->GetHighlighted());
+ QVERIFY(!prismaticJointData->hotspot->GetHighlighted());
+ QVERIFY(!screwJointData->hotspot->GetHighlighted());
+
+ // select the joints and verify that they are selected
+ jointMaker->SetSelected(revoluteJointData->hotspot, true);
+ QVERIFY(revoluteJointData->hotspot->GetHighlighted());
+
+ jointMaker->SetSelected(prismaticJointData->hotspot, true);
+ QVERIFY(prismaticJointData->hotspot->GetHighlighted());
+
+ jointMaker->SetSelected(screwJointData->hotspot, true);
+ QVERIFY(screwJointData->hotspot->GetHighlighted());
+
+ // deselect and verify
+ jointMaker->SetSelected(revoluteJointData->hotspot, false);
+ QVERIFY(!revoluteJointData->hotspot->GetHighlighted());
+
+ jointMaker->SetSelected(prismaticJointData->hotspot, false);
+ QVERIFY(!prismaticJointData->hotspot->GetHighlighted());
+
+ jointMaker->SetSelected(screwJointData->hotspot, false);
+ QVERIFY(!screwJointData->hotspot->GetHighlighted());
+
+ // select one and verify all
+ jointMaker->SetSelected(prismaticJointData->hotspot, true);
+ QVERIFY(prismaticJointData->hotspot->GetHighlighted());
+ QVERIFY(!revoluteJointData->hotspot->GetHighlighted());
+ QVERIFY(!screwJointData->hotspot->GetHighlighted());
+
+ delete jointMaker;
mainWindow->close();
delete mainWindow;
}
diff --git a/gazebo/gui/model/JointMaker_TEST.hh b/gazebo/gui/model/JointMaker_TEST.hh
index 83e98db..e441b05 100644
--- a/gazebo/gui/model/JointMaker_TEST.hh
+++ b/gazebo/gui/model/JointMaker_TEST.hh
@@ -30,6 +30,15 @@ class JointMaker_TEST : public QTestFixture
/// \brief Test creating and removing joints
private slots: void CreateRemoveJoint();
+
+ /// \brief Test values of joint default properties.
+ private slots: void JointDefaultProperties();
+
+ /// \brief Test toggling joint visualization.
+ private slots: void ShowJoints();
+
+ /// \brief Tests selecting joints in the model editor
+ private slots: void Selection();
};
#endif
diff --git a/gazebo/gui/model/LinkConfig.cc b/gazebo/gui/model/LinkConfig.cc
new file mode 100644
index 0000000..8d32ac0
--- /dev/null
+++ b/gazebo/gui/model/LinkConfig.cc
@@ -0,0 +1,89 @@
+/*
+ * 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/msgs/msgs.hh"
+#include "gazebo/gui/ConfigWidget.hh"
+#include "gazebo/gui/model/LinkConfig.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+LinkConfig::LinkConfig()
+{
+ this->setObjectName("LinkConfig");
+
+ // Message
+ msgs::Link linkMsg;
+
+ // ConfigWidget
+ this->configWidget = new ConfigWidget;
+ configWidget->Load(&linkMsg);
+
+ // set default values
+ // TODO: auto-fill them with SDF defaults
+ this->configWidget->SetDoubleWidgetValue("inertial::mass", 1.0);
+ this->configWidget->SetDoubleWidgetValue("inertial::ixx", 1.0);
+ this->configWidget->SetDoubleWidgetValue("inertial::iyy", 1.0);
+ this->configWidget->SetDoubleWidgetValue("inertial::izz", 1.0);
+ this->configWidget->SetBoolWidgetValue("gravity", true);
+ this->configWidget->SetBoolWidgetValue("self_collide", false);
+ this->configWidget->SetBoolWidgetValue("kinematic", false);
+
+ this->configWidget->SetWidgetVisible("id", false);
+ this->configWidget->SetWidgetVisible("name", false);
+ this->configWidget->SetWidgetVisible("canonical", false);
+ this->configWidget->SetWidgetVisible("enabled", false);
+ this->configWidget->SetWidgetReadOnly("id", true);
+ this->configWidget->SetWidgetReadOnly("name", true);
+ this->configWidget->SetWidgetReadOnly("canonical", true);
+ this->configWidget->SetWidgetReadOnly("enabled", true);
+
+ // Scroll area
+ QScrollArea *scrollArea = new QScrollArea;
+ scrollArea->setWidget(this->configWidget);
+ scrollArea->setWidgetResizable(true);
+
+ // Layout
+ QVBoxLayout *generalLayout = new QVBoxLayout;
+ generalLayout->addWidget(scrollArea);
+
+ this->setLayout(generalLayout);
+}
+
+/////////////////////////////////////////////////
+LinkConfig::~LinkConfig()
+{
+}
+
+/////////////////////////////////////////////////
+void LinkConfig::Update(ConstLinkPtr _linkMsg)
+{
+ this->configWidget->UpdateFromMsg(_linkMsg.get());
+}
+
+/////////////////////////////////////////////////
+void LinkConfig::SetPose(const math::Pose &_pose)
+{
+ this->configWidget->SetPoseWidgetValue("pose", _pose);
+}
+
+/////////////////////////////////////////////////
+msgs::Link *LinkConfig::GetData() const
+{
+ return dynamic_cast<msgs::Link *>(this->configWidget->GetMsg());
+}
diff --git a/gazebo/gui/model/LinkConfig.hh b/gazebo/gui/model/LinkConfig.hh
new file mode 100644
index 0000000..018563e
--- /dev/null
+++ b/gazebo/gui/model/LinkConfig.hh
@@ -0,0 +1,67 @@
+/*
+ * 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 _LINK_CONFIG_HH_
+#define _LINK_CONFIG_HH_
+
+#include <string>
+
+#include "gazebo/math/Pose.hh"
+#include "gazebo/msgs/msgs.hh"
+
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class ConfigWidget;
+
+ /// \addtogroup gazebo_gui
+ /// \{
+
+ /// \class LinkConfig LinkConfig.hh
+ /// \brief A tab for configuring properties of a link.
+ class LinkConfig : public QWidget
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ public: LinkConfig();
+
+ /// \brief Destructor
+ public: ~LinkConfig();
+
+ /// \brief Update the link config widget with a link msg.
+ /// \param[in] _linkMsg Link message.
+ public: void Update(ConstLinkPtr _linkMsg);
+
+ /// \brief Get the msg containing all link data.
+ /// \return Link msg.
+ public: msgs::Link *GetData() const;
+
+ /// \brief Set the pose of the link.
+ /// \param[in] _pose Pose to set the link to.
+ public: void SetPose(const math::Pose &_pose);
+
+ /// \brief config widget for configuring link properties.
+ private: ConfigWidget *configWidget;
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/gui/model/LinkInspector.cc b/gazebo/gui/model/LinkInspector.cc
new file mode 100644
index 0000000..79b7c6c
--- /dev/null
+++ b/gazebo/gui/model/LinkInspector.cc
@@ -0,0 +1,144 @@
+/*
+ * 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/common/Console.hh"
+
+#include "gazebo/msgs/msgs.hh"
+#include "gazebo/gui/ConfigWidget.hh"
+
+
+#include "gazebo/gui/model/LinkConfig.hh"
+#include "gazebo/gui/model/VisualConfig.hh"
+#include "gazebo/gui/model/CollisionConfig.hh"
+#include "gazebo/gui/model/LinkInspector.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+LinkInspector::LinkInspector(QWidget *_parent) : QDialog(_parent)
+{
+ this->setObjectName("LinkInspector");
+ this->setWindowTitle(tr("Link Inspector"));
+ this->setWindowFlags(Qt::WindowStaysOnTopHint);
+
+ QLabel *linkLabel = new QLabel(tr("Name:"));
+ this->linkNameLabel = new QLabel(tr(""));
+
+ QHBoxLayout *nameLayout = new QHBoxLayout;
+ nameLayout->addWidget(linkLabel);
+ nameLayout->addWidget(this->linkNameLabel, QSizePolicy::Maximum);
+ nameLayout->setAlignment(this->linkNameLabel, Qt::AlignLeft);
+
+ this->linkConfig = new LinkConfig;
+ this->visualConfig = new VisualConfig;
+ this->collisionConfig = new CollisionConfig;
+
+ // Create the main tab widget for all components in a link
+ this->tabWidget = new QTabWidget();
+ this->tabWidget->setObjectName("linkInspectorTab");
+ this->tabWidget->setMinimumHeight(300);
+ this->tabWidget->setMinimumWidth(560);
+
+ this->tabWidget->addTab(this->linkConfig, "Link");
+ this->tabWidget->addTab(this->visualConfig, "Visual");
+ this->tabWidget->addTab(this->collisionConfig, "Collision");
+
+ QHBoxLayout *buttonsLayout = new QHBoxLayout;
+ QPushButton *cancelButton = new QPushButton(tr("Cancel"));
+ connect(cancelButton, SIGNAL(clicked()), this, SLOT(OnCancel()));
+ QPushButton *applyButton = new QPushButton(tr("Apply"));
+ connect(applyButton, SIGNAL(clicked()), this, SLOT(OnApply()));
+ QPushButton *OKButton = new QPushButton(tr("OK"));
+ OKButton->setDefault(true);
+ connect(OKButton, SIGNAL(clicked()), this, SLOT(OnOK()));
+ buttonsLayout->addWidget(cancelButton);
+ buttonsLayout->addWidget(applyButton);
+ buttonsLayout->addWidget(OKButton);
+ buttonsLayout->setAlignment(Qt::AlignRight);
+
+ QVBoxLayout *mainLayout = new QVBoxLayout;
+ mainLayout->addLayout(nameLayout);
+ mainLayout->addWidget(tabWidget);
+ mainLayout->addLayout(buttonsLayout);
+ this->setLayout(mainLayout);
+}
+
+/////////////////////////////////////////////////
+LinkInspector::~LinkInspector()
+{
+ delete this->linkConfig;
+ this->linkConfig = NULL;
+ delete this->visualConfig;
+ this->visualConfig = NULL;
+ delete this->collisionConfig;
+ this->collisionConfig = NULL;
+}
+
+/////////////////////////////////////////////////
+void LinkInspector::SetName(const std::string &_name)
+{
+ this->linkNameLabel->setText(tr(_name.c_str()));
+}
+
+/////////////////////////////////////////////////
+std::string LinkInspector::GetName() const
+{
+ return this->linkNameLabel->text().toStdString();
+}
+
+/////////////////////////////////////////////////
+LinkConfig *LinkInspector::GetLinkConfig() const
+{
+ return this->linkConfig;
+}
+
+/////////////////////////////////////////////////
+VisualConfig *LinkInspector::GetVisualConfig() const
+{
+ return this->visualConfig;
+}
+
+/////////////////////////////////////////////////
+CollisionConfig *LinkInspector::GetCollisionConfig() const
+{
+ return this->collisionConfig;
+}
+
+/////////////////////////////////////////////////
+void LinkInspector::OnCancel()
+{
+ this->close();
+}
+
+/////////////////////////////////////////////////
+void LinkInspector::OnApply()
+{
+ emit Applied();
+}
+
+/////////////////////////////////////////////////
+void LinkInspector::OnOK()
+{
+ emit Accepted();
+}
+
+/////////////////////////////////////////////////
+void LinkInspector::enterEvent(QEvent */*_event*/)
+{
+ QApplication::setOverrideCursor(Qt::ArrowCursor);
+}
diff --git a/gazebo/gui/model/LinkInspector.hh b/gazebo/gui/model/LinkInspector.hh
new file mode 100644
index 0000000..c325132
--- /dev/null
+++ b/gazebo/gui/model/LinkInspector.hh
@@ -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.
+ *
+*/
+
+#ifndef _LINK_INSPECTOR_HH_
+#define _LINK_INSPECTOR_HH_
+
+#include <string>
+
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class LinkConfig;
+ class VisualConfig;
+ class CollisionConfig;
+
+ class LinkInspector : public QDialog
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ /// \param[in] _parent Parent QWidget.
+ public: LinkInspector(QWidget *_parent = 0);
+
+ /// \brief Destructor
+ public: ~LinkInspector();
+
+ /// \brief Set the name of the link.
+ /// \param[in] Name to set the link to.
+ public: void SetName(const std::string &_name);
+
+ /// \brief Get the name of the link.
+ /// \return Name of the link.
+ public: std::string GetName() const;
+
+ /// \brief Get configurations of the link.
+ /// \return Tab widget with link configurations.
+ public: LinkConfig *GetLinkConfig() const;
+
+ /// \brief Get visual configurations of the link.
+ /// \return Tab widget with visual configurations.
+ public: VisualConfig *GetVisualConfig() const;
+
+ /// \brief Get collision configurations of the link.
+ /// \return Tab widget with visual configurations.
+ public: CollisionConfig *GetCollisionConfig() const;
+
+ /// \brief Qt event emiited when the mouse enters this widget.
+ /// \param[in] _event Qt event.
+ protected: virtual void enterEvent(QEvent *_event);
+
+ /// \brief Set the item name.
+ /// \param[in] _name Name to set to.
+ // public: void SetName(const std::string &_name);
+
+ /// \brief Qt signal emitted to indicate that changes should be applied.
+ Q_SIGNALS: void Applied();
+
+ /// \brief Qt signal emitted to indicate that changes should be applied
+ /// and the inspector closed.
+ Q_SIGNALS: void Accepted();
+
+ /// \brief Qt callback when the Cancel button is pressed.
+ private slots: void OnCancel();
+
+ /// \brief Qt callback when the Apply button is pressed.
+ private slots: void OnApply();
+
+ /// \brief Qt callback when the Ok button is pressed.
+ private slots: void OnOK();
+
+ /// \brief Main tab widget within the link inspector.
+ private: QTabWidget *tabWidget;
+
+ /// \brief Label that displays the name of the link.
+ private: QLabel* linkNameLabel;
+
+ /// \brief Widget with configurable link properties.
+ private: LinkConfig *linkConfig;
+
+ /// \brief Widget with configurable visual properties.
+ private: VisualConfig *visualConfig;
+
+ /// \brief Widget with configurable collision properties.
+ private: CollisionConfig *collisionConfig;
+ };
+ /// \}
+ }
+}
+
+#endif
diff --git a/gazebo/gui/model/ModelCreator.cc b/gazebo/gui/model/ModelCreator.cc
index 85f31ed..7e075dd 100644
--- a/gazebo/gui/model/ModelCreator.cc
+++ b/gazebo/gui/model/ModelCreator.cc
@@ -15,23 +15,29 @@
*
*/
+#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/thread/recursive_mutex.hpp>
#include <boost/filesystem.hpp>
#include <sstream>
#include <string>
-#include "gazebo/common/KeyEvent.hh"
#include "gazebo/common/Exception.hh"
+#include "gazebo/common/SVGLoader.hh"
#include "gazebo/rendering/UserCamera.hh"
-#include "gazebo/rendering/Visual.hh"
+#include "gazebo/rendering/Material.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/math/Quaternion.hh"
#include "gazebo/transport/Publisher.hh"
#include "gazebo/transport/Node.hh"
-
-#include "gazebo/physics/Inertial.hh"
+#include "gazebo/transport/TransportIface.hh"
#include "gazebo/gui/Actions.hh"
#include "gazebo/gui/KeyEventHandler.hh"
@@ -41,31 +47,35 @@
#include "gazebo/gui/ModelManipulator.hh"
#include "gazebo/gui/ModelSnap.hh"
#include "gazebo/gui/ModelAlign.hh"
+#include "gazebo/gui/SaveDialog.hh"
+#include "gazebo/gui/MainWindow.hh"
#include "gazebo/gui/model/ModelData.hh"
+#include "gazebo/gui/model/LinkInspector.hh"
#include "gazebo/gui/model/JointMaker.hh"
+#include "gazebo/gui/model/ModelEditorEvents.hh"
#include "gazebo/gui/model/ModelCreator.hh"
using namespace gazebo;
using namespace gui;
+const std::string ModelCreator::modelDefaultName = "Untitled";
+const std::string ModelCreator::previewName = "ModelPreview";
+
/////////////////////////////////////////////////
ModelCreator::ModelCreator()
{
this->active = false;
- this->modelName = "";
this->modelTemplateSDF.reset(new sdf::SDF);
- this->modelTemplateSDF->SetFromString(this->GetTemplateSDFString());
+ this->modelTemplateSDF->SetFromString(ModelData::GetTemplateSDFString());
+
+ this->updateMutex = new boost::recursive_mutex();
this->manipMode = "";
- this->boxCounter = 0;
- this->cylinderCounter = 0;
- this->sphereCounter = 0;
+ this->linkCounter = 0;
this->modelCounter = 0;
- this->editTransparency = 0.4;
-
this->node = transport::NodePtr(new transport::Node());
this->node->Init();
this->makerPub = this->node->Advertise<msgs::Factory>("~/factory");
@@ -74,8 +84,47 @@ ModelCreator::ModelCreator()
this->jointMaker = new JointMaker();
connect(g_editModelAct, SIGNAL(toggled(bool)), this, SLOT(OnEdit(bool)));
- connect(g_deleteAct, SIGNAL(DeleteSignal(const std::string &)), this,
- SLOT(OnDelete(const std::string &)));
+ this->inspectAct = new QAction(tr("Open Link Inspector"), this);
+ connect(this->inspectAct, SIGNAL(triggered()), this,
+ SLOT(OnOpenInspector()));
+
+ if (g_deleteAct)
+ {
+ connect(g_deleteAct, SIGNAL(DeleteSignal(const std::string &)), this,
+ SLOT(OnDelete(const std::string &)));
+ }
+
+ this->connections.push_back(
+ gui::Events::ConnectEditModel(
+ boost::bind(&ModelCreator::OnEditModel, this, _1)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectSaveModelEditor(
+ boost::bind(&ModelCreator::OnSave, this)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectSaveAsModelEditor(
+ boost::bind(&ModelCreator::OnSaveAs, this)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectNewModelEditor(
+ boost::bind(&ModelCreator::OnNew, this)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectExitModelEditor(
+ boost::bind(&ModelCreator::OnExit, this)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectModelNameChanged(
+ boost::bind(&ModelCreator::OnNameChanged, this, _1)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectModelChanged(
+ boost::bind(&ModelCreator::ModelChanged, this)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectOpenLinkInspector(
+ boost::bind(&ModelCreator::OpenInspector, this, _1)));
this->connections.push_back(
gui::Events::ConnectAlignMode(
@@ -89,11 +138,34 @@ ModelCreator::ModelCreator()
event::Events::ConnectSetSelectedEntity(
boost::bind(&ModelCreator::OnSetSelectedEntity, this, _1, _2)));
- g_copyAct->setEnabled(false);
- g_pasteAct->setEnabled(false);
+ this->connections.push_back(
+ gui::model::Events::ConnectSetSelectedLink(
+ boost::bind(&ModelCreator::OnSetSelectedLink, this, _1, _2)));
+
+ this->connections.push_back(
+ gui::Events::ConnectScaleEntity(
+ boost::bind(&ModelCreator::OnEntityScaleChanged, this, _1, _2)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectShowLinkContextMenu(
+ boost::bind(&ModelCreator::ShowContextMenu, this, _1)));
+
+ this->connections.push_back(
+ event::Events::ConnectPreRender(
+ boost::bind(&ModelCreator::Update, this)));
+
+ if (g_copyAct)
+ {
+ g_copyAct->setEnabled(false);
+ connect(g_copyAct, SIGNAL(triggered()), this, SLOT(OnCopy()));
+ }
+ if (g_pasteAct)
+ {
+ g_pasteAct->setEnabled(false);
+ connect(g_pasteAct, SIGNAL(triggered()), this, SLOT(OnPaste()));
+ }
- connect(g_copyAct, SIGNAL(triggered()), this, SLOT(OnCopy()));
- connect(g_pasteAct, SIGNAL(triggered()), this, SLOT(OnPaste()));
+ this->saveDialog = new SaveDialog(SaveDialog::MODEL);
this->Reset();
}
@@ -101,7 +173,10 @@ ModelCreator::ModelCreator()
/////////////////////////////////////////////////
ModelCreator::~ModelCreator()
{
- this->Reset();
+ while (!this->allLinks.empty())
+ this->RemoveLinkImpl(this->allLinks.begin()->first);
+
+ this->allLinks.clear();
this->node->Fini();
this->node.reset();
this->modelTemplateSDF.reset();
@@ -109,6 +184,9 @@ ModelCreator::~ModelCreator()
this->makerPub.reset();
this->connections.clear();
+ delete this->saveDialog;
+ delete this->updateMutex;
+
delete jointMaker;
}
@@ -118,6 +196,7 @@ void ModelCreator::OnEdit(bool _checked)
if (_checked)
{
this->active = true;
+ this->modelCounter++;
KeyEventHandler::Instance()->AddPressFilter("model_creator",
boost::bind(&ModelCreator::OnKeyPress, this, _1));
@@ -151,10 +230,344 @@ void ModelCreator::OnEdit(bool _checked)
}
/////////////////////////////////////////////////
+void ModelCreator::OnEditModel(const std::string &_modelName)
+{
+ if (!gui::get_active_camera() ||
+ !gui::get_active_camera()->GetScene())
+ {
+ gzerr << "Unable to edit model. GUI camera or scene is NULL"
+ << std::endl;
+ return;
+ }
+
+ if (!this->active)
+ {
+ gzwarn << "Model Editor must be active before loading a model. " <<
+ "Not loading model " << _modelName << std::endl;
+ return;
+ }
+
+ // Get SDF model element from model name
+ // TODO replace with entity_info and parse gazebo.msgs.Model msgs
+ // or handle model_sdf requests in world.
+ boost::shared_ptr<msgs::Response> response =
+ transport::request(gui::get_world(), "world_sdf");
+
+ msgs::GzString msg;
+ // Make sure the response is correct
+ if (response->type() == msg.GetTypeName())
+ {
+ // Parse the response message
+ msg.ParseFromString(response->serialized_data());
+
+ // Parse the string into sdf
+ sdf::SDF sdfParsed;
+ sdfParsed.SetFromString(msg.data());
+
+ // Check that sdf contains world
+ if (sdfParsed.Root()->HasElement("world") &&
+ sdfParsed.Root()->GetElement("world")->HasElement("model"))
+ {
+ sdf::ElementPtr world = sdfParsed.Root()->GetElement("world");
+ sdf::ElementPtr model = world->GetElement("model");
+ while (model)
+ {
+ if (model->GetAttribute("name")->GetAsString() == _modelName)
+ {
+ this->LoadSDF(model);
+
+ // Hide the model from the scene to substitute with the preview visual
+ this->SetModelVisible(_modelName, false);
+
+ rendering::ScenePtr scene = gui::get_active_camera()->GetScene();
+ rendering::VisualPtr visual = scene->GetVisual(_modelName);
+
+ math::Pose pose;
+ if (visual)
+ {
+ pose = visual->GetWorldPose();
+ this->previewVisual->SetWorldPose(pose);
+ }
+
+ this->serverModelName = _modelName;
+ this->serverModelSDF = model;
+ this->modelPose = pose;
+
+ return;
+ }
+ model = model->GetNextElement("model");
+ }
+ gzwarn << "Couldn't find SDF for " << _modelName << ". Not loading it."
+ << std::endl;
+ }
+ }
+ else
+ {
+ GZ_ASSERT(response->type() == msg.GetTypeName(),
+ "Received incorrect response from 'world_sdf' request.");
+ }
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::LoadSDF(sdf::ElementPtr _modelElem)
+{
+ // Reset preview visual in case there was something already loaded
+ this->Reset();
+
+ // Model general info
+ // Keep previewModel with previewName to avoid conflicts
+ if (_modelElem->HasAttribute("name"))
+ this->SetModelName(_modelElem->Get<std::string>("name"));
+
+ if (_modelElem->HasElement("pose"))
+ this->modelPose = _modelElem->Get<math::Pose>("pose");
+ else
+ this->modelPose = math::Pose::Zero;
+ this->previewVisual->SetPose(this->modelPose);
+
+ if (_modelElem->HasElement("static"))
+ this->isStatic = _modelElem->Get<bool>("static");
+ if (_modelElem->HasElement("allow_auto_disable"))
+ this->autoDisable = _modelElem->Get<bool>("allow_auto_disable");
+ gui::model::Events::modelPropertiesChanged(this->isStatic, this->autoDisable,
+ this->modelPose, this->GetModelName());
+
+ // Links
+ if (!_modelElem->HasElement("link"))
+ {
+ gzerr << "Can't load a model without links." << std::endl;
+ return;
+ }
+ sdf::ElementPtr linkElem = _modelElem->GetElement("link");
+ while (linkElem)
+ {
+ this->CreateLinkFromSDF(linkElem);
+ linkElem = linkElem->GetNextElement("link");
+ }
+
+ // Joints
+ std::stringstream preivewModelName;
+ preivewModelName << this->previewName << "_" << this->modelCounter;
+ sdf::ElementPtr jointElem;
+ if (_modelElem->HasElement("joint"))
+ jointElem = _modelElem->GetElement("joint");
+
+ while (jointElem)
+ {
+ this->jointMaker->CreateJointFromSDF(jointElem, preivewModelName.str());
+ jointElem = jointElem->GetNextElement("joint");
+ }
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::OnNew()
+{
+ this->Stop();
+
+ if (this->allLinks.empty())
+ {
+ this->Reset();
+ gui::model::Events::newModel();
+ return;
+ }
+ QString msg;
+ QMessageBox msgBox(QMessageBox::Warning, QString("New"), msg);
+ QPushButton *cancelButton = msgBox.addButton("Cancel",
+ QMessageBox::RejectRole);
+ msgBox.setEscapeButton(cancelButton);
+ QPushButton *saveButton = new QPushButton("Save");
+
+ switch (this->currentSaveState)
+ {
+ case ALL_SAVED:
+ {
+ msg.append("Are you sure you want to close this model and open a new "
+ "canvas?\n\n");
+ QPushButton *newButton =
+ msgBox.addButton("New Canvas", QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(newButton);
+ break;
+ }
+ case UNSAVED_CHANGES:
+ case NEVER_SAVED:
+ {
+ msg.append("You have unsaved changes. Do you want to save this model "
+ "and open a new canvas?\n\n");
+ msgBox.addButton("Don't Save", QMessageBox::DestructiveRole);
+ msgBox.addButton(saveButton, QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(saveButton);
+ break;
+ }
+ default:
+ return;
+ }
+
+ msgBox.setText(msg);
+
+ msgBox.exec();
+
+ if (msgBox.clickedButton() != cancelButton)
+ {
+ if (msgBox.clickedButton() == saveButton)
+ {
+ if (!this->OnSave())
+ {
+ return;
+ }
+ }
+
+ this->Reset();
+ gui::model::Events::newModel();
+ }
+}
+
+/////////////////////////////////////////////////
+bool ModelCreator::OnSave()
+{
+ this->Stop();
+
+ switch (this->currentSaveState)
+ {
+ case UNSAVED_CHANGES:
+ {
+ this->SaveModelFiles();
+ gui::model::Events::saveModel(this->modelName);
+ return true;
+ }
+ case NEVER_SAVED:
+ {
+ return this->OnSaveAs();
+ }
+ default:
+ return false;
+ }
+}
+
+/////////////////////////////////////////////////
+bool ModelCreator::OnSaveAs()
+{
+ this->Stop();
+
+ if (this->saveDialog->OnSaveAs())
+ {
+ // Prevent changing save location
+ this->currentSaveState = ALL_SAVED;
+ // Get name set by user
+ this->SetModelName(this->saveDialog->GetModelName());
+ // Update name on palette
+ gui::model::Events::saveModel(this->modelName);
+ // Generate and save files
+ this->SaveModelFiles();
+ return true;
+ }
+ return false;
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::OnNameChanged(const std::string &_name)
+{
+ if (_name.compare(this->modelName) == 0)
+ return;
+
+ this->SetModelName(_name);
+ this->ModelChanged();
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::OnExit()
+{
+ this->Stop();
+
+ if (this->allLinks.empty())
+ {
+ if (!this->serverModelName.empty())
+ this->SetModelVisible(this->serverModelName, true);
+ this->Reset();
+ gui::model::Events::newModel();
+ gui::model::Events::finishModel();
+ return;
+ }
+
+ switch (this->currentSaveState)
+ {
+ case ALL_SAVED:
+ {
+ QString msg("Are you ready to exit?\n\n");
+ QMessageBox msgBox(QMessageBox::NoIcon, QString("Exit"), msg);
+
+ QPushButton *cancelButton = msgBox.addButton("Cancel",
+ QMessageBox::RejectRole);
+ QPushButton *exitButton =
+ msgBox.addButton("Exit", QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(exitButton);
+ msgBox.setEscapeButton(cancelButton);
+
+ msgBox.exec();
+ if (msgBox.clickedButton() == cancelButton)
+ {
+ return;
+ }
+ this->FinishModel();
+ break;
+ }
+ case UNSAVED_CHANGES:
+ case NEVER_SAVED:
+ {
+ QString msg("Save Changes before exiting?\n\n");
+
+ QMessageBox msgBox(QMessageBox::NoIcon, QString("Exit"), msg);
+ QPushButton *cancelButton = msgBox.addButton("Cancel",
+ QMessageBox::RejectRole);
+ msgBox.addButton("Don't Save, Exit", QMessageBox::DestructiveRole);
+ QPushButton *saveButton = msgBox.addButton("Save and Exit",
+ QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(cancelButton);
+ msgBox.setDefaultButton(saveButton);
+
+ msgBox.exec();
+ if (msgBox.clickedButton() == cancelButton)
+ return;
+
+ if (msgBox.clickedButton() == saveButton)
+ {
+ if (!this->OnSave())
+ {
+ return;
+ }
+ }
+ break;
+ }
+ default:
+ return;
+ }
+
+ // Create entity on main window up to the saved point
+ if (this->currentSaveState != NEVER_SAVED)
+ this->FinishModel();
+ else
+ this->SetModelVisible(this->serverModelName, true);
+
+ this->Reset();
+
+ gui::model::Events::newModel();
+ gui::model::Events::finishModel();
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::SaveModelFiles()
+{
+ this->saveDialog->GenerateConfig();
+ this->saveDialog->SaveToConfig();
+ this->GenerateSDF();
+ this->saveDialog->SaveToSDF(this->modelSDF);
+ this->currentSaveState = ALL_SAVED;
+}
+
+/////////////////////////////////////////////////
std::string ModelCreator::CreateModel()
{
this->Reset();
- return this->modelName;
+ return this->folderName;
}
/////////////////////////////////////////////////
@@ -166,268 +579,522 @@ void ModelCreator::AddJoint(const std::string &_type)
}
/////////////////////////////////////////////////
-std::string ModelCreator::AddBox(const math::Vector3 &_size,
- const math::Pose &_pose)
+std::string ModelCreator::AddShape(LinkType _type,
+ const math::Vector3 &_size, const math::Pose &_pose,
+ const std::string &_uri, unsigned int _samples)
{
- if (!this->modelVisual)
+ if (!this->previewVisual)
{
this->Reset();
}
- std::ostringstream linkNameStream;
- linkNameStream << "unit_box_" << this->boxCounter++;
+ std::stringstream linkNameStream;
+ linkNameStream << this->previewName << "_" << this->modelCounter
+ << "::link_" << this->linkCounter++;
std::string linkName = linkNameStream.str();
rendering::VisualPtr linkVisual(new rendering::Visual(linkName,
- this->modelVisual));
+ this->previewVisual));
linkVisual->Load();
+ linkVisual->SetTransparency(ModelData::GetEditTransparency());
std::ostringstream visualName;
- visualName << linkName << "_visual";
+ visualName << linkName << "::visual";
rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
linkVisual));
- sdf::ElementPtr visualElem = this->modelTemplateSDF->root
+ sdf::ElementPtr visualElem = this->modelTemplateSDF->Root()
->GetElement("model")->GetElement("link")->GetElement("visual");
-
sdf::ElementPtr geomElem = visualElem->GetElement("geometry");
geomElem->ClearElements();
- ((geomElem->AddElement("box"))->AddElement("size"))->Set(_size);
+
+ if (_type == LINK_CYLINDER)
+ {
+ sdf::ElementPtr cylinderElem = geomElem->AddElement("cylinder");
+ (cylinderElem->GetElement("radius"))->Set(_size.x*0.5);
+ (cylinderElem->GetElement("length"))->Set(_size.z);
+ }
+ else if (_type == LINK_SPHERE)
+ {
+ ((geomElem->AddElement("sphere"))->GetElement("radius"))->Set(_size.x*0.5);
+ }
+ else if (_type == LINK_MESH)
+ {
+ sdf::ElementPtr meshElem = geomElem->AddElement("mesh");
+ meshElem->GetElement("scale")->Set(_size);
+ meshElem->GetElement("uri")->Set(_uri);
+ }
+ else if (_type == LINK_POLYLINE)
+ {
+ QFileInfo info(QString::fromStdString(_uri));
+ if (!info.isFile() || info.completeSuffix().toLower() != "svg")
+ {
+ gzerr << "File [" << _uri << "] not found or invalid!" << std::endl;
+ return std::string();
+ }
+
+ common::SVGLoader svgLoader(_samples);
+ std::vector<common::SVGPath> paths;
+ svgLoader.Parse(_uri, paths);
+
+ if (paths.empty())
+ {
+ gzerr << "No paths found on file [" << _uri << "]" << std::endl;
+ return std::string();
+ }
+
+ // SVG paths do not map to sdf polylines, because we now allow a contour
+ // to be made of multiple svg disjoint paths.
+ // For this reason, we compute the closed polylines that can be extruded
+ // in this step
+ std::vector< std::vector<ignition::math::Vector2d> > closedPolys;
+ std::vector< std::vector<ignition::math::Vector2d> > openPolys;
+ svgLoader.PathsToClosedPolylines(paths, 0.05, closedPolys, openPolys);
+ if (closedPolys.empty())
+ {
+ gzerr << "No closed polylines found on file [" << _uri << "]"
+ << std::endl;
+ return std::string();
+ }
+ if (!openPolys.empty())
+ {
+ gzmsg << "There are " << openPolys.size() << "open polylines. "
+ << "They will be ignored." << std::endl;
+ }
+ // Find extreme values to center the polylines
+ ignition::math::Vector2d min(paths[0].polylines[0][0]);
+ ignition::math::Vector2d max(min);
+
+ for (auto const &poly : closedPolys)
+ {
+ for (auto const &pt : poly)
+ {
+ if (pt.X() < min.X())
+ min.X() = pt.X();
+ if (pt.Y() < min.Y())
+ min.Y() = pt.Y();
+ if (pt.X() > max.X())
+ max.X() = pt.X();
+ if (pt.Y() > max.Y())
+ max.Y() = pt.Y();
+ }
+ }
+ for (auto const &poly : closedPolys)
+ {
+ sdf::ElementPtr polylineElem = geomElem->AddElement("polyline");
+ polylineElem->GetElement("height")->Set(_size.z);
+
+ for (auto const &p : poly)
+ {
+ // Translate to center
+ ignition::math::Vector2d pt = p - min - (max-min)*0.5;
+ // Swap X and Y so Z will point up
+ // (in 2D it points into the screen)
+ sdf::ElementPtr pointElem = polylineElem->AddElement("point");
+ pointElem->Set(
+ ignition::math::Vector2d(pt.Y()*_size.y, pt.X()*_size.x));
+ }
+ }
+ }
+ else
+ {
+ if (_type != LINK_BOX)
+ {
+ gzwarn << "Unknown link type '" << _type << "'. " <<
+ "Adding a box" << std::endl;
+ }
+ ((geomElem->AddElement("box"))->GetElement("size"))->Set(_size);
+ }
visVisual->Load(visualElem);
+ this->CreateLink(visVisual);
- linkVisual->SetTransparency(this->editTransparency);
linkVisual->SetPose(_pose);
- if (_pose == math::Pose::Zero)
+
+ // insert over ground plane for now
+ math::Vector3 linkPos = linkVisual->GetWorldPose().pos;
+ if (_type == LINK_BOX || _type == LINK_CYLINDER || _type == LINK_SPHERE)
{
- linkVisual->SetPosition(math::Vector3(_pose.pos.x, _pose.pos.y,
- _pose.pos.z + _size.z/2));
+ linkPos.z = _size.z * 0.5;
}
+ // override orientation as it's more natural to insert objects upright rather
+ // than inserting it in the model frame.
+ linkVisual->SetWorldPose(math::Pose(linkPos, math::Quaternion()));
- this->CreatePart(visVisual);
this->mouseVisual = linkVisual;
return linkName;
}
/////////////////////////////////////////////////
-std::string ModelCreator::AddSphere(double _radius,
- const math::Pose &_pose)
+void ModelCreator::CreateLink(const rendering::VisualPtr &_visual)
{
- if (!this->modelVisual)
- this->Reset();
+ LinkData *link = new LinkData();
+ MainWindow *mainWindow = gui::get_main_window();
+ if (mainWindow)
+ {
+ connect(gui::get_main_window(), SIGNAL(Close()), link->inspector,
+ SLOT(close()));
+ }
- std::ostringstream linkNameStream;
- linkNameStream << "unit_sphere_" << this->sphereCounter++;
- std::string linkName = linkNameStream.str();
+ link->linkVisual = _visual->GetParent();
+ link->AddVisual(_visual);
- rendering::VisualPtr linkVisual(new rendering::Visual(
- linkName, this->modelVisual));
- linkVisual->Load();
+ // override transparency
+ _visual->SetTransparency(_visual->GetTransparency() *
+ (1-ModelData::GetEditTransparency()-0.1)
+ + ModelData::GetEditTransparency());
- std::ostringstream visualName;
- visualName << linkName << "_visual";
- rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
- linkVisual));
- sdf::ElementPtr visualElem = this->modelTemplateSDF->root
- ->GetElement("model")->GetElement("link")->GetElement("visual");
+ // create collision with identical geometry
+ rendering::VisualPtr collisionVis =
+ _visual->Clone(link->linkVisual->GetName() + "::collision",
+ link->linkVisual);
- sdf::ElementPtr geomElem = visualElem->GetElement("geometry");
- geomElem->ClearElements();
- ((geomElem->AddElement("sphere"))->GetElement("radius"))->Set(_radius);
+ // orange
+ collisionVis->SetMaterial("Gazebo/Orange");
+ collisionVis->SetTransparency(
+ math::clamp(ModelData::GetEditTransparency() * 2.0, 0.0, 0.8));
+ // fix for transparency alpha compositing
+ Ogre::MovableObject *colObj = collisionVis->GetSceneNode()->
+ getAttachedObject(0);
+ colObj->setRenderQueueGroup(colObj->getRenderQueueGroup()+1);
+ link->AddCollision(collisionVis);
- visVisual->Load(visualElem);
+ std::string linkName = link->linkVisual->GetName();
+
+ std::string leafName = linkName;
+ size_t idx = linkName.find_last_of("::");
+ if (idx != std::string::npos)
+ leafName = linkName.substr(idx+1);
+
+ link->SetName(leafName);
- linkVisual->SetTransparency(this->editTransparency);
- linkVisual->SetPose(_pose);
- if (_pose == math::Pose::Zero)
{
- linkVisual->SetPosition(math::Vector3(_pose.pos.x, _pose.pos.y,
- _pose.pos.z + _radius));
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+ this->allLinks[linkName] = link;
+ if (this->canonicalLink.empty())
+ this->canonicalLink = linkName;
}
- this->CreatePart(visVisual);
- this->mouseVisual = linkVisual;
+ rendering::ScenePtr scene = link->linkVisual->GetScene();
- return linkName;
+ this->ModelChanged();
}
/////////////////////////////////////////////////
-std::string ModelCreator::AddCylinder(double _radius, double _length,
- const math::Pose &_pose)
+LinkData *ModelCreator::CloneLink(const std::string &_linkName)
{
- if (!this->modelVisual)
- this->Reset();
-
- std::ostringstream linkNameStream;
- linkNameStream << "unit_cylinder_" << this->cylinderCounter++;
- std::string linkName = linkNameStream.str();
-
- rendering::VisualPtr linkVisual(new rendering::Visual(
- linkName, this->modelVisual));
- linkVisual->Load();
-
- std::ostringstream visualName;
- visualName << linkName << "_visual";
- rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
- linkVisual));
- sdf::ElementPtr visualElem = this->modelTemplateSDF->root
- ->GetElement("model")->GetElement("link")->GetElement("visual");
-
- sdf::ElementPtr geomElem = visualElem->GetElement("geometry");
- geomElem->ClearElements();
- sdf::ElementPtr cylinderElem = geomElem->AddElement("cylinder");
- (cylinderElem->GetElement("radius"))->Set(_radius);
- (cylinderElem->GetElement("length"))->Set(_length);
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
- visVisual->Load(visualElem);
+ auto it = this->allLinks.find(_linkName);
+ if (it == allLinks.end())
+ {
+ gzerr << "No link with name: " << _linkName << " found." << std::endl;
+ return NULL;
+ }
- linkVisual->SetTransparency(this->editTransparency);
- linkVisual->SetPose(_pose);
- if (_pose == math::Pose::Zero)
+ // generate unique name.
+ std::string newName = _linkName + "_clone";
+ auto itName = this->allLinks.find(newName);
+ int nameCounter = 0;
+ while (itName != this->allLinks.end())
{
- linkVisual->SetPosition(math::Vector3(_pose.pos.x, _pose.pos.y,
- _pose.pos.z + _length/2));
+ std::stringstream newLinkName;
+ newLinkName << _linkName << "_clone_" << nameCounter++;
+ newName = newLinkName.str();
+ itName = this->allLinks.find(newName);
}
- this->CreatePart(visVisual);
- this->mouseVisual = linkVisual;
+ std::string leafName = newName;
+ size_t idx = newName.find_last_of("::");
+ if (idx != std::string::npos)
+ leafName = newName.substr(idx+1);
+ LinkData *link = it->second->Clone(leafName);
- return linkName;
+ this->allLinks[newName] = link;
+
+ this->ModelChanged();
+
+ return link;
}
/////////////////////////////////////////////////
-std::string ModelCreator::AddCustom(const std::string &_path,
- const math::Vector3 &_scale, const math::Pose &_pose)
+void ModelCreator::CreateLinkFromSDF(sdf::ElementPtr _linkElem)
{
- if (!this->modelVisual)
- this->Reset();
+ LinkData *link = new LinkData();
+ MainWindow *mainWindow = gui::get_main_window();
+ if (mainWindow)
+ {
+ connect(gui::get_main_window(), SIGNAL(Close()), link->inspector,
+ SLOT(close()));
+ }
- std::string path = _path;
+ link->Load(_linkElem);
- std::ostringstream linkNameStream;
- linkNameStream << "custom_" << this->customCounter++;
+ // Link
+ std::stringstream linkNameStream;
+ std::string leafName = link->GetName();
+
+ linkNameStream << this->previewName << "_" << this->modelCounter << "::";
+ linkNameStream << leafName;
std::string linkName = linkNameStream.str();
- rendering::VisualPtr linkVisual(new rendering::Visual(this->modelName + "::" +
- linkName, this->modelVisual));
+ if (this->canonicalLink.empty())
+ this->canonicalLink = linkName;
+
+ link->SetName(leafName);
+
+ // if link name is scoped, it could mean that it's from an included model.
+ // The joint maker needs to know about this in order to specify the correct
+ // parent and child links in sdf generation step.
+ if (leafName.find("::") != std::string::npos)
+ this->jointMaker->AddScopedLinkName(leafName);
+
+ rendering::VisualPtr linkVisual(new rendering::Visual(linkName,
+ this->previewVisual));
linkVisual->Load();
+ linkVisual->SetPose(link->GetPose());
+ link->linkVisual = linkVisual;
- std::ostringstream visualName;
- visualName << linkName << "_visual";
- rendering::VisualPtr visVisual(new rendering::Visual(visualName.str(),
- linkVisual));
- sdf::ElementPtr visualElem = this->modelTemplateSDF->root
- ->GetElement("model")->GetElement("link")->GetElement("visual");
+ // Visuals
+ int visualIndex = 0;
+ sdf::ElementPtr visualElem;
- sdf::ElementPtr geomElem = visualElem->GetElement("geometry");
- geomElem->ClearElements();
- sdf::ElementPtr meshElem = geomElem->AddElement("mesh");
- meshElem->GetElement("scale")->Set(_scale);
- meshElem->GetElement("uri")->Set(path);
- visVisual->Load(visualElem);
+ if (_linkElem->HasElement("visual"))
+ visualElem = _linkElem->GetElement("visual");
- linkVisual->SetTransparency(this->editTransparency);
- linkVisual->SetPose(_pose);
- if (_pose == math::Pose::Zero)
+ linkVisual->SetTransparency(ModelData::GetEditTransparency());
+
+ while (visualElem)
{
- linkVisual->SetPosition(math::Vector3(_pose.pos.x, _pose.pos.y,
- _pose.pos.z + _scale.z/2));
+ // Visual name
+ std::string visualName;
+ if (visualElem->HasAttribute("name"))
+ {
+ visualName = linkName + "::" + visualElem->Get<std::string>("name");
+ visualIndex++;
+ }
+ else
+ {
+ std::stringstream visualNameStream;
+ visualNameStream << linkName << "::visual_" << visualIndex++;
+ visualName = visualNameStream.str();
+ gzwarn << "SDF missing visual name attribute. Created name " << visualName
+ << std::endl;
+ }
+ rendering::VisualPtr visVisual(new rendering::Visual(visualName,
+ linkVisual));
+ visVisual->Load(visualElem);
+
+ // Visual pose
+ math::Pose visualPose;
+ if (visualElem->HasElement("pose"))
+ visualPose = visualElem->Get<math::Pose>("pose");
+ else
+ visualPose.Set(0, 0, 0, 0, 0, 0);
+ visVisual->SetPose(visualPose);
+
+ // Add to link
+ link->AddVisual(visVisual);
+
+ // override transparency
+ visVisual->SetTransparency(visVisual->GetTransparency() *
+ (1-ModelData::GetEditTransparency()-0.1)
+ + ModelData::GetEditTransparency());
+
+ visualElem = visualElem->GetNextElement("visual");
}
- this->CreatePart(visVisual);
- this->mouseVisual = linkVisual;
+ // Collisions
+ int collisionIndex = 0;
+ sdf::ElementPtr collisionElem;
- return linkName;
-}
+ if (_linkElem->HasElement("collision"))
+ collisionElem = _linkElem->GetElement("collision");
-/////////////////////////////////////////////////
-void ModelCreator::CreatePart(const rendering::VisualPtr &_visual)
-{
- PartData *part = new PartData;
+ while (collisionElem)
+ {
+ // Collision name
+ std::string collisionName;
+ if (collisionElem->HasAttribute("name"))
+ {
+ collisionName = linkName + "::" + collisionElem->Get<std::string>("name");
+ collisionIndex++;
+ }
+ else
+ {
+ std::ostringstream collisionNameStream;
+ collisionNameStream << linkName << "::collision_" << collisionIndex++;
+ collisionName = collisionNameStream.str();
+ gzwarn << "SDF missing collision name attribute. Created name " <<
+ collisionName << std::endl;
+ }
+ rendering::VisualPtr colVisual(new rendering::Visual(collisionName,
+ linkVisual));
- part->partVisual = _visual->GetParent();
- part->visuals.push_back(_visual);
+ // Collision pose
+ math::Pose collisionPose;
+ if (collisionElem->HasElement("pose"))
+ collisionPose = collisionElem->Get<math::Pose>("pose");
+ else
+ collisionPose.Set(0, 0, 0, 0, 0, 0);
+
+ // Make a visual element from the collision element
+ sdf::ElementPtr colVisualElem = this->modelTemplateSDF->Root()
+ ->GetElement("model")->GetElement("link")->GetElement("visual");
+
+ sdf::ElementPtr geomElem = colVisualElem->GetElement("geometry");
+ geomElem->ClearElements();
+ geomElem->Copy(collisionElem->GetElement("geometry"));
+
+ colVisual->Load(colVisualElem);
+ colVisual->SetPose(collisionPose);
+ colVisual->SetMaterial("Gazebo/Orange");
+ colVisual->SetTransparency(
+ math::clamp(ModelData::GetEditTransparency() * 2.0, 0.0, 0.8));
+ // fix for transparency alpha compositing
+ Ogre::MovableObject *colObj = colVisual->GetSceneNode()->
+ getAttachedObject(0);
+ colObj->setRenderQueueGroup(colObj->getRenderQueueGroup()+1);
+
+ // Add to link
+ link->AddCollision(colVisual);
+
+ collisionElem = collisionElem->GetNextElement("collision");
+ }
+
+ {
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+ this->allLinks[linkName] = link;
+ gui::model::Events::linkInserted(linkName);
+ }
- part->name = part->partVisual->GetName();
- part->pose = part->partVisual->GetWorldPose();
- part->gravity = true;
- part->selfCollide = false;
- part->kinematic = false;
+ rendering::ScenePtr scene = link->linkVisual->GetScene();
- this->allParts[part->name] = part;
+ this->ModelChanged();
}
/////////////////////////////////////////////////
-void ModelCreator::RemovePart(const std::string &_partName)
+void ModelCreator::RemoveLinkImpl(const std::string &_linkName)
{
- if (!this->modelVisual)
+ if (!this->previewVisual)
{
this->Reset();
return;
}
- if (this->allParts.find(_partName) == this->allParts.end())
+ LinkData *link = NULL;
+ {
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+ if (this->allLinks.find(_linkName) == this->allLinks.end())
+ return;
+ link = this->allLinks[_linkName];
+ }
+
+ if (!link)
return;
- PartData *part = this->allParts[_partName];
- if (!part)
- return;
+ // Copy before reference is deleted.
+ std::string linkName(_linkName);
+
+ rendering::ScenePtr scene = link->linkVisual->GetScene();
+ if (scene)
+ {
+ for (auto &it : link->visuals)
+ {
+ rendering::VisualPtr vis = it.first;
+ scene->RemoveVisual(vis);
+ }
+ scene->RemoveVisual(link->linkVisual);
+ for (auto &colIt : link->collisions)
+ {
+ rendering::VisualPtr vis = colIt.first;
+ scene->RemoveVisual(vis);
+ }
+
+ scene->RemoveVisual(link->linkVisual);
+ }
- rendering::ScenePtr scene = part->partVisual->GetScene();
- for (unsigned int i = 0; i < part->visuals.size(); ++i)
+ link->linkVisual.reset();
{
- rendering::VisualPtr vis = part->visuals[i];
- scene->RemoveVisual(vis);
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+ this->allLinks.erase(_linkName);
+ delete link;
}
- scene->RemoveVisual(part->partVisual);
- part->visuals.clear();
- part->partVisual.reset();
+ gui::model::Events::linkRemoved(linkName);
- this->allParts.erase(_partName);
+ this->ModelChanged();
}
/////////////////////////////////////////////////
void ModelCreator::Reset()
{
- if (!gui::get_active_camera() ||
- !gui::get_active_camera()->GetScene())
- return;
+ delete this->saveDialog;
+ this->saveDialog = new SaveDialog(SaveDialog::MODEL);
this->jointMaker->Reset();
- this->selectedVisuals.clear();
- g_copyAct->setEnabled(false);
- g_pasteAct->setEnabled(false);
+ this->selectedLinks.clear();
- std::stringstream ss;
- ss << "defaultModel_" << this->modelCounter++;
- this->modelName = ss.str();
+ if (g_copyAct)
+ g_copyAct->setEnabled(false);
- rendering::ScenePtr scene = gui::get_active_camera()->GetScene();
+ if (g_pasteAct)
+ g_pasteAct->setEnabled(false);
+
+ this->currentSaveState = NEVER_SAVED;
+ this->SetModelName(this->modelDefaultName);
+ this->serverModelName = "";
+ this->serverModelSDF.reset();
+ this->serverModelVisible.clear();
+ this->canonicalLink = "";
+
+ this->modelTemplateSDF.reset(new sdf::SDF);
+ this->modelTemplateSDF->SetFromString(ModelData::GetTemplateSDFString());
+
+ this->modelSDF.reset(new sdf::SDF);
this->isStatic = false;
this->autoDisable = true;
+ gui::model::Events::modelPropertiesChanged(this->isStatic, this->autoDisable,
+ this->modelPose, this->GetModelName());
+
+ while (!this->allLinks.empty())
+ this->RemoveLinkImpl(this->allLinks.begin()->first);
+ this->allLinks.clear();
- while (this->allParts.size() > 0)
- this->RemovePart(this->allParts.begin()->first);
- this->allParts.clear();
+ if (!gui::get_active_camera() ||
+ !gui::get_active_camera()->GetScene())
+ return;
- if (this->modelVisual)
- scene->RemoveVisual(this->modelVisual);
+ rendering::ScenePtr scene = gui::get_active_camera()->GetScene();
+ if (this->previewVisual)
+ scene->RemoveVisual(this->previewVisual);
- this->modelVisual.reset(new rendering::Visual(this->modelName,
+ this->previewVisual.reset(new rendering::Visual(this->previewName,
scene->GetWorldVisual()));
- this->modelVisual->Load();
+ this->previewVisual->Load();
this->modelPose = math::Pose::Zero;
- this->modelVisual->SetPose(this->modelPose);
- scene->AddVisual(this->modelVisual);
+ this->previewVisual->SetPose(this->modelPose);
}
/////////////////////////////////////////////////
void ModelCreator::SetModelName(const std::string &_modelName)
{
this->modelName = _modelName;
+ this->saveDialog->SetModelName(_modelName);
+
+ this->folderName = this->saveDialog->
+ GetFolderNameFromModelName(this->modelName);
+
+ if (this->currentSaveState == NEVER_SAVED)
+ {
+ // Set new saveLocation
+ boost::filesystem::path oldPath(this->saveDialog->GetSaveLocation());
+
+ boost::filesystem::path newPath = oldPath.parent_path() / this->folderName;
+ this->saveDialog->SetSaveLocation(newPath.string());
+ }
}
/////////////////////////////////////////////////
@@ -440,136 +1107,128 @@ std::string ModelCreator::GetModelName() const
void ModelCreator::SetStatic(bool _static)
{
this->isStatic = _static;
+ this->ModelChanged();
}
/////////////////////////////////////////////////
void ModelCreator::SetAutoDisable(bool _auto)
{
this->autoDisable = _auto;
+ this->ModelChanged();
}
/////////////////////////////////////////////////
-void ModelCreator::SaveToSDF(const std::string &_savePath)
+void ModelCreator::FinishModel()
{
- std::ofstream savefile;
- boost::filesystem::path path;
- path = boost::filesystem::operator/(_savePath, this->modelName + ".sdf");
- savefile.open(path.string().c_str());
- if (savefile.is_open())
- {
- savefile << this->modelSDF->ToString();
- savefile.close();
- }
- else
+ if (!this->serverModelName.empty())
{
- gzerr << "Unable to open file for writing: '" << path.string().c_str()
- << "'. Possibly a permission issue." << std::endl;
- }
-}
+ // delete model on server first before spawning the updated one.
+ transport::request(gui::get_world(), "entity_delete",
+ this->serverModelName);
+ int timeoutCounter = 0;
+ int timeout = 100;
+ while (timeoutCounter < timeout)
+ {
+ boost::shared_ptr<msgs::Response> response =
+ transport::request(gui::get_world(), "entity_info",
+ this->serverModelName);
+ // Make sure the response is correct
+ if (response->response() == "nonexistent")
+ break;
-/////////////////////////////////////////////////
-void ModelCreator::FinishModel()
-{
+ common::Time::MSleep(100);
+ timeoutCounter++;
+ }
+ }
event::Events::setSelectedEntity("", "normal");
- this->Reset();
this->CreateTheEntity();
+ this->Reset();
}
/////////////////////////////////////////////////
void ModelCreator::CreateTheEntity()
{
+ if (!this->modelSDF->Root()->HasElement("model"))
+ {
+ gzerr << "Generated invalid SDF! Cannot create entity." << std::endl;
+ return;
+ }
+
msgs::Factory msg;
+ // Create a new name if the model exists
+ sdf::ElementPtr modelElem = this->modelSDF->Root()->GetElement("model");
+ std::string modelElemName = modelElem->Get<std::string>("name");
+ if (has_entity_name(modelElemName))
+ {
+ int i = 0;
+ while (has_entity_name(modelElemName))
+ {
+ modelElemName = modelElem->Get<std::string>("name") + "_" +
+ boost::lexical_cast<std::string>(i++);
+ }
+ modelElem->GetAttribute("name")->Set(modelElemName);
+ }
+
msg.set_sdf(this->modelSDF->ToString());
+ msgs::Set(msg.mutable_pose(), this->modelPose.Ign());
this->makerPub->Publish(msg);
}
/////////////////////////////////////////////////
-std::string ModelCreator::GetTemplateSDFString()
+void ModelCreator::AddLink(LinkType _type)
{
- std::ostringstream newModelStr;
- newModelStr << "<sdf version ='" << SDF_VERSION << "'>"
- << "<model name='template_model'>"
- << "<pose>0 0 0.0 0 0 0</pose>"
- << "<link name ='link'>"
- << "<visual name ='visual'>"
- << "<pose>0 0 0.0 0 0 0</pose>"
- << "<geometry>"
- << "<box>"
- << "<size>1.0 1.0 1.0</size>"
- << "</box>"
- << "</geometry>"
- << "<material>"
- << "<script>"
- << "<uri>file://media/materials/scripts/gazebo.material</uri>"
- << "<name>Gazebo/Grey</name>"
- << "</script>"
- << "</material>"
- << "</visual>"
- << "</link>"
- << "<static>true</static>"
- << "</model>"
- << "</sdf>";
-
- return newModelStr.str();
-}
+ if (!this->previewVisual)
+ {
+ this->Reset();
+ }
-/////////////////////////////////////////////////
-void ModelCreator::AddPart(PartType _type)
-{
this->Stop();
- this->addPartType = _type;
- if (_type != PART_NONE)
- {
- switch (_type)
- {
- case PART_BOX:
- {
- this->AddBox();
- break;
- }
- case PART_SPHERE:
- {
- this->AddSphere();
- break;
- }
- case PART_CYLINDER:
- {
- this->AddCylinder();
- break;
- }
- default:
- {
- gzwarn << "Unknown part type '" << _type << "'. " <<
- "Part not added" << std::endl;
- break;
- }
- }
- }
+ this->addLinkType = _type;
+ if (_type != LINK_NONE)
+ this->AddShape(_type);
}
/////////////////////////////////////////////////
void ModelCreator::Stop()
{
- if (this->addPartType != PART_NONE && this->mouseVisual)
+ if (this->addLinkType != LINK_NONE && this->mouseVisual)
{
- for (unsigned int i = 0; i < this->mouseVisual->GetChildCount(); ++i)
- this->RemovePart(this->mouseVisual->GetName());
+ this->RemoveLinkImpl(this->mouseVisual->GetName());
this->mouseVisual.reset();
+ emit LinkAdded();
}
if (this->jointMaker)
this->jointMaker->Stop();
}
/////////////////////////////////////////////////
+void ModelCreator::OnDelete()
+{
+ if (this->inspectName.empty())
+ return;
+
+ this->OnDelete(this->inspectName);
+ this->inspectName = "";
+}
+
+/////////////////////////////////////////////////
void ModelCreator::OnDelete(const std::string &_entity)
{
+ this->RemoveLink(_entity);
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::RemoveLink(const std::string &_entity)
+{
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+
// if it's a link
- if (this->allParts.find(_entity) != this->allParts.end())
+ if (this->allLinks.find(_entity) != this->allLinks.end())
{
if (this->jointMaker)
- this->jointMaker->RemoveJointsByPart(_entity);
- this->RemovePart(_entity);
+ this->jointMaker->RemoveJointsByLink(_entity);
+ this->RemoveLinkImpl(_entity);
return;
}
@@ -579,14 +1238,16 @@ void ModelCreator::OnDelete(const std::string &_entity)
if (vis)
{
rendering::VisualPtr parentLink = vis->GetParent();
- if (this->allParts.find(parentLink->GetName()) != this->allParts.end())
+ std::string parentLinkName = parentLink->GetName();
+
+ if (this->allLinks.find(parentLinkName) != this->allLinks.end())
{
// remove the parent link if it's the only child
if (parentLink->GetChildCount() == 1)
{
if (this->jointMaker)
- this->jointMaker->RemoveJointsByPart(parentLink->GetName());
- this->RemovePart(parentLink->GetName());
+ this->jointMaker->RemoveJointsByLink(parentLink->GetName());
+ this->RemoveLinkImpl(parentLink->GetName());
return;
}
}
@@ -602,15 +1263,13 @@ bool ModelCreator::OnKeyPress(const common::KeyEvent &_event)
}
else if (_event.key == Qt::Key_Delete)
{
- if (!this->selectedVisuals.empty())
+ if (!this->selectedLinks.empty())
{
- for (std::vector<rendering::VisualPtr>::iterator it
- = this->selectedVisuals.begin(); it != this->selectedVisuals.end();)
+ for (auto linkVis : this->selectedLinks)
{
- (*it)->SetHighlighted(false);
- this->OnDelete((*it)->GetName());
- it = this->selectedVisuals.erase(it);
+ this->OnDelete(linkVis->GetName());
}
+ this->DeselectAll();
}
}
else if (_event.control)
@@ -642,7 +1301,7 @@ bool ModelCreator::OnMousePress(const common::MouseEvent &_event)
return true;
}
- rendering::VisualPtr vis = userCamera->GetVisual(_event.pos);
+ rendering::VisualPtr vis = userCamera->GetVisual(_event.Pos());
if (vis)
{
if (!vis->IsPlane() && gui::get_entity_id(vis->GetRootVisual()->GetName()))
@@ -667,75 +1326,88 @@ bool ModelCreator::OnMouseRelease(const common::MouseEvent &_event)
if (!userCamera)
return false;
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+
if (this->mouseVisual)
{
- // set the part data pose
- if (this->allParts.find(this->mouseVisual->GetName()) !=
- this->allParts.end())
+ if (_event.Button() == common::MouseEvent::RIGHT)
+ return true;
+
+ // set the link data pose
+ if (this->allLinks.find(this->mouseVisual->GetName()) !=
+ this->allLinks.end())
{
- PartData *part = this->allParts[this->mouseVisual->GetName()];
- part->pose = this->mouseVisual->GetWorldPose();
+ LinkData *link = this->allLinks[this->mouseVisual->GetName()];
+ link->SetPose(this->mouseVisual->GetWorldPose()-this->modelPose);
+ gui::model::Events::linkInserted(this->mouseVisual->GetName());
}
// reset and return
- emit PartAdded();
+ emit LinkAdded();
this->mouseVisual.reset();
- this->AddPart(PART_NONE);
+ this->AddLink(LINK_NONE);
return true;
}
- rendering::VisualPtr vis = userCamera->GetVisual(_event.pos);
+ rendering::VisualPtr vis = userCamera->GetVisual(_event.Pos());
if (vis)
{
- rendering::VisualPtr partVis = vis->GetParent();
- // Is part
- if (this->allParts.find(partVis->GetName()) !=
- this->allParts.end())
+ rendering::VisualPtr linkVis = vis->GetParent();
+ // Is link
+ if (this->allLinks.find(linkVis->GetName()) !=
+ this->allLinks.end())
{
// Handle snap from GLWidget
if (g_snapAct->isChecked())
return false;
+ // trigger link inspector on right click
+ if (_event.Button() == common::MouseEvent::RIGHT)
+ {
+ this->inspectName = vis->GetParent()->GetName();
+
+ this->ShowContextMenu(this->inspectName);
+ return true;
+ }
+
// Not in multi-selection mode.
if (!(QApplication::keyboardModifiers() & Qt::ControlModifier))
{
this->DeselectAll();
-
- // Highlight and selected clicked part
- partVis->SetHighlighted(true);
- this->selectedVisuals.push_back(partVis);
+ this->SetSelected(linkVis, true);
}
// Multi-selection mode
else
{
- std::vector<rendering::VisualPtr>::iterator it =
- std::find(this->selectedVisuals.begin(),
- this->selectedVisuals.end(), partVis);
- // Highlight and select clicked part if not already selected
- if (it == this->selectedVisuals.end())
+ auto it = std::find(this->selectedLinks.begin(),
+ this->selectedLinks.end(), linkVis);
+ // Highlight and select clicked link if not already selected
+ if (it == this->selectedLinks.end())
{
- partVis->SetHighlighted(true);
- this->selectedVisuals.push_back(partVis);
+ this->SetSelected(linkVis, true);
}
// Deselect if already selected
else
{
- partVis->SetHighlighted(false);
- this->selectedVisuals.erase(it);
+ this->SetSelected(linkVis, false);
}
}
- g_copyAct->setEnabled(!this->selectedVisuals.empty());
- g_alignAct->setEnabled(this->selectedVisuals.size() > 1);
+
+ if (this->manipMode == "translate" || this->manipMode == "rotate" ||
+ this->manipMode == "scale")
+ {
+ this->OnManipMode(this->manipMode);
+ }
return true;
}
- // Not part
+ // Not link
else
{
this->DeselectAll();
g_alignAct->setEnabled(false);
- g_copyAct->setEnabled(!this->selectedVisuals.empty());
+ g_copyAct->setEnabled(!this->selectedLinks.empty());
if (!vis->IsPlane())
return true;
@@ -745,6 +1417,45 @@ bool ModelCreator::OnMouseRelease(const common::MouseEvent &_event)
}
/////////////////////////////////////////////////
+void ModelCreator::ShowContextMenu(const std::string &_link)
+{
+ auto it = this->allLinks.find(_link);
+ if (it == this->allLinks.end())
+ return;
+
+ this->inspectName = _link;
+ QMenu menu;
+ if (this->inspectAct)
+ {
+ menu.addAction(this->inspectAct);
+
+ if (this->jointMaker)
+ {
+ std::vector<JointData *> joints = this->jointMaker->GetJointDataByLink(
+ _link);
+
+ if (!joints.empty())
+ {
+ QMenu *jointsMenu = menu.addMenu(tr("Open Joint Inspector"));
+
+ for (auto joint : joints)
+ {
+ QAction *jointAct = new QAction(tr(joint->name.c_str()), this);
+ connect(jointAct, SIGNAL(triggered()), joint,
+ SLOT(OnOpenInspector()));
+ jointsMenu->addAction(jointAct);
+ }
+ }
+ }
+ }
+ QAction *deleteAct = new QAction(tr("Delete"), this);
+ connect(deleteAct, SIGNAL(triggered()), this, SLOT(OnDelete()));
+ menu.addAction(deleteAct);
+
+ menu.exec(QCursor::pos());
+}
+
+/////////////////////////////////////////////////
bool ModelCreator::OnMouseMove(const common::MouseEvent &_event)
{
this->lastMouseEvent = _event;
@@ -754,12 +1465,12 @@ bool ModelCreator::OnMouseMove(const common::MouseEvent &_event)
if (!this->mouseVisual)
{
- rendering::VisualPtr vis = userCamera->GetVisual(_event.pos);
+ rendering::VisualPtr vis = userCamera->GetVisual(_event.Pos());
if (vis && !vis->IsPlane())
{
// Main window models always handled here
- if (this->allParts.find(vis->GetParent()->GetName()) ==
- this->allParts.end())
+ if (this->allLinks.find(vis->GetParent()->GetName()) ==
+ this->allLinks.end())
{
// Prevent highlighting for snapping
if (this->manipMode == "snap" || this->manipMode == "select" ||
@@ -770,7 +1481,7 @@ bool ModelCreator::OnMouseMove(const common::MouseEvent &_event)
userCamera->HandleMouseEvent(_event);
}
// Allow ModelManipulator to work while dragging handle over this
- else if (_event.dragging)
+ else if (_event.Dragging())
{
ModelManipulator::Instance()->OnMouseMoveEvent(_event);
}
@@ -784,7 +1495,7 @@ bool ModelCreator::OnMouseMove(const common::MouseEvent &_event)
pose.pos = ModelManipulator::GetMousePositionOnPlane(
userCamera, _event);
- if (!_event.shift)
+ if (!_event.Shift())
{
pose.pos = ModelManipulator::SnapPoint(pose.pos);
}
@@ -798,15 +1509,17 @@ bool ModelCreator::OnMouseMove(const common::MouseEvent &_event)
/////////////////////////////////////////////////
bool ModelCreator::OnMouseDoubleClick(const common::MouseEvent &_event)
{
- // open the part inspector on double click
- rendering::VisualPtr vis = gui::get_active_camera()->GetVisual(_event.pos);
+ // open the link inspector on double click
+ rendering::VisualPtr vis = gui::get_active_camera()->GetVisual(_event.Pos());
if (!vis)
return false;
- if (this->allParts.find(vis->GetParent()->GetName()) !=
- this->allParts.end())
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+
+ if (this->allLinks.find(vis->GetParent()->GetName()) !=
+ this->allLinks.end())
{
- // TODO open inspector.
+ this->OpenInspector(vis->GetParent()->GetName());
return true;
}
@@ -814,17 +1527,43 @@ bool ModelCreator::OnMouseDoubleClick(const common::MouseEvent &_event)
}
/////////////////////////////////////////////////
+void ModelCreator::OnOpenInspector()
+{
+ if (this->inspectName.empty())
+ return;
+
+ this->OpenInspector(this->inspectName);
+ this->inspectName = "";
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::OpenInspector(const std::string &_name)
+{
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+ LinkData *link = this->allLinks[_name];
+ if (!link)
+ {
+ gzerr << "Link [" << _name << "] not found." << std::endl;
+ return;
+ }
+ link->SetPose(link->linkVisual->GetWorldPose()-this->modelPose);
+ link->UpdateConfig();
+ link->inspector->move(QCursor::pos());
+ link->inspector->show();
+}
+
+/////////////////////////////////////////////////
void ModelCreator::OnCopy()
{
if (!g_editModelAct->isChecked())
return;
- if (!this->selectedVisuals.empty())
+ if (!this->selectedLinks.empty())
{
- this->copiedPartNames.clear();
- for (unsigned int i = 0; i < this->selectedVisuals.size(); ++i)
+ this->copiedLinkNames.clear();
+ for (auto vis : this->selectedLinks)
{
- this->copiedPartNames.push_back(this->selectedVisuals[i]->GetName());
+ this->copiedLinkNames.push_back(vis->GetName());
}
g_pasteAct->setEnabled(true);
}
@@ -833,58 +1572,32 @@ void ModelCreator::OnCopy()
/////////////////////////////////////////////////
void ModelCreator::OnPaste()
{
- if (this->copiedPartNames.empty() || !g_editModelAct->isChecked())
+ if (this->copiedLinkNames.empty() || !g_editModelAct->isChecked())
{
return;
}
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+
// For now, only copy the last selected model
- boost::unordered_map<std::string, PartData *>::iterator it =
- this->allParts.find(this->copiedPartNames.back());
- if (it != this->allParts.end())
+ auto it = this->allLinks.find(this->copiedLinkNames.back());
+ if (it != this->allLinks.end())
{
- PartData *copiedPart = it->second;
- if (!copiedPart)
+ LinkData *copiedLink = it->second;
+ if (!copiedLink)
return;
this->Stop();
this->DeselectAll();
- std::string linkName = copiedPart->name + "_clone";
-
- if (!this->modelVisual)
+ if (!this->previewVisual)
{
this->Reset();
}
- rendering::VisualPtr linkVisual(new rendering::Visual(
- linkName, this->modelVisual));
- linkVisual->Load();
-
- std::ostringstream visualName;
- visualName << linkName << "_visual";
- rendering::VisualPtr visVisual;
-
- math::Pose clonePose;
- math::Vector3 cloneScale;
-
- if (copiedPart->visuals.empty())
- {
- visVisual = rendering::VisualPtr(new rendering::Visual(visualName.str(),
- linkVisual));
- sdf::ElementPtr visualElem = this->modelTemplateSDF->root
- ->GetElement("model")->GetElement("link")->GetElement("visual");
- visVisual->Load(visualElem);
- }
- else
- {
- rendering::VisualPtr copiedVisual = copiedPart->visuals.back();
- visVisual = copiedVisual->Clone(visualName.str(), linkVisual);
- clonePose = copiedVisual->GetWorldPose();
- cloneScale = copiedVisual->GetParent()->GetScale();
- visVisual->SetScale(math::Vector3::One);
- }
+ LinkData* clonedLink = this->CloneLink(it->first);
+ math::Pose clonePose = copiedLink->linkVisual->GetWorldPose();
rendering::UserCameraPtr userCamera = gui::get_active_camera();
if (userCamera)
{
@@ -895,13 +1608,9 @@ void ModelCreator::OnPaste()
clonePose.pos.y = mousePosition.y;
}
- linkVisual->SetScale(cloneScale);
- linkVisual->SetWorldPose(clonePose);
- linkVisual->SetTransparency(this->editTransparency);
-
- this->addPartType = PART_CUSTOM;
- this->CreatePart(visVisual);
- this->mouseVisual = linkVisual;
+ clonedLink->linkVisual->SetWorldPose(clonePose);
+ this->addLinkType = LINK_MESH;
+ this->mouseVisual = clonedLink->linkVisual;
}
}
@@ -915,108 +1624,66 @@ JointMaker *ModelCreator::GetJointMaker() const
void ModelCreator::GenerateSDF()
{
sdf::ElementPtr modelElem;
- sdf::ElementPtr linkElem;
this->modelSDF.reset(new sdf::SDF);
- this->modelSDF->SetFromString(this->GetTemplateSDFString());
+ this->modelSDF->SetFromString(ModelData::GetTemplateSDFString());
- modelElem = this->modelSDF->root->GetElement("model");
+ modelElem = this->modelSDF->Root()->GetElement("model");
- linkElem = modelElem->GetElement("link");
- sdf::ElementPtr templateLinkElem = linkElem->Clone();
- sdf::ElementPtr templateVisualElem = templateLinkElem->GetElement(
- "visual")->Clone();
- sdf::ElementPtr templateCollisionElem = templateLinkElem->GetElement(
- "collision")->Clone();
modelElem->ClearElements();
- std::stringstream visualNameStream;
- std::stringstream collisionNameStream;
+ modelElem->GetAttribute("name")->Set(this->folderName);
- modelElem->GetAttribute("name")->Set(this->modelName);
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
- boost::unordered_map<std::string, PartData *>::iterator partsIt;
-
- // set center of all parts to be origin
- math::Vector3 mid;
- for (partsIt = this->allParts.begin(); partsIt != this->allParts.end();
- ++partsIt)
+ if (this->serverModelName.empty())
{
- PartData *part = partsIt->second;
- mid += part->partVisual->GetWorldPose().pos;
+ // set center of all links to be origin
+ // TODO set a better origin other than the centroid
+ math::Vector3 mid;
+ for (auto &linksIt : this->allLinks)
+ {
+ LinkData *link = linksIt.second;
+ mid += link->GetPose().pos;
+ }
+ if (!this->allLinks.empty())
+ mid /= this->allLinks.size();
+ this->modelPose.pos = mid;
}
- mid /= this->allParts.size();
- this->origin.pos = mid;
- modelElem->GetElement("pose")->Set(this->origin);
- // loop through all parts and generate sdf
- for (partsIt = this->allParts.begin(); partsIt != this->allParts.end();
- ++partsIt)
+ // Update preview model and link poses in case they changed
+ for (auto &linksIt : this->allLinks)
{
- visualNameStream.str("");
- collisionNameStream.str("");
-
- PartData *part = partsIt->second;
- sdf::ElementPtr newLinkElem = templateLinkElem->Clone();
- newLinkElem->ClearElements();
- newLinkElem->GetAttribute("name")->Set(part->name);
- newLinkElem->GetElement("pose")->Set(part->partVisual->GetWorldPose()
- - this->origin);
- newLinkElem->GetElement("gravity")->Set(part->gravity);
- newLinkElem->GetElement("self_collide")->Set(part->selfCollide);
- newLinkElem->GetElement("kinematic")->Set(part->kinematic);
-
- modelElem->InsertElement(newLinkElem);
+ this->previewVisual->SetWorldPose(this->modelPose);
+ LinkData *link = linksIt.second;
+ link->SetPose(link->linkVisual->GetWorldPose() - this->modelPose);
+ link->linkVisual->SetPose(link->GetPose());
+ }
- for (unsigned int i = 0; i < part->visuals.size(); ++i)
+ // generate canonical link sdf first.
+ if (!this->canonicalLink.empty())
+ {
+ auto canonical = this->allLinks.find(this->canonicalLink);
+ if (canonical != this->allLinks.end())
{
- sdf::ElementPtr visualElem = templateVisualElem->Clone();
- sdf::ElementPtr collisionElem = templateCollisionElem->Clone();
+ LinkData *link = canonical->second;
+ link->UpdateConfig();
- rendering::VisualPtr visual = part->visuals[i];
-
- visualElem->GetAttribute("name")->Set(visual->GetName());
- collisionElem->GetAttribute("name")->Set(
- visual->GetParent()->GetName() + "_collision");
- visualElem->GetElement("pose")->Set(visual->GetPose());
- collisionElem->GetElement("pose")->Set(visual->GetPose());
+ sdf::ElementPtr newLinkElem = this->GenerateLinkSDF(link);
+ modelElem->InsertElement(newLinkElem);
+ }
+ }
- sdf::ElementPtr geomElem = visualElem->GetElement("geometry");
- geomElem->ClearElements();
+ // loop through rest of all links and generate sdf
+ for (auto &linksIt : this->allLinks)
+ {
+ if (linksIt.first == this->canonicalLink)
+ continue;
- math::Vector3 scale = visual->GetParent()->GetScale();
- if (visual->GetParent()->GetName().find("unit_box") != std::string::npos)
- {
- sdf::ElementPtr boxElem = geomElem->AddElement("box");
- (boxElem->GetElement("size"))->Set(scale);
- }
- else if (visual->GetParent()->GetName().find("unit_cylinder")
- != std::string::npos)
- {
- sdf::ElementPtr cylinderElem = geomElem->AddElement("cylinder");
- (cylinderElem->GetElement("radius"))->Set(scale.x/2.0);
- (cylinderElem->GetElement("length"))->Set(scale.z);
- }
- else if (visual->GetParent()->GetName().find("unit_sphere")
- != std::string::npos)
- {
- sdf::ElementPtr sphereElem = geomElem->AddElement("sphere");
- (sphereElem->GetElement("radius"))->Set(scale.x/2.0);
- }
- else if (visual->GetParent()->GetName().find("custom")
- != std::string::npos)
- {
- sdf::ElementPtr customElem = geomElem->AddElement("mesh");
- (customElem->GetElement("scale"))->Set(scale);
- (customElem->GetElement("uri"))->Set(visual->GetMeshName());
- }
- sdf::ElementPtr geomElemClone = geomElem->Clone();
- geomElem = collisionElem->GetElement("geometry");
- geomElem->ClearElements();
- geomElem->InsertElement(geomElemClone->GetFirstElement());
+ LinkData *link = linksIt.second;
+ link->UpdateConfig();
- newLinkElem->InsertElement(visualElem);
- newLinkElem->InsertElement(collisionElem);
- }
+ sdf::ElementPtr newLinkElem = this->GenerateLinkSDF(link);
+ modelElem->InsertElement(newLinkElem);
}
// Add joint sdf elements
@@ -1035,27 +1702,114 @@ void ModelCreator::GenerateSDF()
// Model settings
modelElem->GetElement("static")->Set(this->isStatic);
modelElem->GetElement("allow_auto_disable")->Set(this->autoDisable);
+
+ // If we're editing an existing model, copy the original plugin sdf elements
+ // since we are not generating them.
+ if (this->serverModelSDF)
+ {
+ if (this->serverModelSDF->HasElement("plugin"))
+ {
+ sdf::ElementPtr pluginElem = this->serverModelSDF->GetElement("plugin");
+ while (pluginElem)
+ {
+ modelElem->InsertElement(pluginElem->Clone());
+ pluginElem = pluginElem->GetNextElement("plugin");
+ }
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr ModelCreator::GenerateLinkSDF(LinkData *_link)
+{
+ std::stringstream visualNameStream;
+ std::stringstream collisionNameStream;
+ visualNameStream.str("");
+ collisionNameStream.str("");
+
+ sdf::ElementPtr newLinkElem = _link->linkSDF->Clone();
+ newLinkElem->GetElement("pose")->Set(_link->linkVisual->GetWorldPose()
+ - this->modelPose);
+
+ // visuals
+ for (auto const &it : _link->visuals)
+ {
+ rendering::VisualPtr visual = it.first;
+ msgs::Visual visualMsg = it.second;
+ sdf::ElementPtr visualElem = visual->GetSDF()->Clone();
+
+ visualElem->GetElement("transparency")->Set<double>(
+ visualMsg.transparency());
+ newLinkElem->InsertElement(visualElem);
+ }
+
+ // collisions
+ for (auto const &colIt : _link->collisions)
+ {
+ sdf::ElementPtr collisionElem = msgs::CollisionToSDF(colIt.second);
+ newLinkElem->InsertElement(collisionElem);
+ }
+ return newLinkElem;
}
/////////////////////////////////////////////////
void ModelCreator::OnAlignMode(const std::string &_axis,
const std::string &_config, const std::string &_target, bool _preview)
{
- ModelAlign::Instance()->AlignVisuals(this->selectedVisuals, _axis, _config,
+ ModelAlign::Instance()->AlignVisuals(this->selectedLinks, _axis, _config,
_target, !_preview);
}
/////////////////////////////////////////////////
void ModelCreator::DeselectAll()
{
- if (!this->selectedVisuals.empty())
+ while (!this->selectedLinks.empty())
+ {
+ rendering::VisualPtr vis = this->selectedLinks[0];
+ vis->SetHighlighted(false);
+ this->selectedLinks.erase(this->selectedLinks.begin());
+ model::Events::setSelectedLink(vis->GetName(), false);
+ }
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::SetSelected(const std::string &_name, const bool _selected)
+{
+ auto it = this->allLinks.find(_name);
+ if (it == this->allLinks.end())
+ return;
+
+ this->SetSelected((*it).second->linkVisual, _selected);
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::SetSelected(rendering::VisualPtr _linkVis,
+ const bool _selected)
+{
+ if (!_linkVis)
+ return;
+
+ _linkVis->SetHighlighted(_selected);
+ auto it = std::find(this->selectedLinks.begin(),
+ this->selectedLinks.end(), _linkVis);
+ if (_selected)
+ {
+ if (it == this->selectedLinks.end())
+ {
+ this->selectedLinks.push_back(_linkVis);
+ model::Events::setSelectedLink(_linkVis->GetName(), _selected);
+ }
+ }
+ else
{
- for (unsigned int i = 0; i < this->selectedVisuals.size(); ++i)
+ if (it != this->selectedLinks.end())
{
- this->selectedVisuals[i]->SetHighlighted(false);
+ this->selectedLinks.erase(it);
+ model::Events::setSelectedLink(_linkVis->GetName(), _selected);
}
- this->selectedVisuals.clear();
}
+ g_copyAct->setEnabled(!this->selectedLinks.empty());
+ g_alignAct->setEnabled(this->selectedLinks.size() > 1);
}
/////////////////////////////////////////////////
@@ -1066,24 +1820,22 @@ void ModelCreator::OnManipMode(const std::string &_mode)
this->manipMode = _mode;
- if (!this->selectedVisuals.empty())
+ if (!this->selectedLinks.empty())
{
ModelManipulator::Instance()->SetAttachedVisual(
- this->selectedVisuals.back());
+ this->selectedLinks.back());
}
ModelManipulator::Instance()->SetManipulationMode(_mode);
ModelSnap::Instance()->Reset();
// deselect 0 to n-1 models.
- if (this->selectedVisuals.size() > 1)
+ if (this->selectedLinks.size() > 1)
{
- for (std::vector<rendering::VisualPtr>::iterator it
- = this->selectedVisuals.begin(); it != --this->selectedVisuals.end();)
- {
- (*it)->SetHighlighted(false);
- it = this->selectedVisuals.erase(it);
- }
+ rendering::VisualPtr link =
+ this->selectedLinks[this->selectedLinks.size()-1];
+ this->DeselectAll();
+ this->SetSelected(link, true);
}
}
@@ -1093,3 +1845,104 @@ void ModelCreator::OnSetSelectedEntity(const std::string &/*_name*/,
{
this->DeselectAll();
}
+
+/////////////////////////////////////////////////
+void ModelCreator::OnSetSelectedLink(const std::string &_name,
+ const bool _selected)
+{
+ this->SetSelected(_name, _selected);
+}
+
+
+/////////////////////////////////////////////////
+void ModelCreator::ModelChanged()
+{
+ if (this->currentSaveState != NEVER_SAVED)
+ this->currentSaveState = UNSAVED_CHANGES;
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::Update()
+{
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+
+ // Check if any links have been moved or resized and trigger ModelChanged
+ for (auto &linksIt : this->allLinks)
+ {
+ LinkData *link = linksIt.second;
+ if (link->GetPose() != link->linkVisual->GetPose())
+ {
+ link->SetPose(link->linkVisual->GetWorldPose() - this->modelPose);
+ this->ModelChanged();
+ }
+ for (auto &scaleIt : this->linkScaleUpdate)
+ {
+ if (link->linkVisual->GetName() == scaleIt.first)
+ link->SetScale(scaleIt.second);
+ }
+ if (!this->linkScaleUpdate.empty())
+ this->ModelChanged();
+ this->linkScaleUpdate.clear();
+ }
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::OnEntityScaleChanged(const std::string &_name,
+ const math::Vector3 &_scale)
+{
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+ for (auto linksIt : this->allLinks)
+ {
+ if (_name == linksIt.first ||
+ _name.find(linksIt.first) != std::string::npos)
+ {
+ this->linkScaleUpdate[linksIt.first] = _scale;
+ break;
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::SetModelVisible(const std::string &_name, bool _visible)
+{
+ rendering::ScenePtr scene = gui::get_active_camera()->GetScene();
+ rendering::VisualPtr visual = scene->GetVisual(_name);
+ if (!visual)
+ return;
+
+ this->SetModelVisible(visual, _visible);
+
+ if (_visible)
+ visual->SetHighlighted(false);
+}
+
+/////////////////////////////////////////////////
+void ModelCreator::SetModelVisible(rendering::VisualPtr _visual, bool _visible)
+{
+ if (!_visual)
+ return;
+
+ for (unsigned int i = 0; i < _visual->GetChildCount(); ++i)
+ this->SetModelVisible(_visual->GetChild(i), _visible);
+
+ if (!_visible)
+ {
+ // store original visibility
+ this->serverModelVisible[_visual->GetId()] = _visual->GetVisible();
+ _visual->SetVisible(_visible);
+ }
+ else
+ {
+ // restore original visibility
+ auto it = this->serverModelVisible.find(_visual->GetId());
+ if (it != this->serverModelVisible.end())
+ {
+ _visual->SetVisible(it->second, false);
+ }
+ }
+}
+/////////////////////////////////////////////////
+ModelCreator::SaveState ModelCreator::GetCurrentSaveState() const
+{
+ return this->currentSaveState;
+}
diff --git a/gazebo/gui/model/ModelCreator.hh b/gazebo/gui/model/ModelCreator.hh
index a245657..41d788e 100644
--- a/gazebo/gui/model/ModelCreator.hh
+++ b/gazebo/gui/model/ModelCreator.hh
@@ -17,20 +17,28 @@
#ifndef _MODEL_CREATOR_HH_
#define _MODEL_CREATOR_HH_
-#include <boost/unordered/unordered_map.hpp>
#include <sdf/sdf.hh>
#include <list>
+#include <map>
#include <string>
#include <vector>
#include "gazebo/common/KeyEvent.hh"
-#include "gazebo/gui/qt.h"
-#include "gazebo/gui/model/JointMaker.hh"
+#include "gazebo/common/MouseEvent.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/transport/TransportTypes.hh"
+#include "gazebo/rendering/Visual.hh"
+#include "gazebo/gui/model/LinkInspector.hh"
+#include "gazebo/gui/qt.h"
+
#include "gazebo/util/system.hh"
+namespace boost
+{
+ class recursive_mutex;
+}
+
namespace gazebo
{
namespace msgs
@@ -40,31 +48,49 @@ namespace gazebo
namespace gui
{
- class PartData;
+ class LinkData;
+ class SaveDialog;
+ class JointMaker;
/// \addtogroup gazebo_gui
/// \{
/// \class ModelCreator ModelCreator.hh
- /// \brief Create and manage 3D visuals of a model with parts and joints.
- class GAZEBO_VISIBLE ModelCreator : public QObject
+ /// \brief Create and manage 3D visuals of a model with links and joints.
+ class GZ_GUI_MODEL_VISIBLE ModelCreator : public QObject
{
Q_OBJECT
- /// \enum Joint types
- /// \brief Unique identifiers for joint types that can be created.
- public: enum PartType
+ /// \enum Link types
+ /// \brief Unique identifiers for link types that can be created.
+ public: enum LinkType
{
/// \brief none
- PART_NONE,
+ LINK_NONE,
/// \brief Box
- PART_BOX,
+ LINK_BOX,
/// \brief Sphere
- PART_SPHERE,
+ LINK_SPHERE,
/// \brief Cylinder
- PART_CYLINDER,
- /// \brief Custom
- PART_CUSTOM
+ LINK_CYLINDER,
+ /// \brief Imported 3D mesh
+ LINK_MESH,
+ /// \brief Extruded polyline
+ LINK_POLYLINE
+ };
+
+ /// \enum SaveState
+ /// \brief Save states for the model editor.
+ public: enum SaveState
+ {
+ // NEVER_SAVED: The model has never been saved.
+ NEVER_SAVED,
+
+ // ALL_SAVED: All changes have been saved.
+ ALL_SAVED,
+
+ // UNSAVED_CHANGES: Has been saved before, but has unsaved changes.
+ UNSAVED_CHANGES
};
/// \brief Constructor
@@ -81,9 +107,47 @@ namespace gazebo
/// \return Name of model.
public: std::string GetModelName() const;
+ /// \brief Set save state upon a change to the model.
+ public: void ModelChanged();
+
+ /// \brief Callback for newing the model.
+ private: void OnNew();
+
+ /// \brief Helper function to manage writing files to disk.
+ public: void SaveModelFiles();
+
+ /// \brief Callback for saving the model.
+ /// \return True if the user chose to save, false if the user cancelled.
+ private: bool OnSave();
+
+ /// \brief Callback for selecting a folder and saving the model.
+ /// \return True if the user chose to save, false if the user cancelled.
+ private: bool OnSaveAs();
+
+ /// \brief Callback for when the name is changed through the Palette.
+ /// \param[in] _modelName The newly entered model name.
+ private: void OnNameChanged(const std::string &_modelName);
+
+ /// \brief Callback received when exiting the editor mode.
+ private: void OnExit();
+
+ /// \brief Update callback on PreRender.
+ private: void Update();
+
/// \brief Finish the model and create the entity on the gzserver.
public: void FinishModel();
+ /// \brief Add a link to the model.
+ /// \param[in] _type Type of link to add: box, cylinder, or sphere.
+ /// \param[in] _size Size of the link.
+ /// \param[in] _pose Pose of the link.
+ /// \param[in] _samples Number of samples for polyline.
+ /// \return Name of the link that has been added.
+ public: std::string AddShape(LinkType _type,
+ const math::Vector3 &_size = math::Vector3::One,
+ const math::Pose &_pose = math::Pose::Zero,
+ const std::string &_uri = "", unsigned int _samples = 5);
+
/// \brief Add a box to the model.
/// \param[in] _size Size of the box.
/// \param[in] _pose Pose of the box.
@@ -107,10 +171,10 @@ namespace gazebo
public: std::string AddCylinder(double _radius = 0.5,
double _length = 1.0, const math::Pose &_pose = math::Pose::Zero);
- /// \brief Add a custom part to the model
- /// \param[in] _name Name of the custom part.
- /// \param[in] _scale Scale of the custom part.
- /// \param[in] _pose Pose of the custom part.
+ /// \brief Add a custom link to the model
+ /// \param[in] _name Name of the custom link.
+ /// \param[in] _scale Scale of the custom link.
+ /// \param[in] _pose Pose of the custom link.
/// \return Name of the custom that has been added.
public: std::string AddCustom(const std::string &_name,
const math::Vector3 &_scale = math::Vector3::One,
@@ -120,9 +184,9 @@ namespace gazebo
/// \param[in] _type Type of joint to add.
public: void AddJoint(const std::string &_type);
- /// \brief Remove a part from the model.
- /// \param[in] _partName Name of the part to remove
- public: void RemovePart(const std::string &_partName);
+ /// \brief Remove a link from the model.
+ /// \param[in] _linkName Name of the link to remove
+ public: void RemoveLink(const std::string &_linkName);
/// \brief Set the model to be static
/// \param[in] _static True to make the model static.
@@ -132,31 +196,56 @@ namespace gazebo
/// \param[in] _auto True to allow the model to auto disable.
public: void SetAutoDisable(bool _auto);
- /// \brief Save model to SDF format.
- /// \param[in] _savePath Path to save the SDF to.
- public: void SaveToSDF(const std::string &_savePath);
-
/// \brief Reset the model creator and the SDF.
public: void Reset();
- /// \brief Stop the process of adding a part or joint to the model.
+ /// \brief Stop the process of adding a link or joint to the model.
public: void Stop();
/// \brief Get joint maker
/// \return Joint maker
public: JointMaker *GetJointMaker() const;
- /// \brief Add a part to the model
- /// \param[in] _type Type of part to be added
- public: void AddPart(PartType _type);
+ /// \brief Set the select state of a link.
+ /// \param[in] _name Name of the link.
+ /// \param[in] _selected True to select the link.
+ public: void SetSelected(const std::string &_name, const bool selected);
+
+ /// \brief Set the select state of a link visual.
+ /// \param[in] _linkVis Pointer to the link visual.
+ /// \param[in] _selected True to select the link.
+ public: void SetSelected(rendering::VisualPtr _linkVis,
+ const bool selected);
- /// \brief Generate the SDF from model part and joint visuals.
+ /// \brief Get current save state.
+ /// \return Current save state.
+ public: enum SaveState GetCurrentSaveState() const;
+
+ /// \brief Add a link to the model
+ /// \param[in] _type Type of link to be added
+ public: void AddLink(LinkType _type);
+
+ /// \brief Generate the SDF from model link and joint visuals.
public: void GenerateSDF();
+ /// \brief Helper function to generate link sdf from link data.
+ /// \param[in] _link Link data used to generate the sdf.
+ /// \return SDF element describing the link.
+ private: sdf::ElementPtr GenerateLinkSDF(LinkData *_link);
+
+ /// \brief Internal helper function to remove a link without removing
+ /// the joints.
+ /// \param[in] _linkName Name of the link to remove
+ private: void RemoveLinkImpl(const std::string &_linkName);
+
/// \brief QT callback when entering model edit mode
/// \param[in] _checked True if the menu item is checked
private slots: void OnEdit(bool _checked);
+ /// \brief QT callback when there's a request to edit an existing model.
+ /// \param[in] _modelName Name of model to be edited.
+ private slots: void OnEditModel(const std::string &_modelName);
+
/// \brief Qt callback when the copy action is triggered.
private slots: void OnCopy();
@@ -194,16 +283,36 @@ namespace gazebo
/// \brief Callback when an entity is selected.
/// \param[in] _name Name of entity.
- /// \param[in] _mode Select model
+ /// \param[in] _mode Select mode
private: void OnSetSelectedEntity(const std::string &_name,
const std::string &_mode);
- /// \brief Create part with default properties from a visual
- /// \param[in] _visual Visual used to create the part.
- private: void CreatePart(const rendering::VisualPtr &_visual);
-
- /// \brief Open the part inspector.
- /// \param[in] _name Name of part.
+ /// \brief Callback when a link is selected.
+ /// \param[in] _name Name of link.
+ /// \param[in] _selected True if the link is selected, false if
+ /// deselected.
+ private: void OnSetSelectedLink(const std::string &_name,
+ const bool _selected);
+
+ /// \brief Create link with default properties from a visual. This
+ /// function creates a link that will become the parent of the
+ /// input visual. A collision visual with the same geometry as the input
+ /// visual will also be added to the link.
+ /// \param[in] _visual Visual used to create the link.
+ private: void CreateLink(const rendering::VisualPtr &_visual);
+
+ /// \brief Clone an existing link.
+ /// \param[in] _linkName Name of link to be cloned.
+ /// \return Cloned link.
+ private: LinkData *CloneLink(const std::string &_linkName);
+
+ /// \brief Create a link from an SDF.
+ /// \param[in] _link SDF element of the link that will be used to
+ /// recreate its visual representation in the model editor.
+ private: void CreateLinkFromSDF(sdf::ElementPtr _linkElem);
+
+ /// \brief Open the link inspector.
+ /// \param[in] _name Name of link.
private: void OpenInspector(const std::string &_name);
// Documentation inherited
@@ -216,9 +325,10 @@ namespace gazebo
/// \return Name of the model created.
private: std::string CreateModel();
- /// \brief Get a template SDF string of a simple model.
- /// \return Template SDF string of a simple model.
- private: std::string GetTemplateSDFString();
+ /// \brief Load a model SDF file and create visuals in the model editor.
+ /// This is used mainly when editing existing models.
+ /// \param[in] _sdf SDF of a model to be loaded
+ private: void LoadSDF(sdf::ElementPtr _sdf);
/// \brief Callback when a specific alignment configuration is set.
/// \param[in] _axis Axis of alignment: x, y, or z.
@@ -230,15 +340,45 @@ namespace gazebo
const std::string &_config, const std::string &_target,
bool _preview);
- /// \brief Deselect all currently selected visuals.
+ /// \brief Callback when an entity's scale has changed.
+ /// \param[in] _name Name of entity.
+ /// \param[in] _scale New scale.
+ private: void OnEntityScaleChanged(const std::string &_name,
+ const math::Vector3 &_scale);
+
+ /// \brief Deselect all currently selected link visuals.
private: void DeselectAll();
+ /// \brief Set visibilty of a visual recursively while storing their
+ /// original values
+ /// \param[in] _name Name of visual.
+ /// \param[in] _visible True to set the visual to be visible.
+ private: void SetModelVisible(const std::string &_name, bool _visible);
+
+ /// \brief Set visibilty of a visual recursively while storing their
+ /// original values
+ /// \param[in] _visual Pointer to the visual.
+ /// \param[in] _visible True to set the visual to be visible.
+ private: void SetModelVisible(rendering::VisualPtr _visual,
+ bool _visible);
+
+ /// \brief Show a link's context menu
+ /// \param[in] _link Name of link the context menu is associated with.
+ private: void ShowContextMenu(const std::string &_link);
+
+ /// \brief Qt callback when a delete signal has been emitted. This is
+ /// currently triggered by the context menu via right click.
+ private slots: void OnDelete();
+
/// \brief Qt callback when a delete signal has been emitted.
/// \param[in] _name Name of the entity to delete.
- private slots: void OnDelete(const std::string &_name="");
+ private slots: void OnDelete(const std::string &_name);
+
+ /// \brief Qt Callback to open link inspector
+ private slots: void OnOpenInspector();
- /// \brief Qt signal when the a part has been added.
- Q_SIGNALS: void PartAdded();
+ /// \brief Qt signal when the a link has been added.
+ Q_SIGNALS: void LinkAdded();
/// \brief The model in SDF format.
private: sdf::SDFPtr modelSDF;
@@ -249,8 +389,14 @@ namespace gazebo
/// \brief Name of the model.
private: std::string modelName;
+ /// \brief Folder name, which is the model name without spaces.
+ private: std::string folderName;
+
+ /// \brief Name of the model preview.
+ private: static const std::string previewName;
+
/// \brief The root visual of the model.
- private: rendering::VisualPtr modelVisual;
+ private: rendering::VisualPtr previewVisual;
/// \brief The root visual of the model.
private: rendering::VisualPtr mouseVisual;
@@ -267,29 +413,17 @@ namespace gazebo
/// \brief A list of gui editor events connected to the model creator.
private: std::vector<event::ConnectionPtr> connections;
- /// \brief Counter for the number of boxes in the model.
- private: int boxCounter;
-
- /// \brief Counter for the number of cylinders in the model.
- private: int cylinderCounter;
-
- /// \brief Counter for the number of spheres in the model.
- private: int sphereCounter;
-
- /// \brief Counter for the number of custom parts in the model.
- private: int customCounter;
+ /// \brief Counter for the number of links in the model.
+ private: int linkCounter;
/// \brief Counter for generating a unique model name.
private: int modelCounter;
- /// \brief Transparency value for model being edited.
- private: double editTransparency;
+ /// \brief Type of link being added.
+ private: LinkType addLinkType;
- /// \brief Type of part being added.
- private: PartType addPartType;
-
- /// \brief A map of model part names to and their visuals.
- private: boost::unordered_map<std::string, PartData *> allParts;
+ /// \brief A map of model link names to and their visuals.
+ private: std::map<std::string, LinkData *> allLinks;
/// \brief Transport node
private: transport::NodePtr node;
@@ -308,23 +442,55 @@ namespace gazebo
/// \brief origin of the model.
private: math::Pose origin;
- /// \brief A list of selected visuals.
- private: std::vector<rendering::VisualPtr> selectedVisuals;
+ /// \brief A list of selected link visuals.
+ private: std::vector<rendering::VisualPtr> selectedLinks;
- /// \brief Names of parts copied through g_copyAct
- private: std::vector<std::string> copiedPartNames;
+ /// \brief Names of links copied through g_copyAct
+ private: std::vector<std::string> copiedLinkNames;
/// \brief The last mouse event
private: common::MouseEvent lastMouseEvent;
- /// \brief Part visual that is currently being inspected.
- private: rendering::VisualPtr inspectVis;
+ /// \brief Qt action for opening the link inspector.
+ private: QAction *inspectAct;
+
+ /// \brief Name of link that is currently being inspected.
+ private: std::string inspectName;
/// \brief True if the model editor mode is active.
private: bool active;
/// \brief Current model manipulation mode.
private: std::string manipMode;
+
+ /// \brief Default name of the model.
+ private: static const std::string modelDefaultName;
+
+ /// \brief A dialog with options to save the model.
+ private: SaveDialog *saveDialog;
+
+ /// \brief Store the current save state of the model.
+ private: enum SaveState currentSaveState;
+
+ /// \brief Mutex to protect updates
+ private: boost::recursive_mutex *updateMutex;
+
+ /// \brief A list of link names whose scale has changed externally.
+ private: std::map<std::string, math::Vector3> linkScaleUpdate;
+
+ /// \brief Name of model on the server that is being edited here in the
+ /// model editor.
+ private: std::string serverModelName;
+
+ /// \brief SDF element of the model on the server.
+ private: sdf::ElementPtr serverModelSDF;
+
+ /// \brief A map of all visuals of the model to be edited to their
+ /// visibility.
+ private: std::map<uint32_t, bool> serverModelVisible;
+
+ /// \brief Name of the canonical link in the model
+ private: std::string canonicalLink;
};
/// \}
}
diff --git a/gazebo/gui/model/ModelCreator_TEST.cc b/gazebo/gui/model/ModelCreator_TEST.cc
new file mode 100644
index 0000000..d28e75d
--- /dev/null
+++ b/gazebo/gui/model/ModelCreator_TEST.cc
@@ -0,0 +1,205 @@
+/*
+ * 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/gui/Actions.hh"
+#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/MainWindow.hh"
+#include "gazebo/gui/model/ModelEditorEvents.hh"
+#include "gazebo/gui/model/ModelCreator.hh"
+#include "gazebo/gui/model/ModelCreator_TEST.hh"
+
+using namespace gazebo;
+
+/////////////////////////////////////////////////
+void ModelCreator_TEST::SaveState()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/empty.world");
+
+ // Create the main window.
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+ gazebo::rendering::ScenePtr scene = cam->GetScene();
+ QVERIFY(scene != NULL);
+
+ // Start never saved
+ gui::ModelCreator *modelCreator = new gui::ModelCreator();
+ QCOMPARE(modelCreator->GetCurrentSaveState(), gui::ModelCreator::NEVER_SAVED);
+
+ // Inserting a link and it still is never saved
+ modelCreator->AddShape(gui::ModelCreator::LINK_CYLINDER);
+ gazebo::rendering::VisualPtr cylinder =
+ scene->GetVisual("ModelPreview_0::link_0");
+ QVERIFY(cylinder != NULL);
+ QCOMPARE(modelCreator->GetCurrentSaveState(),
+ gui::ModelCreator::NEVER_SAVED);
+
+ // Save all changes
+ modelCreator->SaveModelFiles();
+ QCOMPARE(modelCreator->GetCurrentSaveState(),
+ gui::ModelCreator::ALL_SAVED);
+
+ // Insert another link to have unsaved changes
+ modelCreator->AddShape(gui::ModelCreator::LINK_BOX);
+ QCOMPARE(modelCreator->GetCurrentSaveState(),
+ gui::ModelCreator::UNSAVED_CHANGES);
+
+ // Save all changes
+ modelCreator->SaveModelFiles();
+ QCOMPARE(modelCreator->GetCurrentSaveState(),
+ gui::ModelCreator::ALL_SAVED);
+
+ // Move a link to have unsaved changes
+ cylinder->SetWorldPose(math::Pose(1, 2, 3, 4, 5, 6));
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+ QCOMPARE(modelCreator->GetCurrentSaveState(),
+ gui::ModelCreator::UNSAVED_CHANGES);
+
+ // Save all changes
+ modelCreator->SaveModelFiles();
+ QCOMPARE(modelCreator->GetCurrentSaveState(),
+ gui::ModelCreator::ALL_SAVED);
+
+ // Remove a link to have unsaved changes
+ modelCreator->RemoveLink(cylinder->GetName());
+ QCOMPARE(modelCreator->GetCurrentSaveState(),
+ gui::ModelCreator::UNSAVED_CHANGES);
+
+ // Save all changes
+ modelCreator->SaveModelFiles();
+ QCOMPARE(modelCreator->GetCurrentSaveState(),
+ gui::ModelCreator::ALL_SAVED);
+
+ delete modelCreator;
+ modelCreator = NULL;
+ mainWindow->close();
+ delete mainWindow;
+ mainWindow = NULL;
+}
+
+/////////////////////////////////////////////////
+void ModelCreator_TEST::Selection()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/empty.world");
+
+ // Create the main window.
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // Get the user camera and scene
+ gazebo::rendering::UserCameraPtr cam = gazebo::gui::get_active_camera();
+ QVERIFY(cam != NULL);
+ gazebo::rendering::ScenePtr scene = cam->GetScene();
+ QVERIFY(scene != NULL);
+
+ // Start never saved
+ gui::ModelCreator *modelCreator = new gui::ModelCreator();
+ QVERIFY(modelCreator);
+
+ // Inserting a few links
+ modelCreator->AddShape(gui::ModelCreator::LINK_CYLINDER);
+ gazebo::rendering::VisualPtr cylinder =
+ scene->GetVisual("ModelPreview_0::link_0");
+ QVERIFY(cylinder != NULL);
+
+ modelCreator->AddShape(gui::ModelCreator::LINK_BOX);
+ gazebo::rendering::VisualPtr box =
+ scene->GetVisual("ModelPreview_0::link_1");
+ QVERIFY(box != NULL);
+
+ modelCreator->AddShape(gui::ModelCreator::LINK_SPHERE);
+ gazebo::rendering::VisualPtr sphere =
+ scene->GetVisual("ModelPreview_0::link_2");
+ QVERIFY(sphere != NULL);
+
+ // verify initial selected state
+ QVERIFY(!cylinder->GetHighlighted());
+ QVERIFY(!box->GetHighlighted());
+ QVERIFY(!sphere->GetHighlighted());
+
+ // select the shapes and verify that they are selected
+ modelCreator->SetSelected(cylinder, true);
+ QVERIFY(cylinder->GetHighlighted());
+
+ modelCreator->SetSelected(box, true);
+ QVERIFY(box->GetHighlighted());
+
+ modelCreator->SetSelected(sphere, true);
+ QVERIFY(sphere->GetHighlighted());
+
+ // deselect and verify
+ modelCreator->SetSelected(cylinder, false);
+ QVERIFY(!cylinder->GetHighlighted());
+
+ modelCreator->SetSelected(box, false);
+ QVERIFY(!box->GetHighlighted());
+
+ modelCreator->SetSelected(sphere, false);
+ QVERIFY(!sphere->GetHighlighted());
+
+ // select one and verify all
+ modelCreator->SetSelected(cylinder, true);
+ QVERIFY(cylinder->GetHighlighted());
+ QVERIFY(!box->GetHighlighted());
+ QVERIFY(!sphere->GetHighlighted());
+
+ delete modelCreator;
+ modelCreator = NULL;
+ mainWindow->close();
+ delete mainWindow;
+ mainWindow = NULL;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(ModelCreator_TEST)
diff --git a/gazebo/gui/model/ModelCreator_TEST.hh b/gazebo/gui/model/ModelCreator_TEST.hh
new file mode 100644
index 0000000..41cb0d1
--- /dev/null
+++ b/gazebo/gui/model/ModelCreator_TEST.hh
@@ -0,0 +1,35 @@
+/*
+ * 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_MODEL_CREATOR_TEST_HH_
+#define _GAZEBO_MODEL_CREATOR_TEST_HH_
+
+#include "gazebo/gui/QTestFixture.hh"
+
+/// \brief A test class for the ModelCreator class.
+class ModelCreator_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Tests changing save states in the model editor
+ private slots: void SaveState();
+
+ /// \brief Tests selecting links in the model editor
+ private slots: void Selection();
+};
+
+#endif
diff --git a/gazebo/gui/model/ModelData.cc b/gazebo/gui/model/ModelData.cc
new file mode 100644
index 0000000..afee0d4
--- /dev/null
+++ b/gazebo/gui/model/ModelData.cc
@@ -0,0 +1,871 @@
+/*
+ * 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.
+ *
+ */
+
+#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/thread/recursive_mutex.hpp>
+
+#include "gazebo/rendering/Material.hh"
+#include "gazebo/rendering/Scene.hh"
+#include "gazebo/rendering/ogre_gazebo.h"
+
+#include "gazebo/gui/model/LinkInspector.hh"
+#include "gazebo/gui/model/VisualConfig.hh"
+#include "gazebo/gui/model/LinkConfig.hh"
+#include "gazebo/gui/model/CollisionConfig.hh"
+
+#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/model/ModelData.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+std::string ModelData::GetTemplateSDFString()
+{
+ std::ostringstream newModelStr;
+ newModelStr << "<sdf version ='" << SDF_VERSION << "'>"
+ << "<model name='template_model'>"
+ << "<pose>0 0 0.0 0 0 0</pose>"
+ << "<link name ='link'>"
+ << "<visual name ='visual'>"
+ << "<pose>0 0 0.0 0 0 0</pose>"
+ << "<geometry>"
+ << "<box>"
+ << "<size>1.0 1.0 1.0</size>"
+ << "</box>"
+ << "</geometry>"
+ << "<material>"
+ << "<lighting>true</lighting>"
+ << "<script>"
+ << "<uri>file://media/materials/scripts/gazebo.material</uri>"
+ << "<name>Gazebo/Grey</name>"
+ << "</script>"
+ << "</material>"
+ << "</visual>"
+ << "</link>"
+ << "<static>true</static>"
+ << "</model>"
+ << "</sdf>";
+
+ return newModelStr.str();
+}
+
+/////////////////////////////////////////////////
+double ModelData::GetEditTransparency()
+{
+ return 0.4;
+}
+
+/////////////////////////////////////////////////
+LinkData::LinkData()
+{
+ this->linkSDF.reset(new sdf::Element);
+ sdf::initFile("link.sdf", this->linkSDF);
+
+ this->scale = math::Vector3::One;
+
+ this->inspector = new LinkInspector();
+ this->inspector->setModal(false);
+ connect(this->inspector, SIGNAL(Applied()), this, SLOT(OnApply()));
+ connect(this->inspector, SIGNAL(Accepted()), this, SLOT(OnAccept()));
+ connect(this->inspector->GetVisualConfig(),
+ SIGNAL(VisualAdded(const std::string &)),
+ this, SLOT(OnAddVisual(const std::string &)));
+
+ connect(this->inspector->GetCollisionConfig(),
+ SIGNAL(CollisionAdded(const std::string &)),
+ this, SLOT(OnAddCollision(const std::string &)));
+
+ connect(this->inspector->GetVisualConfig(),
+ SIGNAL(VisualRemoved(const std::string &)), this,
+ SLOT(OnRemoveVisual(const std::string &)));
+
+ connect(this->inspector->GetCollisionConfig(),
+ SIGNAL(CollisionRemoved(const std::string &)),
+ this, SLOT(OnRemoveCollision(const std::string &)));
+
+ // note the destructor removes this connection with the assumption that it is
+ // the first one in the vector
+ this->connections.push_back(event::Events::ConnectPreRender(
+ boost::bind(&LinkData::Update, this)));
+ this->updateMutex = new boost::recursive_mutex();
+}
+
+/////////////////////////////////////////////////
+LinkData::~LinkData()
+{
+ event::Events::DisconnectPreRender(this->connections[0]);
+ delete this->inspector;
+}
+
+/////////////////////////////////////////////////
+std::string LinkData::GetName() const
+{
+ return this->linkSDF->Get<std::string>("name");
+}
+
+/////////////////////////////////////////////////
+void LinkData::SetName(const std::string &_name)
+{
+ this->linkSDF->GetAttribute("name")->Set(_name);
+ this->inspector->SetName(_name);
+}
+
+/////////////////////////////////////////////////
+math::Pose LinkData::GetPose() const
+{
+ return this->linkSDF->Get<math::Pose>("pose");
+}
+
+/////////////////////////////////////////////////
+void LinkData::SetPose(const math::Pose &_pose)
+{
+ this->linkSDF->GetElement("pose")->Set(_pose);
+
+ LinkConfig *linkConfig = this->inspector->GetLinkConfig();
+ linkConfig->SetPose(_pose);
+}
+
+/////////////////////////////////////////////////
+void LinkData::SetScale(const math::Vector3 &_scale)
+{
+ VisualConfig *visualConfig = this->inspector->GetVisualConfig();
+ for (auto it = this->visuals.begin(); it != this->visuals.end(); ++it)
+ {
+ std::string name = it->first->GetName();
+ std::string linkName = this->linkVisual->GetName();
+ std::string leafName =
+ name.substr(name.find(linkName)+linkName.size()+2);
+ visualConfig->SetGeometry(leafName, it->first->GetScale());
+ }
+
+ CollisionConfig *collisionConfig = this->inspector->GetCollisionConfig();
+ for (auto it = this->collisions.begin(); it != this->collisions.end(); ++it)
+ {
+ std::string name = it->first->GetName();
+ std::string linkName = this->linkVisual->GetName();
+ std::string leafName =
+ name.substr(name.find(linkName)+linkName.size()+2);
+ collisionConfig->SetGeometry(leafName, it->first->GetScale());
+ }
+
+ this->scale = _scale;
+}
+
+/////////////////////////////////////////////////
+math::Vector3 LinkData::GetScale() const
+{
+ return this->scale;
+}
+
+/////////////////////////////////////////////////
+void LinkData::Load(sdf::ElementPtr _sdf)
+{
+ LinkConfig *linkConfig = this->inspector->GetLinkConfig();
+
+ this->SetName(_sdf->Get<std::string>("name"));
+ this->SetPose(_sdf->Get<math::Pose>("pose"));
+
+ msgs::LinkPtr linkMsgPtr(new msgs::Link);
+ if (_sdf->HasElement("inertial"))
+ {
+ sdf::ElementPtr inertialElem = _sdf->GetElement("inertial");
+ this->linkSDF->GetElement("inertial")->Copy(inertialElem);
+
+ msgs::Inertial *inertialMsg = linkMsgPtr->mutable_inertial();
+
+ if (inertialElem->HasElement("mass"))
+ {
+ double mass = inertialElem->Get<double>("mass");
+ inertialMsg->set_mass(mass);
+ }
+
+ if (inertialElem->HasElement("pose"))
+ {
+ ignition::math::Pose3d inertialPose =
+ inertialElem->Get<ignition::math::Pose3d>("pose");
+ msgs::Set(inertialMsg->mutable_pose(), inertialPose);
+ }
+
+ if (inertialElem->HasElement("inertia"))
+ {
+ sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia");
+ inertialMsg->set_ixx(inertiaElem->Get<double>("ixx"));
+ inertialMsg->set_ixy(inertiaElem->Get<double>("ixy"));
+ inertialMsg->set_ixz(inertiaElem->Get<double>("ixz"));
+ inertialMsg->set_iyy(inertiaElem->Get<double>("iyy"));
+ inertialMsg->set_iyz(inertiaElem->Get<double>("iyz"));
+ inertialMsg->set_izz(inertiaElem->Get<double>("izz"));
+ }
+ }
+ if (_sdf->HasElement("self_collide"))
+ {
+ sdf::ElementPtr selfCollideSDF = _sdf->GetElement("self_collide");
+ linkMsgPtr->set_self_collide(selfCollideSDF->Get<bool>(""));
+ this->linkSDF->InsertElement(selfCollideSDF->Clone());
+ }
+ if (_sdf->HasElement("kinematic"))
+ {
+ sdf::ElementPtr kinematicSDF = _sdf->GetElement("kinematic");
+ linkMsgPtr->set_kinematic(kinematicSDF->Get<bool>());
+ this->linkSDF->InsertElement(kinematicSDF->Clone());
+ }
+ if (_sdf->HasElement("must_be_base_link"))
+ {
+ sdf::ElementPtr baseLinkSDF = _sdf->GetElement("must_be_base_link");
+ // TODO link.proto is missing the must_be_base_link field.
+ // linkMsgPtr->set_must_be_base_link(baseLinkSDF->Get<bool>());
+ this->linkSDF->InsertElement(baseLinkSDF->Clone());
+ }
+ if (_sdf->HasElement("velocity_decay"))
+ {
+ sdf::ElementPtr velocityDecaySDF = _sdf->GetElement("velocity_decay");
+ // TODO link.proto is missing the velocity_decay field.
+ // linkMsgPtr->set_velocity_decay(velocityDecaySDF->Get<double>());
+ this->linkSDF->InsertElement(velocityDecaySDF->Clone());
+ }
+ linkConfig->Update(linkMsgPtr);
+
+ if (_sdf->HasElement("sensor"))
+ {
+ sdf::ElementPtr sensorElem = _sdf->GetElement("sensor");
+ while (sensorElem)
+ {
+ this->linkSDF->InsertElement(sensorElem->Clone());
+ sensorElem = sensorElem->GetNextElement("sensor");
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void LinkData::UpdateConfig()
+{
+ // set new geom size if scale has changed.
+ VisualConfig *visualConfig = this->inspector->GetVisualConfig();
+ for (auto &it : this->visuals)
+ {
+ std::string name = it.first->GetName();
+ std::string leafName = name;
+ size_t idx = name.find_last_of("::");
+ if (idx != std::string::npos)
+ leafName = name.substr(idx+1);
+ visualConfig->SetGeometry(leafName, it.first->GetScale(),
+ it.first->GetMeshName());
+
+ msgs::Visual *updateMsg = visualConfig->GetData(leafName);
+ msgs::Visual visualMsg = it.second;
+ updateMsg->clear_scale();
+ msgs::Material *matMsg = updateMsg->mutable_material();
+ // clear empty colors so they are not used by visual updates
+ common::Color emptyColor;
+ if (msgs::Convert(matMsg->ambient()) == emptyColor)
+ matMsg->clear_ambient();
+ if (msgs::Convert(matMsg->diffuse()) == emptyColor)
+ matMsg->clear_diffuse();
+ if (msgs::Convert(matMsg->specular()) == emptyColor)
+ matMsg->clear_specular();
+ if (msgs::Convert(matMsg->emissive()) == emptyColor)
+ matMsg->clear_emissive();
+
+ if (matMsg->has_diffuse())
+ matMsg->mutable_diffuse()->set_a(1.0-updateMsg->transparency());
+
+ visualMsg.CopyFrom(*updateMsg);
+ it.second = visualMsg;
+ }
+ CollisionConfig *collisionConfig = this->inspector->GetCollisionConfig();
+ for (auto &colIt : this->collisions)
+ {
+ std::string name = colIt.first->GetName();
+ std::string leafName = name;
+ size_t idx = name.find_last_of("::");
+ if (idx != std::string::npos)
+ leafName = name.substr(idx+1);
+ collisionConfig->SetGeometry(leafName, colIt.first->GetScale(),
+ colIt.first->GetMeshName());
+
+ msgs::Collision *updateMsg = collisionConfig->GetData(leafName);
+ msgs::Collision collisionMsg = colIt.second;
+ collisionMsg.CopyFrom(*updateMsg);
+ colIt.second = collisionMsg;
+ }
+}
+
+/////////////////////////////////////////////////
+void LinkData::AddVisual(rendering::VisualPtr _visual)
+{
+ VisualConfig *visualConfig = this->inspector->GetVisualConfig();
+ msgs::Visual visualMsg = msgs::VisualFromSDF(_visual->GetSDF());
+
+ this->visuals[_visual] = visualMsg;
+
+ std::string visName = _visual->GetName();
+ std::string leafName = visName;
+ size_t idx = visName.find_last_of("::");
+ if (idx != std::string::npos)
+ leafName = visName.substr(idx+1);
+
+ visualConfig->AddVisual(leafName, &visualMsg);
+}
+
+/////////////////////////////////////////////////
+void LinkData::AddCollision(rendering::VisualPtr _collisionVis)
+{
+ CollisionConfig *collisionConfig = this->inspector->GetCollisionConfig();
+ msgs::Visual visualMsg = msgs::VisualFromSDF(_collisionVis->GetSDF());
+
+ sdf::ElementPtr collisionSDF(new sdf::Element);
+ sdf::initFile("collision.sdf", collisionSDF);
+
+ std::string visName = _collisionVis->GetName();
+ std::string leafName = visName;
+ size_t idx = visName.find_last_of("::");
+ if (idx != std::string::npos)
+ leafName = visName.substr(idx+1);
+
+ msgs::Collision collisionMsg;
+ collisionMsg.set_name(leafName);
+ msgs::Geometry *geomMsg = collisionMsg.mutable_geometry();
+ geomMsg->CopyFrom(visualMsg.geometry());
+ msgs::Pose *poseMsg = collisionMsg.mutable_pose();
+ poseMsg->CopyFrom(visualMsg.pose());
+
+ this->collisions[_collisionVis] = collisionMsg;
+ collisionConfig->AddCollision(leafName, &collisionMsg);
+}
+
+/////////////////////////////////////////////////
+LinkData* LinkData::Clone(const std::string &_newName)
+{
+ LinkData *cloneLink = new LinkData();
+
+ cloneLink->Load(this->linkSDF);
+ cloneLink->SetName(_newName);
+
+ std::string linkVisualName = this->linkVisual->GetName();
+ std::string cloneVisName = _newName;
+ size_t linkIdx = linkVisualName.find("::");
+ if (linkIdx != std::string::npos)
+ cloneVisName = linkVisualName.substr(0, linkIdx+2) + _newName;
+
+ // clone linkVisual;
+ rendering::VisualPtr linkVis(new rendering::Visual(cloneVisName,
+ this->linkVisual->GetParent()));
+ linkVis->Load();
+
+ cloneLink->linkVisual = linkVis;
+
+ for (auto &visIt : this->visuals)
+ {
+ std::string newVisName = visIt.first->GetName();
+ size_t idx = newVisName.find_last_of("::");
+ if (idx != std::string::npos)
+ newVisName = cloneVisName + newVisName.substr(idx-1);
+ else
+ newVisName = cloneVisName + "::" + newVisName;
+
+ rendering::VisualPtr cloneVis =
+ visIt.first->Clone(newVisName, cloneLink->linkVisual);
+
+ // override transparency
+ cloneVis->SetTransparency(visIt.second.transparency());
+ cloneLink->AddVisual(cloneVis);
+ cloneVis->SetTransparency(visIt.second.transparency() *
+ (1-ModelData::GetEditTransparency()-0.1)
+ + ModelData::GetEditTransparency());
+ }
+
+ for (auto &colIt : this->collisions)
+ {
+ std::string newColName = colIt.first->GetName();
+ size_t idx = newColName.find_last_of("::");
+ if (idx != std::string::npos)
+ newColName = cloneVisName + newColName.substr(idx-1);
+ else
+ newColName = cloneVisName + "::" + newColName;
+ rendering::VisualPtr collisionVis = colIt.first->Clone(newColName,
+ cloneLink->linkVisual);
+ collisionVis->SetTransparency(
+ math::clamp(ModelData::GetEditTransparency() * 2.0, 0.0, 0.8));
+ // fix for transparency alpha compositing
+ Ogre::MovableObject *colObj = collisionVis->GetSceneNode()->
+ getAttachedObject(0);
+ colObj->setRenderQueueGroup(colObj->getRenderQueueGroup()+1);
+ cloneLink->AddCollision(collisionVis);
+ }
+ return cloneLink;
+}
+
+/////////////////////////////////////////////////
+void LinkData::OnAccept()
+{
+ if (this->Apply())
+ this->inspector->accept();
+}
+
+/////////////////////////////////////////////////
+void LinkData::OnApply()
+{
+ this->Apply();
+}
+
+/////////////////////////////////////////////////
+bool LinkData::Apply()
+{
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+ LinkConfig *linkConfig = this->inspector->GetLinkConfig();
+
+ this->linkSDF = msgs::LinkToSDF(*linkConfig->GetData(), this->linkSDF);
+ this->linkVisual->SetPose(this->GetPose());
+
+ std::vector<msgs::Visual *> visualUpdateMsgsTemp;
+ std::vector<msgs::Collision *> collisionUpdateMsgsTemp;
+
+ // update visuals
+ if (!this->visuals.empty())
+ {
+ VisualConfig *visualConfig = this->inspector->GetVisualConfig();
+ for (auto &it : this->visuals)
+ {
+ std::string name = it.first->GetName();
+ std::string leafName = name;
+ size_t idx = name.find_last_of("::");
+ if (idx != std::string::npos)
+ leafName = name.substr(idx+1);
+ msgs::Visual *updateMsg = visualConfig->GetData(leafName);
+ if (updateMsg)
+ {
+ msgs::Visual visualMsg = it.second;
+
+ // check if the geometry is valid
+ msgs::Geometry *geomMsg = updateMsg->mutable_geometry();
+
+ // warnings when changing from/to polyline
+ for (auto &vis : this->visuals)
+ {
+ if (vis.second.name() != updateMsg->name())
+ continue;
+
+ if (vis.second.mutable_geometry()->type() != geomMsg->type())
+ {
+ // Changing from polyline, give option to cancel
+ if (vis.second.mutable_geometry()->type() ==
+ msgs::Geometry::POLYLINE)
+ {
+ std::string msg =
+ "Once you change the geometry, you can't go "
+ "back to polyline.\n\n"
+ "Do you wish to continue?\n";
+
+ QMessageBox msgBox(QMessageBox::Warning,
+ QString("Changing polyline geometry"), QString(msg.c_str()));
+
+ QPushButton *cancelButton =
+ msgBox.addButton("Cancel", QMessageBox::RejectRole);
+ QPushButton *saveButton = msgBox.addButton("Ok",
+ QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(saveButton);
+ msgBox.setEscapeButton(cancelButton);
+ msgBox.exec();
+ if (msgBox.clickedButton() != saveButton)
+ return false;
+ }
+ // Changing to polyline: not allowed
+ else if (geomMsg->type() == msgs::Geometry::POLYLINE)
+ {
+ std::string msg =
+ "It's not possible to change into polyline.\n"
+ "Please select another geometry type for ["
+ + leafName + "].";
+
+ QMessageBox::warning(linkConfig, QString(
+ "Invalid geometry conversion"), QString(msg.c_str()),
+ QMessageBox::Ok, QMessageBox::Ok);
+ return false;
+ }
+ }
+ }
+
+ if (geomMsg->type() == msgs::Geometry::MESH)
+ {
+ msgs::MeshGeom *meshGeom = geomMsg->mutable_mesh();
+ QFileInfo info(QString::fromStdString(meshGeom->filename()));
+ if (!info.isFile() || (info.completeSuffix().toLower() != "dae" &&
+ info.completeSuffix().toLower() != "stl"))
+ {
+ std::string msg = "\"" + meshGeom->filename() +
+ "\" is not a valid mesh file.\nPlease select another file for ["
+ + leafName + "].";
+
+ QMessageBox::warning(linkConfig, QString("Invalid Mesh File"),
+ QString(msg.c_str()), QMessageBox::Ok, QMessageBox::Ok);
+ return false;
+ }
+ }
+
+ // update the visualMsg that will be used to generate the sdf.
+ updateMsg->clear_scale();
+ msgs::Material *matMsg = updateMsg->mutable_material();
+ msgs::Material::Script *scriptMsg = matMsg->mutable_script();
+
+ common::Color emptyColor;
+ common::Color matAmbient;
+ common::Color matDiffuse;
+ common::Color matSpecular;
+ common::Color matEmissive;
+ rendering::Material::GetMaterialAsColor(scriptMsg->name(), matAmbient,
+ matDiffuse, matSpecular, matEmissive);
+
+ common::Color ambient = msgs::Convert(matMsg->ambient());
+ common::Color diffuse = msgs::Convert(matMsg->diffuse());
+ common::Color specular = msgs::Convert(matMsg->specular());
+ common::Color emissive = msgs::Convert(matMsg->emissive());
+
+ if (ambient == emptyColor)
+ {
+ matMsg->clear_ambient();
+ ambient = matAmbient;
+ }
+ if (diffuse == emptyColor)
+ {
+ matMsg->clear_diffuse();
+ diffuse = matDiffuse;
+ }
+ if (specular == emptyColor)
+ {
+ matMsg->clear_specular();
+ specular = matSpecular;
+ }
+ if (emissive == emptyColor)
+ {
+ matMsg->clear_emissive();
+ emissive = matEmissive;
+ }
+
+ visualConfig->SetMaterial(leafName, scriptMsg->name(), ambient,
+ diffuse, specular, emissive);
+
+ visualMsg.CopyFrom(*updateMsg);
+ it.second = visualMsg;
+
+ visualUpdateMsgsTemp.push_back(updateMsg);
+ }
+ }
+ }
+
+ // update collisions
+ if (!this->collisions.empty())
+ {
+ CollisionConfig *collisionConfig =
+ this->inspector->GetCollisionConfig();
+ for (auto &it : this->collisions)
+ {
+ std::string name = it.first->GetName();
+ std::string leafName = name;
+ size_t idx = name.find_last_of("::");
+ if (idx != std::string::npos)
+ leafName = name.substr(idx+1);
+ msgs::Collision *updateMsg = collisionConfig->GetData(leafName);
+ if (updateMsg)
+ {
+ msgs::Collision collisionMsg = it.second;
+
+ // check if the geometry is valid
+ msgs::Geometry *geomMsg = updateMsg->mutable_geometry();
+
+ // warnings when changing from/to polyline
+ for (auto &col : this->collisions)
+ {
+ if (col.second.name() != updateMsg->name())
+ continue;
+
+ if (col.second.mutable_geometry()->type() != geomMsg->type())
+ {
+ // Changing from polyline, give option to cancel
+ if (col.second.mutable_geometry()->type() ==
+ msgs::Geometry::POLYLINE)
+ {
+ std::string msg =
+ "Once you change the geometry, you can't go "
+ "back to polyline.\n\n"
+ "Do you wish to continue?\n";
+
+ QMessageBox msgBox(QMessageBox::Warning,
+ QString("Changing polyline geometry"), QString(msg.c_str()));
+
+ QPushButton *cancelButton =
+ msgBox.addButton("Cancel", QMessageBox::RejectRole);
+ QPushButton *saveButton = msgBox.addButton("Ok",
+ QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(saveButton);
+ msgBox.setEscapeButton(cancelButton);
+ msgBox.exec();
+ if (msgBox.clickedButton() != saveButton)
+ return false;
+ }
+ // Changing to polyline: not allowed
+ else if (geomMsg->type() == msgs::Geometry::POLYLINE)
+ {
+ std::string msg =
+ "It's not possible to change into polyline.\n"
+ "Please select another geometry type for ["
+ + leafName + "].";
+
+ QMessageBox::warning(linkConfig, QString(
+ "Invalid geometry conversion"), QString(msg.c_str()),
+ QMessageBox::Ok, QMessageBox::Ok);
+ return false;
+ }
+ }
+ }
+
+ if (geomMsg->type() == msgs::Geometry::MESH)
+ {
+ msgs::MeshGeom *meshGeom = geomMsg->mutable_mesh();
+ QFileInfo info(QString::fromStdString(meshGeom->filename()));
+ if (!info.isFile() || (info.completeSuffix().toLower() != "dae" &&
+ info.completeSuffix().toLower() != "stl"))
+ {
+ std::string msg = "\"" + meshGeom->filename() +
+ "\" is not a valid mesh file.\nPlease select another file for ["
+ + leafName + "].";
+
+ QMessageBox::warning(linkConfig, QString("Invalid Mesh File"),
+ QString(msg.c_str()), QMessageBox::Ok, QMessageBox::Ok);
+ return false;
+ }
+ }
+
+ collisionMsg.CopyFrom(*updateMsg);
+ it.second = collisionMsg;
+
+ collisionUpdateMsgsTemp.push_back(updateMsg);
+ }
+ }
+ }
+
+ // Only send update messages if all visuals and collisions are valid
+ this->visualUpdateMsgs.insert(this->visualUpdateMsgs.end(),
+ visualUpdateMsgsTemp.begin(), visualUpdateMsgsTemp.end());
+ this->collisionUpdateMsgs.insert(this->collisionUpdateMsgs.end(),
+ collisionUpdateMsgsTemp.begin(), collisionUpdateMsgsTemp.end());
+ return true;
+}
+
+/////////////////////////////////////////////////
+void LinkData::OnAddVisual(const std::string &_name)
+{
+ // add a visual when the user adds a visual via the inspector's visual tab
+ VisualConfig *visualConfig = this->inspector->GetVisualConfig();
+
+ std::ostringstream visualName;
+ visualName << this->linkVisual->GetName() << "::" << _name;
+
+ rendering::VisualPtr visVisual;
+ rendering::VisualPtr refVisual;
+ if (!this->visuals.empty())
+ {
+ // add new visual by cloning last instance
+ refVisual = this->visuals.rbegin()->first;
+ visVisual = refVisual->Clone(visualName.str(), this->linkVisual);
+ }
+ else
+ {
+ // create new visual based on sdf template (box)
+ sdf::SDFPtr modelTemplateSDF(new sdf::SDF);
+ modelTemplateSDF->SetFromString(
+ ModelData::GetTemplateSDFString());
+
+ visVisual.reset(new rendering::Visual(visualName.str(),
+ this->linkVisual));
+ sdf::ElementPtr visualElem = modelTemplateSDF->Root()
+ ->GetElement("model")->GetElement("link")->GetElement("visual");
+ visVisual->Load(visualElem);
+ }
+
+ msgs::Visual visualMsg = msgs::VisualFromSDF(visVisual->GetSDF());
+ // store the correct transparency setting
+ if (refVisual)
+ visualMsg.set_transparency(this->visuals[refVisual].transparency());
+
+ msgs::VisualPtr visualMsgPtr(new msgs::Visual);
+ visualMsgPtr->CopyFrom(visualMsg);
+ visualConfig->UpdateVisual(_name, visualMsgPtr);
+ this->visuals[visVisual] = visualMsg;
+ visVisual->SetTransparency(visualMsg.transparency() *
+ (1-ModelData::GetEditTransparency()-0.1)
+ + ModelData::GetEditTransparency());
+}
+
+/////////////////////////////////////////////////
+void LinkData::OnAddCollision(const std::string &_name)
+{
+ // add a collision when the user adds a collision via the inspector's
+ // collision tab
+ CollisionConfig *collisionConfig = this->inspector->GetCollisionConfig();
+
+ std::stringstream collisionName;
+ collisionName << this->linkVisual->GetName() << "::" << _name;
+
+ rendering::VisualPtr collisionVis;
+ if (!this->collisions.empty())
+ {
+ // add new collision by cloning last instance
+ collisionVis = this->collisions.rbegin()->first->Clone(collisionName.str(),
+ this->linkVisual);
+ }
+ else
+ {
+ // create new collision based on sdf template (box)
+ sdf::SDFPtr modelTemplateSDF(new sdf::SDF);
+ modelTemplateSDF->SetFromString(
+ ModelData::GetTemplateSDFString());
+
+ collisionVis.reset(new rendering::Visual(collisionName.str(),
+ this->linkVisual));
+ sdf::ElementPtr collisionElem = modelTemplateSDF->Root()
+ ->GetElement("model")->GetElement("link")->GetElement("visual");
+ collisionVis->Load(collisionElem);
+ collisionVis->SetMaterial("Gazebo/Orange");
+ }
+
+ msgs::Visual visualMsg = msgs::VisualFromSDF(collisionVis->GetSDF());
+ msgs::Collision collisionMsg;
+ collisionMsg.set_name(_name);
+ msgs::Geometry *geomMsg = collisionMsg.mutable_geometry();
+ geomMsg->CopyFrom(visualMsg.geometry());
+
+ msgs::CollisionPtr collisionMsgPtr(new msgs::Collision);
+ collisionMsgPtr->CopyFrom(collisionMsg);
+ collisionConfig->UpdateCollision(_name, collisionMsgPtr);
+ this->collisions[collisionVis] = collisionMsg;
+
+ collisionVis->SetTransparency(
+ math::clamp(ModelData::GetEditTransparency() * 2.0, 0.0, 0.8));
+
+ // fix for transparency alpha compositing
+ Ogre::MovableObject *colObj = collisionVis->GetSceneNode()->
+ getAttachedObject(0);
+ colObj->setRenderQueueGroup(colObj->getRenderQueueGroup()+1);
+}
+
+/////////////////////////////////////////////////
+void LinkData::OnRemoveVisual(const std::string &_name)
+{
+ // find and remove visual when the user removes it in the
+ // inspector's visual tab
+ std::ostringstream name;
+ name << this->linkVisual->GetName() << "::" << _name;
+ std::string visualName = name.str();
+
+ for (auto it = this->visuals.begin(); it != this->visuals.end(); ++it)
+ {
+ if (visualName == it->first->GetName())
+ {
+ this->linkVisual->DetachVisual(it->first);
+ this->linkVisual->GetScene()->RemoveVisual(it->first);
+ this->visuals.erase(it);
+ break;
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void LinkData::OnRemoveCollision(const std::string &_name)
+{
+ // find and remove collision visual when the user removes it in the
+ // inspector's collision tab
+ std::ostringstream name;
+ name << this->linkVisual->GetName() << "::" << _name;
+ std::string collisionName = name.str();
+
+ for (auto it = this->collisions.begin(); it != this->collisions.end(); ++it)
+ {
+ if (collisionName == it->first->GetName())
+ {
+ this->linkVisual->DetachVisual(it->first);
+ this->linkVisual->GetScene()->RemoveVisual(it->first);
+ this->collisions.erase(it);
+ break;
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void LinkData::Update()
+{
+ boost::recursive_mutex::scoped_lock lock(*this->updateMutex);
+
+ while (!this->visualUpdateMsgs.empty())
+ {
+ boost::shared_ptr<gazebo::msgs::Visual> updateMsgPtr;
+ updateMsgPtr.reset(new msgs::Visual);
+ updateMsgPtr->CopyFrom(*this->visualUpdateMsgs.front());
+
+ this->visualUpdateMsgs.erase(this->visualUpdateMsgs.begin());
+ for (auto &it : this->visuals)
+ {
+ if (it.second.name() == updateMsgPtr->name())
+ {
+ // make visual semi-transparent here
+ // but generated sdf will use the correct transparency value
+ it.first->UpdateFromMsg(updateMsgPtr);
+ it.first->SetTransparency(updateMsgPtr->transparency() *
+ (1-ModelData::GetEditTransparency()-0.1)
+ + ModelData::GetEditTransparency());
+ break;
+ }
+ }
+ }
+
+ while (!this->collisionUpdateMsgs.empty())
+ {
+ msgs::Collision collisionMsg = *this->collisionUpdateMsgs.front();
+ this->collisionUpdateMsgs.erase(this->collisionUpdateMsgs.begin());
+ for (auto &it : this->collisions)
+ {
+ if (it.second.name() == collisionMsg.name())
+ {
+ msgs::Visual collisionVisMsg;
+ msgs::Geometry *geomMsg = collisionVisMsg.mutable_geometry();
+ geomMsg->CopyFrom(collisionMsg.geometry());
+ msgs::Pose *poseMsg = collisionVisMsg.mutable_pose();
+ poseMsg->CopyFrom(collisionMsg.pose());
+
+ boost::shared_ptr<gazebo::msgs::Visual> updateMsgPtr;
+ updateMsgPtr.reset(new msgs::Visual);
+ updateMsgPtr->CopyFrom(collisionVisMsg);
+ std::string origGeomType = it.first->GetGeometryType();
+ it.first->UpdateFromMsg(updateMsgPtr);
+
+ // fix for transparency alpha compositing
+ if (it.first->GetGeometryType() != origGeomType)
+ {
+ Ogre::MovableObject *colObj = it.first->GetSceneNode()->
+ getAttachedObject(0);
+ colObj->setRenderQueueGroup(colObj->getRenderQueueGroup()+1);
+ }
+ break;
+ }
+ }
+ }
+}
diff --git a/gazebo/gui/model/ModelData.hh b/gazebo/gui/model/ModelData.hh
index 18380b6..ee951e7 100644
--- a/gazebo/gui/model/ModelData.hh
+++ b/gazebo/gui/model/ModelData.hh
@@ -17,44 +17,156 @@
#ifndef _MODEL_DATA_HH_
#define _MODEL_DATA_HH_
+#include <map>
#include <string>
#include <vector>
#include "gazebo/rendering/Visual.hh"
-#include "gazebo/physics/PhysicsTypes.hh"
+#include "gazebo/gui/model/LinkInspector.hh"
-#include "gazebo/gui/qt.h"
+namespace boost
+{
+ class recursive_mutex;
+}
+
+namespace boost
+{
+ class recursive_mutex;
+}
namespace gazebo
{
namespace gui
{
- /// \class PartData PartData.hh
- /// \brief Helper class to store part data
- class PartData : public QObject
+ class LinkInspector;
+
+ class ModelData
+ {
+ /// \brief Get a template SDF string of a simple model.
+ /// \return Template SDF string of a simple model.
+ public: static std::string GetTemplateSDFString();
+
+ /// \brief Get the default transparency setting for entities in model
+ /// editor.
+ public: static double GetEditTransparency();
+ };
+
+ /// \class LinkData LinkData.hh
+ /// \brief Helper class to store link data
+ class LinkData : public QObject
{
Q_OBJECT
- /// \brief Name of part.
- public: std::string name;
+ /// \brief Constructor
+ public: LinkData();
+
+ /// \brief Destructor
+ public: ~LinkData();
+
+ /// \brief Get the name of the link.
+ /// \return Name of link.
+ public: std::string GetName() const;
+
+ /// \brief Set the name of the link.
+ /// \param[in] _name Name of link.
+ public: void SetName(const std::string &_name);
+
+ /// \brief Get the pose of the link.
+ /// \return Pose of link.
+ public: math::Pose GetPose() const;
+
+ /// \brief Set the pose of the link.
+ /// \param[in] _pose Pose of link.
+ public: void SetPose(const math::Pose &_pose);
+
+ /// \brief Load the link with data from SDF.
+ /// \param[in] _sdf Link SDF element.
+ public: void Load(sdf::ElementPtr _sdf);
+
+ /// \brief Get the scale of the link.
+ /// \return Scale of link.
+ public: math::Vector3 GetScale() const;
+
+ /// \brief Set the scale of the link.
+ /// \param[in] _scale Scale of link.
+ public: void SetScale(const math::Vector3 &_scale);
+
+ /// \brief Add a visual to the link.
+ /// \param[in] _visual Visual to be added.
+ public: void AddVisual(rendering::VisualPtr _visual);
+
+ /// \brief Add a collision to the link.
+ /// \param[in] _collisionVis Visual representing the collision.
+ public: void AddCollision(rendering::VisualPtr _collisionVis);
+
+ /// \brief Update the inspector widget if necessary.
+ public: void UpdateConfig();
+
+ /// \brief Clone the link data.
+ /// \param[in] _newName Name to give to the cloned link.
+ /// \return A clone of this link data.
+ public: LinkData *Clone(const std::string &_newName);
+
+ /// \brief Update callback on PreRender.
+ private: void Update();
+
+ /// \brief Apply inspector configurations.
+ /// \return True if successful.
+ private: bool Apply();
+
+ /// \brief Qt Callback when link inspector configurations are to be
+ /// applied and inspector should be closed.
+ private slots: void OnAccept();
+
+ /// \brief Qt Callback when link inspector configurations are to be
+ /// applied.
+ private slots: void OnApply();
+
+ /// \brief Qt callback when a new visual is to be added.
+ /// \param[in] _name Name of visual.
+ private slots: void OnAddVisual(const std::string &_name);
+
+ /// \brief Qt callback when a new collision is to be added.
+ /// \param[in] _name Name of collision.
+ private slots: void OnAddCollision(const std::string &_name);
+
+ /// \brief Qt callback when a visual is to be removed.
+ /// \param[in] _name Name of visual.
+ private slots: void OnRemoveVisual(const std::string &_name);
+
+ /// \brief Qt callback when a collision is to be removed.
+ /// \param[in] _name Name of collision.
+ private slots: void OnRemoveCollision(const std::string &_name);
+
+ /// \brief All the event connections.
+ private: std::vector<event::ConnectionPtr> connections;
+
+ /// \brief Mutex to protect visual and collision update messages.
+ private: boost::recursive_mutex *updateMutex;
+
+ /// \brief SDF representing the link data.
+ public: sdf::ElementPtr linkSDF;
+
+ /// \brief Scale of link.
+ public: math::Vector3 scale;
- /// \brief True to enable gravity on part.
- public: bool gravity;
+ /// \brief Visual representing this link.
+ public: rendering::VisualPtr linkVisual;
- /// \brief True to allow self collision.
- public: bool selfCollide;
+ /// \brief Visuals of the link.
+ public: std::map<rendering::VisualPtr, msgs::Visual> visuals;
- /// \brief True to make part kinematic.
- public: bool kinematic;
+ /// \brief Msgs for updating visuals.
+ public: std::vector<msgs::Visual *> visualUpdateMsgs;
- /// \brief Pose of part.
- public: math::Pose pose;
+ /// \brief Msgs for updating collision visuals.
+ public: std::vector<msgs::Collision *> collisionUpdateMsgs;
- /// \brief Visual representing this part.
- public: rendering::VisualPtr partVisual;
+ /// \brief Collisions of the link.
+ public: std::map<rendering::VisualPtr, msgs::Collision> collisions;
- /// \brief Visuals of the part.
- public: std::vector<rendering::VisualPtr> visuals;
+ /// \brief Inspector for configuring link properties.
+ public: LinkInspector *inspector;
};
}
}
diff --git a/gazebo/gui/model/ModelEditor.cc b/gazebo/gui/model/ModelEditor.cc
index 6038c4d..2eae41a 100644
--- a/gazebo/gui/model/ModelEditor.cc
+++ b/gazebo/gui/model/ModelEditor.cc
@@ -17,8 +17,10 @@
#include <string>
+#include "gazebo/gazebo_config.h"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Events.hh"
+#include "gazebo/common/Assert.hh"
#include "gazebo/gui/qt.h"
#include "gazebo/gui/Actions.hh"
@@ -29,19 +31,87 @@
#include "gazebo/gui/model/ModelEditorEvents.hh"
#include "gazebo/gui/model/ModelCreator.hh"
#include "gazebo/gui/model/JointMaker.hh"
+#include "gazebo/gui/model/ModelEditorPrivate.hh"
#include "gazebo/gui/model/ModelEditor.hh"
+#ifdef HAVE_GRAPHVIZ
+#include "gazebo/gui/model/SchematicViewWidget.hh"
+#endif
+
using namespace gazebo;
using namespace gui;
/////////////////////////////////////////////////
ModelEditor::ModelEditor(MainWindow *_mainWindow)
- : Editor(_mainWindow)
+ : Editor(_mainWindow), dataPtr(new ModelEditorPrivate)
{
- this->active = false;
+ this->dataPtr->active = false;
// Create the model editor tab
- this->modelPalette = new ModelEditorPalette(_mainWindow);
- this->Init("modelEditorTab", "Model Editor", this->modelPalette);
+ this->dataPtr->modelPalette = new ModelEditorPalette(_mainWindow);
+ this->Init("modelEditorTab", "Model Editor", this->dataPtr->modelPalette);
+
+ this->dataPtr->schematicViewAct = NULL;
+ this->dataPtr->svWidget = NULL;
+#ifdef HAVE_GRAPHVIZ
+ RenderWidget *renderWidget = _mainWindow->GetRenderWidget();
+ this->dataPtr->svWidget = new SchematicViewWidget(renderWidget);
+ this->dataPtr->svWidget->setSizePolicy(QSizePolicy::Expanding,
+ QSizePolicy::Expanding);
+ this->dataPtr->svWidget->Init();
+ renderWidget->InsertWidget(0, this->dataPtr->svWidget);
+ this->dataPtr->svWidget->hide();
+
+ this->dataPtr->schematicViewAct =
+ new QAction(tr("Schematic View"), this->mainWindow);
+ this->dataPtr->schematicViewAct->setStatusTip(tr("Sch&ematic View"));
+ this->dataPtr->schematicViewAct->setShortcut(tr("Ctrl+E"));
+ this->dataPtr->schematicViewAct->setCheckable(true);
+ connect(this->dataPtr->schematicViewAct, SIGNAL(toggled(bool)), this,
+ SLOT(OnSchematicView(bool)));
+#endif
+
+ this->dataPtr->newAct = new QAction(tr("&New"), this->mainWindow);
+ this->dataPtr->newAct->setStatusTip(tr("New"));
+ this->dataPtr->newAct->setShortcut(tr("Ctrl+N"));
+ this->dataPtr->newAct->setCheckable(false);
+ connect(this->dataPtr->newAct, SIGNAL(triggered()), this, SLOT(New()));
+
+ this->dataPtr->saveAct = new QAction(tr("&Save"), this->mainWindow);
+ this->dataPtr->saveAct->setStatusTip(tr("Save"));
+ this->dataPtr->saveAct->setShortcut(tr("Ctrl+S"));
+ this->dataPtr->saveAct->setCheckable(false);
+ connect(this->dataPtr->saveAct, SIGNAL(triggered()), this, SLOT(Save()));
+
+ this->dataPtr->saveAsAct = new QAction(tr("&Save As"), this->mainWindow);
+ this->dataPtr->saveAsAct->setStatusTip(tr("Save As"));
+ this->dataPtr->saveAsAct->setShortcut(tr("Ctrl+Shift+S"));
+ this->dataPtr->saveAsAct->setCheckable(false);
+ connect(this->dataPtr->saveAsAct, SIGNAL(triggered()), this, SLOT(SaveAs()));
+
+ this->dataPtr->exitAct = new QAction(tr("E&xit Model Editor"),
+ this->mainWindow);
+ this->dataPtr->exitAct->setStatusTip(tr("Exit Model Editor"));
+ this->dataPtr->exitAct->setShortcut(tr("Ctrl+X"));
+ this->dataPtr->exitAct->setCheckable(false);
+ connect(this->dataPtr->exitAct, SIGNAL(triggered()), this, SLOT(Exit()));
+
+ this->dataPtr->showJointsAct = new QAction(tr("Joints"), this);
+ this->dataPtr->showJointsAct->setStatusTip(tr("Show Joints"));
+ this->dataPtr->showJointsAct->setCheckable(true);
+ this->dataPtr->showJointsAct->setChecked(true);
+ connect(this->dataPtr->showJointsAct, SIGNAL(toggled(bool)),
+ this->dataPtr->modelPalette->GetModelCreator()->GetJointMaker(),
+ SLOT(ShowJoints(bool)));
+
+ // Clone actions from main window
+ this->dataPtr->showToolbarsAct =
+ this->mainWindow->CloneAction(g_showToolbarsAct, this);
+ this->dataPtr->fullScreenAct =
+ this->mainWindow->CloneAction(g_fullScreenAct, this);
+ this->dataPtr->cameraOrthoAct =
+ this->mainWindow->CloneAction(g_cameraOrthoAct, this);
+ this->dataPtr->cameraPerspectiveAct =
+ this->mainWindow->CloneAction(g_cameraPerspectiveAct, this);
connect(g_editModelAct, SIGNAL(toggled(bool)), this, SLOT(OnEdit(bool)));
@@ -50,27 +120,27 @@ ModelEditor::ModelEditor(MainWindow *_mainWindow)
boost::bind(&ModelEditor::OnFinish, this)));
// Add a joint icon to the render widget toolbar
- this->jointAct = new QAction(QIcon(":/images/draw_link.svg"),
+ this->dataPtr->jointAct = new QAction(QIcon(":/images/draw_link.svg"),
tr("Joint"), this);
- this->jointAct->setCheckable(true);
+ this->dataPtr->jointAct->setCheckable(true);
// set up the action group so that only one action is active at one time.
QActionGroup *actionGroup = g_arrowAct->actionGroup();
if (actionGroup)
{
- this->jointAct->setActionGroup(actionGroup);
+ this->dataPtr->jointAct->setActionGroup(actionGroup);
connect(actionGroup, SIGNAL(triggered(QAction *)),
this, SLOT(OnAction(QAction *)));
}
QToolBar *toolbar = this->mainWindow->GetRenderWidget()->GetToolbar();
- this->jointButton = new QToolButton(toolbar);
- this->jointButton->setObjectName("jointToolButton");
- this->jointButton->setCheckable(false);
- this->jointButton->setFixedWidth(15);
- this->jointButton->setPopupMode(QToolButton::InstantPopup);
- QMenu *jointMenu = new QMenu(this->jointButton);
- this->jointButton->setMenu(jointMenu);
+ this->dataPtr->jointButton = new QToolButton(toolbar);
+ this->dataPtr->jointButton->setObjectName("jointToolButton");
+ this->dataPtr->jointButton->setCheckable(false);
+ this->dataPtr->jointButton->setFixedWidth(15);
+ this->dataPtr->jointButton->setPopupMode(QToolButton::InstantPopup);
+ QMenu *jointMenu = new QMenu(this->dataPtr->jointButton);
+ this->dataPtr->jointButton->setMenu(jointMenu);
QAction *revoluteJointAct = new QAction(tr("Revolute"), this);
QAction *revolute2JointAct = new QAction(tr("Revolute2"), this);
QAction *prismaticJointAct = new QAction(tr("Prismatic"), this);
@@ -101,79 +171,176 @@ ModelEditor::ModelEditor(MainWindow *_mainWindow)
jointActionGroup->addAction(screwJointAct);
jointActionGroup->setExclusive(true);
- this->jointSeparatorAct = toolbar->addSeparator();
- toolbar->addAction(this->jointAct);
- this->jointTypeAct = toolbar->addWidget(this->jointButton);
- this->jointAct->setVisible(false);
- this->jointSeparatorAct->setVisible(false);
- this->jointTypeAct->setVisible(false);
+ QAction *toolbarSpacer = toolbar->findChild<QAction *>(
+ "toolbarSpacerAction");
+ GZ_ASSERT(toolbarSpacer, "Toolbar spacer not found");
+
+ this->dataPtr->jointSeparatorAct = toolbar->insertSeparator(toolbarSpacer);
+ toolbar->insertAction(toolbarSpacer, this->dataPtr->jointAct);
+ this->dataPtr->jointTypeAct = toolbar->insertWidget(toolbarSpacer,
+ this->dataPtr->jointButton);
+ this->dataPtr->jointAct->setVisible(false);
+ this->dataPtr->jointSeparatorAct->setVisible(false);
+ this->dataPtr->jointTypeAct->setVisible(false);
- this->signalMapper = new QSignalMapper(this);
- connect(this->signalMapper, SIGNAL(mapped(const QString)),
+ this->dataPtr->signalMapper = new QSignalMapper(this);
+ connect(this->dataPtr->signalMapper, SIGNAL(mapped(const QString)),
this, SLOT(OnAddJoint(const QString)));
- connect(revoluteJointAct, SIGNAL(triggered()), this->signalMapper,
+ connect(revoluteJointAct, SIGNAL(triggered()), this->dataPtr->signalMapper,
SLOT(map()));
- this->signalMapper->setMapping(revoluteJointAct,
+ this->dataPtr->signalMapper->setMapping(revoluteJointAct,
revoluteJointAct->text().toLower());
- connect(revolute2JointAct, SIGNAL(triggered()), this->signalMapper,
+ connect(revolute2JointAct, SIGNAL(triggered()), this->dataPtr->signalMapper,
SLOT(map()));
- this->signalMapper->setMapping(revolute2JointAct,
+ this->dataPtr->signalMapper->setMapping(revolute2JointAct,
revolute2JointAct->text().toLower());
- connect(prismaticJointAct, SIGNAL(triggered()), this->signalMapper,
+ connect(prismaticJointAct, SIGNAL(triggered()), this->dataPtr->signalMapper,
SLOT(map()));
- this->signalMapper->setMapping(prismaticJointAct,
+ this->dataPtr->signalMapper->setMapping(prismaticJointAct,
prismaticJointAct->text().toLower());
- connect(ballJointAct, SIGNAL(triggered()), this->signalMapper,
+ connect(ballJointAct, SIGNAL(triggered()), this->dataPtr->signalMapper,
SLOT(map()));
- this->signalMapper->setMapping(ballJointAct,
+ this->dataPtr->signalMapper->setMapping(ballJointAct,
ballJointAct->text().toLower());
- connect(universalJointAct, SIGNAL(triggered()), this->signalMapper,
+ connect(universalJointAct, SIGNAL(triggered()), this->dataPtr->signalMapper,
SLOT(map()));
- this->signalMapper->setMapping(universalJointAct,
+ this->dataPtr->signalMapper->setMapping(universalJointAct,
universalJointAct->text().toLower());
- connect(screwJointAct, SIGNAL(triggered()), this->signalMapper,
+ connect(screwJointAct, SIGNAL(triggered()), this->dataPtr->signalMapper,
SLOT(map()));
- this->signalMapper->setMapping(screwJointAct,
+ this->dataPtr->signalMapper->setMapping(screwJointAct,
screwJointAct->text().toLower());
// set default joint type.
revoluteJointAct->setChecked(true);
- this->selectedJointType = revoluteJointAct->text().toLower().toStdString();
- connect(this->jointAct, SIGNAL(triggered()), this,
+ this->dataPtr->selectedJointType =
+ revoluteJointAct->text().toLower().toStdString();
+ connect(this->dataPtr->jointAct, SIGNAL(triggered()), this,
SLOT(OnAddSelectedJoint()));
- connect(this->modelPalette->GetModelCreator()->GetJointMaker(),
+ connect(this->dataPtr->modelPalette->GetModelCreator()->GetJointMaker(),
SIGNAL(JointAdded()), this, SLOT(OnJointAdded()));
+
+ this->dataPtr->menuBar = NULL;
}
/////////////////////////////////////////////////
ModelEditor::~ModelEditor()
{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+////////////////////////////////////////////////
+void ModelEditor::AddItemToPalette(QWidget *_item,
+ const std::string &_category)
+{
+ if (!_item)
+ {
+ gzerr << "Item is NULL" << std::endl;
+ return;
+ }
+
+ this->dataPtr->modelPalette->AddItem(_item, _category);
+}
+
+////////////////////////////////////////////////
+void ModelEditor::Save()
+{
+ gui::model::Events::saveModelEditor();
+}
+
+////////////////////////////////////////////////
+void ModelEditor::SaveAs()
+{
+ gui::model::Events::saveAsModelEditor();
+}
+
+/////////////////////////////////////////////////
+void ModelEditor::New()
+{
+ gui::model::Events::newModelEditor();
+}
+
+/////////////////////////////////////////////////
+void ModelEditor::Exit()
+{
+ gui::model::Events::exitModelEditor();
+}
+
+/////////////////////////////////////////////////
+void ModelEditor::OnSchematicView(bool _show)
+{
+ if (!this->dataPtr->svWidget)
+ return;
+
+ if (_show)
+ {
+#ifdef HAVE_GRAPHVIZ
+ this->dataPtr->svWidget->show();
+ }
+ else
+ {
+ this->dataPtr->svWidget->hide();
+#endif
+ }
+}
+
+/////////////////////////////////////////////////
+void ModelEditor::CreateMenus()
+{
+ if (this->dataPtr->menuBar)
+ return;
+
+ this->dataPtr->menuBar = new QMenuBar;
+ this->dataPtr->menuBar->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
+
+ QMenu *fileMenu = this->dataPtr->menuBar->addMenu(tr("&File"));
+ fileMenu->addAction(this->dataPtr->newAct);
+ fileMenu->addAction(this->dataPtr->saveAct);
+ fileMenu->addAction(this->dataPtr->saveAsAct);
+ fileMenu->addAction(this->dataPtr->exitAct);
+
+ QMenu *cameraMenu = this->dataPtr->menuBar->addMenu(tr("&Camera"));
+ cameraMenu->addAction(this->dataPtr->cameraOrthoAct);
+ cameraMenu->addAction(this->dataPtr->cameraPerspectiveAct);
+
+ QMenu *viewMenu = this->dataPtr->menuBar->addMenu(tr("&View"));
+ viewMenu->addAction(this->dataPtr->showJointsAct);
+
+ QMenu *windowMenu = this->dataPtr->menuBar->addMenu(tr("&Window"));
+ if (this->dataPtr->schematicViewAct)
+ {
+ windowMenu->addAction(this->dataPtr->schematicViewAct);
+ windowMenu->addSeparator();
+ }
+ windowMenu->addAction(this->dataPtr->showToolbarsAct);
+ windowMenu->addAction(this->dataPtr->fullScreenAct);
}
/////////////////////////////////////////////////
void ModelEditor::OnAddSelectedJoint()
{
- this->OnAddJoint(tr(this->selectedJointType.c_str()));
+ this->OnAddJoint(tr(this->dataPtr->selectedJointType.c_str()));
}
/////////////////////////////////////////////////
void ModelEditor::OnAddJoint(const QString &_type)
{
std::string type = _type.toStdString();
- this->modelPalette->AddJoint(type);
- this->selectedJointType = type;
- this->jointAct->setChecked(true);
+ this->dataPtr->modelPalette->CreateJoint(type);
+ this->dataPtr->selectedJointType = type;
+ this->dataPtr->jointAct->setChecked(true);
gui::Events::manipMode("joint");
}
/////////////////////////////////////////////////
void ModelEditor::OnJointAdded()
{
- if (this->jointAct->isChecked())
+ if (this->dataPtr->jointAct->isChecked())
{
- this->jointAct->setChecked(false);
+ this->dataPtr->jointAct->setChecked(false);
g_arrowAct->trigger();
}
}
@@ -181,19 +348,39 @@ void ModelEditor::OnJointAdded()
/////////////////////////////////////////////////
void ModelEditor::OnEdit(bool /*_checked*/)
{
- if (!this->active)
+ if (!this->dataPtr->active)
{
+ this->CreateMenus();
+ this->dataPtr->mainWindowPaused = this->mainWindow->IsPaused();
this->mainWindow->Pause();
this->mainWindow->ShowLeftColumnWidget("modelEditorTab");
+ this->mainWindow->ShowMenuBar(this->dataPtr->menuBar);
+ if (!g_showToolbarsAct->isChecked())
+ g_showToolbarsAct->trigger();
+ this->mainWindow->GetRenderWidget()->ShowTimePanel(false);
}
else
{
this->mainWindow->ShowLeftColumnWidget();
- this->mainWindow->Play();
+ this->mainWindow->ShowMenuBar();
+ this->mainWindow->GetRenderWidget()->ShowTimePanel(true);
+ if (!this->dataPtr->mainWindowPaused)
+ this->mainWindow->Play();
}
- this->active = !this->active;
+
+#ifdef HAVE_GRAPHVIZ
+ if (this->dataPtr->svWidget && this->dataPtr->schematicViewAct)
+ {
+ this->dataPtr->svWidget->setVisible(
+ !this->dataPtr->active && this->dataPtr->schematicViewAct->isChecked());
+ if (!this->dataPtr->active)
+ this->dataPtr->svWidget->Reset();
+ }
+#endif
+
+ this->dataPtr->active = !this->dataPtr->active;
this->ToggleToolbar();
- // g_editModelAct->setChecked(this->active);
+ // g_editModelAct->setChecked(this->dataPtr->active);
}
/////////////////////////////////////////////////
@@ -206,14 +393,15 @@ void ModelEditor::OnFinish()
/////////////////////////////////////////////////
void ModelEditor::OnAction(QAction *_action)
{
- if (_action != this->jointAct)
- this->modelPalette->AddJoint("none");
+ if (_action != this->dataPtr->jointAct)
+ this->dataPtr->modelPalette->CreateJoint("none");
}
/////////////////////////////////////////////////
void ModelEditor::ToggleToolbar()
{
- QToolBar *toolbar = this->mainWindow->GetRenderWidget()->GetToolbar();
+ QToolBar *toolbar =
+ this->mainWindow->GetRenderWidget()->GetToolbar();
QList<QAction *> actions = toolbar->actions();
for (int i = 0; i < actions.size(); ++i)
@@ -226,7 +414,9 @@ void ModelEditor::ToggleToolbar()
actions[i] == g_copyAct ||
actions[i] == g_pasteAct ||
actions[i] == g_alignButtonAct ||
- actions[i] == g_snapAct)
+ actions[i] == g_viewAngleButtonAct ||
+ actions[i] == g_snapAct ||
+ actions[i]->objectName() == "toolbarSpacerAction")
{
actions[i]->setVisible(true);
if (i > 0 && actions[i-1]->isSeparator())
@@ -236,11 +426,11 @@ void ModelEditor::ToggleToolbar()
}
else
{
- actions[i]->setVisible(!this->active);
+ actions[i]->setVisible(!this->dataPtr->active);
}
}
- this->jointAct->setVisible(this->active);
- this->jointTypeAct->setVisible(this->active);
- this->jointSeparatorAct->setVisible(this->active);
+ this->dataPtr->jointAct->setVisible(this->dataPtr->active);
+ this->dataPtr->jointTypeAct->setVisible(this->dataPtr->active);
+ this->dataPtr->jointSeparatorAct->setVisible(this->dataPtr->active);
}
diff --git a/gazebo/gui/model/ModelEditor.hh b/gazebo/gui/model/ModelEditor.hh
index 2b4f559..be26122 100644
--- a/gazebo/gui/model/ModelEditor.hh
+++ b/gazebo/gui/model/ModelEditor.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _MODEL_EDITOR_HH_
-#define _MODEL_EDITOR_HH_
+#ifndef _GAZEBO_MODEL_EDITOR_HH_
+#define _GAZEBO_MODEL_EDITOR_HH_
#include <string>
@@ -27,11 +27,11 @@ namespace gazebo
{
namespace gui
{
- class ModelEditorPalette;
+ class ModelEditorPrivate;
/// \class ModelEditor ModelEditor.hh gui/gui.hh
/// \brief Interface to the terrain editor.
- class GAZEBO_VISIBLE ModelEditor : public Editor
+ class GZ_GUI_MODEL_VISIBLE ModelEditor : public Editor
{
Q_OBJECT
@@ -42,6 +42,28 @@ namespace gazebo
/// \brief Destuctor.
public: virtual ~ModelEditor();
+ /// \brief Add an item to palette.
+ /// \param[in] _Item item to add.
+ /// \param[in] _category Category to add the item too.
+ public: void AddItemToPalette(QWidget *_item,
+ const std::string &_category = "");
+
+ /// \brief Qt callback when the model editor's save action is
+ /// triggered.
+ private slots: void Save();
+
+ /// \brief Qt callback when the model editor's save as action is
+ /// triggered.
+ private slots: void SaveAs();
+
+ /// \brief Qt callback when the model editor's new action is
+ /// triggered.
+ private slots: void New();
+
+ /// \brief Qt callback when the model editor's exit action is
+ /// triggered.
+ private slots: void Exit();
+
/// \brief QT callback when entering model edit mode
/// \param[in] _checked True if the menu item is checked
private slots: void OnEdit(bool _checked);
@@ -60,36 +82,22 @@ namespace gazebo
/// \param[in] _action Triggered action.
private slots: void OnAction(QAction *_action);
+ /// \brief Show the schematic view widget
+ /// \param[in] _show True to show the widget, false to hide it.
+ private slots: void OnSchematicView(bool _show);
+
/// \brief Callback when the model has been completed.
private: void OnFinish();
/// \brief Toggle main window's toolbar to display model editor icons.
private: void ToggleToolbar();
- /// \brief Contains all the model editor tools.
- private: ModelEditorPalette *modelPalette;
-
- /// \brief True if model editor is active.
- private: bool active;
-
- /// \brief Qt action for selecting and adding a joint in the model editor.
- private: QAction *jointTypeAct;
-
- /// \brief Qt action for adding a previously selected joint in the
- /// model editor.
- private: QAction *jointAct;
-
- /// \brief A separator for the joint icon.
- private: QAction *jointSeparatorAct;
-
- /// \brief Qt tool button associated with the joint action.
- private: QToolButton *jointButton;
-
- /// \brief Qt signal mapper for mapping add jointsignals.
- private: QSignalMapper *signalMapper;
+ /// \brief Create menus
+ private: void CreateMenus();
- /// \brief Previously selected joint type.
- private: std::string selectedJointType;
+ /// \internal
+ /// \brief Pointer to private data.
+ private: ModelEditorPrivate *dataPtr;
};
}
}
diff --git a/gazebo/gui/model/ModelEditorEvents.cc b/gazebo/gui/model/ModelEditorEvents.cc
index 7fb6c87..081ab12 100644
--- a/gazebo/gui/model/ModelEditorEvents.cc
+++ b/gazebo/gui/model/ModelEditorEvents.cc
@@ -21,3 +21,27 @@ using namespace gazebo;
using namespace gui;
event::EventT<void ()> model::Events::finishModel;
+event::EventT<bool ()> model::Events::saveAsModelEditor;
+event::EventT<bool ()> model::Events::saveModelEditor;
+event::EventT<void ()> model::Events::newModelEditor;
+event::EventT<void ()> model::Events::exitModelEditor;
+event::EventT<void ()> model::Events::modelChanged;
+event::EventT<void (std::string)> model::Events::modelNameChanged;
+event::EventT<void (bool, bool, const math::Pose &, const std::string &)>
+ model::Events::modelPropertiesChanged;
+
+event::EventT<void (std::string)> model::Events::saveModel;
+event::EventT<void ()> model::Events::newModel;
+
+event::EventT<void (std::string)> model::Events::linkInserted;
+event::EventT<void (std::string, std::string, std::string, std::string)>
+ model::Events::jointInserted;
+event::EventT<void (std::string)> model::Events::linkRemoved;
+event::EventT<void (std::string)> model::Events::jointRemoved;
+event::EventT<void (std::string)> model::Events::openLinkInspector;
+event::EventT<void (std::string)> model::Events::openJointInspector;
+event::EventT<void (std::string, std::string)> model::Events::jointNameChanged;
+event::EventT<void (std::string)> model::Events::showLinkContextMenu;
+event::EventT<void (std::string)> model::Events::showJointContextMenu;
+event::EventT<void (std::string, bool)> model::Events::setSelectedLink;
+event::EventT<void (std::string, bool)> model::Events::setSelectedJoint;
diff --git a/gazebo/gui/model/ModelEditorEvents.hh b/gazebo/gui/model/ModelEditorEvents.hh
index b8c1e70..179aa18 100644
--- a/gazebo/gui/model/ModelEditorEvents.hh
+++ b/gazebo/gui/model/ModelEditorEvents.hh
@@ -18,6 +18,7 @@
#define _MODEL_EDITOR_EVENTS_HH_
#include <string>
+#include "gazebo/math/Pose.hh"
#include "gazebo/common/Event.hh"
#include "gazebo/util/system.hh"
@@ -27,24 +28,359 @@ namespace gazebo
{
namespace model
{
- class GAZEBO_VISIBLE Events
+ class GZ_GUI_MODEL_VISIBLE Events
{
- /// \brief Connect a boost::slot to the finish model signal
+ /// \brief Connect a boost::slot to the finish model signal.
/// \param[in] _subscriber the subscriber to this event
/// \return a connection
public: template<typename T>
- static event::ConnectionPtr
- ConnectFinishModel(T _subscriber)
+ static event::ConnectionPtr ConnectFinishModel(T _subscriber)
{ return finishModel.Connect(_subscriber); }
- /// \brief Disconnect a boost::slot to the finish model signal
+ /// \brief Disconnect a boost::slot to the finish model signal.
/// \param[in] _subscriber the subscriber to this event
public: static void DisconnectFinishModel(
event::ConnectionPtr _subscriber)
{ finishModel.Disconnect(_subscriber); }
+ /// \brief Connect a Gazebo event to the save signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectSaveModelEditor(T _subscriber)
+ { return saveModelEditor.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the save signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectSaveModelEditor(
+ event::ConnectionPtr _subscriber)
+ { saveModelEditor.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the save as signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectSaveAsModelEditor(T _subscriber)
+ { return saveAsModelEditor.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the save as signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectSaveAsModelEditor(
+ event::ConnectionPtr _subscriber)
+ { saveAsModelEditor.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the new signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectNewModelEditor(T _subscriber)
+ { return newModelEditor.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the new signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectNewModelEditor(
+ event::ConnectionPtr _subscriber)
+ { newModelEditor.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the exit signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectExitModelEditor(T _subscriber)
+ { return exitModelEditor.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the exit signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectExitModelEditor(
+ event::ConnectionPtr _subscriber)
+ { exitModelEditor.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the model changed signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectModelChanged(T _subscriber)
+ { return modelChanged.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the model changed signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectModelChanged(
+ event::ConnectionPtr _subscriber)
+ { modelChanged.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the name changed signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectModelNameChanged(T _subscriber)
+ { return modelNameChanged.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the name changed signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectModelNameChanged(
+ event::ConnectionPtr _subscriber)
+ { modelNameChanged.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the model properties changed
+ /// signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr
+ ConnectModelPropertiesChanged(T _subscriber)
+ { return modelPropertiesChanged.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the model properties changed
+ /// signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectModelPropertiesChanged(
+ event::ConnectionPtr _subscriber)
+ { modelPropertiesChanged.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the save model signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectSaveModel(T _subscriber)
+ { return saveModel.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the save model signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectSaveModel(
+ event::ConnectionPtr _subscriber)
+ { saveModel.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the new model signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectNewModel(T _subscriber)
+ { return newModel.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the new model signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectNewModel(
+ event::ConnectionPtr _subscriber)
+ { newModel.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the link inserted signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectLinkInserted(T _subscriber)
+ { return linkInserted.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the link inserted signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectLinkInserted(
+ event::ConnectionPtr _subscriber)
+ { linkInserted.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the joint inserted signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectJointInserted(T _subscriber)
+ { return jointInserted.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the joint inserted signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectJointInserted(
+ event::ConnectionPtr _subscriber)
+ { jointInserted.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the link removed signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectLinkRemoved(T _subscriber)
+ { return linkRemoved.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the link removed signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectLinkRemoved(
+ event::ConnectionPtr _subscriber)
+ { linkRemoved.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the joint removed signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectJointRemoved(T _subscriber)
+ { return jointRemoved.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the joint removed signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectJointRemoved(
+ event::ConnectionPtr _subscriber)
+ { jointRemoved.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the open link inspector signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectOpenLinkInspector(T _subscriber)
+ { return openLinkInspector.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the open link inspector
+ /// signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectOpenLinkInspector(
+ event::ConnectionPtr _subscriber)
+ { openLinkInspector.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the open joint inspector signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectOpenJointInspector(T _subscriber)
+ { return openJointInspector.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the open joint inspector
+ /// signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectOpenJointInspector(
+ event::ConnectionPtr _subscriber)
+ { openJointInspector.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the joint name changed signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T>
+ static event::ConnectionPtr ConnectJointNameChanged(T _subscriber)
+ { return jointNameChanged.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the joint name changed signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectJointNameChanged(
+ event::ConnectionPtr _subscriber)
+ { jointNameChanged.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the show link context menu signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T> static event::ConnectionPtr
+ ConnectShowLinkContextMenu(T _subscriber)
+ { return showLinkContextMenu.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the show link context menu
+ /// signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectShowLinkContextMenu(
+ event::ConnectionPtr _subscriber)
+ { showLinkContextMenu.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the show joint context menu signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T> static event::ConnectionPtr
+ ConnectShowJointContextMenu(T _subscriber)
+ { return showJointContextMenu.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the show joint context menu
+ /// signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectShowJointContextMenu(
+ event::ConnectionPtr _subscriber)
+ { showJointContextMenu.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the set selected link signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T> static event::ConnectionPtr
+ ConnectSetSelectedLink(T _subscriber)
+ { return setSelectedLink.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the set selected link signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectSetSelectedLink(
+ event::ConnectionPtr _subscriber)
+ { setSelectedLink.Disconnect(_subscriber); }
+
+ /// \brief Connect a Gazebo event to the set selected joint signal.
+ /// \param[in] _subscriber the subscriber to this event
+ /// \return a connection
+ public: template<typename T> static event::ConnectionPtr
+ ConnectSetSelectedJoint(T _subscriber)
+ { return setSelectedJoint.Connect(_subscriber); }
+
+ /// \brief Disconnect a Gazebo event from the set selected joint signal.
+ /// \param[in] _subscriber the subscriber to this event
+ public: static void DisconnectSetSelectedJoint(
+ event::ConnectionPtr _subscriber)
+ { setSelectedJoint.Disconnect(_subscriber); }
+
/// \brief A model has been completed and uploaded onto the server.
public: static event::EventT<void ()> finishModel;
+
+ /// \brief Request to save the model.
+ public: static event::EventT<bool ()> saveModelEditor;
+
+ /// \brief Request to save the model as.
+ public: static event::EventT<bool ()> saveAsModelEditor;
+
+ /// \brief Request to start a new model.
+ public: static event::EventT<void ()> newModelEditor;
+
+ /// \brief Request to exit the editor.
+ public: static event::EventT<void ()> exitModelEditor;
+
+ /// \brief Model has been changed.
+ public: static event::EventT<void ()> modelChanged;
+
+ /// \brief Name was changed in the editor palette.
+ public: static event::EventT<void (std::string)> modelNameChanged;
+
+ /// \brief Notify that model properties have been changed.
+ // The properties are: is_static, auto_disable, pose, name.
+ public: static event::EventT<void (bool, bool, const math::Pose &,
+ const std::string &)> modelPropertiesChanged;
+
+ /// \brief Notify that model has been saved.
+ public: static event::EventT<void (std::string)> saveModel;
+
+ /// \brief Notify that model has been newed.
+ public: static event::EventT<void ()> newModel;
+
+ /// \brief Notify that a link has been inserted.
+ public: static event::EventT<void (std::string)> linkInserted;
+
+ /// \brief Notify that a joint has been inserted. The first string is
+ /// the joint's unique id, the second string is the joint name, the
+ /// third is the parent link's name, the fourth is the child link's
+ /// name. All names scoped.
+ public: static event::EventT<void (std::string, std::string,
+ std::string, std::string)> jointInserted;
+
+ /// \brief Notify that a link has been removed.
+ public: static event::EventT<void (std::string)> linkRemoved;
+
+ /// \brief Notify that a joint has been removed.
+ public: static event::EventT<void (std::string)> jointRemoved;
+
+ /// \brief Request to open the link inspector.
+ public: static event::EventT<void (std::string)> openLinkInspector;
+
+ /// \brief Request to open the joint inspector.
+ public: static event::EventT<void (std::string)> openJointInspector;
+
+ /// \brief Notify that the joint name has been changed. The first
+ /// string is the joint's unique id and the second string is the
+ /// new joint name.
+ public: static event::EventT<void (std::string, std::string)>
+ jointNameChanged;
+
+ /// \brief Request to show the link context menu.
+ public: static event::EventT<void (std::string)> showLinkContextMenu;
+
+ /// \brief Request to show the joint context menu.
+ public: static event::EventT<void (std::string)> showJointContextMenu;
+
+ /// \brief Request to select or deselect a link.
+ public: static event::EventT<void (std::string, bool)> setSelectedLink;
+
+ /// \brief Request to select or deselect a joint.
+ public: static event::EventT<void (std::string, bool)> setSelectedJoint;
};
}
}
diff --git a/gazebo/gui/model/ModelEditorPalette.cc b/gazebo/gui/model/ModelEditorPalette.cc
index fb7a317..0bb9dc1 100644
--- a/gazebo/gui/model/ModelEditorPalette.cc
+++ b/gazebo/gui/model/ModelEditorPalette.cc
@@ -27,7 +27,7 @@
#include "gazebo/gui/KeyEventHandler.hh"
#include "gazebo/gui/MouseEventHandler.hh"
#include "gazebo/gui/GuiEvents.hh"
-#include "gazebo/gui/SaveDialog.hh"
+#include "gazebo/gui/model/ExtrudeDialog.hh"
#include "gazebo/gui/model/ImportDialog.hh"
#include "gazebo/gui/model/JointMaker.hh"
#include "gazebo/gui/model/ModelEditorPalette.hh"
@@ -42,72 +42,63 @@ ModelEditorPalette::ModelEditorPalette(QWidget *_parent)
{
this->setObjectName("modelEditorPalette");
+ this->modelDefaultName = "Untitled";
+
QVBoxLayout *mainLayout = new QVBoxLayout;
- this->modelTreeWidget = new QTreeWidget();
- this->modelTreeWidget->setColumnCount(1);
- this->modelTreeWidget->setIndentation(10);
- this->modelTreeWidget->setContextMenuPolicy(Qt::CustomContextMenu);
- this->modelTreeWidget->header()->hide();
- this->modelTreeWidget->setFocusPolicy(Qt::NoFocus);
+ // Simple Shapes
+ QLabel *shapesLabel = new QLabel(tr(
+ "<font size=4 color='white'>Simple Shapes</font>"));
- this->modelTreeWidget->setSelectionMode(QAbstractItemView::NoSelection);
- connect(this->modelTreeWidget, SIGNAL(itemClicked(QTreeWidgetItem *, int)),
- this, SLOT(OnItemSelection(QTreeWidgetItem *, int)));
-
- // Parts tree item
- this->modelItem =
- new QTreeWidgetItem(static_cast<QTreeWidgetItem*>(0),
- QStringList(QString("Link Palette")));
- this->modelTreeWidget->addTopLevelItem(this->modelItem);
-
- QTreeWidgetItem *simpleShapesItem =
- new QTreeWidgetItem(static_cast<QTreeWidgetItem*>(0),
- QStringList(QString("Simple Shapes")));
- this->modelItem->addChild(simpleShapesItem);
-
- QTreeWidgetItem *simpleShapesChildItem =
- new QTreeWidgetItem(static_cast<QTreeWidgetItem*>(0));
- simpleShapesItem->addChild(simpleShapesChildItem);
-
- // Shapes buttons
- QWidget *modelWidget = new QWidget;
- QWidget *customWidget = new QWidget;
- QGridLayout *partsLayout = new QGridLayout;
- QGridLayout *customLayout = new QGridLayout;
-
- // cylinder button
- QPushButton *cylinderButton = new QPushButton(tr("Cylinder"), this);
+ QHBoxLayout *shapesLayout = new QHBoxLayout;
+
+ QSize toolButtonSize(70, 70);
+ QSize iconSize(40, 40);
+
+ // Cylinder button
+ QToolButton *cylinderButton = new QToolButton(this);
+ cylinderButton->setFixedSize(toolButtonSize);
+ cylinderButton->setToolTip(tr("Cylinder"));
+ cylinderButton->setIcon(QPixmap(":/images/cylinder.png"));
+ cylinderButton->setToolButtonStyle(Qt::ToolButtonIconOnly);
+ cylinderButton->setIconSize(QSize(iconSize));
cylinderButton->setCheckable(true);
cylinderButton->setChecked(false);
connect(cylinderButton, SIGNAL(clicked()), this, SLOT(OnCylinder()));
+ shapesLayout->addWidget(cylinderButton);
// Sphere button
- QPushButton *sphereButton = new QPushButton(tr("Sphere"), this);
+ QToolButton *sphereButton = new QToolButton(this);
+ sphereButton->setFixedSize(toolButtonSize);
+ sphereButton->setToolTip(tr("Sphere"));
+ sphereButton->setIcon(QPixmap(":/images/sphere.png"));
+ sphereButton->setToolButtonStyle(Qt::ToolButtonIconOnly);
+ sphereButton->setIconSize(QSize(iconSize));
sphereButton->setCheckable(true);
sphereButton->setChecked(false);
connect(sphereButton, SIGNAL(clicked()), this, SLOT(OnSphere()));
+ shapesLayout->addWidget(sphereButton);
// Box button
- QPushButton *boxButton = new QPushButton(tr("Box"), this);
+ QToolButton *boxButton = new QToolButton(this);
+ boxButton->setFixedSize(toolButtonSize);
+ boxButton->setToolTip(tr("Box"));
+ boxButton->setIcon(QPixmap(":/images/box.png"));
+ boxButton->setToolButtonStyle(Qt::ToolButtonIconOnly);
+ boxButton->setIconSize(QSize(iconSize));
boxButton->setCheckable(true);
boxButton->setChecked(false);
connect(boxButton, SIGNAL(clicked()), this, SLOT(OnBox()));
+ shapesLayout->addWidget(boxButton);
- partsLayout->addWidget(cylinderButton, 0, 0);
- partsLayout->addWidget(sphereButton, 0, 1);
- partsLayout->addWidget(boxButton, 0, 2);
- modelWidget->setLayout(partsLayout);
-
- // custom button
- QTreeWidgetItem *customItem =
- new QTreeWidgetItem(static_cast<QTreeWidgetItem*>(0),
- QStringList(QString("Custom")));
- this->modelItem->addChild(customItem);
+ // Custom Shapes
+ QLabel *customShapesLabel = new QLabel(tr(
+ "<font size=4 color='white'>Custom Shapes</font>"));
- QTreeWidgetItem *customChildItem =
- new QTreeWidgetItem(static_cast<QTreeWidgetItem*>(0));
- customItem->addChild(customChildItem);
+ QHBoxLayout *customLayout = new QHBoxLayout;
+ customLayout->setAlignment(Qt::AlignLeft);
+ customLayout->addItem(new QSpacerItem(30, 30, QSizePolicy::Minimum,
+ QSizePolicy::Minimum));
QPushButton *customButton = new QPushButton(tr("Add"), this);
customButton->setMaximumWidth(60);
@@ -115,88 +106,134 @@ ModelEditorPalette::ModelEditorPalette(QWidget *_parent)
customButton->setChecked(false);
connect(customButton, SIGNAL(clicked()), this, SLOT(OnCustom()));
customLayout->addWidget(customButton, 0, 0);
- customWidget->setLayout(customLayout);
-
- this->modelTreeWidget->setItemWidget(simpleShapesChildItem, 0, modelWidget);
- this->modelTreeWidget->setItemWidget(customChildItem, 0, customWidget);
- this->modelItem->setExpanded(true);
- simpleShapesItem->setExpanded(true);
- simpleShapesChildItem->setExpanded(true);
- customItem->setExpanded(true);
- customChildItem->setExpanded(true);
-
- this->partButtonGroup = new QButtonGroup;
- this->partButtonGroup->addButton(cylinderButton);
- this->partButtonGroup->addButton(sphereButton);
- this->partButtonGroup->addButton(boxButton);
- this->partButtonGroup->addButton(customButton);
-
- // model settings tree item
- this->modelSettingsItem =
- new QTreeWidgetItem(static_cast<QTreeWidgetItem*>(0),
- QStringList(QString("Model Settings")));
- this->modelTreeWidget->addTopLevelItem(this->modelSettingsItem);
-
- QTreeWidgetItem *modelSettingsChildItem =
- new QTreeWidgetItem(static_cast<QTreeWidgetItem*>(0));
- this->modelSettingsItem->addChild(modelSettingsChildItem);
-
- QWidget *modelSettingsWidget = new QWidget;
- QVBoxLayout *modelSettingsLayout = new QVBoxLayout;
- QGridLayout *dynamicsLayout = new QGridLayout;
+ // Button group
+ this->linkButtonGroup = new QButtonGroup;
+ this->linkButtonGroup->addButton(cylinderButton);
+ this->linkButtonGroup->addButton(sphereButton);
+ this->linkButtonGroup->addButton(boxButton);
+ this->linkButtonGroup->addButton(customButton);
+
+ // Model Settings
+ QLabel *settingsLabel = new QLabel(tr(
+ "<font size=4 color='white'>Model Settings</font>"));
+
+ QGridLayout *settingsLayout = new QGridLayout;
+
+ // Model name
+ QLabel *modelLabel = new QLabel(tr("Model Name: "));
+ this->modelNameEdit = new QLineEdit();
+ this->modelNameEdit->setText(tr(this->modelDefaultName.c_str()));
+ connect(this->modelNameEdit, SIGNAL(textChanged(QString)), this,
+ SLOT(OnNameChanged(QString)));
+
+ // Static
QLabel *staticLabel = new QLabel(tr("Static:"));
this->staticCheck = new QCheckBox;
this->staticCheck->setChecked(false);
connect(this->staticCheck, SIGNAL(clicked()), this, SLOT(OnStatic()));
+ // Auto disable
QLabel *autoDisableLabel = new QLabel(tr("Auto-disable:"));
this->autoDisableCheck = new QCheckBox;
this->autoDisableCheck->setChecked(true);
connect(this->autoDisableCheck, SIGNAL(clicked()), this,
SLOT(OnAutoDisable()));
- dynamicsLayout->addWidget(staticLabel, 0, 0);
- dynamicsLayout->addWidget(this->staticCheck, 0, 1);
- dynamicsLayout->addWidget(autoDisableLabel, 1, 0);
- dynamicsLayout->addWidget(this->autoDisableCheck, 1, 1);
-
- modelSettingsLayout->addLayout(dynamicsLayout);
- modelSettingsWidget->setLayout(modelSettingsLayout);
- this->modelTreeWidget->setItemWidget(modelSettingsChildItem, 0,
- modelSettingsWidget);
- this->modelSettingsItem->setExpanded(true);
- modelSettingsChildItem->setExpanded(true);
-
- // plugin
- this->pluginItem =
- new QTreeWidgetItem(static_cast<QTreeWidgetItem*>(0),
- QStringList(QString("Plugin")));
- // this->modelTreeWidget->addTopLevelItem(this->pluginItem);
-
- // save buttons
- QPushButton *discardButton = new QPushButton(tr("Discard"));
- connect(discardButton, SIGNAL(clicked()), this, SLOT(OnDiscard()));
-
- this->saveButton = new QPushButton(tr("Save As"));
- connect(this->saveButton, SIGNAL(clicked()), this, SLOT(OnSave()));
-
- QPushButton *doneButton = new QPushButton(tr("Done"));
- connect(doneButton, SIGNAL(clicked()), this, SLOT(OnDone()));
-
- QHBoxLayout *buttonsLayout = new QHBoxLayout;
- buttonsLayout->addWidget(discardButton);
- buttonsLayout->addWidget(this->saveButton);
- buttonsLayout->addWidget(doneButton);
- buttonsLayout->setAlignment(Qt::AlignCenter);
+ settingsLayout->addWidget(modelLabel, 0, 0);
+ settingsLayout->addWidget(this->modelNameEdit, 0, 1);
+ settingsLayout->addWidget(staticLabel, 1, 0);
+ settingsLayout->addWidget(this->staticCheck, 1, 1);
+ settingsLayout->addWidget(autoDisableLabel, 2, 0);
+ settingsLayout->addWidget(this->autoDisableCheck, 2, 1);
this->modelCreator = new ModelCreator();
- connect(modelCreator, SIGNAL(PartAdded()), this, SLOT(OnPartAdded()));
-
+ connect(modelCreator, SIGNAL(LinkAdded()), this, SLOT(OnLinkAdded()));
+
+ this->otherItemsLayout = new QVBoxLayout();
+ this->otherItemsLayout->setContentsMargins(0, 0, 0, 0);
+
+ // Palette layout
+ QVBoxLayout *paletteLayout = new QVBoxLayout();
+ paletteLayout->addWidget(shapesLabel);
+ paletteLayout->addLayout(shapesLayout);
+ paletteLayout->addWidget(customShapesLabel);
+ paletteLayout->addLayout(customLayout);
+ paletteLayout->addLayout(this->otherItemsLayout);
+ paletteLayout->addItem(new QSpacerItem(30, 30, QSizePolicy::Minimum,
+ QSizePolicy::Minimum));
+ paletteLayout->setAlignment(Qt::AlignTop | Qt::AlignHCenter);
+ QWidget *paletteWidget = new QWidget();
+ paletteWidget->setLayout(paletteLayout);
+
+ // Model tree
+ this->modelTreeWidget = new QTreeWidget();
+ this->modelTreeWidget->setObjectName("modelTreeWidget");
+ this->modelTreeWidget->setColumnCount(1);
+ this->modelTreeWidget->setContextMenuPolicy(Qt::CustomContextMenu);
+ this->modelTreeWidget->header()->hide();
+ this->modelTreeWidget->setFocusPolicy(Qt::NoFocus);
+ this->modelTreeWidget->setSelectionMode(QAbstractItemView::ExtendedSelection);
+ this->modelTreeWidget->setSelectionBehavior(QAbstractItemView::SelectRows);
+ this->modelTreeWidget->setVerticalScrollMode(
+ QAbstractItemView::ScrollPerPixel);
+
+ // Links
+ this->linksItem = new QTreeWidgetItem(
+ static_cast<QTreeWidgetItem *>(0),
+ QStringList(QString("%1").arg(tr("Links"))));
+ this->linksItem->setData(0, Qt::UserRole, QVariant(tr("Links")));
+ QFont linksFont = this->linksItem->font(0);
+ linksFont.setBold(true);
+ linksFont.setPointSize(1.1 * linksFont.pointSize());
+ this->linksItem->setFont(0, linksFont);
+ this->modelTreeWidget->addTopLevelItem(this->linksItem);
+
+ // Joints
+ this->jointsItem = new QTreeWidgetItem(
+ static_cast<QTreeWidgetItem*>(0),
+ QStringList(QString("%1").arg(tr("Joints"))));
+ this->jointsItem->setData(0, Qt::UserRole, QVariant(tr("Joints")));
+ this->jointsItem->setFont(0, linksFont);
+ this->modelTreeWidget->addTopLevelItem(this->jointsItem);
+
+ connect(this->modelTreeWidget,
+ SIGNAL(itemDoubleClicked(QTreeWidgetItem *, int)),
+ this, SLOT(OnItemDoubleClicked(QTreeWidgetItem *, int)));
+
+ connect(this->modelTreeWidget,
+ SIGNAL(itemClicked(QTreeWidgetItem *, int)),
+ this, SLOT(OnItemClicked(QTreeWidgetItem *, int)));
+
+ connect(this->modelTreeWidget, SIGNAL(itemSelectionChanged()),
+ this, SLOT(OnItemSelectionChanged()));
+
+ connect(this->modelTreeWidget,
+ SIGNAL(customContextMenuRequested(const QPoint &)),
+ this, SLOT(OnCustomContextMenu(const QPoint &)));
+
+ // Model layout
+ QVBoxLayout *modelLayout = new QVBoxLayout();
+ modelLayout->addWidget(settingsLabel);
+ modelLayout->addLayout(settingsLayout);
+ modelLayout->addWidget(this->modelTreeWidget);
+ modelLayout->setAlignment(Qt::AlignTop | Qt::AlignHCenter);
+ QWidget *modelWidget = new QWidget();
+ modelWidget->setLayout(modelLayout);
+
+ // Main layout
QFrame *frame = new QFrame;
QVBoxLayout *frameLayout = new QVBoxLayout;
- frameLayout->addWidget(this->modelTreeWidget, 0);
- frameLayout->addLayout(buttonsLayout);
+
+ QSplitter *splitter = new QSplitter(Qt::Vertical, this);
+ splitter->addWidget(paletteWidget);
+ splitter->addWidget(modelWidget);
+ splitter->setStretchFactor(0, 1);
+ splitter->setStretchFactor(1, 2);
+ splitter->setCollapsible(0, false);
+ splitter->setCollapsible(1, false);
+
+ frameLayout->addWidget(splitter);
frameLayout->setContentsMargins(0, 0, 0, 0);
frame->setLayout(frameLayout);
@@ -205,25 +242,61 @@ ModelEditorPalette::ModelEditorPalette(QWidget *_parent)
this->setLayout(mainLayout);
this->layout()->setContentsMargins(0, 0, 0, 0);
- this->saved = false;
- this->saveLocation = QDir::homePath().toStdString();
- this->modelName = "default";
-
KeyEventHandler::Instance()->AddPressFilter("model_editor",
boost::bind(&ModelEditorPalette::OnKeyPress, this, _1));
-}
-/////////////////////////////////////////////////
-ModelEditorPalette::~ModelEditorPalette()
-{
+ // Connections
+ this->connections.push_back(
+ gui::model::Events::ConnectSaveModel(
+ boost::bind(&ModelEditorPalette::OnSaveModel, this, _1)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectNewModel(
+ boost::bind(&ModelEditorPalette::OnNewModel, this)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectModelPropertiesChanged(
+ boost::bind(&ModelEditorPalette::OnModelPropertiesChanged, this, _1, _2,
+ _3, _4)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectLinkInserted(
+ boost::bind(&ModelEditorPalette::OnLinkInserted, this, _1)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectJointInserted(
+ boost::bind(&ModelEditorPalette::OnJointInserted, this, _1, _2, _3, _4)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectLinkRemoved(
+ boost::bind(&ModelEditorPalette::OnLinkRemoved, this, _1)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectJointRemoved(
+ boost::bind(&ModelEditorPalette::OnJointRemoved, this, _1)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectJointNameChanged(
+ boost::bind(&ModelEditorPalette::OnJointNameChanged, this, _1, _2)));
+
+ this->connections.push_back(
+ event::Events::ConnectSetSelectedEntity(
+ boost::bind(&ModelEditorPalette::OnSetSelectedEntity, this, _1, _2)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectSetSelectedLink(
+ boost::bind(&ModelEditorPalette::OnSetSelectedLink, this, _1, _2)));
+
+ this->connections.push_back(
+ gui::model::Events::ConnectSetSelectedJoint(
+ boost::bind(&ModelEditorPalette::OnSetSelectedJoint, this, _1, _2)));
}
/////////////////////////////////////////////////
-void ModelEditorPalette::OnItemSelection(QTreeWidgetItem *_item,
- int /*_column*/)
+ModelEditorPalette::~ModelEditorPalette()
{
- if (_item && _item->childCount() > 0)
- _item->setExpanded(!_item->isExpanded());
+ delete this->modelCreator;
+ this->modelCreator = NULL;
}
/////////////////////////////////////////////////
@@ -232,7 +305,7 @@ void ModelEditorPalette::OnCylinder()
event::Events::setSelectedEntity("", "normal");
g_arrowAct->trigger();
- this->modelCreator->AddPart(ModelCreator::PART_CYLINDER);
+ this->modelCreator->AddLink(ModelCreator::LINK_CYLINDER);
}
/////////////////////////////////////////////////
@@ -241,7 +314,7 @@ void ModelEditorPalette::OnSphere()
event::Events::setSelectedEntity("", "normal");
g_arrowAct->trigger();
- this->modelCreator->AddPart(ModelCreator::PART_SPHERE);
+ this->modelCreator->AddLink(ModelCreator::LINK_SPHERE);
}
/////////////////////////////////////////////////
@@ -250,7 +323,7 @@ void ModelEditorPalette::OnBox()
event::Events::setSelectedEntity("", "normal");
g_arrowAct->trigger();
- this->modelCreator->AddPart(ModelCreator::PART_BOX);
+ this->modelCreator->AddLink(ModelCreator::LINK_BOX);
}
/////////////////////////////////////////////////
@@ -260,31 +333,88 @@ void ModelEditorPalette::OnCustom()
importDialog.deleteLater();
if (importDialog.exec() == QDialog::Accepted)
{
- event::Events::setSelectedEntity("", "normal");
- g_arrowAct->trigger();
- this->modelCreator->AddCustom(importDialog.GetImportPath());
+ QFileInfo info(QString::fromStdString(importDialog.GetImportPath()));
+ if (info.isFile())
+ {
+ event::Events::setSelectedEntity("", "normal");
+ g_arrowAct->trigger();
+ if (info.completeSuffix().toLower() == "dae" ||
+ info.completeSuffix().toLower() == "stl")
+ {
+ this->modelCreator->AddShape(ModelCreator::LINK_MESH,
+ math::Vector3::One, math::Pose::Zero, importDialog.GetImportPath());
+ }
+ else if (info.completeSuffix().toLower() == "svg")
+ {
+ ExtrudeDialog extrudeDialog(importDialog.GetImportPath(), this);
+ extrudeDialog.deleteLater();
+ if (extrudeDialog.exec() == QDialog::Accepted)
+ {
+ this->modelCreator->AddShape(ModelCreator::LINK_POLYLINE,
+ math::Vector3(1.0/extrudeDialog.GetResolution(),
+ 1.0/extrudeDialog.GetResolution(),
+ extrudeDialog.GetThickness()),
+ math::Pose::Zero, importDialog.GetImportPath(),
+ extrudeDialog.GetSamples());
+ }
+ else
+ {
+ this->OnCustom();
+ }
+ }
+ }
}
else
{
// this unchecks the custom button
- this->OnPartAdded();
+ this->OnLinkAdded();
}
}
/////////////////////////////////////////////////
-void ModelEditorPalette::AddJoint(const std::string &_type)
+void ModelEditorPalette::AddItem(QWidget *_item,
+ const std::string &_category)
+{
+ std::string category = _category;
+ if (category.empty())
+ category = "Other";
+
+ auto iter = this->categories.find(category);
+ QGridLayout *catLayout = NULL;
+ if (iter == this->categories.end())
+ {
+ catLayout = new QGridLayout();
+ this->categories[category] = catLayout;
+
+ std::string catStr =
+ "<font size=4 color='white'>" + category + "</font>";
+ QLabel *catLabel = new QLabel(tr(catStr.c_str()));
+ this->otherItemsLayout->addWidget(catLabel);
+ this->otherItemsLayout->addLayout(catLayout);
+ }
+ else
+ catLayout = iter->second;
+
+ int rowWidth = 3;
+ int row = catLayout->count() / rowWidth;
+ int col = catLayout->count() % rowWidth;
+ catLayout->addWidget(_item, row, col);
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::CreateJoint(const std::string &_type)
{
event::Events::setSelectedEntity("", "normal");
this->modelCreator->AddJoint(_type);
}
/////////////////////////////////////////////////
-void ModelEditorPalette::OnPartAdded()
+void ModelEditorPalette::OnLinkAdded()
{
- this->partButtonGroup->setExclusive(false);
- if (this->partButtonGroup->checkedButton())
- this->partButtonGroup->checkedButton()->setChecked(false);
- this->partButtonGroup->setExclusive(true);
+ this->linkButtonGroup->setExclusive(false);
+ if (this->linkButtonGroup->checkedButton())
+ this->linkButtonGroup->checkedButton()->setChecked(false);
+ this->linkButtonGroup->setExclusive(true);
}
/////////////////////////////////////////////////
@@ -300,88 +430,334 @@ void ModelEditorPalette::OnStatic()
}
/////////////////////////////////////////////////
-void ModelEditorPalette::OnSave()
+void ModelEditorPalette::OnModelPropertiesChanged(
+ bool _static, bool _autoDisable, const math::Pose &/*_pose*/,
+ const std::string &_name)
+{
+ this->staticCheck->setChecked(_static);
+ this->autoDisableCheck->setChecked(_autoDisable);
+ this->modelNameEdit->setText(tr(_name.c_str()));
+}
+
+/////////////////////////////////////////////////
+bool ModelEditorPalette::OnKeyPress(const common::KeyEvent &_event)
+{
+ if (_event.key == Qt::Key_Escape)
+ {
+ // call the slots to uncheck the buttons
+ this->OnLinkAdded();
+ }
+ if (_event.key == Qt::Key_Delete)
+ {
+ event::Events::setSelectedEntity("", "normal");
+ g_arrowAct->trigger();
+ }
+ return false;
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnItemSelectionChanged()
{
- SaveDialog saveDialog;
- saveDialog.deleteLater();
- saveDialog.SetTitle("Save Model");
- saveDialog.SetSaveName(this->modelCreator->GetModelName());
- saveDialog.SetSaveLocation(QDir::homePath().toStdString());
- saveDialog.SetFileExtension("sdf");
- if (saveDialog.exec() == QDialog::Accepted)
+ QList<QTreeWidgetItem *> items = this->modelTreeWidget->selectedItems();
+
+ // update and signal new selection
+ for (auto const item : items)
+ {
+ int idx = this->selected.indexOf(item);
+ if (idx >= 0)
+ {
+ this->selected.removeAt(idx);
+ continue;
+ }
+ std::string name = item->data(0, Qt::UserRole).toString().toStdString();
+ std::string type = item->data(1, Qt::UserRole).toString().toStdString();
+
+ if (type == "Link")
+ gui::model::Events::setSelectedLink(name, true);
+ else if (type == "Joint")
+ gui::model::Events::setSelectedJoint(name, true);
+ }
+
+ // deselect
+ for (auto const item : this->selected)
{
- this->modelName = saveDialog.GetSaveName();
- this->saveLocation = saveDialog.GetSaveLocation();
- this->modelCreator->SetModelName(this->modelName);
- this->modelCreator->GenerateSDF();
- this->modelCreator->SaveToSDF(this->saveLocation);
- this->saveButton->setText("&Save");
+ if (item)
+ {
+ std::string name = item->data(0, Qt::UserRole).toString().toStdString();
+ std::string type = item->data(1, Qt::UserRole).toString().toStdString();
+
+ if (type == "Link")
+ gui::model::Events::setSelectedLink(name, false);
+ else if (type == "Joint")
+ gui::model::Events::setSelectedJoint(name, false);
+ }
}
+
+ this->selected = items;
}
/////////////////////////////////////////////////
-void ModelEditorPalette::OnDiscard()
+void ModelEditorPalette::OnSetSelectedEntity(const std::string &/*_name*/,
+ const std::string &/*_mode*/)
{
- int ret = QMessageBox::warning(0, QString("Discard"),
- QString("Are you sure you want to discard\n"
- "your model? All of your work will\n"
- "be lost."),
- QMessageBox::Discard | QMessageBox::Cancel,
- QMessageBox::Cancel);
-
- switch (ret)
+ // deselect all
+ for (auto &item : this->selected)
{
- case QMessageBox::Discard:
- this->modelCreator->Reset();
- this->saveButton->setText("&Save As");
- this->saveLocation = QDir::homePath().toStdString();
- break;
- case QMessageBox::Cancel:
- // Do nothing
- break;
- default:
+ if (item)
+ item->setSelected(false);
+ }
+}
+
+/////////////////////////////////////////////////
+ModelCreator *ModelEditorPalette::GetModelCreator()
+{
+ return this->modelCreator;
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnNameChanged(const QString &_name)
+{
+ gui::model::Events::modelNameChanged(_name.toStdString());
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnNewModel()
+{
+ this->modelNameEdit->setText(tr(this->modelDefaultName.c_str()));
+
+ this->ClearModelTree();
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnSaveModel(const std::string &_saveName)
+{
+ this->modelNameEdit->setText(tr(_saveName.c_str()));
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnItemDoubleClicked(QTreeWidgetItem *_item,
+ int /*_column*/)
+{
+ if (_item)
+ {
+ std::string name = _item->data(0, Qt::UserRole).toString().toStdString();
+ std::string type = _item->data(1, Qt::UserRole).toString().toStdString();
+
+ if (type == "Link")
+ gui::model::Events::openLinkInspector(name);
+ else if (type == "Joint")
+ gui::model::Events::openJointInspector(name);
+ }
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnItemClicked(QTreeWidgetItem *_item,
+ int /*_column*/)
+{
+ if (_item)
+ {
+ if (this->selected.empty())
+ return;
+
+ QTreeWidgetItem *item = this->selected[0];
+ std::string selectedType =
+ item->data(1, Qt::UserRole).toString().toStdString();
+
+ std::string type = _item->data(1, Qt::UserRole).toString().toStdString();
+
+ if (type != selectedType)
+ this->DeselectType(selectedType);
+ }
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::DeselectType(const std::string &_type)
+{
+ QObject::disconnect(this->modelTreeWidget, SIGNAL(itemSelectionChanged()),
+ this, SLOT(OnItemSelectionChanged()));
+
+ for (auto it = this->selected.begin(); it != this->selected.end();)
+ {
+ std::string name = (*it)->data(0, Qt::UserRole).toString().toStdString();
+ std::string type = (*it)->data(1, Qt::UserRole).toString().toStdString();
+ if (type == _type)
+ {
+ (*it)->setSelected(false);
+ it = this->selected.erase(it);
+ if (type == "Link")
+ gui::model::Events::setSelectedLink(name, false);
+ else if (type == "Joint")
+ gui::model::Events::setSelectedJoint(name, false);
+ }
+ else
+ ++it;
+ }
+
+ this->selected = this->modelTreeWidget->selectedItems();
+
+ QObject::connect(this->modelTreeWidget, SIGNAL(itemSelectionChanged()),
+ this, SLOT(OnItemSelectionChanged()));
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnCustomContextMenu(const QPoint &_pt)
+{
+ QTreeWidgetItem *item = this->modelTreeWidget->itemAt(_pt);
+
+ if (item)
+ {
+ std::string name = item->data(0, Qt::UserRole).toString().toStdString();
+ std::string type = item->data(1, Qt::UserRole).toString().toStdString();
+
+ if (type == "Link")
+ gui::model::Events::showLinkContextMenu(name);
+ else if (type == "Joint")
+ gui::model::Events::showJointContextMenu(name);
+ }
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnLinkInserted(const std::string &_linkName)
+{
+ std::string leafName = _linkName;
+ size_t idx = _linkName.find_last_of("::");
+ if (idx != std::string::npos)
+ leafName = _linkName.substr(idx+1);
+
+ QTreeWidgetItem *newLinkItem = new QTreeWidgetItem(this->linksItem,
+ QStringList(QString("%1").arg(QString::fromStdString(leafName))));
+
+ newLinkItem->setData(0, Qt::UserRole, _linkName.c_str());
+ newLinkItem->setData(1, Qt::UserRole, "Link");
+ this->modelTreeWidget->addTopLevelItem(newLinkItem);
+
+ this->linksItem->setExpanded(true);
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnJointInserted(const std::string &_jointId,
+ const std::string &_jointName, const std::string &/*_parentName*/,
+ const std::string &/*_childName*/)
+{
+ std::string leafName = _jointName;
+ size_t idx = _jointName.find_last_of("::");
+ if (idx != std::string::npos)
+ leafName = _jointName.substr(idx+1);
+
+ QTreeWidgetItem *newJointItem = new QTreeWidgetItem(this->jointsItem,
+ QStringList(QString("%1").arg(QString::fromStdString(leafName))));
+
+ newJointItem->setData(0, Qt::UserRole, _jointId.c_str());
+ newJointItem->setData(1, Qt::UserRole, "Joint");
+ this->modelTreeWidget->addTopLevelItem(newJointItem);
+
+ this->jointsItem->setExpanded(true);
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnLinkRemoved(const std::string &_linkId)
+{
+ std::unique_lock<std::recursive_mutex> lock(this->updateMutex);
+ for (int i = 0; i < this->linksItem->childCount(); ++i)
+ {
+ QTreeWidgetItem *item = this->linksItem->child(i);
+ if (!item)
+ continue;
+ std::string listData = item->data(0, Qt::UserRole).toString().toStdString();
+
+ if (listData == _linkId)
+ {
+ this->linksItem->takeChild(this->linksItem->indexOfChild(item));
break;
+ }
}
}
/////////////////////////////////////////////////
-void ModelEditorPalette::OnDone()
+void ModelEditorPalette::OnJointRemoved(const std::string &_jointId)
{
- SaveDialog saveDialog;
- saveDialog.SetTitle("Save Model");
- saveDialog.SetSaveName(this->modelCreator->GetModelName());
- saveDialog.SetSaveLocation(QDir::homePath().toStdString());
- saveDialog.SetFileExtension("sdf");
- if (saveDialog.exec() == QDialog::Accepted)
+ std::unique_lock<std::recursive_mutex> lock(this->updateMutex);
+ for (int i = 0; i < this->jointsItem->childCount(); ++i)
{
- this->modelName = saveDialog.GetSaveName();
- this->saveLocation = saveDialog.GetSaveLocation();
- this->modelCreator->SetModelName(this->modelName);
- this->modelCreator->GenerateSDF();
- this->modelCreator->SaveToSDF(this->saveLocation);
- this->modelCreator->FinishModel();
- gui::model::Events::finishModel();
+ QTreeWidgetItem *item = this->jointsItem->child(i);
+ if (!item)
+ continue;
+ std::string listData = item->data(0, Qt::UserRole).toString().toStdString();
+
+ if (listData == _jointId)
+ {
+ this->jointsItem->takeChild(this->jointsItem->indexOfChild(item));
+ break;
+ }
}
}
/////////////////////////////////////////////////
-bool ModelEditorPalette::OnKeyPress(const common::KeyEvent &_event)
+void ModelEditorPalette::ClearModelTree()
{
- if (_event.key == Qt::Key_Escape)
+ std::unique_lock<std::recursive_mutex> lock(this->updateMutex);
+ // Remove all links
+ this->linksItem->takeChildren();
+ // Remove all joints
+ this->jointsItem->takeChildren();
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnJointNameChanged(const std::string &_jointId,
+ const std::string &_newJointName)
+{
+ std::unique_lock<std::recursive_mutex> lock(this->updateMutex);
+ for (int i = 0; i < this->jointsItem->childCount(); ++i)
{
- // call the slots to uncheck the buttons
- this->OnPartAdded();
+ QTreeWidgetItem *item = this->jointsItem->child(i);
+ if (!item)
+ continue;
+ std::string listData = item->data(0, Qt::UserRole).toString().toStdString();
+
+ if (listData == _jointId)
+ {
+ item->setText(0, QString::fromStdString(_newJointName));
+ break;
+ }
}
- if (_event.key == Qt::Key_Delete)
+}
+
+/////////////////////////////////////////////////
+void ModelEditorPalette::OnSetSelectedLink(const std::string &_name,
+ bool _selected)
+{
+ std::unique_lock<std::recursive_mutex> lock(this->updateMutex);
+ for (int i = 0; i < this->linksItem->childCount(); ++i)
{
- event::Events::setSelectedEntity("", "normal");
- g_arrowAct->trigger();
+ QTreeWidgetItem *item = this->linksItem->child(i);
+ if (!item)
+ continue;
+ std::string listData = item->data(0, Qt::UserRole).toString().toStdString();
+
+ if (listData == _name)
+ {
+ item->setSelected(_selected);
+ break;
+ }
}
- return false;
}
/////////////////////////////////////////////////
-ModelCreator *ModelEditorPalette::GetModelCreator()
+void ModelEditorPalette::OnSetSelectedJoint(const std::string &_name,
+ bool _selected)
{
- return this->modelCreator;
+ std::unique_lock<std::recursive_mutex> lock(this->updateMutex);
+ for (int i = 0; i < this->jointsItem->childCount(); ++i)
+ {
+ QTreeWidgetItem *item = this->jointsItem->child(i);
+ if (!item)
+ continue;
+ std::string listData = item->data(0, Qt::UserRole).toString().toStdString();
+
+ if (listData == _name)
+ {
+ item->setSelected(_selected);
+ break;
+ }
+ }
}
diff --git a/gazebo/gui/model/ModelEditorPalette.hh b/gazebo/gui/model/ModelEditorPalette.hh
index f478072..d3a5f9b 100644
--- a/gazebo/gui/model/ModelEditorPalette.hh
+++ b/gazebo/gui/model/ModelEditorPalette.hh
@@ -15,10 +15,13 @@
*
*/
-#ifndef _MODEL_EDITOR_PALETTE_HH_
-#define _MODEL_EDITOR_PALETTE_HH_
+#ifndef _GAZEBO_MODEL_EDITOR_PALETTE_HH_
+#define _GAZEBO_MODEL_EDITOR_PALETTE_HH_
+#include <mutex>
+#include <map>
#include <string>
+#include <vector>
#include "gazebo/rendering/RenderTypes.hh"
#include "gazebo/common/Event.hh"
@@ -42,7 +45,7 @@ namespace gazebo
/// \class ModelEditorPalette ModelEditorPalette.hh
/// \brief A palette of model items which can be added to the editor.
- class GAZEBO_VISIBLE ModelEditorPalette : public QWidget
+ class GZ_GUI_MODEL_VISIBLE ModelEditorPalette : public QWidget
{
Q_OBJECT
@@ -53,9 +56,15 @@ namespace gazebo
/// \brief Destructor
public: ~ModelEditorPalette();
+ /// \brief Add an item to the model editor palette.
+ /// \param[in] _Item item to add.
+ /// \param[in] _category Category to add the item too.
+ public: void AddItem(QWidget *_item,
+ const std::string &_category = "Other");
+
/// \brief Add a joint to the model.
/// \param[in] _type Type of joint to add.
- public: void AddJoint(const std::string &_type);
+ public: void CreateJoint(const std::string &_type);
/// \brief Get the model creator.
/// \return a pointer to the model creator.
@@ -66,10 +75,28 @@ namespace gazebo
/// \return True if the event was handled
private: bool OnKeyPress(const common::KeyEvent &_event);
- /// \brief Received item selection user input.
- /// \param[in] _item Item selected.
- /// \param[in] _column Column index.
- private slots: void OnItemSelection(QTreeWidgetItem *_item, int _column);
+ /// \brief Callback when an entity is selected.
+ /// \param[in] _name Name of entity.
+ /// \param[in] _mode Select mode
+ private: void OnSetSelectedEntity(const std::string &_name,
+ const std::string &_mode);
+
+ /// \brief Callback when a link is selected.
+ /// \param[in] _name Name of link.
+ /// \param[in] _selected True if the link is selected, false if
+ /// deselected.
+ private: void OnSetSelectedLink(const std::string &_name, bool _selected);
+
+ /// \brief Callback when a joint is selected.
+ /// \param[in] _name Name of joint.
+ /// \param[in] _selected True if the joint is selected, false if
+ /// deselected.
+ private: void OnSetSelectedJoint(const std::string &_name,
+ bool _selected);
+
+ /// \brief Helper function to deselect a link or a joint.
+ /// \param[in] _type Type: Link or Joint.
+ private: void DeselectType(const std::string &_type);
/// \brief Qt callback when cylinder button is clicked.
private slots: void OnCylinder();
@@ -83,8 +110,8 @@ namespace gazebo
/// \brief Qt callback when custom button is clicked.
private slots: void OnCustom();
- /// \brief Qt callback when a part has been added.
- private slots: void OnPartAdded();
+ /// \brief Qt callback when a link has been added.
+ private slots: void OnLinkAdded();
/// \brief Qt callback when the model is to be made static.
private slots: void OnStatic();
@@ -92,51 +119,117 @@ namespace gazebo
/// \brief Qt callback when the model is allowed to auto disable at rest.
private slots: void OnAutoDisable();
- /// \brief Qt callback when the model is to be saved.
- private slots: void OnSave();
+ /// \brief Qt callback when the Model Name field is changed.
+ /// \param[in] _name New name.
+ private slots: void OnNameChanged(const QString &_name);
- /// \brief Qt callback when the model is to be discarded.
- private slots: void OnDiscard();
+ /// \brief Qt callback when a tree item has been double clicked.
+ /// \param[in] _item Item clicked.
+ /// \param[in] _column Column index.
+ private slots: void OnItemDoubleClicked(QTreeWidgetItem *_item,
+ int _column);
- /// \brief Qt callback when model editing is complete.
- private slots: void OnDone();
+ /// \brief Qt callback when a tree item has been clicked.
+ /// \param[in] _item Item clicked.
+ /// \param[in] _column Column index.
+ private slots: void OnItemClicked(QTreeWidgetItem *_item, int _column);
+
+ /// \brief Qt callback when selected items have changed.
+ private slots: void OnItemSelectionChanged();
+
+ /// \brief Qt callback when the context menu signal is triggered.
+ /// \param[in] _pt Position of the context menu event that the widget
+ /// receives.
+ private slots: void OnCustomContextMenu(const QPoint &_pt);
+
+ /// \brief Add a link to the tree.
+ /// \param[in] _linkName Scoped link name.
+ private: void OnLinkInserted(const std::string &_linkName);
+
+ /// \brief Add a joint to the tree.
+ /// \param[in] _jointId Unique joint identifying name.
+ /// \param[in] _jointName Scoped name which can be changed by the user.
+ /// \param[in] _jointName Scoped name of the parent link.
+ /// \param[in] _jointName Scoped name of the child link.
+ private: void OnJointInserted(const std::string &_jointId,
+ const std::string &_jointName, const std::string &_parentName,
+ const std::string &_childName);
+
+ /// \brief Remove a link from the tree.
+ /// \param[in] _linkId Unique link identifying name.
+ private: void OnLinkRemoved(const std::string &_linkId);
+
+ /// \brief Remove a joint from the tree.
+ /// \param[in] _jointId Unique joint identifying name.
+ private: void OnJointRemoved(const std::string &_jointId);
+
+ /// \brief Remove all links and joints from the tree.
+ private: void ClearModelTree();
+
+ /// \brief Update a joint item text in the tree.
+ /// \param[in] _jointId Unique joint identifying name.
+ /// \param[in] _newJointName New scoped joint name.
+ private: void OnJointNameChanged(const std::string &_jointId,
+ const std::string &_newJointName);
+
+ /// \brief Callback when user has provided information on where to save
+ /// the model to.
+ /// \param[in] _saveName Name of model being saved.
+ private: void OnSaveModel(const std::string &_saveName);
+
+ /// \brief Event received when the user starts a new model.
+ private: void OnNewModel();
+
+ /// \brief Event received when the model properties changed.
+ /// \param[in] _static New static property of the model.
+ /// \param[in] _autoDisable New allow_auto_disable property of the model.
+ /// \param[in] _pose New model pose.
+ /// \param[in] _name New name.
+ private: void OnModelPropertiesChanged(bool _static, bool _autoDisable,
+ const math::Pose &_pose, const std::string &_name);
+
+ /// \brief A list of gui editor events connected to this palette.
+ private: std::vector<event::ConnectionPtr> connections;
+
+ /// \brief Links button group.
+ private: QButtonGroup *linkButtonGroup;
- /// \brief Widget that display model properties.
- private: QTreeWidget *modelTreeWidget;
+ /// \brief Model creator.
+ private: ModelCreator *modelCreator;
- /// \brief Model settings item in the tree widget.
- private: QTreeWidgetItem *modelSettingsItem;
+ /// \brief Static checkbox, true to create a static model.
+ private: QCheckBox *staticCheck;
- /// \brief Model parts item in the tree widget.
- private: QTreeWidgetItem *modelItem;
+ /// \brief Auto disable checkbox, true to allow model to auto-disable at
+ /// rest.
+ private: QCheckBox *autoDisableCheck;
- /// \brief Plugin item in the tree widget.
- private: QTreeWidgetItem *pluginItem;
+ /// \brief Default name of the model.
+ private: std::string modelDefaultName;
- /// \brief Parts button group.
- private: QButtonGroup *partButtonGroup;
+ /// \brief Edit the name of the model.
+ private: QLineEdit *modelNameEdit;
- /// \brief Model creator.
- private: ModelCreator *modelCreator;
+ /// \brief The tree holding all links and joints.
+ private: QTreeWidget *modelTreeWidget;
- /// \brief Save button.
- private: QPushButton *saveButton;
+ /// \brief Parent item for all links.
+ private: QTreeWidgetItem *linksItem;
- /// \brief Indicate whether the model has been saved before or not.
- private: bool saved;
+ /// \brief Parent item for all joints.
+ private: QTreeWidgetItem *jointsItem;
- /// \brief Path to where the model is saved.
- private: std::string saveLocation;
+ /// \brief Mutex to protect updates.
+ private: std::recursive_mutex updateMutex;
- /// \brief Name of model being edited.
- private: std::string modelName;
+ /// \brief Keeps track of selected items.
+ private: QList<QTreeWidgetItem *> selected;
- /// \brief Static checkbox, true to create a static model.
- private: QCheckBox *staticCheck;
+ /// \brief Layout for other items in the palette.
+ private: QVBoxLayout *otherItemsLayout;
- /// \brief Auto disable checkbox, true to allow model to auto-disable at
- /// rest.
- private: QCheckBox *autoDisableCheck;
+ /// \brief Map of categories to their layout
+ private: std::map<std::string, QGridLayout *> categories;
};
}
}
diff --git a/gazebo/gui/model/ModelEditorPalette_TEST.cc b/gazebo/gui/model/ModelEditorPalette_TEST.cc
new file mode 100644
index 0000000..17ed47f
--- /dev/null
+++ b/gazebo/gui/model/ModelEditorPalette_TEST.cc
@@ -0,0 +1,85 @@
+/*
+ * 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/gui/MainWindow.hh"
+#include "gazebo/gui/model/ModelEditorPalette.hh"
+#include "gazebo/gui/model/ModelEditorPalette_TEST.hh"
+
+using namespace gazebo;
+
+/////////////////////////////////////////////////
+void ModelEditorPalette_TEST::AddItem()
+{
+ gui::ModelEditorPalette *palette = new gui::ModelEditorPalette();
+ QVERIFY(palette);
+
+ // add a custom tool button to the palette
+ QToolButton *testButton = new QToolButton();
+ testButton->setObjectName("my_tool_button");
+
+ // test adding without specifying category
+ palette->AddItem(testButton);
+
+ // verify that the button is added.
+ QToolButton *retButton =
+ palette->findChild<QToolButton *>("my_tool_button");
+ QVERIFY(retButton);
+ QVERIFY(retButton == testButton);
+
+ // add another custom tool button to the palette
+ QToolButton *testButton2 = new QToolButton();
+ testButton2->setObjectName("my_tool_button2");
+
+ // test adding with category
+ palette->AddItem(testButton2, "category");
+
+ // verify that the button is added.
+ QToolButton *retButton2 =
+ palette->findChild<QToolButton *>("my_tool_button2");
+ QVERIFY(retButton2);
+ QVERIFY(retButton2 == testButton2);
+
+ // add a button to an existing category
+ QToolButton *testButton3 = new QToolButton();
+ testButton3->setObjectName("my_tool_button3");
+
+ palette->AddItem(testButton3, "category");
+
+ // verify that the button is added.
+ QToolButton *retButton3 =
+ palette->findChild<QToolButton *>("my_tool_button3");
+ QVERIFY(retButton3);
+ QVERIFY(retButton3 == testButton3);
+
+ // add a button with an empty category
+ QToolButton *testButton4 = new QToolButton();
+ testButton4->setObjectName("my_tool_button4");
+
+ palette->AddItem(testButton4, "");
+
+ // verify that the button is added.
+ QToolButton *retButton4 =
+ palette->findChild<QToolButton *>("my_tool_button4");
+ QVERIFY(retButton4);
+ QVERIFY(retButton4 == testButton4);
+
+ delete palette;
+ palette = NULL;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(ModelEditorPalette_TEST)
diff --git a/gazebo/gui/model/ModelEditorPalette_TEST.hh b/gazebo/gui/model/ModelEditorPalette_TEST.hh
new file mode 100644
index 0000000..be41365
--- /dev/null
+++ b/gazebo/gui/model/ModelEditorPalette_TEST.hh
@@ -0,0 +1,32 @@
+/*
+ * 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_MODEL_EDITOR_PALETTE_TEST_HH_
+#define _GAZEBO_MODEL_EDITOR_PALETTE_TEST_HH_
+
+#include "gazebo/gui/QTestFixture.hh"
+
+/// \brief A test class for the ModelEditorPalette class.
+class ModelEditorPalette_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Tests adding an item to the palette.
+ private slots: void AddItem();
+};
+
+#endif
diff --git a/gazebo/gui/model/ModelEditorPrivate.hh b/gazebo/gui/model/ModelEditorPrivate.hh
new file mode 100644
index 0000000..bcf5e7e
--- /dev/null
+++ b/gazebo/gui/model/ModelEditorPrivate.hh
@@ -0,0 +1,103 @@
+/*
+ * 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_MODEL_EDITOR_PRIVATE_HH_
+#define _GAZEBO_MODEL_EDITOR_PRIVATE_HH_
+
+#include <string>
+
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class SchematicViewWidget;
+ class ModelEditorPalette;
+
+ /// \internal
+ /// \class ModelEditor ModelEditor.hh
+ /// \brief Private data for the ModelEditor class.
+ class ModelEditorPrivate
+ {
+ /// \brief Menubar containing actions related to the editor.
+ public: QMenuBar *menuBar;
+
+ /// \brief Contains all the model editor tools.
+ public: ModelEditorPalette *modelPalette;
+
+ /// \brief True if model editor is active.
+ public: bool active;
+
+ /// \brief Qt action for selecting and adding a joint in the model editor.
+ public: QAction *jointTypeAct;
+
+ /// \brief Qt action for adding a previously selected joint in the
+ /// model editor.
+ public: QAction *jointAct;
+
+ /// \brief A separator for the joint icon.
+ public: QAction *jointSeparatorAct;
+
+ /// \brief Qt tool button associated with the joint action.
+ public: QToolButton *jointButton;
+
+ /// \brief Qt signal mapper for mapping add jointsignals.
+ public: QSignalMapper *signalMapper;
+
+ /// \brief Previously selected joint type.
+ public: std::string selectedJointType;
+
+ /// \brief Action to save model.
+ public: QAction *saveAct;
+
+ /// \brief Action to save model as.
+ public: QAction *saveAsAct;
+
+ /// \brief Action to start a new model.
+ public: QAction *newAct;
+
+ /// \brief Action to exit the editor.
+ public: QAction *exitAct;
+
+ /// \brief Action to show joints.
+ public: QAction *showJointsAct;
+
+ /// \brief Action to show/hide the schematic view.
+ public: QAction *schematicViewAct;
+
+ /// \brief Pointer to the schematic view widget.
+ public: SchematicViewWidget *svWidget;
+
+ /// \brief Show toolbars action cloned from main window.
+ public: QAction *showToolbarsAct;
+
+ /// \brief Fullscreen action cloned from main window.
+ public: QAction *fullScreenAct;
+
+ /// \brief Camera orthographic view action cloned from main window.
+ public: QAction *cameraOrthoAct;
+
+ /// \brief Camera perspective view action cloned from main window.
+ public: QAction *cameraPerspectiveAct;
+
+ /// \brief Save the main window paused state to use when returning.
+ public: bool mainWindowPaused;
+ };
+ }
+}
+#endif
diff --git a/gazebo/gui/model/ModelEditor_TEST.cc b/gazebo/gui/model/ModelEditor_TEST.cc
new file mode 100644
index 0000000..50183d3
--- /dev/null
+++ b/gazebo/gui/model/ModelEditor_TEST.cc
@@ -0,0 +1,70 @@
+/*
+ * 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/gui/MainWindow.hh"
+#include "gazebo/gui/model/ModelEditor.hh"
+#include "gazebo/gui/model/ModelEditor_TEST.hh"
+
+using namespace gazebo;
+
+/////////////////////////////////////////////////
+void ModelEditor_TEST::AddItemToPalette()
+{
+ this->resMaxPercentChange = 5.0;
+ this->shareMaxPercentChange = 2.0;
+
+ this->Load("worlds/empty.world");
+
+ // Create the main window.
+ gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow();
+ QVERIFY(mainWindow != NULL);
+ mainWindow->Load();
+ mainWindow->Init();
+ mainWindow->show();
+
+ // Process some events, and draw the screen
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ gazebo::common::Time::MSleep(30);
+ QCoreApplication::processEvents();
+ mainWindow->repaint();
+ }
+
+ // verify we have a model editor widget
+ gui::ModelEditor *modelEditor =
+ dynamic_cast<gui::ModelEditor *>(mainWindow->GetEditor("model"));
+ QVERIFY(modelEditor);
+
+ // add a custom push button to the model editor palette
+ QPushButton *testButton = new QPushButton("TEST_BUTTON");
+ testButton->setObjectName("my_custom_test_button");
+ modelEditor->AddItemToPalette(testButton, "test_categorty");
+
+ QPushButton *retButton =
+ mainWindow->findChild<QPushButton *>("my_custom_test_button");
+
+ // verify that the push button is added.
+ QVERIFY(retButton);
+ QVERIFY(retButton->text().toStdString() == "TEST_BUTTON");
+
+ mainWindow->close();
+ delete mainWindow;
+ mainWindow = NULL;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(ModelEditor_TEST)
diff --git a/gazebo/gui/model/ModelEditor_TEST.hh b/gazebo/gui/model/ModelEditor_TEST.hh
new file mode 100644
index 0000000..101ddd2
--- /dev/null
+++ b/gazebo/gui/model/ModelEditor_TEST.hh
@@ -0,0 +1,32 @@
+/*
+ * 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_MODEL_EDITOR_TEST_HH_
+#define _GAZEBO_MODEL_EDITOR_TEST_HH_
+
+#include "gazebo/gui/QTestFixture.hh"
+
+/// \brief A test class for the ModelEditor class.
+class ModelEditor_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Tests adding an item to the model editor palette.
+ private slots: void AddItemToPalette();
+};
+
+#endif
diff --git a/gazebo/gui/model/SchematicViewWidget.cc b/gazebo/gui/model/SchematicViewWidget.cc
new file mode 100644
index 0000000..c5b0be4
--- /dev/null
+++ b/gazebo/gui/model/SchematicViewWidget.cc
@@ -0,0 +1,208 @@
+/*
+ * 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/common/Events.hh"
+
+#include "gazebo/gui/model/GraphScene.hh"
+#include "gazebo/gui/model/GraphView.hh"
+#include "gazebo/gui/model/ModelEditorEvents.hh"
+#include "gazebo/gui/model/SchematicViewWidget.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+SchematicViewWidget::SchematicViewWidget(QWidget *_parent)
+ : QWidget(_parent)
+{
+ this->setObjectName("SchematicViewWidget");
+
+ this->scene = new GraphScene(this);
+ this->view = new GraphView(_parent);
+
+ this->minimumWidth = 500;
+ this->minimumHeight = 500;
+
+ QHBoxLayout *canvasLayout = new QHBoxLayout(this);
+ canvasLayout->addWidget(view);
+ canvasLayout->setAlignment(Qt::AlignHCenter);
+
+ this->view->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
+ this->view->setScene(this->scene);
+ this->view->centerOn(QPointF(0, 0));
+ this->view->setDragMode(QGraphicsView::ScrollHandDrag);
+ this->view->show();
+
+ canvasLayout->setContentsMargins(0, 0, 0, 0);
+ canvasLayout->setSpacing(0);
+ this->setLayout(canvasLayout);
+}
+
+/////////////////////////////////////////////////
+void SchematicViewWidget::Reset()
+{
+ this->edges.clear();
+ this->scene->clear();
+ this->FitInView();
+}
+
+/////////////////////////////////////////////////
+void SchematicViewWidget::Init()
+{
+ this->connections.push_back(gui::model::Events::ConnectLinkInserted(
+ boost::bind(&SchematicViewWidget::AddNode, this, _1)));
+
+ this->connections.push_back(gui::model::Events::ConnectLinkRemoved(
+ boost::bind(&SchematicViewWidget::RemoveNode, this, _1)));
+
+ this->connections.push_back(gui::model::Events::ConnectJointInserted(
+ boost::bind(&SchematicViewWidget::AddEdge, this, _1, _2, _3, _4)));
+
+ this->connections.push_back(gui::model::Events::ConnectJointRemoved(
+ boost::bind(&SchematicViewWidget::RemoveEdge, this, _1)));
+}
+
+/////////////////////////////////////////////////
+std::string SchematicViewWidget::GetLeafName(const std::string &_scopedName)
+{
+ if (_scopedName.empty())
+ return "";
+
+ std::string leafName = _scopedName;
+ size_t idx = _scopedName.find_last_of("::");
+ if (idx != std::string::npos)
+ leafName = _scopedName.substr(idx+1);
+ return leafName;
+}
+
+/////////////////////////////////////////////////
+void SchematicViewWidget::AddNode(const std::string &_node)
+{
+ std::string node = this->GetLeafName(_node);
+
+ if (this->scene->HasNode(node))
+ return;
+
+ // this must be called before making changes to the graph
+ this->scene->clearLayout();
+
+ this->scene->AddNode(node);
+ this->scene->applyLayout();
+
+ this->FitInView();
+}
+
+/////////////////////////////////////////////////
+unsigned int SchematicViewWidget::GetNodeCount() const
+{
+ return static_cast<unsigned int>(this->scene->nodeCount());
+}
+
+/////////////////////////////////////////////////
+void SchematicViewWidget::RemoveNode(const std::string &_node)
+{
+ std::string node = this->GetLeafName(_node);
+
+ if (!this->scene->HasNode(node))
+ return;
+
+ // this must be called before making changes to the graph
+ this->scene->clearLayout();
+ this->scene->RemoveNode(node);
+
+ this->scene->applyLayout();
+ this->FitInView();
+}
+
+/////////////////////////////////////////////////
+bool SchematicViewWidget::HasNode(const std::string &_name) const
+{
+ return this->scene->HasNode(_name);
+}
+
+/////////////////////////////////////////////////
+void SchematicViewWidget::AddEdge(const std::string &_id,
+ const std::string &/*_name*/, const std::string &_parent,
+ const std::string &_child)
+{
+ std::string parentNode = this->GetLeafName(_parent);
+ std::string childNode = this->GetLeafName(_child);
+
+ this->edges[_id] = std::make_pair(parentNode, childNode);
+
+ // this must be called before making changes to the graph
+ this->scene->clearLayout();
+
+ this->scene->AddEdge(_id, parentNode, childNode);
+ this->scene->applyLayout();
+
+ this->FitInView();
+}
+
+/////////////////////////////////////////////////
+void SchematicViewWidget::RemoveEdge(const std::string &_id)
+{
+ auto it = this->edges.find(_id);
+ if (it != this->edges.end())
+ {
+ // this must be called before making changes to the graph
+ this->scene->clearLayout();
+
+ this->scene->RemoveEdge(_id);
+ this->scene->applyLayout();
+
+ this->FitInView();
+
+ this->edges.erase(it);
+ }
+}
+
+/////////////////////////////////////////////////
+unsigned int SchematicViewWidget::GetEdgeCount() const
+{
+ return static_cast<unsigned int>(this->scene->edgeCount());
+}
+
+/////////////////////////////////////////////////
+void SchematicViewWidget::resizeEvent(QResizeEvent */*_event*/)
+{
+ this->FitInView();
+}
+
+/////////////////////////////////////////////////
+void SchematicViewWidget::FitInView()
+{
+ QRectF newRect;
+ QRectF sceneRect = this->scene->itemsBoundingRect();
+
+ int sceneCenterX = sceneRect.x() + sceneRect.width()*0.5;
+ int sceneCenterY = sceneRect.y() + sceneRect.height()*0.5;
+
+ int sceneWidth = std::max(static_cast<int>(sceneRect.width()),
+ this->minimumWidth);
+ int sceneHeight = std::max(static_cast<int>(sceneRect.height()),
+ this->minimumHeight);
+
+ newRect.setX(sceneCenterX - sceneWidth*0.5);
+ newRect.setY(sceneCenterY - sceneHeight*0.5);
+ newRect.setWidth(sceneWidth);
+ newRect.setHeight(sceneHeight);
+
+ this->view->fitInView(newRect, Qt::KeepAspectRatio);
+ this->view->centerOn(sceneCenterX, sceneCenterY);
+ this->scene->setSceneRect(newRect);
+}
diff --git a/gazebo/gui/model/SchematicViewWidget.hh b/gazebo/gui/model/SchematicViewWidget.hh
new file mode 100644
index 0000000..b4a1df7
--- /dev/null
+++ b/gazebo/gui/model/SchematicViewWidget.hh
@@ -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.
+ *
+*/
+
+#ifndef _GAZEBO_SCHEMATIC_VIEW_WIDGET_HH_
+#define _GAZEBO_SCHEMATIC_VIEW_WIDGET_HH_
+
+#include <utility>
+#include <map>
+#include <string>
+#include <vector>
+
+#include "gazebo/common/CommonTypes.hh"
+#include "gazebo/gui/qt.h"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class GraphView;
+ class GraphScene;
+
+ /// \class SchematicViewWidget SchematicViewWidget.hh
+ /// \brief The parent widget of the CML editor
+ class GZ_GUI_MODEL_VISIBLE SchematicViewWidget : public QWidget
+ {
+ /// \brief Constructor
+ /// \param[in] _parent Parent QWidget.
+ public: SchematicViewWidget(QWidget *_parent = 0);
+
+ /// \brief Destructor
+ public: ~SchematicViewWidget() = default;
+
+ /// \brief Initialize the widget to listen to events/topics
+ public: void Init();
+
+ /// \brief Reset the widget and clear the scene.
+ public: void Reset();
+
+ /// \brief Add a node to the scene in the widget.
+ /// \param[in] _node Name of node.
+ public: void AddNode(const std::string &_node);
+
+ /// \brief Remove a node from the scene in the widget
+ /// \param[in] _node Name of node.
+ public: void RemoveNode(const std::string &_node);
+
+ /// \brief Check if a node exists in the scene in the widget.
+ /// \param[in] _name Name of the name.
+ /// \return True if the node exists.
+ public: bool HasNode(const std::string &_name) const;
+
+ /// \brief Add an edge to the scene in the widget
+ /// \param[in] _id Unique id of edge.
+ /// \param[in] _name Name of edge.
+ /// \param[in] _parent Name of parent node.
+ /// \param[in] _child Name of child node.
+ public: void AddEdge(const std::string &_id, const std::string &_name,
+ const std::string &_parent, const std::string &_child);
+
+ /// \brief Remove an edge from the scene in the widget
+ /// \param[in] _id Unique id of edge.
+ public: void RemoveEdge(const std::string &_id);
+
+ /// \brief Get number of nodes in the scene.
+ /// \return Number of nodes.
+ public: unsigned int GetNodeCount() const;
+
+ /// \brief Get number of edges in the scene.
+ /// \return Number of edges.
+ public: unsigned int GetEdgeCount() const;
+
+ /// \brief Scales the view to ensure the items of the scene are visible.
+ public: void FitInView();
+
+ /// \brief Helper function to get the leaf name from a scoped name.
+ /// \param[in] _scopedName Scoped name.
+ /// \return Leaf name.
+ private: std::string GetLeafName(const std::string &_scopedName);
+
+ /// \brief Qt event received when the widget is being resized
+ /// \param[in] _event Resize event.
+ private: void resizeEvent(QResizeEvent *_event);
+
+ /// \brief Qt Graphics Scene where graphics items are drawn in
+ private: GraphScene *scene;
+
+ /// \brief Qt Graphics View attached to the scene
+ private: GraphView *view;
+
+ /// \brief Minimum width of the Qt graphics scene
+ private: int minimumWidth;
+
+ /// \brief Minimum height of the Qt graphics scene
+ private: int minimumHeight;
+
+ /// \brief A map of joint id to parent-child link pair.
+ private: std::map<std::string, std::pair<std::string, std::string>>
+ edges;
+
+ /// \brief A list of gazebo event connections.
+ private: std::vector<event::ConnectionPtr> connections;
+ };
+ }
+}
+
+#endif
diff --git a/gazebo/gui/model/SchematicViewWidget_TEST.cc b/gazebo/gui/model/SchematicViewWidget_TEST.cc
new file mode 100644
index 0000000..5825edf
--- /dev/null
+++ b/gazebo/gui/model/SchematicViewWidget_TEST.cc
@@ -0,0 +1,82 @@
+/*
+ * 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/gui/model/SchematicViewWidget.hh"
+#include "gazebo/gui/model/SchematicViewWidget_TEST.hh"
+
+using namespace gazebo;
+
+/////////////////////////////////////////////////
+void SchematicViewWidget_TEST::AddRemove()
+{
+ gui::SchematicViewWidget *svWidget = new gui::SchematicViewWidget();
+ QVERIFY(svWidget);
+
+ // add nodes
+ QCOMPARE(svWidget->GetNodeCount(), 0u);
+ svWidget->AddNode("node_a");
+ Q_ASSERT(svWidget->HasNode("node_a"));
+ QCOMPARE(svWidget->GetNodeCount(), 1u);
+ svWidget->AddNode("node_b");
+ Q_ASSERT(svWidget->HasNode("node_b"));
+ QCOMPARE(svWidget->GetNodeCount(), 2u);
+ svWidget->AddNode("node_c");
+ Q_ASSERT(svWidget->HasNode("node_c"));
+ QCOMPARE(svWidget->GetNodeCount(), 3u);
+ svWidget->AddNode("node_d");
+ Q_ASSERT(svWidget->HasNode("node_d"));
+ QCOMPARE(svWidget->GetNodeCount(), 4u);
+ // remove node
+ svWidget->RemoveNode("node_d");
+ Q_ASSERT(!svWidget->HasNode("node_d"));
+ QCOMPARE(svWidget->GetNodeCount(), 3u);
+ // removing a node that doesn't exist doesn't break anything
+ svWidget->RemoveNode("node_d");
+ QCOMPARE(svWidget->GetNodeCount(), 3u);
+ // add it back
+ svWidget->AddNode("node_d");
+ Q_ASSERT(svWidget->HasNode("node_d"));
+ QCOMPARE(svWidget->GetNodeCount(), 4u);
+
+ // add edges
+ QCOMPARE(svWidget->GetEdgeCount(), 0u);
+ svWidget->AddEdge("id_0", "edge_0", "node_a", "node_b");
+ QCOMPARE(svWidget->GetEdgeCount(), 1u);
+ svWidget->AddEdge("id_1", "edge_1", "node_b", "node_c");
+ QCOMPARE(svWidget->GetEdgeCount(), 2u);
+ svWidget->AddEdge("id_2", "edge_2", "node_a", "node_c");
+ QCOMPARE(svWidget->GetEdgeCount(), 3u);
+ // remove edge
+ svWidget->RemoveEdge("id_2");
+ QCOMPARE(svWidget->GetEdgeCount(), 2u);
+ // removing an edge that doesn't exist doesn't break anything
+ svWidget->RemoveEdge("id_2");
+ QCOMPARE(svWidget->GetEdgeCount(), 2u);
+ // add it back
+ svWidget->AddEdge("id_2", "edge_2", "node_a", "node_c");
+ QCOMPARE(svWidget->GetEdgeCount(), 3u);
+
+ // remove node and its edges
+ svWidget->RemoveNode("node_b");
+ QCOMPARE(svWidget->GetNodeCount(), 3u);
+ QCOMPARE(svWidget->GetEdgeCount(), 1u);
+
+ delete svWidget;
+}
+
+// Generate a main function for the test
+QTEST_MAIN(SchematicViewWidget_TEST)
diff --git a/gazebo/gui/model/SchematicViewWidget_TEST.hh b/gazebo/gui/model/SchematicViewWidget_TEST.hh
new file mode 100644
index 0000000..31560c1
--- /dev/null
+++ b/gazebo/gui/model/SchematicViewWidget_TEST.hh
@@ -0,0 +1,32 @@
+/*
+ * 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_SCHEMATIC_VIEW_WIDGET_TEST_HH_
+#define _GAZEBO_SCHEMATIC_VIEW_WIDGET_TEST_HH_
+
+#include "gazebo/gui/QTestFixture.hh"
+
+/// \brief A test class for the SchematicView class.
+class SchematicViewWidget_TEST : public QTestFixture
+{
+ Q_OBJECT
+
+ /// \brief Test adding and removing nodes.
+ private slots: void AddRemove();
+};
+
+#endif
diff --git a/gazebo/gui/model/VisualConfig.cc b/gazebo/gui/model/VisualConfig.cc
new file mode 100644
index 0000000..7f8dc7a
--- /dev/null
+++ b/gazebo/gui/model/VisualConfig.cc
@@ -0,0 +1,333 @@
+/*
+ * 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/msgs/msgs.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/rendering/Material.hh"
+#include "gazebo/gui/ConfigWidget.hh"
+#include "gazebo/gui/model/VisualConfig.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+VisualConfig::VisualConfig()
+{
+ this->setObjectName("VisualConfig");
+
+ // Layout for list
+ this->listLayout = new QVBoxLayout();
+ this->listLayout->setContentsMargins(0, 0, 0, 0);
+ this->listLayout->setAlignment(Qt::AlignTop);
+
+ // Widget for list, which will be scrollable
+ QWidget *listWidget = new QWidget();
+ listWidget->setLayout(this->listLayout);
+ listWidget->setStyleSheet("QWidget{background-color: #808080}");
+
+ // Scroll area for list
+ QScrollArea *scrollArea = new QScrollArea;
+ scrollArea->setWidget(listWidget);
+ scrollArea->setWidgetResizable(true);
+
+ // Add Visual button
+ QPushButton *addVisualButton = new QPushButton(tr("+ &Another Visual"));
+ addVisualButton->setMaximumWidth(200);
+ connect(addVisualButton, SIGNAL(clicked()), this, SLOT(OnAddVisual()));
+
+ // Main layout
+ QVBoxLayout *mainLayout = new QVBoxLayout;
+ mainLayout->addWidget(scrollArea);
+ mainLayout->addWidget(addVisualButton);
+ mainLayout->setContentsMargins(0, 0, 0, 0);
+ this->setLayout(mainLayout);
+
+ this->counter = 0;
+ this->signalMapper = new QSignalMapper(this);
+
+ connect(this->signalMapper, SIGNAL(mapped(int)),
+ this, SLOT(OnRemoveVisual(int)));
+}
+
+/////////////////////////////////////////////////
+VisualConfig::~VisualConfig()
+{
+ while (!this->configs.empty())
+ {
+ auto config = this->configs.begin();
+ this->configs.erase(config);
+ }
+}
+
+/////////////////////////////////////////////////
+void VisualConfig::OnAddVisual()
+{
+ std::stringstream visualIndex;
+ visualIndex << "visual_" << this->counter;
+ this->AddVisual(visualIndex.str());
+ emit VisualAdded(visualIndex.str());
+}
+
+/////////////////////////////////////////////////
+unsigned int VisualConfig::GetVisualCount() const
+{
+ return this->configs.size();
+}
+
+/////////////////////////////////////////////////
+void VisualConfig::Reset()
+{
+ for (auto &it : this->configs)
+ {
+ this->listLayout->removeWidget(it.second->widget);
+ delete it.second;
+ }
+
+ this->configs.clear();
+}
+
+/////////////////////////////////////////////////
+void VisualConfig::AddVisual(const std::string &_name,
+ const msgs::Visual *_visualMsg)
+{
+ // Header button
+ QRadioButton *headerButton = new QRadioButton();
+ headerButton->setChecked(false);
+ headerButton->setFocusPolicy(Qt::NoFocus);
+ headerButton->setText(QString(_name.c_str()));
+ headerButton->setStyleSheet(
+ "QRadioButton {\
+ color: #d0d0d0;\
+ }\
+ QRadioButton::indicator::unchecked {\
+ image: url(:/images/right_arrow.png);\
+ }\
+ QRadioButton::indicator::checked {\
+ image: url(:/images/down_arrow.png);\
+ }");
+
+ // Remove button
+ QToolButton *removeVisualButton = new QToolButton(this);
+ removeVisualButton->setFixedSize(QSize(30, 30));
+ removeVisualButton->setToolTip("Remove " + QString(_name.c_str()));
+ removeVisualButton->setIcon(QPixmap(":/images/trashcan.png"));
+ removeVisualButton->setToolButtonStyle(Qt::ToolButtonIconOnly);
+ removeVisualButton->setIconSize(QSize(16, 16));
+ removeVisualButton->setCheckable(false);
+ connect(removeVisualButton, SIGNAL(clicked()), this->signalMapper,
+ SLOT(map()));
+ this->signalMapper->setMapping(removeVisualButton, this->counter);
+
+ // Header Layout
+ QHBoxLayout *headerLayout = new QHBoxLayout;
+ headerLayout->setContentsMargins(0, 0, 0, 0);
+ headerLayout->addWidget(headerButton);
+ headerLayout->addWidget(removeVisualButton);
+
+ // Header widget
+ QWidget *headerWidget = new QWidget;
+ headerWidget->setLayout(headerLayout);
+
+ // ConfigWidget
+ msgs::Visual msgToLoad;
+ if (_visualMsg)
+ msgToLoad = *_visualMsg;
+
+ // set default values
+ // TODO: auto-fill them with SDF defaults
+ msgs::Material *matMsg = msgToLoad.mutable_material();
+ if (!matMsg->has_lighting())
+ matMsg->set_lighting(true);
+
+ ConfigWidget *configWidget = new ConfigWidget;
+ configWidget->Load(&msgToLoad);
+ configWidget->hide();
+
+ configWidget->SetWidgetVisible("id", false);
+ configWidget->SetWidgetVisible("name", false);
+ configWidget->SetWidgetVisible("parent_name", false);
+ configWidget->SetWidgetVisible("parent_id", false);
+ configWidget->SetWidgetVisible("delete_me", false);
+ configWidget->SetWidgetVisible("is_static", false);
+ configWidget->SetWidgetVisible("visible", false);
+ configWidget->SetWidgetVisible("scale", false);
+ configWidget->SetWidgetVisible("plugin", false);
+ configWidget->SetWidgetVisible("type", false);
+ configWidget->SetWidgetReadOnly("id", true);
+ configWidget->SetWidgetReadOnly("name", true);
+ configWidget->SetWidgetReadOnly("parent_name", true);
+ configWidget->SetWidgetReadOnly("parent_id", true);
+ configWidget->SetWidgetReadOnly("delete_me", true);
+ configWidget->SetWidgetReadOnly("is_static", true);
+ configWidget->SetWidgetReadOnly("visible", true);
+ configWidget->SetWidgetReadOnly("scale", true);
+ configWidget->SetWidgetReadOnly("plugin", true);
+ configWidget->SetWidgetReadOnly("type", true);
+
+ // Item layout
+ QVBoxLayout *itemLayout = new QVBoxLayout();
+ itemLayout->addWidget(headerWidget);
+ itemLayout->addWidget(configWidget);
+
+ // Put the layout in a widget which can be added/deleted
+ QWidget *item = new QWidget();
+ item->setLayout(itemLayout);
+
+ // Add to the list
+ this->listLayout->addWidget(item);
+
+ // Fill ConfigData
+ VisualConfigData *configData = new VisualConfigData;
+ configData->configWidget = configWidget;
+ configData->id = this->counter;
+ configData->widget = item;
+ configData->name = _name;
+ connect(headerButton, SIGNAL(toggled(bool)), configData,
+ SLOT(OnToggleItem(bool)));
+ this->configs[this->counter] = configData;
+
+ this->counter++;
+}
+
+/////////////////////////////////////////////////
+void VisualConfig::UpdateVisual(const std::string &_name,
+ ConstVisualPtr _visualMsg)
+{
+ for (auto &it : this->configs)
+ {
+ if (it.second->name == _name)
+ {
+ VisualConfigData *configData = it.second;
+ configData->configWidget->UpdateFromMsg(_visualMsg.get());
+ break;
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void VisualConfig::OnRemoveVisual(int _id)
+{
+ auto it = this->configs.find(_id);
+ if (it == this->configs.end())
+ {
+ gzerr << "Visual not found " << std::endl;
+ return;
+ }
+
+ VisualConfigData *configData = this->configs[_id];
+
+ // Ask for confirmation
+ std::string msg;
+
+ if (this->configs.size() == 1)
+ {
+ msg = "Are you sure you want to remove " +
+ configData->name + "?\n\n" +
+ "This is the only visual. \n" +
+ "Without visuals, this link won't be visible.\n";
+ }
+ else
+ {
+ msg = "Are you sure you want to remove " +
+ configData->name + "?\n";
+ }
+
+ QMessageBox msgBox(QMessageBox::Warning, QString("Remove visual?"),
+ QString(msg.c_str()));
+ msgBox.setWindowFlags(Qt::WindowStaysOnTopHint);
+
+ QPushButton *cancelButton =
+ msgBox.addButton("Cancel", QMessageBox::RejectRole);
+ QPushButton *removeButton = msgBox.addButton("Remove",
+ QMessageBox::AcceptRole);
+ msgBox.setDefaultButton(removeButton);
+ msgBox.setEscapeButton(cancelButton);
+ msgBox.exec();
+ if (msgBox.clickedButton() != removeButton)
+ return;
+
+ // Remove
+ this->listLayout->removeWidget(configData->widget);
+ delete configData->widget;
+
+ emit VisualRemoved(configData->name);
+ this->configs.erase(it);
+}
+
+/////////////////////////////////////////////////
+msgs::Visual *VisualConfig::GetData(const std::string &_name) const
+{
+ for (auto const &it : this->configs)
+ {
+ if (it.second->name == _name)
+ return dynamic_cast<msgs::Visual *>(it.second->configWidget->GetMsg());
+ }
+ return NULL;
+}
+
+/////////////////////////////////////////////////
+void VisualConfig::SetGeometry(const std::string &_name,
+ const math::Vector3 &_size, const std::string &_uri)
+{
+ for (auto &it : this->configs)
+ {
+ if (it.second->name == _name)
+ {
+ math::Vector3 dimensions;
+ std::string uri;
+ std::string type = it.second->configWidget->GetGeometryWidgetValue(
+ "geometry", dimensions, uri);
+ it.second->configWidget->SetGeometryWidgetValue("geometry", type,
+ _size, _uri);
+ break;
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void VisualConfig::SetMaterial(const std::string &_name,
+ const std::string &_materialName, const common::Color &_ambient,
+ const common::Color &_diffuse, const common::Color &_specular,
+ const common::Color &_emissive)
+{
+ for (auto &it : this->configs)
+ {
+ if (it.second->name == _name)
+ {
+ it.second->configWidget->SetStringWidgetValue("material::script::name",
+ _materialName);
+ it.second->configWidget->SetColorWidgetValue("material::ambient",
+ _ambient);
+ it.second->configWidget->SetColorWidgetValue("material::diffuse",
+ _diffuse);
+ it.second->configWidget->SetColorWidgetValue("material::specular",
+ _specular);
+ it.second->configWidget->SetColorWidgetValue("material::emissive",
+ _emissive);
+ break;
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void VisualConfigData::OnToggleItem(bool _checked)
+{
+ if (_checked)
+ this->configWidget->show();
+ else
+ this->configWidget->hide();
+}
diff --git a/gazebo/gui/model/VisualConfig.hh b/gazebo/gui/model/VisualConfig.hh
new file mode 100644
index 0000000..28f68f8
--- /dev/null
+++ b/gazebo/gui/model/VisualConfig.hh
@@ -0,0 +1,147 @@
+/*
+ * 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 _VISUAL_CONFIG_HH_
+#define _VISUAL_CONFIG_HH_
+
+#include <map>
+#include <string>
+
+#include "gazebo/math/Pose.hh"
+
+#include "gazebo/gui/qt.h"
+#include "gazebo/gui/model/ModelData.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class ConfigWidget;
+
+ /// \addtogroup gazebo_gui
+ /// \{
+
+ /// \class VisualConfigData VisualConfig.hh
+ /// \brief A class of widgets used for configuring visual properties.
+ class VisualConfigData : public QWidget
+ {
+ Q_OBJECT
+
+ /// \brief Qt callback when this item's button has been pressed.
+ /// \param[in] _checked Whether it was checked or unchecked.
+ private slots: void OnToggleItem(bool _checked);
+
+ /// \brief Unique ID of this visual config.
+ public: int id;
+
+ /// \brief Name of the visual.
+ public: std::string name;
+
+ /// \brief Config widget for configuring visual properties.
+ public: ConfigWidget *configWidget;
+
+ /// \brief Widget associated with this data.
+ public: QWidget *widget;
+ };
+
+ /// \class VisualConfig VisualConfig.hh
+ /// \brief A tab for configuring visual properties of a link.
+ class VisualConfig : public QWidget
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ public: VisualConfig();
+
+ /// \brief Destructor
+ public: ~VisualConfig();
+
+ /// \brief Add a visual widget to the tab.
+ /// \param[in] _name Name of visual added.
+ /// \param[in] _visualMsg Msg containing information of the visual
+ /// to be added.
+ public: void AddVisual(const std::string &_name,
+ const msgs::Visual *_visualMsg = NULL);
+
+ /// \brief Update a visual widget from a visual msg.
+ /// \param[in] _name Name of visual to be updated.
+ /// \param[in] _visualMsg Msg used to update the visual widget values.
+ public: void UpdateVisual(const std::string &_name,
+ ConstVisualPtr _visualMsg);
+
+ /// \brief Reset the visual tab.
+ public: void Reset();
+
+ /// \brief Get the number of visuals.
+ /// \return Number of visuals.
+ public: unsigned int GetVisualCount() const;
+
+ /// \brief Get the msg containing all visual data.
+ /// \param[in] _name Name of visual.
+ /// \return Visual msg.
+ public: msgs::Visual *GetData(const std::string &_name) const;
+
+ /// \brief Set the geometry data of a visual
+ /// \param[in] _name Name of visual.
+ /// \param[in] _size Size of the geometry.
+ /// \param[in] _uri URI of the geometry.
+ public: void SetGeometry(const std::string &_name,
+ const math::Vector3 &_size, const std::string &_uri = "");
+
+ /// \brief Set the material of a visual
+ /// \param[in] _name Name of visual.
+ /// \param[in] _materialName Name of material.
+ /// \param[in] _ambient Ambient color of visual.
+ /// \param[in] _diffuse Diffuse color of visual.
+ /// \param[in] _specular Specular color of visual.
+ /// \param[in] _emissive Emissive color of visual.
+ public: void SetMaterial(const std::string &_name,
+ const std::string &_materialName,
+ const common::Color &_ambient, const common::Color &_diffuse,
+ const common::Color &_specular, const common::Color &_emissive);
+
+ /// \brief Qt signal emitted when a visual is removed.
+ /// \param[in] _name Name of visual removed.
+ Q_SIGNALS: void VisualRemoved(const std::string &_name);
+
+ /// \brief Qt signal emitted when a visual is added.
+ /// \param[in] _name Name of visual added.
+ Q_SIGNALS: void VisualAdded(const std::string &_name);
+
+ /// \brief Qt callback when a visual is to be added.
+ private slots: void OnAddVisual();
+
+ /// \brief Qt callback when a visual is to be removed.
+ /// \param[in] _id Id of item to be removed.
+ private slots: void OnRemoveVisual(int _id);
+
+ /// \brief Map of id to visual config widget.
+ private: std::map<int, VisualConfigData *> configs;
+
+ /// \brief Counter for the number of visuals.
+ private: int counter;
+
+ /// \brief Qt signal mapper for mapping remove button signals.
+ private: QSignalMapper *signalMapper;
+
+ /// \brief Layout which holds all visual items.
+ private: QVBoxLayout *listLayout;
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/gui/model/qgv/CMakeLists.txt b/gazebo/gui/model/qgv/CMakeLists.txt
new file mode 100644
index 0000000..aa0254c
--- /dev/null
+++ b/gazebo/gui/model/qgv/CMakeLists.txt
@@ -0,0 +1,48 @@
+add_definitions(-DWITH_CGRAPH)
+add_definitions(-DHAVE_STRING_H)
+
+# Activate Export macro for building the library itself
+# if QGVCORE_LIB is not set or equal 0 (null), import macro is used
+add_definitions(-DQGVCORE_LIB)
+
+include ( ${QT_USE_FILE} )
+
+SET(sources
+ private/QGVCore.cpp
+ private/QGVGraphPrivate.cpp
+ private/QGVEdgePrivate.cpp
+ private/QGVGvcPrivate.cpp
+ private/QGVNodePrivate.cpp
+ QGVEdge.cpp
+ QGVNode.cpp
+ QGVScene.cpp
+ QGVSubGraph.cpp
+)
+
+SET(qt_headers
+ QGVScene.h
+)
+
+SET(headers
+ qgv.h
+ QGVEdge.h
+ QGVNode.h
+ QGVSubGraph.h
+)
+
+add_definitions(${QT_DEFINITIONS})
+
+QT4_WRAP_CPP(headers_MOC ${qt_headers})
+
+include_directories(${GRAPHVIZ_INCLUDE_DIR})
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/private)
+
+add_library(gazebo_gui_model_qgv STATIC ${sources} ${headers_MOC} ${headers})
+
+set_target_properties(gazebo_gui_model_qgv PROPERTIES POSITION_INDEPENDENT_CODE ON)
+
+target_link_libraries(gazebo_gui_model_qgv
+ ${QT_LIBRARIES}
+ ${GRAPHVIZ_LIBRARIES}
+)
diff --git a/gazebo/gui/model/qgv/GraphViz.pri b/gazebo/gui/model/qgv/GraphViz.pri
new file mode 100644
index 0000000..60ee8b4
--- /dev/null
+++ b/gazebo/gui/model/qgv/GraphViz.pri
@@ -0,0 +1,17 @@
+#GraphViz librairie
+DEFINES += WITH_CGRAPH
+INCLUDEPATH += private
+QMAKE_CXXFLAGS += -DQGVCORE_LIB
+
+unix {
+ CONFIG += link_pkgconfig
+ PKGCONFIG += libcdt libgvc libcgraph libgraph
+}
+win32 {
+ #Configure Windows GraphViz path here :
+ GRAPHVIZ_PATH = "D:/Program Files (x86)/Graphviz2.36/"
+ DEFINES += WIN32_DLL
+ DEFINES += GVDLL
+ INCLUDEPATH += $$GRAPHVIZ_PATH/include/graphviz
+ LIBS += -L$$GRAPHVIZ_PATH/lib/release/lib -lgvc -lcgraph -lgraph -lcdt
+}
diff --git a/gazebo/gui/model/qgv/LICENSE.txt b/gazebo/gui/model/qgv/LICENSE.txt
new file mode 100644
index 0000000..4362b49
--- /dev/null
+++ b/gazebo/gui/model/qgv/LICENSE.txt
@@ -0,0 +1,502 @@
+ GNU LESSER GENERAL PUBLIC LICENSE
+ Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL. It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+ Preamble
+
+ The licenses for most software are designed to take away your
+freedom to share and change it. By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+ This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it. You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+ When we speak of free software, we are referring to freedom of use,
+not price. Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+ To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights. These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+ For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you. You must make sure that they, too, receive or can get the source
+code. If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it. And you must show them these terms so they know their rights.
+
+ We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+ To protect each distributor, we want to make it very clear that
+there is no warranty for the free library. Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+ Finally, software patents pose a constant threat to the existence of
+any free program. We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder. Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+ Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License. This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License. We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+ When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library. The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom. The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+ We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License. It also provides other free software developers Less
+of an advantage over competing non-free programs. These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries. However, the Lesser license provides advantages in certain
+special circumstances.
+
+ For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard. To achieve this, non-free programs must be
+allowed to use the library. A more frequent case is that a free
+library does the same job as widely used non-free libraries. In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+ In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software. For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+ Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+ The precise terms and conditions for copying, distribution and
+modification follow. Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library". The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+ GNU LESSER GENERAL PUBLIC LICENSE
+ TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+ 0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+ A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+ The "Library", below, refers to any such software library or work
+which has been distributed under these terms. A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language. (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+ "Source code" for a work means the preferred form of the work for
+making modifications to it. For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+ Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope. The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it). Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+
+ 1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+ You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+ 2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+ a) The modified work must itself be a software library.
+
+ b) You must cause the files modified to carry prominent notices
+ stating that you changed the files and the date of any change.
+
+ c) You must cause the whole of the work to be licensed at no
+ charge to all third parties under the terms of this License.
+
+ d) If a facility in the modified Library refers to a function or a
+ table of data to be supplied by an application program that uses
+ the facility, other than as an argument passed when the facility
+ is invoked, then you must make a good faith effort to ensure that,
+ in the event an application does not supply such function or
+ table, the facility still operates, and performs whatever part of
+ its purpose remains meaningful.
+
+ (For example, a function in a library to compute square roots has
+ a purpose that is entirely well-defined independent of the
+ application. Therefore, Subsection 2d requires that any
+ application-supplied function or table used by this function must
+ be optional: if the application does not supply it, the square
+ root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole. If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works. But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+ 3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library. To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License. (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.) Do not make any other change in
+these notices.
+
+ Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+ This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+ 4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+ If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+ 5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library". Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+ However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library". The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+ When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library. The
+threshold for this to be true is not precisely defined by law.
+
+ If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work. (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+ Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+ 6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+ You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License. You must supply a copy of this License. If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License. Also, you must do one
+of these things:
+
+ a) Accompany the work with the complete corresponding
+ machine-readable source code for the Library including whatever
+ changes were used in the work (which must be distributed under
+ Sections 1 and 2 above); and, if the work is an executable linked
+ with the Library, with the complete machine-readable "work that
+ uses the Library", as object code and/or source code, so that the
+ user can modify the Library and then relink to produce a modified
+ executable containing the modified Library. (It is understood
+ that the user who changes the contents of definitions files in the
+ Library will not necessarily be able to recompile the application
+ to use the modified definitions.)
+
+ b) Use a suitable shared library mechanism for linking with the
+ Library. A suitable mechanism is one that (1) uses at run time a
+ copy of the library already present on the user's computer system,
+ rather than copying library functions into the executable, and (2)
+ will operate properly with a modified version of the library, if
+ the user installs one, as long as the modified version is
+ interface-compatible with the version that the work was made with.
+
+ c) Accompany the work with a written offer, valid for at
+ least three years, to give the same user the materials
+ specified in Subsection 6a, above, for a charge no more
+ than the cost of performing this distribution.
+
+ d) If distribution of the work is made by offering access to copy
+ from a designated place, offer equivalent access to copy the above
+ specified materials from the same place.
+
+ e) Verify that the user has already received a copy of these
+ materials or that you have already sent this user a copy.
+
+ For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it. However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+ It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system. Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+ 7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+ a) Accompany the combined library with a copy of the same work
+ based on the Library, uncombined with any other library
+ facilities. This must be distributed under the terms of the
+ Sections above.
+
+ b) Give prominent notice with the combined library of the fact
+ that part of it is a work based on the Library, and explaining
+ where to find the accompanying uncombined form of the same work.
+
+ 8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License. Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License. However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+ 9. You are not required to accept this License, since you have not
+signed it. However, nothing else grants you permission to modify or
+distribute the Library or its derivative works. These actions are
+prohibited by law if you do not accept this License. Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+ 10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions. You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+ 11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all. For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices. Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+ 12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded. In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+ 13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number. If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation. If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+ 14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission. For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this. Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+ NO WARRANTY
+
+ 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+ 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+ END OF TERMS AND CONDITIONS
+
+ How to Apply These Terms to Your New Libraries
+
+ If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change. You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+ To apply these terms, attach the following notices to the library. It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+ <one line to give the library's name and a brief idea of what it does.>
+ Copyright (C) <year> <name of author>
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 2.1 of the License, or (at your option) any later version.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with this library; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary. Here is a sample; alter the names:
+
+ Yoyodyne, Inc., hereby disclaims all copyright interest in the
+ library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+ <signature of Ty Coon>, 1 April 1990
+ Ty Coon, President of Vice
+
+That's all there is to it!
diff --git a/gazebo/gui/model/qgv/QGVCore.pro b/gazebo/gui/model/qgv/QGVCore.pro
new file mode 100644
index 0000000..8d2e4a3
--- /dev/null
+++ b/gazebo/gui/model/qgv/QGVCore.pro
@@ -0,0 +1,47 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2013-04-17T09:06:06
+#
+#-------------------------------------------------
+
+QT += core
+
+greaterThan(QT_MAJOR_VERSION, 4){
+ QT += widgets
+}
+lessThan(QT_MAJOR_VERSION, 5) {
+ QT += gui
+}
+
+TARGET = QGVCore
+TEMPLATE = lib
+CONFIG += shared
+
+DESTDIR = ../lib
+DLLDESTDIR = ../bin
+
+#GraphViz librairie
+!include(GraphViz.pri) {
+ error("fail open GraphViz.pri")
+ }
+
+SOURCES += QGVScene.cpp \
+ QGVNode.cpp \
+ QGVEdge.cpp \
+ QGVSubGraph.cpp \
+ private/QGVCore.cpp \
+ private/QGVGraphPrivate.cpp \
+ private/QGVGvcPrivate.cpp \
+ private/QGVEdgePrivate.cpp \
+ private/QGVNodePrivate.cpp
+
+HEADERS += QGVScene.h \
+ QGVNode.h \
+ QGVEdge.h \
+ QGVSubGraph.h \
+ private/QGVCore.h \
+ private/QGVGraphPrivate.h \
+ private/QGVGvcPrivate.h \
+ private/QGVEdgePrivate.h \
+ private/QGVNodePrivate.h \
+ qgv.h
diff --git a/gazebo/gui/model/qgv/QGVEdge.cpp b/gazebo/gui/model/qgv/QGVEdge.cpp
new file mode 100644
index 0000000..bcbda20
--- /dev/null
+++ b/gazebo/gui/model/qgv/QGVEdge.cpp
@@ -0,0 +1,207 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#include <QGVEdge.h>
+#include <QGVCore.h>
+#include <QGVScene.h>
+#include <QGVGraphPrivate.h>
+#include <QGVEdgePrivate.h>
+#include <QDebug>
+#include <QPainter>
+
+QGVEdge::QGVEdge(QGVEdgePrivate *edge, QGVScene *qgvscene)
+ : _scene(qgvscene), _edge(edge)
+{
+// setFlag(QGraphicsItem::ItemIsSelectable, true);
+}
+
+QGVEdge::~QGVEdge()
+{
+ _scene->removeItem(this);
+ delete _edge;
+}
+
+QString QGVEdge::label() const
+{
+ return getAttribute("xlabel");
+}
+
+QRectF QGVEdge::boundingRect() const
+{
+ return _path.boundingRect() | _head_arrow.boundingRect() | _tail_arrow.boundingRect() | _label_rect;
+}
+
+QPainterPath QGVEdge::shape() const
+{
+ QPainterPathStroker ps;
+ ps.setCapStyle(_pen.capStyle());
+ ps.setWidth(_pen.widthF() + 10);
+ ps.setJoinStyle(_pen.joinStyle());
+ ps.setMiterLimit(_pen.miterLimit());
+ return ps.createStroke(_path);
+}
+
+void QGVEdge::setLabel(const QString &_xlabel)
+{
+ setAttribute("xlabel", _xlabel);
+}
+
+void QGVEdge::setSource(const QString &_source)
+{
+ this->sourceNode = _source;
+}
+
+void QGVEdge::setTarget(const QString &_target)
+{
+ this->targetNode = _target;
+}
+
+QString QGVEdge::source() const
+{
+ return sourceNode;
+}
+
+QString QGVEdge::target() const
+{
+ return targetNode;
+}
+
+void QGVEdge::paint(QPainter * painter,
+ const QStyleOptionGraphicsItem */*_option*/, QWidget */*_widget*/)
+{
+ painter->save();
+
+ if(isSelected())
+ {
+ QPen tpen(_pen);
+ tpen.setColor(_pen.color().darker(120));
+ tpen.setStyle(Qt::DotLine);
+ painter->setPen(tpen);
+ }
+ else
+ painter->setPen(_pen);
+
+
+ painter->drawPath(_path);
+
+ /*
+ QRectF pp = _path.controlPointRect();
+ if(pp.width() < pp.height())
+ {
+ painter->save();
+ painter->translate(_label_rect.topLeft());
+ painter->rotate(90);
+ painter->drawText(QRectF(QPointF(0, -_label_rect.width()),
+ _label_rect.size()), Qt::AlignCenter, _label);
+ painter->restore();
+ }
+ else
+ */
+ painter->drawText(_label_rect, Qt::AlignCenter, _label);
+
+ painter->setBrush(QBrush(_pen.color(), Qt::SolidPattern));
+ painter->drawPolygon(_head_arrow);
+ painter->drawPolygon(_tail_arrow);
+ painter->restore();
+}
+
+void QGVEdge::setAttribute(const QString &name, const QString &value)
+{
+ agsafeset(_edge->edge(), name.toLocal8Bit().data(),
+ value.toLocal8Bit().data(), (char *)"");
+}
+
+QString QGVEdge::getAttribute(const QString &name) const
+{
+ char* value = agget(_edge->edge(), name.toLocal8Bit().data());
+ if(value)
+ return value;
+ return QString();
+}
+
+void QGVEdge::updateLayout()
+{
+ prepareGeometryChange();
+
+ qreal gheight = QGVCore::graphHeight(_scene->_graph->graph());
+
+ const splines* spl = ED_spl(_edge->edge());
+ _path = QGVCore::toPath(spl, gheight);
+
+
+ //Edge arrows
+ if((spl->list != 0) && (spl->list->size%3 == 1))
+ {
+ if(spl->list->sflag)
+ {
+ _tail_arrow = toArrow(
+ QLineF(QGVCore::toPoint(spl->list->list[0], gheight),
+ QGVCore::toPoint(spl->list->sp, gheight)));
+ }
+
+ if(spl->list->eflag)
+ {
+ _head_arrow = toArrow(
+ QLineF(QGVCore::toPoint(spl->list->list[spl->list->size-1],
+ gheight), QGVCore::toPoint(spl->list->ep, gheight)));
+ }
+ }
+
+ _pen.setWidth(1);
+ _pen.setColor(QGVCore::toColor(getAttribute("color")));
+ _pen.setStyle(QGVCore::toPenStyle(getAttribute("style")));
+
+ // Edge label
+ textlabel_t *xlabel = ED_xlabel(_edge->edge());
+ if(xlabel)
+ {
+ _label = xlabel->text;
+ _label_rect.setSize(QSize(xlabel->dimen.x, xlabel->dimen.y));
+ _label_rect.moveCenter(QGVCore::toPoint(xlabel->pos,
+ QGVCore::graphHeight(_scene->_graph->graph())));
+ }
+
+ setToolTip(getAttribute("tooltip"));
+}
+
+QPolygonF QGVEdge::toArrow(const QLineF &line) const
+{
+ QLineF n = line.normalVector();
+ QPointF o(n.dx() / 3.0, n.dy() / 3.0);
+
+ // Only support normal arrow type
+ QPolygonF polygon;
+ polygon.append(line.p1() + o);
+ polygon.append(line.p2());
+ polygon.append(line.p1() - o);
+
+ return polygon;
+}
+
+QPolygonF QGVEdge::toBox(const QLineF &line) const
+{
+ QLineF n = line.normalVector();
+ QPointF o(n.dx() * 0.5, n.dy() * 0.5);
+
+ QPolygonF polygon;
+ polygon.append(line.p1() + o);
+ polygon.append(line.p2() + o);
+ polygon.append(line.p2() - o);
+ polygon.append(line.p1() - o);
+ return polygon;
+}
diff --git a/gazebo/gui/model/qgv/QGVEdge.h b/gazebo/gui/model/qgv/QGVEdge.h
new file mode 100644
index 0000000..ffe9c2d
--- /dev/null
+++ b/gazebo/gui/model/qgv/QGVEdge.h
@@ -0,0 +1,89 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#ifndef QGVEDGE_H
+#define QGVEDGE_H
+
+#include <qgv.h>
+#include <QGraphicsItem>
+#include <QPen>
+
+class QGVNode;
+class QGVScene;
+class QGVEdgePrivate;
+
+/**
+ * @brief Edge item
+ *
+ */
+class QGVCORE_EXPORT QGVEdge : public QGraphicsItem
+{
+ public:
+ ~QGVEdge();
+
+ QString label() const;
+ QRectF boundingRect() const;
+ QPainterPath shape() const;
+
+ QString source() const;
+ QString target() const;
+
+ void setLabel(const QString &label);
+
+ void setSource(const QString &_source);
+ void setTarget(const QString &_target);
+
+ void paint(QPainter * painter, const QStyleOptionGraphicsItem * option,
+ QWidget * widget = 0);
+
+ void setAttribute(const QString &name, const QString &value);
+ QString getAttribute(const QString &name) const;
+
+ void updateLayout();
+
+ enum { Type = UserType + 3 };
+ int type() const
+ {
+ return Type;
+ }
+
+ private:
+ QGVEdge(QGVEdgePrivate *edge, QGVScene *scene);
+
+ QPolygonF toArrow(const QLineF &normal) const;
+ QPolygonF toBox(const QLineF &normal) const;
+
+ friend class QGVScene;
+ // friend class QGVSubGraph;
+
+ QGVScene *_scene;
+ QGVEdgePrivate* _edge;
+
+ QPainterPath _path;
+ QPen _pen;
+ QPolygonF _head_arrow;
+ QPolygonF _tail_arrow;
+
+ QString _label;
+ QRectF _label_rect;
+
+ QString sourceNode;
+ QString targetNode;
+};
+
+#endif
diff --git a/gazebo/gui/model/qgv/QGVNode.cpp b/gazebo/gui/model/qgv/QGVNode.cpp
new file mode 100644
index 0000000..de3f44e
--- /dev/null
+++ b/gazebo/gui/model/qgv/QGVNode.cpp
@@ -0,0 +1,137 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#include <QGVNode.h>
+#include <QGVCore.h>
+#include <QGVScene.h>
+#include <QGVGraphPrivate.h>
+#include <QGVNodePrivate.h>
+#include <QDebug>
+#include <QPainter>
+#include <iostream>
+QGVNode::QGVNode(QGVNodePrivate *node, QGVScene *qgvscene): _scene(qgvscene),
+ _node(node)
+{
+// this->setFlag(QGraphicsItem::ItemIsSelectable, true);
+// this->setFlag(QGraphicsItem::ItemIsMovable, true);
+}
+
+QGVNode::~QGVNode()
+{
+ _scene->removeItem(this);
+ delete _node;
+}
+
+QString QGVNode::label() const
+{
+ return getAttribute("label");
+}
+
+void QGVNode::setLabel(const QString &_label)
+{
+ setAttribute("label", _label);
+}
+
+QRectF QGVNode::boundingRect() const
+{
+ return _path.boundingRect();
+}
+
+void QGVNode::paint(QPainter * painter,
+ const QStyleOptionGraphicsItem */*_option*/, QWidget */*_widget*/)
+{
+ painter->save();
+
+ painter->setPen(_pen);
+
+ if (isSelected())
+ {
+ QBrush tbrush(_brush);
+ tbrush.setColor(tbrush.color().darker(120));
+ painter->setBrush(tbrush);
+ }
+ else
+ painter->setBrush(_brush);
+
+ painter->drawPath(_path);
+
+ painter->setPen(QGVCore::toColor(getAttribute("labelfontcolor")));
+
+ const QRectF rect = boundingRect();
+ if (_icon.isNull())
+ {
+ painter->drawText(rect, Qt::AlignCenter , QGVNode::label());
+ }
+ else
+ {
+ painter->drawText(rect.adjusted(0,0,0, -rect.height()*2/3),
+ Qt::AlignCenter , QGVNode::label());
+
+ const QRectF img_rect = rect.adjusted(0, rect.height()/3,0, 0);
+ QImage img = _icon.scaled(img_rect.size().toSize(),
+ Qt::KeepAspectRatio, Qt::SmoothTransformation);
+ painter->drawImage(img_rect.topLeft() + QPointF((img_rect.width()
+ - img.rect().width())/2, 0), img);
+ }
+ painter->restore();
+}
+
+void QGVNode::setAttribute(const QString &name, const QString &value)
+{
+ agsafeset(_node->node(), name.toLocal8Bit().data(),
+ value.toLocal8Bit().data(), (char *)"");
+}
+
+QString QGVNode::getAttribute(const QString &name) const
+{
+ char* value = agget(_node->node(), name.toLocal8Bit().data());
+ if (value)
+ return value;
+ return QString();
+}
+
+void QGVNode::setIcon(const QImage &icon)
+{
+ _icon = icon;
+}
+
+void QGVNode::updateLayout()
+{
+ prepareGeometryChange();
+ qreal width = ND_width(_node->node())*DotDefaultDPI;
+ qreal height = ND_height(_node->node())*DotDefaultDPI;
+
+ // Node Position (center)
+ qreal gheight = QGVCore::graphHeight(_scene->_graph->graph());
+ setPos(QGVCore::centerToOrigin(QGVCore::toPoint(ND_coord(_node->node()),
+ gheight), width, height));
+
+ // Node on top
+ setZValue(1);
+
+ // Node path
+ _path = QGVCore::toPath(ND_shape(_node->node())->name,
+ static_cast<polygon_t *>(ND_shape_info(_node->node())), width, height);
+ _pen.setWidth(1);
+
+ _brush.setStyle(QGVCore::toBrushStyle(getAttribute("style")));
+ _brush.setColor(QGVCore::toColor(getAttribute("fillcolor")));
+ _pen.setColor(QGVCore::toColor(getAttribute("color")));
+
+ setToolTip(getAttribute("tooltip"));
+}
diff --git a/gazebo/gui/model/qgv/QGVNode.h b/gazebo/gui/model/qgv/QGVNode.h
new file mode 100644
index 0000000..c1787fe
--- /dev/null
+++ b/gazebo/gui/model/qgv/QGVNode.h
@@ -0,0 +1,75 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+#ifndef QGVNODE_H
+#define QGVNODE_H
+
+#include <qgv.h>
+#include <QGraphicsItem>
+#include <QPen>
+
+class QGVEdge;
+class QGVScene;
+class QGVNodePrivate;
+
+/**
+ * @brief Node item
+ *
+ */
+class QGVCORE_EXPORT QGVNode : public QGraphicsItem
+{
+ public:
+ ~QGVNode();
+
+ QString label() const;
+ void setLabel(const QString &label);
+
+ QRectF boundingRect() const;
+ void paint(QPainter *painter, const QStyleOptionGraphicsItem *option,
+ QWidget *widget = 0);
+ void setAttribute(const QString &label, const QString &value);
+ QString getAttribute(const QString &name) const;
+
+ void setIcon(const QImage &icon);
+
+ enum { Type = UserType + 2 };
+ int type() const
+ {
+ return Type;
+ }
+
+ private:
+ friend class QGVScene;
+ friend class QGVSubGraph;
+ void updateLayout();
+ QGVNode(QGVNodePrivate* node, QGVScene *scene);
+
+ // Not implemented in QGVNode.cpp
+// QPainterPath makeShape(Agnode_t* node) const;
+// QPolygonF makeShapeHelper(Agnode_t* node) const;
+
+ QPainterPath _path;
+ QPen _pen;
+ QBrush _brush;
+ QImage _icon;
+
+ QGVScene *_scene;
+ QGVNodePrivate* _node;
+};
+
+
+#endif
diff --git a/gazebo/gui/model/qgv/QGVScene.cpp b/gazebo/gui/model/qgv/QGVScene.cpp
new file mode 100644
index 0000000..3420d29
--- /dev/null
+++ b/gazebo/gui/model/qgv/QGVScene.cpp
@@ -0,0 +1,342 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+#include "QGVScene.h"
+// The following include allows the automoc to detect, that it must moc this
+// class
+//#include "moc_QGVScene.cpp"
+#include <QDebug>
+
+#include <QGVNode.h>
+#include <QGVEdge.h>
+#include <QGVSubGraph.h>
+
+#include <QGVCore.h>
+#include <QGVGraphPrivate.h>
+#include <QGVGvcPrivate.h>
+#include <QGVEdgePrivate.h>
+#include <QGVNodePrivate.h>
+#include <iostream>
+
+QGVScene::QGVScene(const QString &name, QObject *_parent)
+ : QGraphicsScene(_parent)
+{
+ _context = new QGVGvcPrivate(gvContext());
+ _graph = new QGVGraphPrivate(agopen(name.toLocal8Bit().data(), Agdirected, NULL));
+ // setGraphAttribute("fontname", QFont().family());
+ this->init = false;
+}
+
+QGVScene::~QGVScene()
+{
+ gvFreeLayout(_context->context(), _graph->graph());
+ agclose(_graph->graph());
+ gvFreeContext(_context->context());
+ delete _graph;
+ delete _context;
+}
+
+void QGVScene::setGraphAttribute(const QString &name, const QString &value)
+{
+ agattr(_graph->graph(), AGRAPH, name.toLocal8Bit().data(), value.toLocal8Bit().data());
+}
+
+void QGVScene::setNodeAttribute(const QString &name, const QString &value)
+{
+ agattr(_graph->graph(), AGNODE, name.toLocal8Bit().data(),
+ value.toLocal8Bit().data());
+}
+
+void QGVScene::setEdgeAttribute(const QString &name, const QString &value)
+{
+ agattr(_graph->graph(), AGEDGE, name.toLocal8Bit().data(),
+ value.toLocal8Bit().data());
+}
+
+QGVNode *QGVScene::addNode(const QString &label)
+{
+ Agnode_t *node = agnode(_graph->graph(), label.toLocal8Bit().data(), true);
+ if (node == NULL)
+ {
+ qWarning()<<"Invalid node :"<<label;
+ return 0;
+ }
+
+ QGVNode *item = new QGVNode(new QGVNodePrivate(node), this);
+ item->setLabel(label);
+ addItem(item);
+ _nodes.insert(label, item);
+ return item;
+}
+
+QGVEdge *QGVScene::addEdge(QGVNode *source, QGVNode *target,
+ const QString &label)
+{
+ Agedge_t* edge = agedge(_graph->graph(), source->_node->node(),
+ target->_node->node(), NULL, true);
+ if (edge == NULL)
+ {
+ qWarning() << "Invalid egde :" << label;
+ return 0;
+ }
+
+ QGVEdge *item = new QGVEdge(new QGVEdgePrivate(edge), this);
+
+ item->setSource(source->label());
+ item->setTarget(target->label());
+ addItem(item);
+
+ _edges.insert(label, item);
+ return item;
+}
+
+
+QGVEdge *QGVScene::addEdge(const QString &source, const QString &target,
+ const QString &label)
+{
+ if (_nodes.contains(source) && _nodes.contains(target))
+ return this->addEdge(_nodes[source], _nodes[target], label);
+ return NULL;
+}
+
+
+QGVSubGraph *QGVScene::addSubGraph(const QString &name, bool cluster)
+{
+ Agraph_t* sgraph;
+ if (cluster)
+ {
+ sgraph = agsubg(_graph->graph(),
+ ("cluster_" + name).toLocal8Bit().data(), true);
+ }
+ else
+ sgraph = agsubg(_graph->graph(), name.toLocal8Bit().data(), true);
+
+ if (sgraph == NULL)
+ {
+ qWarning()<<"Invalid subGraph :"<<name;
+ return 0;
+ }
+
+ QGVSubGraph *item = new QGVSubGraph(new QGVGraphPrivate(sgraph), this);
+ addItem(item);
+ _subGraphs.insert(name, item);
+ return item;
+}
+
+void QGVScene::removeNode(const QString &label)
+{
+ if (_nodes.contains(label))
+ {
+ agdelete(_graph->graph(), _nodes[label]->_node->node());
+ removeItem(_nodes[label]);
+ _nodes.remove(label);
+ }
+
+ QList<QString> toRemove;
+
+ for (auto key : _edges.toStdMap())
+ {
+ QGVEdge *e = key.second;
+ if (e->source() == label || e->target() == label)
+ toRemove.append(key.first);
+ }
+ for (auto e : toRemove)
+ this->removeEdge(e);
+}
+
+void QGVScene::removeEdge(const QString &_label)
+{
+ if (_edges.contains(_label))
+ {
+ agdelete(_graph->graph(), _edges[_label]->_edge->edge());
+ removeItem(_edges[_label]);
+ _edges.remove(_label);
+ }
+}
+
+bool QGVScene::hasNode(const QString &name)
+{
+ return _nodes.contains(name);
+}
+
+QGVNode *QGVScene::getNode(const QString &name)
+{
+ return _nodes[name];
+}
+
+int QGVScene::nodeCount() const
+{
+ return _nodes.size();
+}
+
+int QGVScene::edgeCount() const
+{
+ return _edges.size();
+}
+
+void QGVScene::setRootNode(QGVNode *node)
+{
+ Q_ASSERT(_nodes.contains(node->label()));
+ agset(_graph->graph(), (char *)"root", node->label().toLocal8Bit().data());
+}
+
+void QGVScene::loadLayout(const QString &text)
+{
+ _graph->setGraph(QGVCore::agmemread2(text.toLocal8Bit().constData()));
+
+ if (gvLayout(_context->context(), _graph->graph(), "dot") != 0)
+ {
+ qCritical() << "Layout render error" << agerrors() <<
+ QString::fromLocal8Bit(aglasterr());
+ return;
+ }
+
+ // Debug output
+ // gvRenderFilename(_context->context(), _graph->graph(), "png",
+ // "debug.png");
+
+ // Read nodes and edges
+ for (Agnode_t* node = agfstnode(_graph->graph()); node != NULL;
+ node = agnxtnode(_graph->graph(), node))
+ {
+ QGVNode *inode = new QGVNode(new QGVNodePrivate(node), this);
+ inode->updateLayout();
+ addItem(inode);
+ for (Agedge_t* edge = agfstout(_graph->graph(), node); edge != NULL;
+ edge = agnxtout(_graph->graph(), edge))
+ {
+ QGVEdge *iedge = new QGVEdge(new QGVEdgePrivate(edge), this);
+ iedge->updateLayout();
+ addItem(iedge);
+ }
+ }
+ update();
+}
+
+void QGVScene::applyLayout()
+{
+ if (_nodes.empty())
+ return;
+
+ gvFreeLayout(_context->context(), _graph->graph());
+ if (gvLayout(_context->context(), _graph->graph(), "dot") != 0)
+ {
+ qCritical()<<"Layout render error" <<
+ agerrors()<<QString::fromLocal8Bit(aglasterr());
+ return;
+ }
+
+ // Debug output
+ // gvRenderFilename(_context->context(), _graph->graph(), "canon",
+ // "debug.dot");
+ // gvRenderFilename(_context->context(), _graph->graph(), "png",
+ // "debug.png");
+
+ //Update items layout
+ foreach(QGVNode* node, _nodes)
+ node->updateLayout();
+
+ foreach(QGVEdge* edge, _edges)
+ edge->updateLayout();
+
+ foreach(QGVSubGraph* sgraph, _subGraphs)
+ sgraph->updateLayout();
+
+ // Graph label
+ textlabel_t *xlabel = GD_label(_graph->graph());
+ if (xlabel)
+ {
+ QGraphicsTextItem *item = addText(xlabel->text);
+ item->setPos(QGVCore::centerToOrigin(QGVCore::toPoint(xlabel->pos,
+ QGVCore::graphHeight(_graph->graph())), xlabel->dimen.x, -4));
+ }
+
+ update();
+}
+
+void QGVScene::clearLayout()
+{
+ gvFreeLayout(_context->context(), _graph->graph());
+}
+
+void QGVScene::clear()
+{
+ gvFreeLayout(_context->context(), _graph->graph());
+ _nodes.clear();
+ _edges.clear();
+ _subGraphs.clear();
+ QGraphicsScene::clear();
+}
+
+#include <QGraphicsSceneContextMenuEvent>
+void QGVScene::contextMenuEvent(
+ QGraphicsSceneContextMenuEvent *_contextMenuEvent)
+{
+ QGraphicsItem *item = itemAt(_contextMenuEvent->scenePos(), QTransform());
+ if (item)
+ {
+ item->setSelected(true);
+ if (item->type() == QGVNode::Type)
+ emit nodeContextMenu(qgraphicsitem_cast<QGVNode*>(item));
+ else if (item->type() == QGVEdge::Type)
+ emit edgeContextMenu(qgraphicsitem_cast<QGVEdge*>(item));
+ else if (item->type() == QGVSubGraph::Type)
+ emit subGraphContextMenu(qgraphicsitem_cast<QGVSubGraph*>(item));
+ else
+ emit graphContextMenuEvent();
+ }
+ QGraphicsScene::contextMenuEvent(_contextMenuEvent);
+}
+
+void QGVScene::mouseDoubleClickEvent(QGraphicsSceneMouseEvent *mouseEvent)
+{
+ QGraphicsItem *item = itemAt(mouseEvent->scenePos(), QTransform());
+ if (item)
+ {
+ if (item->type() == QGVNode::Type)
+ emit nodeDoubleClick(qgraphicsitem_cast<QGVNode*>(item));
+ else if (item->type() == QGVEdge::Type)
+ emit edgeDoubleClick(qgraphicsitem_cast<QGVEdge*>(item));
+ else if (item->type() == QGVSubGraph::Type)
+ emit subGraphDoubleClick(qgraphicsitem_cast<QGVSubGraph*>(item));
+ }
+ QGraphicsScene::mouseDoubleClickEvent(mouseEvent);
+}
+
+#include <QVarLengthArray>
+#include <QPainter>
+void QGVScene::drawBackground(QPainter * painter, const QRectF & rect)
+{
+ const int gridSize = 25;
+
+ const qreal left = int(rect.left()) - (int(rect.left()) % gridSize);
+ const qreal top = int(rect.top()) - (int(rect.top()) % gridSize);
+
+ QVarLengthArray<QLineF, 100> lines;
+
+ for (qreal x = left; x < rect.right(); x += gridSize)
+ lines.append(QLineF(x, rect.top(), x, rect.bottom()));
+ for (qreal y = top; y < rect.bottom(); y += gridSize)
+ lines.append(QLineF(rect.left(), y, rect.right(), y));
+
+ painter->setRenderHint(QPainter::Antialiasing, false);
+
+ painter->setPen(QColor(Qt::lightGray).lighter(110));
+ painter->drawLines(lines.data(), lines.size());
+ painter->setPen(Qt::black);
+ // painter->drawRect(sceneRect());
+}
diff --git a/gazebo/gui/model/qgv/QGVScene.h b/gazebo/gui/model/qgv/QGVScene.h
new file mode 100644
index 0000000..7ad34a8
--- /dev/null
+++ b/gazebo/gui/model/qgv/QGVScene.h
@@ -0,0 +1,112 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#ifndef QGVSCENE_H
+#define QGVSCENE_H
+
+
+
+#include <QMap>
+#include <QGraphicsScene>
+
+#include "qgv.h"
+
+class QGVNode;
+class QGVEdge;
+class QGVSubGraph;
+
+class QGVGraphPrivate;
+class QGVGvcPrivate;
+
+/**
+ * @brief GraphViz interactive scene
+ *
+ */
+class QGVCORE_EXPORT QGVScene : public QGraphicsScene
+{
+ Q_OBJECT
+ public:
+
+ explicit QGVScene(const QString &name, QObject *parent = 0);
+ ~QGVScene();
+
+ void setGraphAttribute(const QString &name, const QString &value);
+ void setNodeAttribute(const QString &name, const QString &value);
+ void setEdgeAttribute(const QString &name, const QString &value);
+
+ QGVNode* addNode(const QString& label);
+ QGVEdge* addEdge(QGVNode* source, QGVNode* target, const QString& label);
+ QGVEdge *addEdge(const QString &source, const QString &target,
+ const QString &label);
+
+ QGVSubGraph* addSubGraph(const QString& name, bool cluster = true);
+ void removeNode(const QString &label);
+// void removeEdge(const QString& source, const QString& target);
+// void removeEdge(const QPair<QString, QString>& key);
+ void removeEdge(const QString &label);
+
+ bool hasNode(const QString &name);
+ QGVNode *getNode(const QString &name);
+
+ void setRootNode(QGVNode *node);
+
+ int nodeCount() const;
+ int edgeCount() const;
+
+ void loadLayout(const QString &text);
+ void applyLayout();
+ void clearLayout();
+ void clear();
+
+
+ signals:
+ void nodeContextMenu(QGVNode* node);
+ void nodeDoubleClick(QGVNode* node);
+
+ void edgeContextMenu(QGVEdge* edge);
+ void edgeDoubleClick(QGVEdge* edge);
+
+ void subGraphContextMenu(QGVSubGraph* graph);
+ void subGraphDoubleClick(QGVSubGraph* graph);
+
+ void graphContextMenuEvent();
+
+ public slots:
+
+ protected:
+ virtual void contextMenuEvent(
+ QGraphicsSceneContextMenuEvent * contextMenuEvent);
+ virtual void mouseDoubleClickEvent(QGraphicsSceneMouseEvent * mouseEvent);
+ virtual void drawBackground(QPainter * painter, const QRectF & rect);
+ private:
+ friend class QGVNode;
+ friend class QGVEdge;
+ friend class QGVSubGraph;
+
+ QGVGvcPrivate *_context;
+ QGVGraphPrivate *_graph;
+
+ QMap<QString, QGVNode *> _nodes;
+// QMap<QPair<QString, QString>, QGVEdge*> _edges;
+ QMap<QString, QGVEdge *> _edges;
+ QMap<QString, QGVSubGraph *> _subGraphs;
+
+ bool init;
+};
+
+#endif
diff --git a/gazebo/gui/model/qgv/QGVSubGraph.cpp b/gazebo/gui/model/qgv/QGVSubGraph.cpp
new file mode 100644
index 0000000..51925ab
--- /dev/null
+++ b/gazebo/gui/model/qgv/QGVSubGraph.cpp
@@ -0,0 +1,146 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#include "QGVSubGraph.h"
+#include <QGVCore.h>
+#include <QGVScene.h>
+#include <QGVGraphPrivate.h>
+#include <QGVNodePrivate.h>
+#include <QGVNode.h>
+#include <QDebug>
+#include <QPainter>
+
+QGVSubGraph::QGVSubGraph(QGVGraphPrivate *subGraph, QGVScene *_qgvscene)
+ : _scene(_qgvscene), _sgraph(subGraph)
+{
+ // setFlag(QGraphicsItem::ItemIsSelectable, true);
+}
+
+QGVSubGraph::~QGVSubGraph()
+{
+ _scene->removeItem(this);
+ delete _sgraph;
+}
+
+QString QGVSubGraph::name() const
+{
+ return QString::fromLocal8Bit(GD_label(_sgraph->graph())->text);
+}
+
+QGVNode *QGVSubGraph::addNode(const QString &label)
+{
+ Agnode_t *node = agnode(_sgraph->graph(), NULL, TRUE);
+ if (node == NULL)
+ {
+ qWarning()<<"Invalid sub node :"<<label;
+ return 0;
+ }
+ agsubnode(_sgraph->graph(), node, TRUE);
+
+ QGVNode *item = new QGVNode(new QGVNodePrivate(node), _scene);
+ item->setLabel(label);
+ _scene->addItem(item);
+ _scene->_nodes.insert(label, item);
+ _nodes.append(item);
+ return item;
+}
+
+QGVSubGraph *QGVSubGraph::addSubGraph(const QString &_name, bool cluster)
+{
+ Agraph_t* sgraph;
+ if (cluster)
+ {
+ sgraph = agsubg(_sgraph->graph(),
+ ("cluster_" + _name).toLocal8Bit().data(), TRUE);
+ }
+ else
+ sgraph = agsubg(_sgraph->graph(), _name.toLocal8Bit().data(), TRUE);
+
+ if (sgraph == NULL)
+ {
+ qWarning()<<"Invalid subGraph :"<< _name;
+ return 0;
+ }
+
+ QGVSubGraph *item = new QGVSubGraph(new QGVGraphPrivate(sgraph), _scene);
+ _scene->_subGraphs.insert(_name, item);
+ _scene->addItem(item);
+ return item;
+}
+
+QRectF QGVSubGraph::boundingRect() const
+{
+ return QRectF(0,0, _width, _height);
+}
+
+void QGVSubGraph::paint(QPainter * painter,
+ const QStyleOptionGraphicsItem */*option*/, QWidget */*widget*/)
+{
+ painter->save();
+
+ painter->setPen(_pen);
+ painter->setBrush(_brush);
+
+ painter->drawRect(boundingRect());
+ painter->drawText(_label_rect, Qt::AlignCenter, _label);
+ painter->restore();
+}
+
+void QGVSubGraph::setAttribute(const QString &_name, const QString &value)
+{
+ agsafeset(_sgraph->graph(), _name.toLocal8Bit().data(),
+ value.toLocal8Bit().data(), (char *)"");
+}
+
+QString QGVSubGraph::getAttribute(const QString &_name) const
+{
+ char* value = agget(_sgraph->graph(), _name.toLocal8Bit().data());
+ if (value)
+ return value;
+ return QString();
+}
+
+void QGVSubGraph::updateLayout()
+{
+ prepareGeometryChange();
+
+ // SubGraph box
+ boxf bbox = GD_bb(_sgraph->graph());
+ pointf p1 = bbox.UR;
+ pointf p2 = bbox.LL;
+ _width = p1.x - p2.x;
+ _height = p1.y - p2.y;
+
+ qreal gheight = QGVCore::graphHeight(_scene->_graph->graph());
+ setPos(p2.x, gheight - p1.y);
+
+ _pen.setWidth(1);
+ _brush.setStyle(QGVCore::toBrushStyle(getAttribute("style")));
+ _brush.setColor(QGVCore::toColor(getAttribute("fillcolor")));
+ _pen.setColor(QGVCore::toColor(getAttribute("color")));
+
+ // SubGraph label
+ textlabel_t *xlabel = GD_label(_sgraph->graph());
+ if (xlabel)
+ {
+ _label = xlabel->text;
+ _label_rect.setSize(QSize(xlabel->dimen.x, xlabel->dimen.y));
+ _label_rect.moveCenter(QGVCore::toPoint(xlabel->pos,
+ QGVCore::graphHeight(_scene->_graph->graph())) - pos());
+ }
+}
diff --git a/gazebo/gui/model/qgv/QGVSubGraph.h b/gazebo/gui/model/qgv/QGVSubGraph.h
new file mode 100644
index 0000000..e3621ef
--- /dev/null
+++ b/gazebo/gui/model/qgv/QGVSubGraph.h
@@ -0,0 +1,74 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#ifndef QGVSUBGRAPH_H
+#define QGVSUBGRAPH_H
+
+#include <qgv.h>
+#include <QGraphicsItem>
+#include <QPen>
+
+class QGVNode;
+class QGVEdge;
+class QGVScene;
+class QGVGraphPrivate;
+
+/**
+ * @brief SubGraph item
+ *
+ */
+class QGVCORE_EXPORT QGVSubGraph : public QGraphicsItem
+{
+ public:
+ ~QGVSubGraph();
+
+ QString name() const;
+
+ QGVNode* addNode(const QString& label);
+ QGVSubGraph* addSubGraph(const QString& name, bool cluster = true);
+
+ QRectF boundingRect() const;
+ void paint(QPainter *painter, const QStyleOptionGraphicsItem *option,
+ QWidget *widget = 0);
+ void setAttribute(const QString &name, const QString &value);
+ QString getAttribute(const QString &name) const;
+ void updateLayout();
+
+ enum { Type = UserType + 4 };
+ int type() const
+ {
+ return Type;
+ }
+
+ private:
+ friend class QGVScene;
+ QGVSubGraph(QGVGraphPrivate* subGraph, QGVScene *scene);
+
+ double _height, _width;
+ QPen _pen;
+ QBrush _brush;
+
+ QString _label;
+ QRectF _label_rect;
+
+ QGVScene *_scene;
+ QGVGraphPrivate *_sgraph;
+ QList<QGVNode*> _nodes;
+};
+
+#endif
diff --git a/gazebo/gui/model/qgv/private/QGVCore.cpp b/gazebo/gui/model/qgv/private/QGVCore.cpp
new file mode 100644
index 0000000..a7566a7
--- /dev/null
+++ b/gazebo/gui/model/qgv/private/QGVCore.cpp
@@ -0,0 +1,135 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#include "QGVCore.h"
+#include <QDebug>
+
+qreal QGVCore::graphHeight(Agraph_t *graph)
+{
+ return GD_bb(graph).UR.y;
+}
+
+QPointF QGVCore::toPoint(pointf p, qreal gheight)
+{
+ return QPointF(p.x, gheight - p.y);
+}
+
+QPointF QGVCore::toPoint(point p, qreal gheight)
+{
+ return QPointF(p.x, gheight - p.y);
+}
+
+QPointF QGVCore::centerToOrigin(const QPointF &p, qreal width, qreal height)
+{
+ return QPointF(p.x() - width/2, p.y() - height/2);
+}
+
+QPolygonF QGVCore::toPolygon(const polygon_t *poly, qreal width, qreal height)
+{
+ if (poly->peripheries != 1)
+ qWarning("unsupported number of peripheries %d", poly->peripheries);
+
+ const int sides = poly->sides;
+ const pointf* vertices = poly->vertices;
+
+ QPolygonF polygon;
+ for (int side = 0; side < sides; side++)
+ {
+ polygon.append(QPointF(vertices[side].x + width/2,
+ vertices[side].y + height/2));
+ }
+ return polygon;
+}
+
+QPainterPath QGVCore::toPath(const char *type, const polygon_t *poly,
+ qreal width, qreal height)
+{
+ QPainterPath painterPath;
+ if ((strcmp(type, "rectangle") == 0) ||
+ (strcmp(type, "box") == 0) ||
+ (strcmp(type, "hexagon") == 0) ||
+ (strcmp(type, "polygon") == 0) ||
+ (strcmp(type, "diamond") == 0))
+ {
+ QPolygonF polygon = toPolygon(poly, width, height);
+ polygon.append(polygon[0]);
+ painterPath.addPolygon(polygon);
+ }
+ else if ((strcmp(type, "ellipse") == 0) ||
+ (strcmp(type, "circle") == 0))
+ {
+ QPolygonF polygon = toPolygon(poly, width, height);
+ painterPath.addEllipse(QRectF(polygon[0], polygon[1]));
+ }
+ else
+ {
+ qWarning("unsupported shape %s", type);
+ }
+ return painterPath;
+}
+
+QPainterPath QGVCore::toPath(const splines *spl, qreal gheight)
+{
+ QPainterPath painterPath;
+ if ((spl->list != 0) && (spl->list->size%3 == 1))
+ {
+ bezier bez = spl->list[0];
+ // If there is a starting point, draw a line from it to the first
+ // curve point
+ if (bez.sflag)
+ {
+ painterPath.moveTo(toPoint(bez.sp, gheight));
+ painterPath.lineTo(toPoint(bez.list[0], gheight));
+ }
+ else
+ painterPath.moveTo(toPoint(bez.list[0], gheight));
+
+ //Loop over the curve points
+ for(int i=1; i<bez.size; i+=3)
+ {
+ painterPath.cubicTo(toPoint(bez.list[i], gheight),
+ toPoint(bez.list[i+1], gheight), toPoint(bez.list[i+2], gheight));
+ }
+
+ //If there is an ending point, draw a line to it
+ if (bez.eflag)
+ painterPath.lineTo(toPoint(bez.ep, gheight));
+ }
+ return painterPath;
+}
+
+Qt::BrushStyle QGVCore::toBrushStyle(const QString &style)
+{
+ if (style == "filled")
+ return Qt::SolidPattern;
+ return Qt::NoBrush;
+}
+
+Qt::PenStyle QGVCore::toPenStyle(const QString &style)
+{
+ if (style =="dashed")
+ return Qt::DashLine;
+ else if (style == "dotted")
+ return Qt::DotLine;
+ return Qt::SolidLine;
+}
+
+QColor QGVCore::toColor(const QString &color)
+{
+ return QColor(color);
+}
diff --git a/gazebo/gui/model/qgv/private/QGVCore.h b/gazebo/gui/model/qgv/private/QGVCore.h
new file mode 100644
index 0000000..01dd0cf
--- /dev/null
+++ b/gazebo/gui/model/qgv/private/QGVCore.h
@@ -0,0 +1,106 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#ifndef QGVCORE_H
+#define QGVCORE_H
+
+#include <QPointF>
+#include <QPolygonF>
+#include <QPainterPath>
+#include <QColor>
+
+// GraphViz headers
+#include <gvc.h>
+#include <cgraph.h>
+
+const qreal DotDefaultDPI = 72.0;
+
+/**
+ * @brief GraphViz to GraphicsScene conversions
+ *
+ */
+class QGVCore
+{
+ public:
+ static qreal graphHeight(Agraph_t *graph);
+ static QPointF toPoint(pointf p, qreal gheight);
+ static QPointF toPoint(point p, qreal gheight);
+ static QPointF centerToOrigin(const QPointF &p, qreal width, qreal height);
+ static QPolygonF toPolygon(const polygon_t* poly, qreal width,
+ qreal height);
+
+ static QPainterPath toPath(const char *type, const polygon_t *poly,
+ qreal width, qreal height);
+ static QPainterPath toPath(const splines* spl, qreal gheight);
+
+ static Qt::BrushStyle toBrushStyle(const QString &style);
+ static Qt::PenStyle toPenStyle(const QString &style);
+ static QColor toColor(const QString &color);
+
+ typedef struct {
+ const char *data;
+ int len;
+ int cur;
+ } rdr_t;
+
+ static int memiofread(void *chan, char *buf, int bufsize)
+ {
+ const char *ptr;
+ char *optr;
+ char c;
+ int l;
+ rdr_t *s;
+
+ if (bufsize == 0) return 0;
+ s = static_cast<rdr_t *>(chan);
+ if (s->cur >= s->len)
+ return 0;
+ l = 0;
+ ptr = s->data + s->cur;
+ optr = buf;
+ do {
+ *optr++ = c = *ptr++;
+ l++;
+ } while (c && (c != '\n') && (l < bufsize));
+ s->cur += l;
+ return l;
+ }
+
+ static Agraph_t *agmemread2(const char *cp)
+ {
+ Agraph_t* g;
+ rdr_t rdr;
+ Agdisc_t disc;
+ Agiodisc_t memIoDisc;
+
+ memIoDisc.afread = memiofread;
+ memIoDisc.putstr = AgIoDisc.putstr;
+ memIoDisc.flush = AgIoDisc.flush;
+ rdr.data = cp;
+ rdr.len = strlen(cp);
+ rdr.cur = 0;
+
+ disc.mem = &AgMemDisc;
+ disc.id = &AgIdDisc;
+ disc.io = &memIoDisc;
+ g = agread(&rdr, &disc);
+ return g;
+ }
+};
+
+#endif
diff --git a/gazebo/gui/model/qgv/private/QGVEdgePrivate.cpp b/gazebo/gui/model/qgv/private/QGVEdgePrivate.cpp
new file mode 100644
index 0000000..f6146e7
--- /dev/null
+++ b/gazebo/gui/model/qgv/private/QGVEdgePrivate.cpp
@@ -0,0 +1,34 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#include "QGVEdgePrivate.h"
+
+QGVEdgePrivate::QGVEdgePrivate(Agedge_t *_agedge)
+{
+ setEdge(_agedge);
+}
+
+void QGVEdgePrivate::setEdge(Agedge_t *_agedge)
+{
+ _edge = _agedge;
+}
+
+Agedge_t* QGVEdgePrivate::edge() const
+{
+ return _edge;
+}
diff --git a/gazebo/gui/model/qgv/private/QGVEdgePrivate.h b/gazebo/gui/model/qgv/private/QGVEdgePrivate.h
new file mode 100644
index 0000000..c624afe
--- /dev/null
+++ b/gazebo/gui/model/qgv/private/QGVEdgePrivate.h
@@ -0,0 +1,36 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#ifndef QGVEDGEPRIVATE_H
+#define QGVEDGEPRIVATE_H
+
+#include <cgraph.h>
+
+class QGVEdgePrivate
+{
+ public:
+ explicit QGVEdgePrivate(Agedge_t *edge = NULL);
+
+ void setEdge(Agedge_t *edge);
+ Agedge_t* edge() const;
+
+ private:
+ Agedge_t* _edge;
+};
+
+#endif
diff --git a/gazebo/gui/model/qgv/private/QGVGraphPrivate.cpp b/gazebo/gui/model/qgv/private/QGVGraphPrivate.cpp
new file mode 100644
index 0000000..9f97972
--- /dev/null
+++ b/gazebo/gui/model/qgv/private/QGVGraphPrivate.cpp
@@ -0,0 +1,34 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#include "QGVGraphPrivate.h"
+
+QGVGraphPrivate::QGVGraphPrivate(Agraph_t *agraph)
+{
+ setGraph(agraph);
+}
+
+void QGVGraphPrivate::setGraph(Agraph_t *agraph)
+{
+ _graph = agraph;
+}
+
+Agraph_t* QGVGraphPrivate::graph() const
+{
+ return _graph;
+}
diff --git a/gazebo/gui/model/qgv/private/QGVGraphPrivate.h b/gazebo/gui/model/qgv/private/QGVGraphPrivate.h
new file mode 100644
index 0000000..4e5e8fa
--- /dev/null
+++ b/gazebo/gui/model/qgv/private/QGVGraphPrivate.h
@@ -0,0 +1,36 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#ifndef QGVGRAPHPRIVATE_H
+#define QGVGRAPHPRIVATE_H
+
+#include <cgraph.h>
+
+class QGVGraphPrivate
+{
+ public:
+ explicit QGVGraphPrivate(Agraph_t *graph = NULL);
+
+ void setGraph(Agraph_t *graph);
+ Agraph_t* graph() const;
+
+ private:
+ Agraph_t* _graph;
+};
+
+#endif
diff --git a/gazebo/gui/model/qgv/private/QGVGvcPrivate.cpp b/gazebo/gui/model/qgv/private/QGVGvcPrivate.cpp
new file mode 100644
index 0000000..0467e8e
--- /dev/null
+++ b/gazebo/gui/model/qgv/private/QGVGvcPrivate.cpp
@@ -0,0 +1,34 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#include "QGVGvcPrivate.h"
+
+QGVGvcPrivate::QGVGvcPrivate(GVC_t *_gvccontext)
+{
+ setContext(_gvccontext);
+}
+
+void QGVGvcPrivate::setContext(GVC_t *_gvccontext)
+{
+ _context = _gvccontext;
+}
+
+GVC_t* QGVGvcPrivate::context() const
+{
+ return _context;
+}
diff --git a/gazebo/gui/model/qgv/private/QGVGvcPrivate.h b/gazebo/gui/model/qgv/private/QGVGvcPrivate.h
new file mode 100644
index 0000000..c8347ac
--- /dev/null
+++ b/gazebo/gui/model/qgv/private/QGVGvcPrivate.h
@@ -0,0 +1,49 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#ifndef QGVGVCPRIVATE_H
+#define QGVGVCPRIVATE_H
+
+#include <gvc.h>
+
+class QGVGvcPrivate
+{
+ public:
+ explicit QGVGvcPrivate(GVC_t *context = NULL);
+
+ void setContext(GVC_t *context);
+ GVC_t* context() const;
+
+ // operators to implicit cast from QGVGvcPrivate* into
+ // GVC_t* seems not to work,
+ // because of typedef GVC_t
+// inline operator const GVC_t* () const
+// {
+// return const_cast<VC_t*>(context());
+// }
+
+// inline operator struct GVC_t* ()
+// {
+// return context();
+// }
+
+ private:
+ GVC_t* _context;
+};
+
+#endif
diff --git a/gazebo/gui/model/qgv/private/QGVNodePrivate.cpp b/gazebo/gui/model/qgv/private/QGVNodePrivate.cpp
new file mode 100644
index 0000000..2662395
--- /dev/null
+++ b/gazebo/gui/model/qgv/private/QGVNodePrivate.cpp
@@ -0,0 +1,16 @@
+#include "QGVNodePrivate.h"
+
+QGVNodePrivate::QGVNodePrivate(Agnode_t *agnode)
+{
+ setNode(agnode);
+}
+
+void QGVNodePrivate::setNode(Agnode_t *agnode)
+{
+ _node = agnode;
+}
+
+Agnode_t* QGVNodePrivate::node() const
+{
+ return _node;
+}
diff --git a/gazebo/gui/model/qgv/private/QGVNodePrivate.h b/gazebo/gui/model/qgv/private/QGVNodePrivate.h
new file mode 100644
index 0000000..cf5bd95
--- /dev/null
+++ b/gazebo/gui/model/qgv/private/QGVNodePrivate.h
@@ -0,0 +1,36 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#ifndef QGVNODEPRIVATE_H
+#define QGVNODEPRIVATE_H
+
+#include <cgraph.h>
+
+class QGVNodePrivate
+{
+ public:
+ explicit QGVNodePrivate(Agnode_t *node = NULL);
+
+ void setNode(Agnode_t *node);
+ Agnode_t* node() const;
+
+ private:
+ Agnode_t* _node;
+};
+
+#endif
diff --git a/gazebo/gui/model/qgv/qgv.h b/gazebo/gui/model/qgv/qgv.h
new file mode 100644
index 0000000..e93f32d
--- /dev/null
+++ b/gazebo/gui/model/qgv/qgv.h
@@ -0,0 +1,36 @@
+/***************************************************************
+QGVCore
+Copyright (c) 2014, Bergont Nicolas, All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 3.0 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library.
+***************************************************************/
+
+#ifndef QGV_H
+#define QGV_H
+
+#include <QtGlobal>
+
+#if defined(_WIN32) && defined(_MSC_VER)
+
+ #ifdef QGVCORE_LIB
+ #define QGVCORE_EXPORT Q_DECL_EXPORT
+ #else
+ #define QGVCORE_EXPORT Q_DECL_IMPORT
+ #endif
+
+#else
+ #define QGVCORE_EXPORT
+#endif
+
+#endif
diff --git a/gazebo/gui/resources.qrc b/gazebo/gui/resources.qrc
index 3755238..e17b549 100644
--- a/gazebo/gui/resources.qrc
+++ b/gazebo/gui/resources.qrc
@@ -51,8 +51,27 @@
<file>images/z_center.png</file>
<file>images/z_max.png</file>
<file>images/draw_link.svg</file>
+ <file>images/up_spin_arrow.png</file>
+ <file>images/up_spin_arrow_disabled.png</file>
+ <file>images/down_spin_arrow.png</file>
+ <file>images/down_spin_arrow_disabled.png</file>
+ <file>images/log_play.png</file>
+ <file>images/log_pause.png</file>
+ <file>images/log_step_forward.png</file>
+ <file>images/trashcan.png</file>
+ <file>images/log_record.png</file>
+ <file>images/view_angle_top.png</file>
+ <file>images/view_angle_bottom.png</file>
+ <file>images/view_angle_front.png</file>
+ <file>images/view_angle_back.png</file>
+ <file>images/view_angle_left.png</file>
+ <file>images/view_angle_right.png</file>
+ <file>images/view_angle_home.png</file>
+
<file alias='wood.jpg'>../../media/materials/textures/wood.jpg</file>
<file alias='tiles.jpg'>../../media/materials/textures/ceiling_tiled.jpg</file>
<file alias='bricks.png'>../../media/materials/textures/bricks.png</file>
+ <file>images/com.png</file>
+ <file>images/splash.svg</file>
</qresource>
</RCC>
diff --git a/gazebo/gui/style.qss b/gazebo/gui/style.qss
index 0a84c77..063c757 100644
--- a/gazebo/gui/style.qss
+++ b/gazebo/gui/style.qss
@@ -1,6 +1,7 @@
- QGroupBox::title {
+QGroupBox::title
+{
color: #f0f0f0;
- }
+}
QToolButton
{
@@ -217,8 +218,7 @@ QListView
background: #808080;
alternate-background-color: #606060;
- selection-color: #f58113;
- selection-background-color: #808080;
+ selection-background-color: transparent;
show-decoration-selected: 0;
border: 1px solid #737373;
@@ -234,81 +234,58 @@ QListView::item
QTreeView
{
color: #d0d0d0;
- background: #808080;
- alternate-background-color: #606060;
-
- selection-color: #f58113;
selection-background-color: transparent;
+ alternate-background-color: #606060;
border-radius: 4;
show-decoration-selected: 0;
}
-QTreeView::branch:selected,
-QTreeView::branch:selected:active
-QTreeView::branch:selected:!active
-{
- color: #f58113;
-}
-
-QTreeView::item:!selected,
-QTreeView::branch:!selected
+QTreeView::item:hover
{
- color: #d0d0d0;
+ background: #808080;
+ color: #ffffff;
}
-QTreeView::item:selected,
-QTreeView::item:hover
+QTreeView::item:selected
{
color: #f58113;
+ background: #808080;
}
-QTreeView QHeaderView:section
+QTreeView::branch:selected
{
- color: #d0d0d0;
- font-weight: bold;
- background: #404040;
- border: 0px;
+ background-color: #808080;
}
-QSpinBox
+QTreeView::branch:selected:active
{
- color: #353535;
- background: #737373;
- border-radius: 4;
- margin-right: 20px;
+ background-color: #808080;
}
-QSpinBox::up-button
+QTreeView::branch:selected:!active
{
- subcontrol-origin: margin;
- subcontrol-position: top right;
-
- border: 1px outset #727272;
- border-radius: 1px;
- background: #737373;
+ background-color: #808080;
}
-QSpinBox::up-button:pressed
+QTreeView::branch:!open:has-children
{
- border: 2px inset #727272;
+ image: url(:/images/right_arrow.png);
}
-QSpinBox::down-button
+QTreeView::branch:open:has-children:!has-siblings,
+QTreeView::branch:open:has-children:has-siblings
{
- subcontrol-origin: margin;
- subcontrol-position: bottom right;
-
- border: 1px outset #727272;
- border-radius: 1px;
- background: #737373;
+ image: url(:/images/down_arrow.png);
}
-QSpinBox::down-button:pressed
+QTreeView QHeaderView:section
{
- border: 2px inset #727272;
+ color: #d0d0d0;
+ font-weight: bold;
+ background: #404040;
+ border: 0px;
}
-QDoubleSpinBox,
QLineEdit
{
color: #353535;
@@ -321,42 +298,6 @@ QLineEdit:disabled
color: #909090;
}
-QDoubleSpinBox
-{
- margin-right: 20px;
-}
-
-QDoubleSpinBox::up-button
-{
- subcontrol-origin: margin;
- subcontrol-position: top right;
-
- border: 1px outset #727272;
- border-radius: 1px;
- background: #737373;
-}
-
-QDoubleSpinBox::up-button:pressed
-{
- border: 2px inset #727272;
-}
-
-
-QDoubleSpinBox::down-button
-{
- subcontrol-origin: margin;
- subcontrol-position: bottom right;
-
- border: 1px outset #727272;
- border-radius: 1px;
- background: #737373;
-}
-
-QDoubleSpinBox::down-button:pressed
-{
- border: 2px inset #727272;
-}
-
QPushButton
{
/*
@@ -563,7 +504,7 @@ QTabWidget#mainTab::pane,
QTabWidget#buildingEditorTab::pane,
QTabWidget#modelEditorTab::pane,
QTabWidget#terrainEditorTab::pane,
-QTabWidget#partInspectorTab::pane
+QTabWidget#linkInspectorTab::pane
{
border-left: 1px solid #101010;
border-bottom: 1px solid #101010;
@@ -653,19 +594,6 @@ QWidget#renderBottomFrame
border-bottom-right-radius: 10px;
}
-QWidget#timePanel
-{
- max-height: 40px;
- background-color: #808080;
- border-bottom-right-radius: 10px;
-}
-
-QWidget#timePanel > QFrame
-{
- background-color: #808080;
- border-bottom-right-radius: 10px;
-}
-
QScrollArea
{
background-color: #808080;
@@ -762,3 +690,78 @@ QwtScaleWidget
{
background-color: #808080;
}
+
+QAbstractSpinBox
+{
+ padding-top: 2px;
+ padding-bottom: 2px;
+ border: 0px;
+
+ background-color: #737373;
+ color: #353535;
+ selection-color: #ffffff;
+ selection-background-color: #434343;
+
+ border-radius: 3px;
+ min-width: 75px;
+}
+
+QAbstractSpinBox:up-button
+{
+ background-color: transparent;
+ subcontrol-origin: border;
+ subcontrol-position: top right;
+}
+QAbstractSpinBox:down-button
+{
+ background-color: transparent;
+ subcontrol-origin: border;
+ subcontrol-position: bottom right;
+}
+
+QAbstractSpinBox::up-arrow,
+QAbstractSpinBox::up-arrow:disabled,
+QAbstractSpinBox::up-arrow:off
+{
+ image: url(:/images/up_spin_arrow_disabled.png);
+ width: 10px;
+ height: 10px;
+}
+QAbstractSpinBox::up-arrow:hover
+{
+ image: url(:/images/up_spin_arrow.png);
+ width: 10px;
+ height: 10px;
+}
+
+QAbstractSpinBox::down-arrow,
+QAbstractSpinBox::down-arrow:disabled,
+QAbstractSpinBox::down-arrow:off
+{
+ image: url(:/images/down_spin_arrow_disabled.png);
+ width: 10px;
+ height: 10px;
+}
+QAbstractSpinBox::down-arrow:hover
+{
+ image: url(:/images/down_spin_arrow.png);
+ width: 10px;
+ height: 10px;
+}
+
+QLineEdit#logPlayCurrentTime
+{
+ background-color: #808080;
+ color: #cfcfcf;
+ font-size: 15px;
+}
+QLineEdit#logPlayCurrentTime:focus
+{
+ background-color: #707070;
+}
+
+QTextEdit#splashVersionTextEdit
+{
+ color: #f58113;
+ font-size: 10px;
+}
diff --git a/gazebo/gui/terrain/CMakeLists.txt b/gazebo/gui/terrain/CMakeLists.txt
index 3965fa3..32bbd8f 100644
--- a/gazebo/gui/terrain/CMakeLists.txt
+++ b/gazebo/gui/terrain/CMakeLists.txt
@@ -18,6 +18,8 @@ set (resources ../resources.qrc)
QT4_WRAP_CPP(headers_MOC ${qt_headers})
QT4_ADD_RESOURCES(resources_RCC ${resources})
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_GUI_TERRAIN")
+
add_library(gazebo_gui_terrain STATIC ${sources} ${headers_MOC}
${headers} ${resources_RCC})
diff --git a/gazebo/gui/terrain/TerrainEditor.hh b/gazebo/gui/terrain/TerrainEditor.hh
index d40df3d..d679c52 100644
--- a/gazebo/gui/terrain/TerrainEditor.hh
+++ b/gazebo/gui/terrain/TerrainEditor.hh
@@ -29,7 +29,7 @@ namespace gazebo
/// \class TerrainEditor TerrainEditor.hh gui/gui.hh
/// \brief Interface to the terrain editor.
- class GAZEBO_VISIBLE TerrainEditor : public Editor
+ class GZ_GUI_TERRAIN_VISIBLE TerrainEditor : public Editor
{
Q_OBJECT
diff --git a/gazebo/gui/terrain/TerrainEditorPalette.cc b/gazebo/gui/terrain/TerrainEditorPalette.cc
index 564131d..679c032 100644
--- a/gazebo/gui/terrain/TerrainEditorPalette.cc
+++ b/gazebo/gui/terrain/TerrainEditorPalette.cc
@@ -279,7 +279,7 @@ void TerrainEditorPalette::OnSave()
/////////////////////////////////////////////////
bool TerrainEditorPalette::OnMousePress(const common::MouseEvent &_event)
{
- if (_event.button != common::MouseEvent::LEFT)
+ if (_event.Button() != common::MouseEvent::LEFT)
return false;
bool handled = false;
@@ -293,7 +293,7 @@ bool TerrainEditorPalette::OnMousePress(const common::MouseEvent &_event)
// Only try to modify if the heightmap exists, and the LEFT mouse button
// was used.
- if (heightmap && !_event.shift)
+ if (heightmap && !_event.Shift())
{
handled = this->Apply(_event, camera, heightmap);
QApplication::setOverrideCursor(QCursor(Qt::CrossCursor));
@@ -307,7 +307,7 @@ bool TerrainEditorPalette::OnMousePress(const common::MouseEvent &_event)
/////////////////////////////////////////////////
bool TerrainEditorPalette::OnMouseMove(const common::MouseEvent &_event)
{
- if (_event.button != common::MouseEvent::LEFT)
+ if (_event.Button() != common::MouseEvent::LEFT)
return false;
bool handled = false;
@@ -319,9 +319,9 @@ bool TerrainEditorPalette::OnMouseMove(const common::MouseEvent &_event)
// Get a pointer to the heightmap, if the scen is valid.
rendering::Heightmap *heightmap = scene ? scene->GetHeightmap() : NULL;
- if (heightmap && !_event.shift)
+ if (heightmap && !_event.Shift())
{
- if (_event.dragging)
+ if (_event.Dragging())
handled = this->Apply(_event, camera, heightmap);
QApplication::setOverrideCursor(QCursor(Qt::CrossCursor));
}
@@ -343,17 +343,25 @@ bool TerrainEditorPalette::Apply(const common::MouseEvent &_event,
double insideRadius = this->insideRadiusSpin->value() / 10.0;
if (this->state == "lower")
- handled = _heightmap->Lower(_camera, _event.pos, outsideRadius,
+ {
+ handled = _heightmap->Lower(_camera, _event.Pos(), outsideRadius,
insideRadius, weight);
+ }
else if (this->state == "raise")
- handled = _heightmap->Raise(_camera, _event.pos, outsideRadius,
+ {
+ handled = _heightmap->Raise(_camera, _event.Pos(), outsideRadius,
insideRadius, weight);
+ }
else if (this->state == "flatten")
- handled = _heightmap->Flatten(_camera, _event.pos, outsideRadius,
+ {
+ handled = _heightmap->Flatten(_camera, _event.Pos(), outsideRadius,
insideRadius, weight);
+ }
else if (this->state == "smooth")
- handled = _heightmap->Smooth(_camera, _event.pos, outsideRadius,
+ {
+ handled = _heightmap->Smooth(_camera, _event.Pos(), outsideRadius,
insideRadius, weight);
+ }
return handled;
}
diff --git a/gazebo/gui/terrain/TerrainEditorPalette.hh b/gazebo/gui/terrain/TerrainEditorPalette.hh
index 49e4b81..7531e49 100644
--- a/gazebo/gui/terrain/TerrainEditorPalette.hh
+++ b/gazebo/gui/terrain/TerrainEditorPalette.hh
@@ -40,7 +40,7 @@ namespace gazebo
/// \class TerrainEditorPalette TerrainEditorPalette.hh
/// \brief A palette of building items which can be added to the editor.
- class GAZEBO_VISIBLE TerrainEditorPalette : public QWidget
+ class GZ_GUI_TERRAIN_VISIBLE TerrainEditorPalette : public QWidget
{
Q_OBJECT
diff --git a/gazebo/gui/viewers/CMakeLists.txt b/gazebo/gui/viewers/CMakeLists.txt
index 7c5814c..1cfa093 100644
--- a/gazebo/gui/viewers/CMakeLists.txt
+++ b/gazebo/gui/viewers/CMakeLists.txt
@@ -38,8 +38,16 @@ QT4_ADD_RESOURCES(resources_RCC ${resources})
include_directories(${CMAKE_CURRENT_BINARY_DIR})
-gz_add_library(gazebo_gui_viewers ${sources}
- ${headers_MOC} ${headers} ${resources_RCC})
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_GUI_VIEWERS")
+
+if (WIN32)
+ add_library(gazebo_gui_viewers STATIC ${sources}
+ ${headers_MOC} ${headers} ${resources_RCC})
+ target_link_libraries(gazebo_gui_viewers ${general_libraries})
+else()
+ gz_add_library(gazebo_gui_viewers ${sources}
+ ${headers_MOC} ${headers} ${resources_RCC})
+endif()
target_link_libraries(gazebo_gui_viewers
gazebo_common
diff --git a/gazebo/gui/viewers/ImageView.cc b/gazebo/gui/viewers/ImageView.cc
index 3a4d091..a2ddbe4 100644
--- a/gazebo/gui/viewers/ImageView.cc
+++ b/gazebo/gui/viewers/ImageView.cc
@@ -14,6 +14,13 @@
* 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 "gazebo/transport/TransportIface.hh"
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Publisher.hh"
diff --git a/gazebo/gui/viewers/ImageView.hh b/gazebo/gui/viewers/ImageView.hh
index 79cd6fc..de8710c 100644
--- a/gazebo/gui/viewers/ImageView.hh
+++ b/gazebo/gui/viewers/ImageView.hh
@@ -35,7 +35,7 @@ namespace gazebo
{
class ImageViewPrivate;
- class GAZEBO_VISIBLE ImageView : public TopicView
+ class GZ_GUI_VIEWERS_VISIBLE ImageView : public TopicView
{
Q_OBJECT
diff --git a/gazebo/gui/viewers/ImagesView.cc b/gazebo/gui/viewers/ImagesView.cc
index fbc51d2..364a624 100644
--- a/gazebo/gui/viewers/ImagesView.cc
+++ b/gazebo/gui/viewers/ImagesView.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/transport/TransportIface.hh"
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Publisher.hh"
diff --git a/gazebo/gui/viewers/ImagesView.hh b/gazebo/gui/viewers/ImagesView.hh
index 7327457..5b27cfd 100644
--- a/gazebo/gui/viewers/ImagesView.hh
+++ b/gazebo/gui/viewers/ImagesView.hh
@@ -39,7 +39,7 @@ namespace gazebo
{
class ImagesViewPrivate;
- class GAZEBO_VISIBLE ImagesView : public TopicView
+ class GZ_GUI_VIEWERS_VISIBLE ImagesView : public TopicView
{
Q_OBJECT
diff --git a/gazebo/gui/viewers/LaserView.cc b/gazebo/gui/viewers/LaserView.cc
index 340bf2d..8d95523 100644
--- a/gazebo/gui/viewers/LaserView.cc
+++ b/gazebo/gui/viewers/LaserView.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/transport/TransportIface.hh"
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Publisher.hh"
diff --git a/gazebo/gui/viewers/LaserView.hh b/gazebo/gui/viewers/LaserView.hh
index a1b910b..d33df7b 100644
--- a/gazebo/gui/viewers/LaserView.hh
+++ b/gazebo/gui/viewers/LaserView.hh
@@ -34,7 +34,7 @@ namespace gazebo
{
namespace gui
{
- class GAZEBO_VISIBLE LaserView : public TopicView
+ class GZ_GUI_VIEWERS_VISIBLE LaserView : public TopicView
{
Q_OBJECT
diff --git a/gazebo/gui/viewers/TextView.cc b/gazebo/gui/viewers/TextView.cc
index 5ef712b..233beb6 100644
--- a/gazebo/gui/viewers/TextView.cc
+++ b/gazebo/gui/viewers/TextView.cc
@@ -14,6 +14,13 @@
* 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 "gazebo/transport/TransportIface.hh"
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Publisher.hh"
diff --git a/gazebo/gui/viewers/TextView.hh b/gazebo/gui/viewers/TextView.hh
index 058bfc7..a8a9969 100644
--- a/gazebo/gui/viewers/TextView.hh
+++ b/gazebo/gui/viewers/TextView.hh
@@ -33,7 +33,7 @@ namespace gazebo
{
namespace gui
{
- class GAZEBO_VISIBLE TextView : public TopicView
+ class GZ_GUI_VIEWERS_VISIBLE TextView : public TopicView
{
Q_OBJECT
diff --git a/gazebo/gui/viewers/TopicView.cc b/gazebo/gui/viewers/TopicView.cc
index c46c9d6..8d4d794 100644
--- a/gazebo/gui/viewers/TopicView.cc
+++ b/gazebo/gui/viewers/TopicView.cc
@@ -14,6 +14,13 @@
* 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 "gazebo/gui/viewers/ViewFactory.hh"
#include "gazebo/gui/GuiIface.hh"
#include "gazebo/gui/GuiEvents.hh"
diff --git a/gazebo/gui/viewers/TopicView.hh b/gazebo/gui/viewers/TopicView.hh
index e6a5aad..573e0b8 100644
--- a/gazebo/gui/viewers/TopicView.hh
+++ b/gazebo/gui/viewers/TopicView.hh
@@ -35,7 +35,7 @@ namespace gazebo
/// \cond
/// \brief A custom combobox that pull in a list of topics for user
/// selection.
- class GAZEBO_VISIBLE TopicCombo : public QComboBox
+ class GZ_GUI_VIEWERS_VISIBLE TopicCombo : public QComboBox
{
/// \brief Constructor
/// \param[in] _w Parent widget.
@@ -73,7 +73,7 @@ namespace gazebo
/// \endcond
/// \brief Base class for widgets that display topic data.
- class GAZEBO_VISIBLE TopicView : public QDialog
+ class GZ_GUI_VIEWERS_VISIBLE TopicView : public QDialog
{
Q_OBJECT
diff --git a/gazebo/gui/viewers/ViewFactory.hh b/gazebo/gui/viewers/ViewFactory.hh
index 7a903ea..bbc5ea9 100644
--- a/gazebo/gui/viewers/ViewFactory.hh
+++ b/gazebo/gui/viewers/ViewFactory.hh
@@ -40,7 +40,7 @@ namespace gazebo
/// \class ViewFactory ViewFactory.hh gui/viewers/ViewFactory.hh
/// \brief The view factory creates GUI widgets to visualize data on
/// a topic.
- class GAZEBO_VISIBLE ViewFactory
+ class GZ_GUI_VIEWERS_VISIBLE ViewFactory
{
/// \brief Register all known views
public: static void RegisterAll();
@@ -76,11 +76,11 @@ namespace gazebo
/// \param[in] msgtype Type of message to visualize.
/// \param[in] classname C++ class name for the view.
#define GZ_REGISTER_STATIC_VIEWER(msgtype, classname) \
- GAZEBO_VISIBLE TopicView *New##classname(QWidget *_parent) \
+ GZ_GUI_VIEWERS_VISIBLE TopicView *New##classname(QWidget *_parent) \
{ \
return new gazebo::gui::classname(_parent); \
} \
- GAZEBO_VISIBLE \
+ GZ_GUI_VIEWERS_VISIBLE \
void Register##classname() \
{\
ViewFactory::RegisterView(msgtype, New##classname);\
diff --git a/gazebo/gzserver.1.ronn b/gazebo/gzserver.1.ronn
index 02d3aeb..5317b2d 100644
--- a/gazebo/gzserver.1.ronn
+++ b/gazebo/gzserver.1.ronn
@@ -37,6 +37,8 @@ Gazebo server runs simulation and handles commandline options, starts a Master,
Reduce the TCP/IP traffic output by gzserver
* -s, --server-plugin arg :
Load a plugin.
+* -o, --profile arg :
+ Physics preset profile name from the options in the world file.
## AUTHOR
diff --git a/gazebo/math/Angle.cc b/gazebo/math/Angle.cc
index ca87bba..2a7f212 100644
--- a/gazebo/math/Angle.cc
+++ b/gazebo/math/Angle.cc
@@ -50,6 +50,12 @@ Angle::Angle(const Angle &_angle)
}
//////////////////////////////////////////////////
+Angle::Angle(const ignition::math::Angle &_angle)
+{
+ this->value = _angle.Radian();
+}
+
+//////////////////////////////////////////////////
Angle::~Angle()
{
}
@@ -172,4 +178,22 @@ bool Angle::operator>=(const Angle &angle) const
return this->value > angle.value || math::equal(this->value, angle.value);
}
+//////////////////////////////////////////////////
+ignition::math::Angle Angle::Ign() const
+{
+ return ignition::math::Angle(this->value);
+}
+
+//////////////////////////////////////////////////
+Angle &Angle::operator=(const double &_angle)
+{
+ this->value = _angle;
+ return *this;
+}
+//////////////////////////////////////////////////
+Angle &Angle::operator=(const ignition::math::Angle &_angle)
+{
+ this->value = _angle.Radian();
+ return *this;
+}
diff --git a/gazebo/math/Angle.hh b/gazebo/math/Angle.hh
index c97bf95..50e9329 100644
--- a/gazebo/math/Angle.hh
+++ b/gazebo/math/Angle.hh
@@ -15,10 +15,11 @@
*
*/
-#ifndef _ANGLE_HH_
-#define _ANGLE_HH_
+#ifndef _GAZEBO_ANGLE_HH_
+#define _GAZEBO_ANGLE_HH_
#include <math.h>
+#include <ignition/math/Angle.hh>
#include <iostream>
#include "gazebo/util/system.hh"
@@ -49,7 +50,7 @@ namespace gazebo
/// \class Angle Angle.hh math/gzmath.hh
/// \brief An angle and related functions.
- class GAZEBO_VISIBLE Angle
+ class GZ_MATH_VISIBLE Angle
{
/// \brief math::Angle(0)
public: static const Angle Zero;
@@ -74,6 +75,10 @@ namespace gazebo
/// \param[in] _angle Angle to copy
public: Angle(const Angle &_angle);
+ /// \brief Ignition copy constructor
+ /// \param[in] _angle Ignition angle to copy
+ public: Angle(const ignition::math::Angle &_angle);
+
/// \brief Destructor
public: virtual ~Angle();
@@ -139,6 +144,20 @@ namespace gazebo
/// \return angle
public: Angle operator/=(const Angle &_angle);
+ /// \brief Convert this angle to an ignition::math::Angle.
+ /// \return This Angle as an ignition::math::Angle.
+ public: ignition::math::Angle Ign() const;
+
+ /// \brief Assignment operator
+ /// \param[in] _angle Radians
+ /// \return The new angle
+ public: Angle &operator=(const double &_angle);
+
+ /// \brief Assignment operator
+ /// \param[in] _angle ignition::math::Angle to copy
+ /// \return The new angle
+ public: Angle &operator=(const ignition::math::Angle &_angle);
+
/// \brief Equality operator, result = this == _angle
/// \param[in] _angle Angle to check for equality
/// \return true if this == _angle
diff --git a/gazebo/math/Box.cc b/gazebo/math/Box.cc
index b2e3926..59e2f91 100644
--- a/gazebo/math/Box.cc
+++ b/gazebo/math/Box.cc
@@ -41,6 +41,12 @@ Box::Box(const Vector3 &_vec1, const Vector3 &_vec2)
}
//////////////////////////////////////////////////
+Box::Box(const ignition::math::Box &_box)
+ : min(_box.Min()), max(_box.Max())
+{
+}
+
+//////////////////////////////////////////////////
Box::Box(const Box &_b)
: min(_b.min), max(_b.max), extent(_b.extent)
{
@@ -160,3 +166,25 @@ Box Box::operator-(const Vector3 &_v)
{
return Box(this->min - _v, this->max - _v);
}
+
+//////////////////////////////////////////////////
+bool Box::Contains(const Vector3 &_p) const
+{
+ return _p.x >= this->min.x && _p.x <= this->max.x &&
+ _p.y >= this->min.y && _p.y <= this->max.y &&
+ _p.z >= this->min.z && _p.z <= this->max.z;
+}
+
+//////////////////////////////////////////////////
+ignition::math::Box Box::Ign() const
+{
+ return ignition::math::Box(this->min.Ign(), this->max.Ign());
+}
+
+//////////////////////////////////////////////////
+Box &Box::operator=(const ignition::math::Box &_b)
+{
+ this->min = _b.Min();
+ this->max = _b.Max();
+ return *this;
+}
diff --git a/gazebo/math/Box.hh b/gazebo/math/Box.hh
index c5e53b8..82dd400 100644
--- a/gazebo/math/Box.hh
+++ b/gazebo/math/Box.hh
@@ -14,10 +14,12 @@
* limitations under the License.
*
*/
-#ifndef _BOX_HH_
-#define _BOX_HH_
+#ifndef _GAZEBO_BOX_HH_
+#define _GAZEBO_BOX_HH_
#include <iostream>
+#include <ignition/math/Box.hh>
+
#include "gazebo/math/Vector3.hh"
#include "gazebo/util/system.hh"
@@ -30,17 +32,22 @@ namespace gazebo
/// \class Box Box.hh math/gzmath.hh
/// \brief Mathematical representation of a box and related functions.
- class GAZEBO_VISIBLE Box
+ class GZ_MATH_VISIBLE Box
{
/// \brief Default constructor
public: Box();
+
/// \brief Constructor. This constructor will compute the box's
/// minumum and maximum corners based on the two arguments.
/// \param[in] _vec1 One corner of the box
/// \param[in] _vec2 Another corner of the box
public: Box(const Vector3 &_vec1, const Vector3 &_vec2);
+ /// \brief Ignition math copy constructor
+ /// \param[in] _box Ignition box to convert
+ public: Box(const ignition::math::Box &_box);
+
/// \brief Copy Constructor
/// \param[in] _b Box to copy
public: Box(const Box &_b);
@@ -72,6 +79,15 @@ namespace gazebo
/// \param[in] _box Box to add to this box
public: void Merge(const Box &_box);
+ /// \brief Convert this box to an ignition::math::Box.
+ /// \return This Box as an ignition::math::Box.
+ public: ignition::math::Box Ign() const;
+
+ /// \brief Assignment operator for ignition math
+ /// \param[in] _b a new value
+ /// \return The new box
+ public: Box &operator=(const ignition::math::Box &_b);
+
/// \brief Assignment operator. Set this box to the parameter
/// \param[in] _b Box to copy
/// \return The new box.
@@ -97,6 +113,11 @@ namespace gazebo
/// \return The new box
public: Box operator-(const Vector3 &_v);
+ /// \brief Check if a point lies inside or on the box.
+ /// \param[in] _p Point to check.
+ /// \return True if the point is inside or on the box.
+ public: bool Contains(const math::Vector3 &_p) const;
+
/// \brief Output operator
/// \param[in] _out Output stream
/// \param[in] _b Box to output to the stream
diff --git a/gazebo/math/Box_TEST.cc b/gazebo/math/Box_TEST.cc
index eb73b49..dd2ae85 100644
--- a/gazebo/math/Box_TEST.cc
+++ b/gazebo/math/Box_TEST.cc
@@ -99,3 +99,33 @@ TEST_F(BoxTest, OperatorPlus)
EXPECT_TRUE(box == math::Box(math::Vector3(-2, -2, -2),
math::Vector3(4, 4, 4)));
}
+
+/////////////////////////////////////////////////
+TEST_F(BoxTest, Contains)
+{
+ math::Box box = math::Box(math::Vector3(0, 0, 0), math::Vector3(1, 1, 1));
+
+ EXPECT_TRUE(box.Contains(math::Vector3(0, 0, 0)));
+ EXPECT_TRUE(box.Contains(math::Vector3(0, 0, 1)));
+ EXPECT_TRUE(box.Contains(math::Vector3(0, 1, 1)));
+ EXPECT_TRUE(box.Contains(math::Vector3(1, 1, 1)));
+ EXPECT_TRUE(box.Contains(math::Vector3(1, 1, 0)));
+ EXPECT_TRUE(box.Contains(math::Vector3(1, 0, 0)));
+ EXPECT_TRUE(box.Contains(math::Vector3(0.5, 0.5, 0.5)));
+
+ EXPECT_FALSE(box.Contains(math::Vector3(0, 0, -1)));
+ EXPECT_FALSE(box.Contains(math::Vector3(0, -1, -1)));
+ EXPECT_FALSE(box.Contains(math::Vector3(-1, -1, -1)));
+ EXPECT_FALSE(box.Contains(math::Vector3(-1, -1, 0)));
+ EXPECT_FALSE(box.Contains(math::Vector3(-1, 0, 0)));
+
+ EXPECT_FALSE(box.Contains(math::Vector3(0.5, 0.5, -0.5)));
+ EXPECT_FALSE(box.Contains(math::Vector3(0.5, -0.5, 0.5)));
+ EXPECT_FALSE(box.Contains(math::Vector3(-0.5, 0.5, 0.5)));
+ EXPECT_FALSE(box.Contains(math::Vector3(-0.5, -0.5, 0.5)));
+ EXPECT_FALSE(box.Contains(math::Vector3(-0.5, -0.5, -0.5)));
+
+ EXPECT_FALSE(box.Contains(math::Vector3(0, 0, -0.01)));
+ EXPECT_FALSE(box.Contains(math::Vector3(0, -0.01, 0)));
+ EXPECT_FALSE(box.Contains(math::Vector3(-0.01, 0, 0)));
+}
diff --git a/gazebo/math/CMakeLists.txt b/gazebo/math/CMakeLists.txt
index f2de9fa..e7599f3 100644
--- a/gazebo/math/CMakeLists.txt
+++ b/gazebo/math/CMakeLists.txt
@@ -73,9 +73,10 @@ endforeach()
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/gzmath.hh.in
${CMAKE_CURRENT_BINARY_DIR}/gzmath.hh )
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_MATH")
gz_add_library(gazebo_math ${sources})
-target_link_libraries(gazebo_math ${Boost_LIBRARIES})
+target_link_libraries(gazebo_math ${Boost_LIBRARIES} ${IGNITION-MATH_LIBRARIES})
gz_install_library(gazebo_math)
gz_install_includes("math" ${headers} ${CMAKE_CURRENT_BINARY_DIR}/gzmath.hh)
diff --git a/gazebo/math/Filter.hh b/gazebo/math/Filter.hh
index f814466..a80f46a 100644
--- a/gazebo/math/Filter.hh
+++ b/gazebo/math/Filter.hh
@@ -34,7 +34,7 @@ namespace gazebo
/// \class Filter Filter.hh math/gzmath.hh
/// \brief Filter base class
template <class T>
- class GAZEBO_VISIBLE Filter
+ class GZ_MATH_VISIBLE Filter
{
/// \brief Destructor.
public: virtual ~Filter() {}
@@ -60,7 +60,7 @@ namespace gazebo
/// \brief A one-pole DSP filter.
/// \sa http://www.earlevel.com/main/2012/12/15/a-one-pole-filter/
template <class T>
- class GAZEBO_VISIBLE OnePole : public Filter<T>
+ class GZ_MATH_VISIBLE OnePole : public Filter<T>
{
/// \brief Constructor.
public: OnePole() : a0(0), b1(0) {}
@@ -99,7 +99,7 @@ namespace gazebo
/// \class OnePoleQuaternion Filter.hh math/gzmath.hh
/// \brief One-pole quaternion filter.
- class GAZEBO_VISIBLE OnePoleQuaternion : public OnePole<math::Quaternion>
+ class GZ_MATH_VISIBLE OnePoleQuaternion : public OnePole<math::Quaternion>
{
/// \brief Constructor.
public: OnePoleQuaternion()
@@ -128,7 +128,7 @@ namespace gazebo
/// \class OnePoleVector3 Filter.hh math/gzmath.hh
/// \brief One-pole vector3 filter.
- class GAZEBO_VISIBLE OnePoleVector3 : public OnePole<math::Vector3>
+ class GZ_MATH_VISIBLE OnePoleVector3 : public OnePole<math::Vector3>
{
/// \brief Constructor.
public: OnePoleVector3()
@@ -150,7 +150,7 @@ namespace gazebo
/// \brief Bi-quad filter base class.
/// \sa http://www.earlevel.com/main/2003/03/02/the-bilinear-z-transform/
template <class T>
- class GAZEBO_VISIBLE BiQuad : public Filter<T>
+ class GZ_MATH_VISIBLE BiQuad : public Filter<T>
{
/// \brief Constructor.
public: BiQuad()
@@ -223,7 +223,7 @@ namespace gazebo
/// \class BiQuadVector3 Filter.hh math/gzmath.hh
/// \brief BiQuad vector3 filter
- class GAZEBO_VISIBLE BiQuadVector3 : public BiQuad<math::Vector3>
+ class GZ_MATH_VISIBLE BiQuadVector3 : public BiQuad<math::Vector3>
{
/// \brief Constructor.
public: BiQuadVector3()
diff --git a/gazebo/math/Helpers.hh b/gazebo/math/Helpers.hh
index c51255d..651c0e5 100644
--- a/gazebo/math/Helpers.hh
+++ b/gazebo/math/Helpers.hh
@@ -32,6 +32,9 @@
/// \brief Double min value
#define GZ_DBL_MIN std::numeric_limits<double>::min()
+/// \brief Double positive infinite value
+#define GZ_DBL_INF std::numeric_limits<double>::infinity()
+
/// \brief Float maximum value
#define GZ_FLT_MAX std::numeric_limits<float>::max()
@@ -178,7 +181,15 @@ namespace gazebo
template<typename T>
inline T precision(const T &_a, const unsigned int &_precision)
{
- return boost::math::round(_a * pow(10, _precision)) / pow(10, _precision);
+ if (!std::isinf(_a))
+ {
+ return boost::math::round(
+ _a * pow(10, _precision)) / pow(10, _precision);
+ }
+ else
+ {
+ return _a;
+ }
}
/// \brief is this a power of 2?
diff --git a/gazebo/math/Kmeans.hh b/gazebo/math/Kmeans.hh
index 731873d..b093bc4 100644
--- a/gazebo/math/Kmeans.hh
+++ b/gazebo/math/Kmeans.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// k-means partitions the observations into k sets so as to minimize the
/// within-cluster sum of squares.
/// Description based on http://en.wikipedia.org/wiki/K-means_clustering.
- class GAZEBO_VISIBLE Kmeans
+ class GZ_MATH_VISIBLE Kmeans
{
/// \brief constructor
/// \param[in] _obs Set of observations to cluster.
diff --git a/gazebo/math/Matrix3.cc b/gazebo/math/Matrix3.cc
index 4cef406..5c9e6b9 100644
--- a/gazebo/math/Matrix3.cc
+++ b/gazebo/math/Matrix3.cc
@@ -108,6 +108,27 @@ void Matrix3::SetCol(unsigned int _i, const Vector3 &_v)
}
//////////////////////////////////////////////////
+Matrix3 Matrix3::Inverse() const
+{
+ double t0 = + (this->m[2][2]*this->m[1][1] - this->m[2][1]*this->m[1][2]);
+ double t1 = - (this->m[2][2]*this->m[1][0] - this->m[2][0]*this->m[1][2]);
+ double t2 = + (this->m[2][1]*this->m[1][0] - this->m[2][0]*this->m[1][1]);
+
+ double invDet = 1 / (t0*this->m[0][0] + t1*this->m[0][1] + t2*this->m[0][2]);
+
+ return invDet * Matrix3(
+ t0,
+ - (this->m[2][2]*this->m[0][1] - this->m[2][1]*this->m[0][2]),
+ + (this->m[1][2]*this->m[0][1] - this->m[1][1]*this->m[0][2]),
+ t1,
+ + (this->m[2][2]*this->m[0][0] - this->m[2][0]*this->m[0][2]),
+ - (this->m[1][2]*this->m[0][0] - this->m[1][0]*this->m[0][2]),
+ t2,
+ - (this->m[2][1]*this->m[0][0] - this->m[2][0]*this->m[0][1]),
+ + (this->m[1][1]*this->m[0][0] - this->m[1][0]*this->m[0][1]));
+}
+
+//////////////////////////////////////////////////
bool Matrix3::operator==(const Matrix3 &_m) const
{
return math::equal(this->m[0][0], _m[0][0]) &&
diff --git a/gazebo/math/Matrix3.hh b/gazebo/math/Matrix3.hh
index 7f52842..846f881 100644
--- a/gazebo/math/Matrix3.hh
+++ b/gazebo/math/Matrix3.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class Matrix3 Matrix3hh math/gzmath.hh
/// \brief A 3x3 matrix class
- class GAZEBO_VISIBLE Matrix3
+ class GZ_MATH_VISIBLE Matrix3
{
/// \brief Constructor
public: Matrix3();
@@ -76,6 +76,10 @@ namespace gazebo
/// \param[in] _v The value to set in each row of the column
public: void SetCol(unsigned int _c, const Vector3 &_v);
+ /// \brief Return the inverse matrix
+ /// \return Inverse of this matrix.
+ public: Matrix3 Inverse() const;
+
/// \brief returns the element wise difference of two matrices
public: Matrix3 operator-(const Matrix3 &_m) const
{
diff --git a/gazebo/math/Matrix3_TEST.cc b/gazebo/math/Matrix3_TEST.cc
index 539aa84..2e1d472 100644
--- a/gazebo/math/Matrix3_TEST.cc
+++ b/gazebo/math/Matrix3_TEST.cc
@@ -103,3 +103,22 @@ TEST_F(Matrix3Test, Multiplication)
EXPECT_EQ(matrix * matrix, matrix2);
}
}
+
+TEST_F(Matrix3Test, Inverse)
+{
+ // Inverse of identity matrix is itself
+ EXPECT_EQ(math::Matrix3::IDENTITY, math::Matrix3::IDENTITY.Inverse());
+
+ // Matrix multiplied by its inverse results in the identity matrix
+ math::Matrix3 matrix1(-2, 4, 0, 0.1, 9, 55, -7, 1, 26);
+ math::Matrix3 matrix2 = matrix1.Inverse();
+ EXPECT_EQ(matrix1 * matrix2, math::Matrix3::IDENTITY);
+ EXPECT_EQ(matrix2 * matrix1, math::Matrix3::IDENTITY);
+
+ // Inverse of inverse results in the same matrix
+ EXPECT_EQ((matrix1.Inverse()).Inverse(), matrix1);
+
+ // Invert multiplication by scalar
+ double scalar = 2.5;
+ EXPECT_EQ((matrix1 * scalar).Inverse(), matrix1.Inverse() * (1.0/scalar));
+}
diff --git a/gazebo/math/Matrix4.cc b/gazebo/math/Matrix4.cc
index a8d7d23..f17cfdb 100644
--- a/gazebo/math/Matrix4.cc
+++ b/gazebo/math/Matrix4.cc
@@ -51,6 +51,16 @@ Matrix4::Matrix4(const Matrix4 &_m)
}
//////////////////////////////////////////////////
+Matrix4::Matrix4(const ignition::math::Matrix4d &_m)
+{
+ this->Set(_m(0, 0), _m(0, 1), _m(0, 2), _m(0, 3),
+ _m(1, 0), _m(1, 1), _m(1, 2), _m(1, 3),
+ _m(2, 0), _m(2, 1), _m(2, 2), _m(2, 3),
+ _m(3, 0), _m(3, 1), _m(3, 2), _m(3, 3));
+}
+
+
+//////////////////////////////////////////////////
Matrix4::Matrix4(double _v00, double _v01, double _v02, double _v03,
double _v10, double _v11, double _v12, double _v13,
double _v20, double _v21, double _v22, double _v23,
@@ -218,6 +228,16 @@ Matrix4 &Matrix4::operator =(const Matrix4 &_mat)
}
//////////////////////////////////////////////////
+Matrix4 &Matrix4::operator=(const ignition::math::Matrix4d &_m)
+{
+ this->Set(_m(0, 0), _m(0, 1), _m(0, 2), _m(0, 3),
+ _m(1, 0), _m(1, 1), _m(1, 2), _m(1, 3),
+ _m(2, 0), _m(2, 1), _m(2, 2), _m(2, 3),
+ _m(3, 0), _m(3, 1), _m(3, 2), _m(3, 3));
+ return *this;
+}
+
+//////////////////////////////////////////////////
const Matrix4 &Matrix4::operator =(const Matrix3 &mat)
{
this->m[0][0] = mat.m[0][0];
@@ -480,3 +500,13 @@ math::Pose Matrix4::GetAsPose() const
{
return math::Pose(this->GetTranslation(), this->GetRotation());
}
+
+//////////////////////////////////////////////////
+ignition::math::Matrix4d Matrix4::Ign() const
+{
+ return ignition::math::Matrix4d(
+ this->m[0][0], this->m[0][1], this->m[0][2], this->m[0][3],
+ this->m[1][0], this->m[1][1], this->m[1][2], this->m[1][3],
+ this->m[2][0], this->m[2][1], this->m[2][2], this->m[2][3],
+ this->m[3][0], this->m[3][1], this->m[3][2], this->m[3][3]);
+}
diff --git a/gazebo/math/Matrix4.hh b/gazebo/math/Matrix4.hh
index 7f8223c..df19555 100644
--- a/gazebo/math/Matrix4.hh
+++ b/gazebo/math/Matrix4.hh
@@ -14,11 +14,12 @@
* limitations under the License.
*
*/
-#ifndef _MATRIX4_HH_
-#define _MATRIX4_HH_
+#ifndef _GAZEBO_MATRIX4_HH_
+#define _GAZEBO_MATRIX4_HH_
#include <assert.h>
#include <iostream>
+#include <ignition/math/Matrix4.hh>
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Matrix3.hh"
@@ -36,7 +37,7 @@ namespace gazebo
/// \class Matrix4 Matrix4.hh math/gzmath.hh
/// \brief A 3x3 matrix class
- class GAZEBO_VISIBLE Matrix4
+ class GZ_MATH_VISIBLE Matrix4
{
/// \brief Constructor
public: Matrix4();
@@ -45,6 +46,10 @@ namespace gazebo
/// \param _m Matrix to copy
public: Matrix4(const Matrix4 &_m);
+ /// \brief Copy constructor for ignition math
+ /// \param _m Matrix to copy
+ public: Matrix4(const ignition::math::Matrix4d &_m);
+
/// \brief Constructor
/// \param[in] _v00 Row 0, Col 0 value
/// \param[in] _v01 Row 0, Col 1 value
@@ -128,6 +133,7 @@ namespace gazebo
public: Vector3 TransformAffine(const Vector3 &_v) const;
/// \brief Return the inverse matrix
+ /// \return Inverse of this matrix.
public: Matrix4 Inverse() const;
/// \brief Equal operator. this = _mat
@@ -135,6 +141,11 @@ namespace gazebo
/// \return itself
public: Matrix4 &operator =(const Matrix4 &_mat);
+ /// \brief Equal operator for ignition math
+ /// \param _mat Incoming matrix
+ /// \return itself
+ public: Matrix4 &operator=(const ignition::math::Matrix4d &_mat);
+
/// \brief Equal operator for 3x3 matrix
/// \param _mat Incoming matrix
/// \return itself
@@ -178,6 +189,10 @@ namespace gazebo
/// false otherwise
public: bool operator==(const Matrix4 &_m) const;
+ /// \brief Convert this matrix to an ignition::math::Matrix4d.
+ /// \return This matrix as an ignition::math::Matrix4d.
+ public: ignition::math::Matrix4d Ign() const;
+
/// \brief Stream insertion operator
/// \param _out output stream
/// \param _m Matrix to output
diff --git a/gazebo/math/Plane.cc b/gazebo/math/Plane.cc
index 4653885..6014916 100644
--- a/gazebo/math/Plane.cc
+++ b/gazebo/math/Plane.cc
@@ -19,27 +19,37 @@
using namespace gazebo;
using namespace math;
-
+/////////////////////////////////////////////////
Plane::Plane()
{
this->d = 0.0;
}
+/////////////////////////////////////////////////
Plane::Plane(const Vector3 &_normal, double _offset)
{
this->normal = _normal;
this->d = _offset;
}
+/////////////////////////////////////////////////
Plane::Plane(const Vector3 &_normal, const Vector2d &_size, double _offset)
{
this->Set(_normal, _size, _offset);
}
+/////////////////////////////////////////////////
+Plane::Plane(const ignition::math::Planed &_plane)
+{
+ this->Set(_plane.Normal(), _plane.Size(), _plane.Offset());
+}
+
+/////////////////////////////////////////////////
Plane::~Plane()
{
}
+/////////////////////////////////////////////////
void Plane::Set(const Vector3 &_n, const Vector2d &_s, double _offset)
{
this->normal = _n;
@@ -58,6 +68,16 @@ Plane &Plane::operator =(const Plane & _p)
}
//////////////////////////////////////////////////
+Plane &Plane::operator=(const ignition::math::Planed &_p)
+{
+ this->normal = _p.Normal();
+ this->size = _p.Size();
+ this->d = _p.Offset();
+
+ return *this;
+}
+
+//////////////////////////////////////////////////
double Plane::Distance(const Vector3 &_origin, const Vector3 &_dir) const
{
double denom = this->normal.Dot(_dir);
@@ -74,3 +94,9 @@ double Plane::Distance(const Vector3 &_origin, const Vector3 &_dir) const
return t;
}
}
+
+//////////////////////////////////////////////////
+ignition::math::Planed Plane::Ign() const
+{
+ return ignition::math::Planed(this->normal.Ign(), this->size.Ign(), this->d);
+}
diff --git a/gazebo/math/Plane.hh b/gazebo/math/Plane.hh
index fa3cea5..3029ce3 100644
--- a/gazebo/math/Plane.hh
+++ b/gazebo/math/Plane.hh
@@ -14,8 +14,10 @@
* limitations under the License.
*
*/
-#ifndef _PLANE_HH_
-#define _PLANE_HH_
+#ifndef _GAZEBO_PLANE_HH_
+#define _GAZEBO_PLANE_HH_
+
+#include <ignition/math/Plane.hh>
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Vector2d.hh"
@@ -30,7 +32,7 @@ namespace gazebo
/// \class Plane Plane.hh math/gzmath.hh
/// \brief A plane and related functions.
- class GAZEBO_VISIBLE Plane
+ class GZ_MATH_VISIBLE Plane
{
/// \brief Constructor
public: Plane();
@@ -47,6 +49,10 @@ namespace gazebo
public: Plane(const Vector3 &_normal, const Vector2d &_size,
double _offset);
+ /// \brief Copy constructor for ignition::math::Plane
+ /// \param[in] _plane Plane to copy
+ public: Plane(const ignition::math::Planed &_plane);
+
/// \brief Destructor
public: virtual ~Plane();
@@ -69,6 +75,15 @@ namespace gazebo
/// \return itself
public: Plane &operator =(const Plane &_p);
+ /// \brief Equal operator for ignition::math::Plane
+ /// \param _p Ignition math plane
+ /// \return itself
+ public: Plane &operator =(const ignition::math::Planed &_p);
+
+ /// \brief Convert this to igntion::math::Planed
+ /// \return This plane converted to ignition::math::Planed.
+ public: ignition::math::Planed Ign() const;
+
/// \brief Plane normal
public: Vector3 normal;
diff --git a/gazebo/math/Pose.cc b/gazebo/math/Pose.cc
index 2d3766e..f973fba 100644
--- a/gazebo/math/Pose.cc
+++ b/gazebo/math/Pose.cc
@@ -52,6 +52,12 @@ Pose::Pose(const Pose &_pose)
}
//////////////////////////////////////////////////
+Pose::Pose(const ignition::math::Pose3d &_pose)
+ : pos(_pose.Pos()), rot(_pose.Rot())
+{
+}
+
+//////////////////////////////////////////////////
Pose::~Pose()
{
}
@@ -147,6 +153,14 @@ Pose &Pose::operator=(const Pose &_pose)
}
//////////////////////////////////////////////////
+Pose &Pose::operator=(const ignition::math::Pose3d &_pose)
+{
+ this->pos = _pose.Pos();
+ this->rot = _pose.Rot();
+ return *this;
+}
+
+//////////////////////////////////////////////////
Vector3 Pose::CoordPositionAdd(const Vector3 &_pos) const
{
Quaternion tmp;
@@ -239,3 +253,8 @@ void Pose::Round(int _precision)
this->pos.Round(_precision);
}
+//////////////////////////////////////////////////
+ignition::math::Pose3d Pose::Ign() const
+{
+ return ignition::math::Pose3d(this->pos.Ign(), this->rot.Ign());
+}
diff --git a/gazebo/math/Pose.hh b/gazebo/math/Pose.hh
index f6d6138..c5cf69c 100644
--- a/gazebo/math/Pose.hh
+++ b/gazebo/math/Pose.hh
@@ -14,16 +14,13 @@
* limitations under the License.
*
*/
-/* Desc: External interfaces for Gazebo
- * Author: Nate Koenig
- * Date: 03 Apr 2007
- */
-
-#ifndef _POSE_HH_
-#define _POSE_HH_
+#ifndef _GAZEBO_POSE_HH_
+#define _GAZEBO_POSE_HH_
#include <iostream>
+#include <ignition/math/Pose3.hh>
+
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Quaternion.hh"
#include "gazebo/util/system.hh"
@@ -37,7 +34,7 @@ namespace gazebo
/// \class Pose Pose.hh math/gzmath.hh
/// \brief Encapsulates a position and rotation in three space
- class GAZEBO_VISIBLE Pose
+ class GZ_MATH_VISIBLE Pose
{
/// \brief math::Pose(0, 0, 0, 0, 0, 0)
public: static const Pose Zero;
@@ -64,6 +61,10 @@ namespace gazebo
/// \param[in] _pose Pose to copy
public: Pose(const Pose &_pose);
+ /// \brief Copy constructor for ignition math
+ /// \param[in] _pose Pose to copy
+ public: Pose(const ignition::math::Pose3d &_pose);
+
/// \brief Destructor
public: virtual ~Pose();
@@ -159,6 +160,10 @@ namespace gazebo
/// \param[in] _pose Pose to copy
public: Pose &operator=(const Pose &_pose);
+ /// \brief Equal operator for ignition math
+ /// \param[in] _pose Pose to copy
+ public: Pose &operator=(const ignition::math::Pose3d &_pose);
+
/// \brief Add one point to a vector: result = this + pos
/// \param[in] _pos Position to add to this pose
/// \return the resulting position
@@ -215,6 +220,10 @@ namespace gazebo
/// \param[in] _precision
public: void Round(int _precision);
+ /// \brief Convert this pose to an ignition::math::Pose3d object.
+ /// \return This pose represented as an ignition::math::Pose3d.
+ public: ignition::math::Pose3d Ign() const;
+
/// \brief Stream insertion operator
/// \param[in] _out output stream
/// \param[in] _pose pose to output
diff --git a/gazebo/math/Quaternion.cc b/gazebo/math/Quaternion.cc
index 89c5396..3f45cd2 100644
--- a/gazebo/math/Quaternion.cc
+++ b/gazebo/math/Quaternion.cc
@@ -69,6 +69,15 @@ Quaternion::Quaternion(const Quaternion &_qt)
}
//////////////////////////////////////////////////
+Quaternion::Quaternion(const ignition::math::Quaterniond &_qt)
+{
+ this->w = _qt.W();
+ this->x = _qt.X();
+ this->y = _qt.Y();
+ this->z = _qt.Z();
+}
+
+//////////////////////////////////////////////////
Quaternion::~Quaternion()
{
}
@@ -626,3 +635,19 @@ Quaternion Quaternion::Integrate(const Vector3 &_angularVelocity,
return deltaQ * (*this);
}
+
+//////////////////////////////////////////////////
+ignition::math::Quaterniond Quaternion::Ign() const
+{
+ return ignition::math::Quaterniond(this->w, this->x, this->y, this->z);
+}
+
+//////////////////////////////////////////////////
+Quaternion &Quaternion::operator=(const ignition::math::Quaterniond &_v)
+{
+ this->w = _v.W();
+ this->x = _v.X();
+ this->y = _v.Y();
+ this->z = _v.Z();
+ return *this;
+}
diff --git a/gazebo/math/Quaternion.hh b/gazebo/math/Quaternion.hh
index 003a8ae..3601c48 100644
--- a/gazebo/math/Quaternion.hh
+++ b/gazebo/math/Quaternion.hh
@@ -21,6 +21,7 @@
#include <math.h>
#include <iostream>
#include <cmath>
+#include <ignition/math/Quaternion.hh>
#include "gazebo/math/Helpers.hh"
#include "gazebo/math/Angle.hh"
@@ -38,7 +39,7 @@ namespace gazebo
/// \class Quaternion Quaternion.hh math/gzmath.hh
/// \brief A quaternion class
- class GAZEBO_VISIBLE Quaternion
+ class GZ_MATH_VISIBLE Quaternion
{
/// \brief Default Constructor
public: Quaternion();
@@ -68,9 +69,13 @@ namespace gazebo
public: Quaternion(const Vector3 &_rpy);
/// \brief Copy constructor
- /// \param qt Quaternion to copy
+ /// \param[in] _qt Quaternion to copy
public: Quaternion(const Quaternion &_qt);
+ /// \brief Copy constructor for ignition::math::Quaterniond
+ /// \param[in] _qt Ignition math quaterniond to copy
+ public: Quaternion(const ignition::math::Quaterniond &_qt);
+
/// \brief Destructor
public: ~Quaternion();
@@ -78,11 +83,20 @@ namespace gazebo
/// \param[in] _qt Quaternion to copy
public: Quaternion &operator =(const Quaternion &_qt);
+ /// \brief Convert this quaternion to an ignition::math::Quaterniond.
+ /// \return This quaternion as an ignition::math::Quaterniond.
+ public: ignition::math::Quaterniond Ign() const;
+
+ /// \brief Assignment operator for ignition math
+ /// \param[in] _v a new value
+ /// \return The new quaternion.
+ public: Quaternion &operator =(const ignition::math::Quaterniond &_v);
+
/// \brief Invert the quaternion
public: void Invert();
/// \brief Get the inverse of this quaternion
- /// \return Inverse quarenion
+ /// \return Inverse quaternion
public: inline Quaternion GetInverse() const
{
double s = 0;
@@ -110,7 +124,7 @@ namespace gazebo
return q;
}
- /// \brief Set the quatern to the identity
+ /// \brief Set the quaternion to the identity
public: void SetToIdentity();
/// \brief Return the logarithm
@@ -144,42 +158,46 @@ namespace gazebo
public: void Set(double _u, double _x, double _y, double _z);
/// \brief Set the quaternion from Euler angles. The order of operations
- /// are roll, pitch, yaw.
- /// \param[in] vec Euler angle
+ /// is roll, pitch, yaw around a fixed body frame axis
+ /// (the original frame of the object before rotation is applied).
+ /// Roll is a rotation about x, pitch is about y, yaw is about z.
+ /// \param[in] _vec Euler angle
public: void SetFromEuler(const Vector3 &_vec);
/// \brief Set the quaternion from Euler angles.
/// \param[in] _roll Roll angle (radians).
- /// \param[in] _pitch Roll angle (radians).
- /// \param[in] _yaw Roll angle (radians).
+ /// \param[in] _pitch Pitch angle (radians).
+ /// \param[in] _yaw Yaw angle (radians).
public: void SetFromEuler(double _roll, double _pitch, double _yaw);
/// \brief Return the rotation in Euler angles
/// \return This quaternion as an Euler vector
public: Vector3 GetAsEuler() const;
- /// \brief Convert euler angles to quatern.
- /// \param[in]
+ /// \brief Convert euler angles to a quaternion.
+ /// \param[in] _vec The vector of angles to convert.
+ /// \return The converted quaternion.
public: static Quaternion EulerToQuaternion(const Vector3 &_vec);
/// \brief Convert euler angles to quatern.
/// \param[in] _x rotation along x
/// \param[in] _y rotation along y
/// \param[in] _z rotation along z
+ /// \return The converted quaternion.
public: static Quaternion EulerToQuaternion(double _x,
double _y,
double _z);
/// \brief Get the Euler roll angle in radians
- /// \return the roll
+ /// \return the roll component
public: double GetRoll();
/// \brief Get the Euler pitch angle in radians
- /// \return the pitch
+ /// \return the pitch component
public: double GetPitch();
/// \brief Get the Euler yaw angle in radians
- /// \return the yaw
+ /// \return the yaw component
public: double GetYaw();
/// \brief Return rotation as axis and angle
@@ -201,18 +219,18 @@ namespace gazebo
/// \return this quaternion + qt
public: Quaternion operator+=(const Quaternion &_qt);
- /// \brief Substraction operator
- /// \param[in] _qt quaternion to substract
+ /// \brief Subtraction operator
+ /// \param[in] _qt quaternion to subtract
/// \return this quaternion - _qt
public: Quaternion operator-(const Quaternion &_qt) const;
- /// \brief Substraction operator
- /// \param[in] _qt Quaternion for substraction
- /// \return This quatern - qt
+ /// \brief Subtraction operator
+ /// \param[in] _qt Quaternion for subtraction
+ /// \return This quaternion - qt
public: Quaternion operator-=(const Quaternion &_qt);
/// \brief Multiplication operator
- /// \param[in] _qt Quaternion for multiplication
+ /// \param[in] _q Quaternion for multiplication
/// \return This quaternion multiplied by the parameter
public: inline Quaternion operator*(const Quaternion &_q) const
{
@@ -223,18 +241,19 @@ namespace gazebo
this->w*_q.z + this->x*_q.y - this->y*_q.x + this->z*_q.w);
}
- /// \brief Multiplication operator
+ /// \brief Multiplication operator by a scalar.
/// \param[in] _f factor
- /// \return quaternion multiplied by _f
+ /// \return quaternion multiplied by the scalar
public: Quaternion operator*(const double &_f) const;
/// \brief Multiplication operator
/// \param[in] _qt Quaternion for multiplication
- /// \return This quatern multiplied by the parameter
+ /// \return This quaternion multiplied by the parameter
public: Quaternion operator*=(const Quaternion &qt);
/// \brief Vector3 multiplication operator
/// \param[in] _v vector to multiply
+ /// \return The result of the vector multiplication
public: Vector3 operator*(const Vector3 &_v) const;
/// \brief Equal to operator
@@ -263,14 +282,14 @@ namespace gazebo
/// \brief Do the reverse rotation of a vector by this quaternion
/// \param[in] _vec the vector
- /// \return the
+ /// \return the reversed vector
public: Vector3 RotateVectorReverse(Vector3 _vec) const;
- /// \brief See if a quatern is finite (e.g., not nan)
- /// \return True if quatern is finite
+ /// \brief See if a quaternion is finite (e.g., not nan)
+ /// \return True if quaternion is finite
public: bool IsFinite() const;
- /// \brief Correct any nan
+ /// \brief Correct any nan values in this quaternion
public: inline void Correct()
{
if (!std::isfinite(this->x))
@@ -292,6 +311,7 @@ namespace gazebo
}
/// \brief Get the quaternion as a 3x3 matrix
+ /// \return The 3x3 matrix form of the quaternion
public: Matrix3 GetAsMatrix3() const;
/// \brief Get the quaternion as a 4x4 matrix
@@ -299,15 +319,15 @@ namespace gazebo
public: Matrix4 GetAsMatrix4() const;
/// \brief Return the X axis
- /// \return the vector
+ /// \return the X axis of the vector
public: Vector3 GetXAxis() const;
/// \brief Return the Y axis
- /// \return the vector
+ /// \return the Y axis of the vector
public: Vector3 GetYAxis() const;
/// \brief Return the Z axis
- /// \return the vector
+ /// \return the Z axis of the vector
public: Vector3 GetZAxis() const;
/// \brief Round all values to _precision decimal places
@@ -328,6 +348,7 @@ namespace gazebo
/// \param[in] _rkQ the end quaternion
/// \param[in] _shortestPath when true, the rotation may be inverted to
/// get to minimize rotation
+ /// \return The result of the quadratic interpolation
public: static Quaternion Squad(double _fT, const Quaternion &_rkP,
const Quaternion &_rkA, const Quaternion &_rkB,
const Quaternion &_rkQ, bool _shortestPath = false);
@@ -339,6 +360,7 @@ namespace gazebo
/// \param[in] _rkQ the end quaternion
/// \param[in] _shortestPath when true, the rotation may be inverted to
/// get to minimize rotation
+ /// \return The result of the linear interpolation
public: static Quaternion Slerp(double _fT, const Quaternion &_rkP,
const Quaternion &_rkQ, bool _shortestPath = false);
@@ -351,16 +373,16 @@ namespace gazebo
public: Quaternion Integrate(const Vector3 &_angularVelocity,
const double _deltaT) const;
- /// \brief Attributes of the quaternion
+ /// \brief w value of the quaternion
public: double w;
- /// \brief Attributes of the quaternion
+ /// \brief x value of the quaternion
public: double x;
- /// \brief Attributes of the quaternion
+ /// \brief y value of the quaternion
public: double y;
- /// \brief Attributes of the quaternion
+ /// \brief z value of the quaternion
public: double z;
/// \brief Stream insertion operator
@@ -398,4 +420,3 @@ namespace gazebo
}
}
#endif
-
diff --git a/gazebo/math/Quaternion_TEST.cc b/gazebo/math/Quaternion_TEST.cc
index 4b623be..4652ae7 100644
--- a/gazebo/math/Quaternion_TEST.cc
+++ b/gazebo/math/Quaternion_TEST.cc
@@ -205,6 +205,29 @@ TEST_F(QuaternionTest, Quaternion)
EXPECT_TRUE(q.GetInverse().GetZAxis() == math::Vector3(0, 0, 1));
}
+ // Test RPY fixed-body-frame convention:
+ // Rotate each unit vector in roll and pitch
+ {
+ q = math::Quaternion(M_PI/2.0, M_PI/2.0, 0);
+ math::Vector3 v1(1, 0, 0);
+ math::Vector3 r1 = q.RotateVector(v1);
+ // 90 degrees about X does nothing,
+ // 90 degrees about Y sends point down to -Z
+ EXPECT_EQ(r1, math::Vector3(0, 0, -1));
+
+ math::Vector3 v2(0, 1, 0);
+ math::Vector3 r2 = q.RotateVector(v2);
+ // 90 degrees about X sends point to +Z
+ // 90 degrees about Y sends point to +X
+ EXPECT_EQ(r2, math::Vector3(1, 0, 0));
+
+ math::Vector3 v3(0, 0, 1);
+ math::Vector3 r3 = q.RotateVector(v3);
+ // 90 degrees about X sends point to -Y
+ // 90 degrees about Y does nothing
+ EXPECT_EQ(r3, math::Vector3(0, -1, 0));
+ }
+
{
// now try a harder case (axis[1,2,3], rotation[0.3*pi])
// verified with octave
@@ -285,6 +308,70 @@ TEST_F(QuaternionTest, Integrate)
EXPECT_EQ(qYaw.GetAsEuler(), math::Vector3::UnitZ);
}
+ // Integrate sequentially along single axes in order XYZ,
+ // expect rotations to match Euler Angles
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qX = q.Integrate(math::Vector3::UnitX, angle);
+ math::Quaternion qXY = qX.Integrate(math::Vector3::UnitY, angle);
+ EXPECT_EQ(qXY.GetAsEuler(), angle*math::Vector3(1, 1, 0));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qX = q.Integrate(math::Vector3::UnitX, angle);
+ math::Quaternion qXZ = qX.Integrate(math::Vector3::UnitZ, angle);
+ EXPECT_EQ(qXZ.GetAsEuler(), angle*math::Vector3(1, 0, 1));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qY = q.Integrate(math::Vector3::UnitY, angle);
+ math::Quaternion qYZ = qY.Integrate(math::Vector3::UnitZ, angle);
+ EXPECT_EQ(qYZ.GetAsEuler(), angle*math::Vector3(0, 1, 1));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qX = q.Integrate(math::Vector3::UnitX, angle);
+ math::Quaternion qXY = qX.Integrate(math::Vector3::UnitY, angle);
+ math::Quaternion qXYZ = qXY.Integrate(math::Vector3::UnitZ, angle);
+ EXPECT_EQ(qXYZ.GetAsEuler(), angle*math::Vector3::One);
+ }
+
+ // Integrate sequentially along single axes in order ZYX,
+ // expect rotations to not match Euler Angles
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qZ = q.Integrate(math::Vector3::UnitZ, angle);
+ math::Quaternion qZY = qZ.Integrate(math::Vector3::UnitY, angle);
+ EXPECT_NE(qZY.GetAsEuler(), angle*math::Vector3(0, 1, 1));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qZ = q.Integrate(math::Vector3::UnitZ, angle);
+ math::Quaternion qZX = qZ.Integrate(math::Vector3::UnitX, angle);
+ EXPECT_NE(qZX.GetAsEuler(), angle*math::Vector3(1, 0, 1));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qZ = q.Integrate(math::Vector3::UnitZ, angle);
+ math::Quaternion qZY = qZ.Integrate(math::Vector3::UnitY, angle);
+ math::Quaternion qZYX = qZY.Integrate(math::Vector3::UnitX, angle);
+ EXPECT_NE(qZYX.GetAsEuler(), angle*math::Vector3(1, 1, 1));
+ }
+ {
+ const math::Quaternion q(1, 0, 0, 0);
+ const double angle = 0.5;
+ math::Quaternion qY = q.Integrate(math::Vector3::UnitY, angle);
+ math::Quaternion qYX = qY.Integrate(math::Vector3::UnitX, angle);
+ EXPECT_NE(qYX.GetAsEuler(), angle*math::Vector3(1, 1, 0));
+ }
+
// Integrate a full rotation about different axes,
// expect no change.
{
diff --git a/gazebo/math/Rand.cc b/gazebo/math/Rand.cc
index 6986291..c7ca2ad 100644
--- a/gazebo/math/Rand.cc
+++ b/gazebo/math/Rand.cc
@@ -20,7 +20,11 @@
*/
#include <sys/types.h>
-#include <unistd.h>
+#ifdef _WIN32
+ #include <process.h>
+#else
+ #include <unistd.h>
+#endif
#include <ctime>
#include "gazebo/math/Rand.hh"
@@ -32,7 +36,11 @@ using namespace math;
// We don't seed with time for the cases when two processes are started the
// same time (this mostly happens with launch scripts that start a server
// and gui simultaneously).
-uint32_t Rand::seed = getpid();
+#ifdef _WIN32
+ uint32_t Rand::seed = _getpid();
+#else
+ uint32_t Rand::seed = getpid();
+#endif
GeneratorType *Rand::randGenerator = new GeneratorType(seed);
diff --git a/gazebo/math/Rand.hh b/gazebo/math/Rand.hh
index 09ea362..56f0212 100644
--- a/gazebo/math/Rand.hh
+++ b/gazebo/math/Rand.hh
@@ -57,7 +57,7 @@ namespace gazebo
/// \class Rand Rand.hh gzmath/gzmath.hh
/// \brief Random number generator class
- class GAZEBO_VISIBLE Rand
+ class GZ_MATH_VISIBLE Rand
{
/// \brief Set the seed value.
/// \param[in] _seed The seed used to initialize the randon number
diff --git a/gazebo/math/Rand_TEST.cc b/gazebo/math/Rand_TEST.cc
index 82d940e..acc8447 100644
--- a/gazebo/math/Rand_TEST.cc
+++ b/gazebo/math/Rand_TEST.cc
@@ -42,6 +42,8 @@ TEST_F(RandTest, Rand)
EXPECT_GE(i, 1);
i = math::Rand::GetIntNormal(2, 3);
+ EXPECT_LE(i, GZ_INT32_MAX);
+ EXPECT_GE(i, -GZ_INT32_MAX);
// Test setting the random number seed
{
diff --git a/gazebo/math/RotationSpline.hh b/gazebo/math/RotationSpline.hh
index 6039fdd..706ea4c 100644
--- a/gazebo/math/RotationSpline.hh
+++ b/gazebo/math/RotationSpline.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \class RotationSpline RotationSpline.hh math/gzmath.hh
/// \brief Spline for rotations
- class GAZEBO_VISIBLE RotationSpline
+ class GZ_MATH_VISIBLE RotationSpline
{
/// \brief Constructor. Sets the autoCalc to true
public: RotationSpline();
diff --git a/gazebo/math/SignalStats.cc b/gazebo/math/SignalStats.cc
index c9336b0..113a93f 100644
--- a/gazebo/math/SignalStats.cc
+++ b/gazebo/math/SignalStats.cc
@@ -144,10 +144,9 @@ size_t SignalStats::Count() const
std::map<std::string, double> SignalStats::Map() const
{
std::map<std::string, double> map;
- for (SignalStatistic_V::const_iterator iter = this->dataPtr->stats.begin();
- iter != this->dataPtr->stats.end(); ++iter)
+ for (auto const &statistic : this->dataPtr->stats)
{
- map[(*iter)->ShortName()] = (*iter)->Value();
+ map[statistic->ShortName()] = statistic->Value();
}
return map;
}
@@ -155,10 +154,9 @@ std::map<std::string, double> SignalStats::Map() const
//////////////////////////////////////////////////
void SignalStats::InsertData(const double _data)
{
- for (SignalStatistic_V::iterator iter = this->dataPtr->stats.begin();
- iter != this->dataPtr->stats.end(); ++iter)
+ for (auto &statistic : this->dataPtr->stats)
{
- (*iter)->InsertData(_data);
+ statistic->InsertData(_data);
}
}
@@ -220,10 +218,9 @@ bool SignalStats::InsertStatistics(const std::string &_names)
bool result = true;
std::vector<std::string> names;
boost::split(names, _names, boost::is_any_of(","));
- for (std::vector<std::string>::iterator iter = names.begin();
- iter != names.end(); ++iter)
+ for (auto &statistic : names)
{
- result = result && this->InsertStatistic(*iter);
+ result = result && this->InsertStatistic(statistic);
}
return result;
}
@@ -231,10 +228,9 @@ bool SignalStats::InsertStatistics(const std::string &_names)
//////////////////////////////////////////////////
void SignalStats::Reset()
{
- for (SignalStatistic_V::iterator iter = this->dataPtr->stats.begin();
- iter != this->dataPtr->stats.end(); ++iter)
+ for (auto &statistic : this->dataPtr->stats)
{
- (*iter)->Reset();
+ statistic->Reset();
}
}
diff --git a/gazebo/math/SignalStats.hh b/gazebo/math/SignalStats.hh
index 272486e..9707705 100644
--- a/gazebo/math/SignalStats.hh
+++ b/gazebo/math/SignalStats.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class SignalStatistic SignalStats.hh math/gzmath.hh
/// \brief Statistical properties of a discrete time scalar signal.
- class GAZEBO_VISIBLE SignalStatistic
+ class GZ_MATH_VISIBLE SignalStatistic
{
/// \brief Constructor
public: SignalStatistic();
@@ -67,7 +67,7 @@ namespace gazebo
/// \class SignalMean SignalStats.hh math/gzmath.hh
/// \brief Computing the mean value of a discretely sampled signal.
- class GAZEBO_VISIBLE SignalMean : public SignalStatistic
+ class GZ_MATH_VISIBLE SignalMean : public SignalStatistic
{
// Documentation inherited.
public: virtual double Value() const;
@@ -84,7 +84,7 @@ namespace gazebo
/// \class SignalRootMeanSquare SignalStats.hh math/gzmath.hh
/// \brief Computing the square root of the mean squared value
/// of a discretely sampled signal.
- class GAZEBO_VISIBLE SignalRootMeanSquare : public SignalStatistic
+ class GZ_MATH_VISIBLE SignalRootMeanSquare : public SignalStatistic
{
// Documentation inherited.
public: virtual double Value() const;
@@ -102,7 +102,7 @@ namespace gazebo
/// \brief Computing the maximum of the absolute value
/// of a discretely sampled signal.
/// Also known as the maximum norm, infinity norm, or supremum norm.
- class GAZEBO_VISIBLE SignalMaxAbsoluteValue : public SignalStatistic
+ class GZ_MATH_VISIBLE SignalMaxAbsoluteValue : public SignalStatistic
{
// Documentation inherited.
public: virtual double Value() const;
@@ -121,7 +121,7 @@ namespace gazebo
/// \class SignalStats SignalStats.hh math/gzmath.hh
/// \brief Collection of statistics for a scalar signal.
- class GAZEBO_VISIBLE SignalStats
+ class GZ_MATH_VISIBLE SignalStats
{
/// \brief Constructor
public: SignalStats();
diff --git a/gazebo/math/Spline.hh b/gazebo/math/Spline.hh
index f65575f..7555b19 100644
--- a/gazebo/math/Spline.hh
+++ b/gazebo/math/Spline.hh
@@ -34,7 +34,7 @@ namespace gazebo
/// \class Spline Spline.hh math/gzmath.hh
/// \brief Splines
- class GAZEBO_VISIBLE Spline
+ class GZ_MATH_VISIBLE Spline
{
/// \brief constructor
public: Spline();
diff --git a/gazebo/math/Vector2d.cc b/gazebo/math/Vector2d.cc
index 0ea00e5..3cdb571 100644
--- a/gazebo/math/Vector2d.cc
+++ b/gazebo/math/Vector2d.cc
@@ -45,6 +45,12 @@ Vector2d::Vector2d(const Vector2d &_pt)
}
//////////////////////////////////////////////////
+Vector2d::Vector2d(const ignition::math::Vector2d &_v)
+ : x(_v.X()), y(_v.Y())
+{
+}
+
+//////////////////////////////////////////////////
Vector2d::~Vector2d()
{
}
@@ -82,6 +88,15 @@ Vector2d &Vector2d::operator =(const Vector2d &_pt)
}
//////////////////////////////////////////////////
+Vector2d &Vector2d::operator=(const ignition::math::Vector2d &_pt)
+{
+ this->x = _pt.X();
+ this->y = _pt.Y();
+
+ return *this;
+}
+
+//////////////////////////////////////////////////
const Vector2d &Vector2d::operator =(double value)
{
this->x = value;
@@ -209,3 +224,9 @@ double Vector2d::Dot(const Vector2d &_v) const
{
return (this->x * _v.x) + (this->y * _v.y);
}
+
+//////////////////////////////////////////////////
+ignition::math::Vector2d Vector2d::Ign() const
+{
+ return ignition::math::Vector2d(this->x, this->y);
+}
diff --git a/gazebo/math/Vector2d.hh b/gazebo/math/Vector2d.hh
index 98ea048..46fc336 100644
--- a/gazebo/math/Vector2d.hh
+++ b/gazebo/math/Vector2d.hh
@@ -14,13 +14,10 @@
* limitations under the License.
*
*/
-/* Desc: Two dimensional vector
- * Author: Nate Koenig
- * Date: 3 Apr 2007
- */
+#ifndef _GAZEBO_VECTOR2D_HH_
+#define _GAZEBO_VECTOR2D_HH_
-#ifndef _VECTOR2D_HH_
-#define _VECTOR2D_HH_
+#include <ignition/math/Vector2.hh>
#include <math.h>
#include <iostream>
@@ -36,7 +33,7 @@ namespace gazebo
/// \class Vector2d Vector2D.hh math/gzmath.hh
/// \brief Generic double x, y vector
- class GAZEBO_VISIBLE Vector2d
+ class GZ_MATH_VISIBLE Vector2d
{
/// \brief Constructor
public: Vector2d();
@@ -50,6 +47,10 @@ namespace gazebo
/// \param[in] _v the value
public: Vector2d(const Vector2d &_v);
+ /// \brief Copy constructor for ignition math
+ /// \param[in] _v the value
+ public: Vector2d(const ignition::math::Vector2d &_v);
+
/// \brief Destructor
public: virtual ~Vector2d();
@@ -76,6 +77,11 @@ namespace gazebo
/// \return this
public: Vector2d &operator =(const Vector2d &_v);
+ /// \brief Assignment operator for ignition math
+ /// \param[in] _v a value for x and y element
+ /// \return this
+ public: Vector2d &operator=(const ignition::math::Vector2d &_v);
+
/// \brief Assignment operator
/// \param[in] _v the value for x and y element
/// \return this
@@ -163,6 +169,10 @@ namespace gazebo
/// \return the value, or 0 if _index is out of bounds
public: double operator[](unsigned int _index) const;
+ /// \brief Convert this vector to an ignition::math::Vector2d.
+ /// \return This vector as an ignition::math::Vector2d.
+ public: ignition::math::Vector2d Ign() const;
+
/// \brief x data
public: double x;
diff --git a/gazebo/math/Vector2i.cc b/gazebo/math/Vector2i.cc
index 80582e0..e913cf2 100644
--- a/gazebo/math/Vector2i.cc
+++ b/gazebo/math/Vector2i.cc
@@ -44,6 +44,12 @@ Vector2i::Vector2i(const Vector2i &_pt)
}
//////////////////////////////////////////////////
+Vector2i::Vector2i(const ignition::math::Vector2i &_pt)
+ : x(_pt.X()), y(_pt.Y())
+{
+}
+
+//////////////////////////////////////////////////
Vector2i::~Vector2i()
{
}
@@ -81,6 +87,15 @@ Vector2i &Vector2i::operator =(const Vector2i &_pt)
}
//////////////////////////////////////////////////
+Vector2i &Vector2i::operator=(const ignition::math::Vector2i &_pt)
+{
+ this->x = _pt.X();
+ this->y = _pt.Y();
+
+ return *this;
+}
+
+//////////////////////////////////////////////////
const Vector2i &Vector2i::operator =(int _value)
{
this->x = _value;
@@ -206,4 +221,8 @@ int Vector2i::operator[](unsigned int index) const
}
}
-
+//////////////////////////////////////////////////
+ignition::math::Vector2i Vector2i::Ign() const
+{
+ return ignition::math::Vector2i(this->x, this->y);
+}
diff --git a/gazebo/math/Vector2i.hh b/gazebo/math/Vector2i.hh
index bdef347..04c75dd 100644
--- a/gazebo/math/Vector2i.hh
+++ b/gazebo/math/Vector2i.hh
@@ -14,17 +14,14 @@
* limitations under the License.
*
*/
-/* Desc: Two dimensional vector
- * Author: Nate Koenig
- * Date: 3 Apr 2007
- */
-
-#ifndef _VECTOR2I_HH_
-#define _VECTOR2I_HH_
+#ifndef _GAZEBO_VECTOR2I_HH_
+#define _GAZEBO_VECTOR2I_HH_
#include <math.h>
#include <iostream>
#include <fstream>
+#include <ignition/math/Vector2.hh>
+
#include "gazebo/util/system.hh"
namespace gazebo
@@ -36,7 +33,7 @@ namespace gazebo
/// \class Vector2i Vector2i.hh math/gzmath.hh
/// \brief Generic integer x, y vector
- class GAZEBO_VISIBLE Vector2i
+ class GZ_MATH_VISIBLE Vector2i
{
/// \brief Constructor
public: Vector2i();
@@ -46,10 +43,14 @@ namespace gazebo
/// \param[in] _y value along y
public: Vector2i(const int &_x, const int &_y);
- /// \brief Copy onstructor
+ /// \brief Copy constructor
/// \param[in] _pt a point
public: Vector2i(const Vector2i &_pt);
+ /// \brief Copy constructor for ignition math
+ /// \param[in] _pt a point
+ public: Vector2i(const ignition::math::Vector2i &_pt);
+
/// \brief Destructor
public: virtual ~Vector2i();
@@ -76,6 +77,11 @@ namespace gazebo
/// \return this
public: Vector2i &operator =(const Vector2i &_v);
+ /// \brief Assignment operator for ignition math
+ /// \param[in] _v the value
+ /// \return this
+ public: Vector2i &operator =(const ignition::math::Vector2i &_v);
+
/// \brief Assignment operator
/// \param[in] _value the value for x and y
/// \return this
@@ -165,6 +171,11 @@ namespace gazebo
/// \param[in] _index the array index
public: int operator[](unsigned int _index) const;
+ /// \brief Convert this vector to ignition::math::Vector2i
+ /// \return Return an ignition::math::Vector2i representation of this
+ /// vector.
+ public: ignition::math::Vector2i Ign() const;
+
/// \brief x data
public: int x;
diff --git a/gazebo/math/Vector3.cc b/gazebo/math/Vector3.cc
index b76da0b..f47be48 100644
--- a/gazebo/math/Vector3.cc
+++ b/gazebo/math/Vector3.cc
@@ -44,6 +44,12 @@ Vector3::Vector3(const double &_x, const double &_y, const double &_z)
}
//////////////////////////////////////////////////
+Vector3::Vector3(const ignition::math::Vector3d &_v)
+: x(_v.X()), y(_v.Y()), z(_v.Z())
+{
+}
+
+//////////////////////////////////////////////////
Vector3::Vector3(const Vector3 &_pt)
: x(_pt.x), y(_pt.y), z(_pt.z)
{
@@ -368,3 +374,18 @@ bool Vector3::Equal(const Vector3 &_v) const
math::equal(this->y, _v.y) &&
math::equal(this->z, _v.z);
}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d Vector3::Ign() const
+{
+ return ignition::math::Vector3d(this->x, this->y, this->z);
+}
+
+//////////////////////////////////////////////////
+Vector3 &Vector3::operator=(const ignition::math::Vector3d &_v)
+{
+ this->x = _v.X();
+ this->y = _v.Y();
+ this->z = _v.Z();
+ return *this;
+}
diff --git a/gazebo/math/Vector3.hh b/gazebo/math/Vector3.hh
index 48b9c78..f31690a 100644
--- a/gazebo/math/Vector3.hh
+++ b/gazebo/math/Vector3.hh
@@ -14,17 +14,13 @@
* limitations under the License.
*
*/
-/* Desc: The world; all models are collected here
- * Author: Nate Koenig
- * Date: 3 Apr 2007
- */
-
-#ifndef _VECTOR3_HH_
-#define _VECTOR3_HH_
+#ifndef _GAZEBO_VECTOR3_HH_
+#define _GAZEBO_VECTOR3_HH_
#include <math.h>
#include <iostream>
#include <fstream>
+#include <ignition/math/Vector3.hh>
#include "gazebo/math/Helpers.hh"
#include "gazebo/util/system.hh"
@@ -40,7 +36,7 @@ namespace gazebo
/// \brief The Vector3 class represents the generic vector containing 3
/// elements. Since it's commonly used to keep coordinate system
/// related information, its elements are labeled by x, y, z.
- class GAZEBO_VISIBLE Vector3
+ class GZ_MATH_VISIBLE Vector3
{
/// \brief math::Vector3(0, 0, 0)
public: static const Vector3 Zero;
@@ -66,6 +62,10 @@ namespace gazebo
/// \param[in] _z value along z
public: Vector3(const double &_x, const double &_y, const double &_z);
+ /// \brief Ignition math copy constructor
+ /// \param[in] _v a vector
+ public: Vector3(const ignition::math::Vector3d &_v);
+
/// \brief Copy constructor
/// \param[in] _v a vector
public: Vector3(const Vector3 &_v);
@@ -168,9 +168,18 @@ namespace gazebo
/// \return the minimum element
public: double GetMin() const;
- /// \brief Assignment operator
+ /// \brief Convert this vector to an ignition::math::Vector3d.
+ /// \return This vector as an ignition::math::Vector3d.
+ public: ignition::math::Vector3d Ign() const;
+
+ /// \brief Assignment operator for ignition math
/// \param[in] _v a new value
/// \return this
+ public: Vector3 &operator=(const ignition::math::Vector3d &_v);
+
+ /// \brief Assignment operator
+ /// \param[in] _v a new value
+ /// \return The new vector
public: Vector3 &operator =(const Vector3 &_v);
/// \brief Assignment operator
diff --git a/gazebo/math/Vector3Stats.hh b/gazebo/math/Vector3Stats.hh
index 877b783..22eafa6 100644
--- a/gazebo/math/Vector3Stats.hh
+++ b/gazebo/math/Vector3Stats.hh
@@ -34,7 +34,7 @@ namespace gazebo
/// \class Vector3Stats Vector3Stats.hh math/gzmath.hh
/// \brief Collection of statistics for a Vector3 signal.
- class GAZEBO_VISIBLE Vector3Stats
+ class GZ_MATH_VISIBLE Vector3Stats
{
/// \brief Constructor
public: Vector3Stats();
diff --git a/gazebo/math/Vector4.hh b/gazebo/math/Vector4.hh
index 9d112b1..926b0cc 100644
--- a/gazebo/math/Vector4.hh
+++ b/gazebo/math/Vector4.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \class Vector4 Vector4.hh math/gzmath.hh
/// \brief double Generic x, y, z, w vector
- class GAZEBO_VISIBLE Vector4
+ class GZ_MATH_VISIBLE Vector4
{
/// \brief Constructor
public: Vector4();
diff --git a/gazebo/msgs/CMakeLists.txt b/gazebo/msgs/CMakeLists.txt
index 9c61fed..00a79ee 100644
--- a/gazebo/msgs/CMakeLists.txt
+++ b/gazebo/msgs/CMakeLists.txt
@@ -1,10 +1,13 @@
include (${gazebo_cmake_dir}/GazeboUtils.cmake)
+include_directories(${IGNITION-MATH_INCLUDE_DIRS})
set (msgs
+ altimeter.proto
axis.proto
boxgeom.proto
camerasensor.proto
camera_cmd.proto
+ cessna.proto
collision.proto
color.proto
contact.proto
@@ -45,7 +48,10 @@ set (msgs
link.proto
link_data.proto
log_control.proto
+ log_playback_control.proto
+ log_playback_stats.proto
log_status.proto
+ magnetometer.proto
material.proto
meshgeom.proto
model.proto
@@ -75,6 +81,10 @@ set (msgs
raysensor.proto
request.proto
response.proto
+ rest_error.proto
+ rest_login.proto
+ rest_logout.proto
+ rest_post.proto
road.proto
scene.proto
selection.proto
@@ -114,7 +124,10 @@ gz_build_tests(${msgs_tests_sources})
include_directories(${CMAKE_CURRENT_BINARY_DIR})
add_executable(gazebomsgs_out generator/GazeboGenerator.cc generator/gazebo_generator.cc)
-target_link_libraries(gazebomsgs_out ${PROTOBUF_LIBRARY} ${PROTOBUF_PROTOC_LIBRARY} pthread)
+target_link_libraries(gazebomsgs_out ${GZ_PROTOBUF_LIBRARY} ${GZ_PROTOBUF_PROTOC_LIBRARY})
+if (UNIX)
+ target_link_libraries(gazebomsgs_out pthread)
+endif()
set (sources msgs.cc MsgFactory.cc)
set (headers msgs.hh MsgFactory.hh)
@@ -142,30 +155,42 @@ foreach(FIL ${msgs})
list(APPEND PROTO_SRCS "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.pb.cc")
list(APPEND PROTO_HDRS "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.pb.h")
+ # Need to add .exe to executable in windows
+ if (WIN32)
+ set(gazebomsgs_out_binary "gazebomsgs_out.exe")
+ else()
+ set(gazebomsgs_out_binary "gazebomsgs_out")
+ endif()
+
add_custom_command(
OUTPUT
"${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.pb.cc"
"${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.pb.h"
COMMAND ${PROTOBUF_PROTOC_EXECUTABLE}
- ARGS --plugin=protoc-gen-gazebomsgs=${CMAKE_CURRENT_BINARY_DIR}/gazebomsgs_out --cpp_out ${CMAKE_CURRENT_BINARY_DIR} --gazebomsgs_out ${CMAKE_CURRENT_BINARY_DIR} --proto_path ${CMAKE_CURRENT_SOURCE_DIR} ${ABS_FIL}
+ ARGS --plugin=protoc-gen-gazebomsgs=${CMAKE_CURRENT_BINARY_DIR}/${gazebomsgs_out_binary} --cpp_out=dllexport_decl=GZ_MSGS_VISIBLE:${CMAKE_CURRENT_BINARY_DIR} --gazebomsgs_out=${CMAKE_CURRENT_BINARY_DIR} --proto_path=${CMAKE_CURRENT_SOURCE_DIR} ${ABS_FIL}
DEPENDS ${ABS_FIL} gazebomsgs_out
COMMENT "Running C++ protocol buffer compiler on ${FIL}"
VERBATIM )
endforeach()
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_MSGS")
+
set_source_files_properties(${PROTO_SRCS} ${PROTO_HDRS} PROPERTIES GENERATED TRUE)
gz_add_library(gazebo_msgs ${PROTO_SRCS} ${sources})
-# Need to add default visibility to gazebo_msgs
-get_target_property(current_property gazebo_msgs COMPILE_FLAGS)
-if(NOT current_property) # property non-existent or empty
- set_target_properties(gazebo_msgs PROPERTIES GENERATED TRUE COMPILE_FLAGS "-fvisibility=default")
-else()
- set_target_properties(gazebo_msgs PROPERTIES COMPILE_FLAGS "${current_property} -fvisibility=default")
+if (UNIX)
+ # Need to add default visibility to gazebo_msgs
+ get_target_property(current_property gazebo_msgs COMPILE_FLAGS)
+ if(NOT current_property) # property non-existent or empty
+ set_target_properties(gazebo_msgs PROPERTIES GENERATED TRUE COMPILE_FLAGS "-fvisibility=default")
+ else()
+ set_target_properties(gazebo_msgs PROPERTIES COMPILE_FLAGS "${current_property} -fvisibility=default")
+ endif()
endif()
+
target_link_libraries(gazebo_msgs
gazebo_common
gazebo_math
- ${PROTOBUF_LIBRARY}
+ ${GZ_PROTOBUF_LIBRARY}
)
add_dependencies(gazebo_msgs gazebomsgs_out)
diff --git a/gazebo/msgs/altimeter.proto b/gazebo/msgs/altimeter.proto
new file mode 100644
index 0000000..49fa557
--- /dev/null
+++ b/gazebo/msgs/altimeter.proto
@@ -0,0 +1,23 @@
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface Altimeter
+/// \brief Data from an altimeter sensor
+
+import "time.proto";
+
+/// \brief Altimeter sensor data
+message Altimeter
+{
+ /// \brief Timestamp of the altimeter data
+ required Time time = 1;
+
+ /// \brief Vertical position data, in meters.
+ required double vertical_position = 2;
+
+ /// \brief Vertical velocity data, in meters/second.
+ required double vertical_velocity = 3;
+
+ /// \brief Vertical reference.
+ required double vertical_reference = 4;
+}
diff --git a/gazebo/msgs/cessna.proto b/gazebo/msgs/cessna.proto
new file mode 100644
index 0000000..886bf1a
--- /dev/null
+++ b/gazebo/msgs/cessna.proto
@@ -0,0 +1,50 @@
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface Cessna
+/// \brief Cessna message
+
+message Cessna
+{
+ /// \brief Current RPM of the propeller.
+ optional float propeller_speed = 1;
+
+ /// \brief Current left aileron angle in rads.
+ optional float left_aileron = 2;
+
+ /// \brief Current left flap angle in rads.
+ optional float left_flap = 3;
+
+ /// \brief Current right aileron angle in rads.
+ optional float right_aileron = 4;
+
+ /// \brief Current right flap angle in rads.
+ optional float right_flap = 5;
+
+ /// \brief Current elevators angle in rads.
+ optional float elevators = 6;
+
+ /// \brief Current ruddle angle in rads.
+ optional float rudder = 7;
+
+ /// \brief Target RPM of the propeller.
+ optional float cmd_propeller_speed = 8;
+
+ /// \brief Target left aileron angle in rads.
+ optional float cmd_left_aileron = 9;
+
+ /// \brief Target left flap angle in rads.
+ optional float cmd_left_flap = 10;
+
+ /// \brief Target right aileron angle in rads.
+ optional float cmd_right_aileron = 11;
+
+ /// \brief Target right flap angle in rads.
+ optional float cmd_right_flap = 12;
+
+ /// \brief Target elevators angle in rads.
+ optional float cmd_elevators = 13;
+
+ /// \brief Target ruddle angle in rads.
+ optional float cmd_rudder = 14;
+}
diff --git a/gazebo/msgs/generator/GazeboGenerator.cc b/gazebo/msgs/generator/GazeboGenerator.cc
index 7fab35a..b25c0e6 100644
--- a/gazebo/msgs/generator/GazeboGenerator.cc
+++ b/gazebo/msgs/generator/GazeboGenerator.cc
@@ -72,6 +72,7 @@ bool GazeboGenerator::Generate(const FileDescriptor *_file,
io::Printer printer(output.get(), '$');
printer.Print("#include <boost/shared_ptr.hpp>\n", "name", "includes");
+ printer.Print("#include <gazebo/util/system.hh>\n", "name", "includes");
printer.Print("#include \"gazebo/msgs/MsgFactory.hh\"\n",
"name", "includes");
}
diff --git a/gazebo/msgs/geometry.proto b/gazebo/msgs/geometry.proto
index b15a92d..735db3d 100644
--- a/gazebo/msgs/geometry.proto
+++ b/gazebo/msgs/geometry.proto
@@ -1,7 +1,7 @@
package gazebo.msgs;
/// \ingroup gazebo_msgs
-/// \interface Geometry
+/// \interface Geometry
/// \brief Information about a geometry element
@@ -17,14 +17,14 @@ import "polylinegeom.proto";
message Geometry
{
- enum Type
+ enum Type
{
- BOX = 1;
- CYLINDER = 2;
- SPHERE = 3;
- PLANE = 4;
- IMAGE = 5;
- HEIGHTMAP = 6;
+ BOX = 1;
+ CYLINDER = 2;
+ SPHERE = 3;
+ PLANE = 4;
+ IMAGE = 5;
+ HEIGHTMAP = 6;
MESH = 7;
TRIANGLE_FAN = 8;
LINE_STRIP = 9;
@@ -42,5 +42,5 @@ message Geometry
optional MeshGeom mesh = 8;
repeated Vector3d points = 9;
- optional Polyline polyline = 10;
+ repeated Polyline polyline = 10;
}
diff --git a/gazebo/msgs/gui_camera.proto b/gazebo/msgs/gui_camera.proto
index 0e6772e..4d1e979 100644
--- a/gazebo/msgs/gui_camera.proto
+++ b/gazebo/msgs/gui_camera.proto
@@ -15,4 +15,7 @@ message GUICamera
optional string view_controller = 2;
optional Pose pose = 3;
optional TrackVisual track = 4;
+
+ /// \brief Type of projection: "perspective" or "orthographic".
+ optional string projection_type = 5;
}
diff --git a/gazebo/msgs/joint.proto b/gazebo/msgs/joint.proto
index b890e91..f89174e 100644
--- a/gazebo/msgs/joint.proto
+++ b/gazebo/msgs/joint.proto
@@ -1,7 +1,7 @@
package gazebo.msgs;
/// \ingroup gazebo_msgs
-/// \interface Joint
+/// \interface Joint
/// \brief Message for creating joint in rendering::Scene
@@ -20,6 +20,7 @@ message Joint
BALL = 5;
SCREW = 6;
GEARBOX = 7;
+ FIXED = 8;
}
required string name = 1;
diff --git a/gazebo/msgs/log_playback_control.proto b/gazebo/msgs/log_playback_control.proto
new file mode 100644
index 0000000..7da47d8
--- /dev/null
+++ b/gazebo/msgs/log_playback_control.proto
@@ -0,0 +1,29 @@
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface LogPlaybackControl
+/// \brief A message that allows for control of log playback functions.
+
+import "time.proto";
+
+message LogPlaybackControl
+{
+ /// \brief Pause/play the log file.
+ optional bool pause = 1;
+
+ /// \brief Make a relative jump. The value indicates the number of
+ /// iterations that will be executed at once. If a negative
+ /// value is specified, the playback will jump backwards.
+ optional sint32 multi_step = 2;
+
+ /// \brief Jump to the beginning of the log file.
+ optional bool rewind = 3;
+
+ /// \brief Jump to the end of the log file.
+ optional bool forward = 4;
+
+ /// \brief Jump to a specific simulation time in the log file. The
+ /// playback service will load the frame with the closest
+ /// simulation time bigger than the "seek" value.
+ optional Time seek = 5;
+}
diff --git a/gazebo/msgs/log_playback_stats.proto b/gazebo/msgs/log_playback_stats.proto
new file mode 100644
index 0000000..2314ba5
--- /dev/null
+++ b/gazebo/msgs/log_playback_stats.proto
@@ -0,0 +1,17 @@
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface LogPlaybackStatistics
+/// \brief A message with statistics about a log during playback.
+
+
+import "time.proto";
+
+message LogPlaybackStatistics
+{
+ /// \brief Log start time
+ required Time start_time = 1;
+
+ /// \brief Log end time
+ required Time end_time = 2;
+}
diff --git a/gazebo/msgs/magnetometer.proto b/gazebo/msgs/magnetometer.proto
new file mode 100644
index 0000000..0fca409
--- /dev/null
+++ b/gazebo/msgs/magnetometer.proto
@@ -0,0 +1,18 @@
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface Magnetometer
+/// \brief Data from a magnetic field strength sensor
+
+import "time.proto";
+import "vector3d.proto";
+
+/// \brief Message that encapsulates sensor data from a magnetometer.
+message Magnetometer
+{
+ /// \brief Global time at which the magnetic field strength was sampled
+ required Time time = 1;
+
+ /// \brief Magnetic field strength (in Tesla) along body-frame axis
+ required Vector3d field_tesla = 2;
+}
diff --git a/gazebo/msgs/model.proto b/gazebo/msgs/model.proto
index 023eb38..f5e5d9a 100644
--- a/gazebo/msgs/model.proto
+++ b/gazebo/msgs/model.proto
@@ -22,4 +22,5 @@ message Model
optional bool deleted = 7;
repeated Visual visual = 8;
optional Vector3d scale = 9;
+ optional bool self_collide = 10;
}
diff --git a/gazebo/msgs/msgs.cc b/gazebo/msgs/msgs.cc
index b899614..8cb3350 100644
--- a/gazebo/msgs/msgs.cc
+++ b/gazebo/msgs/msgs.cc
@@ -17,12 +17,7 @@
#include <google/protobuf/descriptor.h>
#include <algorithm>
-
-#include "gazebo/math/Vector3.hh"
-#include "gazebo/math/Pose.hh"
-#include "gazebo/math/Quaternion.hh"
-#include "gazebo/math/Plane.hh"
-#include "gazebo/math/Rand.hh"
+#include <ignition/math/Rand.hh>
#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/Image.hh"
@@ -34,6 +29,14 @@ namespace gazebo
{
namespace msgs
{
+ /// \internal
+ /// \brief Internal function to create an SDF element from msgs::Axis.
+ /// It is only intended to be used by JointToSDF.
+ /// \param[in] _msg The msgs::Axis object.
+ /// \param[in] _sdf sdf::ElementPtr to fill with data.
+ void AxisToSDF(const msgs::Axis &_msg, sdf::ElementPtr _sdf);
+
+ /////////////////////////////////////////////
/// Create a request message
msgs::Request *CreateRequest(const std::string &_request,
const std::string &_data)
@@ -42,17 +45,19 @@ namespace gazebo
request->set_request(_request);
request->set_data(_data);
- request->set_id(math::Rand::GetIntUniform(1, 10000));
+ request->set_id(ignition::math::Rand::IntUniform(1, 10000));
return request;
}
+ /////////////////////////////////////////////
const google::protobuf::FieldDescriptor *GetFD(
google::protobuf::Message &message, const std::string &name)
{
return message.GetDescriptor()->FindFieldByName(name);
}
+ /////////////////////////////////////////////
msgs::Header *GetHeader(google::protobuf::Message &message)
{
google::protobuf::Message *msg = NULL;
@@ -71,6 +76,7 @@ namespace gazebo
return (msgs::Header*)msg;
}
+ /////////////////////////////////////////////
void Init(google::protobuf::Message &_message, const std::string &_id)
{
msgs::Header *header = GetHeader(_message);
@@ -83,11 +89,13 @@ namespace gazebo
}
}
+ /////////////////////////////////////////////
void Stamp(msgs::Header *_hdr)
{
Stamp(_hdr->mutable_stamp());
}
+ /////////////////////////////////////////////
void Stamp(msgs::Time *_time)
{
common::Time tm = common::Time::GetWallTime();
@@ -96,6 +104,7 @@ namespace gazebo
_time->set_nsec(tm.nsec);
}
+ /////////////////////////////////////////////
std::string Package(const std::string &type,
const google::protobuf::Message &message)
{
@@ -119,6 +128,7 @@ namespace gazebo
return data;
}
+ /////////////////////////////////////////////
void Set(msgs::Vector3d *_pt, const math::Vector3 &_v)
{
_pt->set_x(_v.x);
@@ -126,12 +136,29 @@ namespace gazebo
_pt->set_z(_v.z);
}
+ /////////////////////////////////////////////
+ void Set(msgs::Vector3d *_pt, const ignition::math::Vector3d &_v)
+ {
+ _pt->set_x(_v.X());
+ _pt->set_y(_v.Y());
+ _pt->set_z(_v.Z());
+ }
+
+ /////////////////////////////////////////////
void Set(msgs::Vector2d *_pt, const math::Vector2d &_v)
{
_pt->set_x(_v.x);
_pt->set_y(_v.y);
}
+ /////////////////////////////////////////////
+ void Set(msgs::Vector2d *_pt, const ignition::math::Vector2d &_v)
+ {
+ _pt->set_x(_v.X());
+ _pt->set_y(_v.Y());
+ }
+
+ /////////////////////////////////////////////
void Set(msgs::Quaternion *_q, const math::Quaternion &_v)
{
_q->set_x(_v.x);
@@ -140,12 +167,30 @@ namespace gazebo
_q->set_w(_v.w);
}
+ /////////////////////////////////////////////
+ void Set(msgs::Quaternion *_q, const ignition::math::Quaterniond &_v)
+ {
+ _q->set_x(_v.X());
+ _q->set_y(_v.Y());
+ _q->set_z(_v.Z());
+ _q->set_w(_v.W());
+ }
+
+ /////////////////////////////////////////////
void Set(msgs::Pose *_p, const math::Pose &_v)
{
- Set(_p->mutable_position(), _v.pos);
- Set(_p->mutable_orientation(), _v.rot);
+ Set(_p->mutable_position(), _v.pos.Ign());
+ Set(_p->mutable_orientation(), _v.rot.Ign());
}
+ /////////////////////////////////////////////
+ void Set(msgs::Pose *_p, const ignition::math::Pose3d &_v)
+ {
+ Set(_p->mutable_position(), _v.Pos());
+ Set(_p->mutable_orientation(), _v.Rot());
+ }
+
+ /////////////////////////////////////////////
void Set(msgs::Color *_c, const common::Color &_v)
{
_c->set_r(_v.r);
@@ -154,6 +199,7 @@ namespace gazebo
_c->set_a(_v.a);
}
+ /////////////////////////////////////////////
void Set(msgs::Time *_t, const common::Time &_v)
{
_t->set_sec(_v.sec);
@@ -176,22 +222,31 @@ namespace gazebo
break;
};
- _s->set_latitude_deg(_v.GetLatitudeReference().Degree());
- _s->set_longitude_deg(_v.GetLongitudeReference().Degree());
- _s->set_heading_deg(_v.GetHeadingOffset().Degree());
+ _s->set_latitude_deg(_v.LatitudeReference().Degree());
+ _s->set_longitude_deg(_v.LongitudeReference().Degree());
+ _s->set_heading_deg(_v.HeadingOffset().Degree());
_s->set_elevation(_v.GetElevationReference());
}
/////////////////////////////////////////////////
void Set(msgs::PlaneGeom *_p, const math::Plane &_v)
{
- Set(_p->mutable_normal(), _v.normal);
+ Set(_p->mutable_normal(), _v.normal.Ign());
_p->mutable_size()->set_x(_v.size.x);
_p->mutable_size()->set_y(_v.size.y);
_p->set_d(_v.d);
}
/////////////////////////////////////////////////
+ void Set(msgs::PlaneGeom *_p, const ignition::math::Planed &_v)
+ {
+ Set(_p->mutable_normal(), _v.Normal());
+ _p->mutable_size()->set_x(_v.Size().X());
+ _p->mutable_size()->set_y(_v.Size().Y());
+ _p->set_d(_v.Offset());
+ }
+
+ /////////////////////////////////////////////////
void Set(common::Image &_img, const msgs::Image &_msg)
{
_img.SetFromData(
@@ -222,40 +277,67 @@ namespace gazebo
/////////////////////////////////////////////////
msgs::Vector3d Convert(const math::Vector3 &_v)
{
+ return Convert(_v.Ign());
+ }
+
+ /////////////////////////////////////////////////
+ msgs::Vector3d Convert(const ignition::math::Vector3d &_v)
+ {
msgs::Vector3d result;
- result.set_x(_v.x);
- result.set_y(_v.y);
- result.set_z(_v.z);
+ result.set_x(_v.X());
+ result.set_y(_v.Y());
+ result.set_z(_v.Z());
return result;
}
/////////////////////////////////////////////////
msgs::Vector2d Convert(const math::Vector2d &_v)
{
+ return Convert(_v.Ign());
+ }
+
+ /////////////////////////////////////////////////
+ msgs::Vector2d Convert(const ignition::math::Vector2d &_v)
+ {
msgs::Vector2d result;
- result.set_x(_v.x);
- result.set_y(_v.y);
+ result.set_x(_v.X());
+ result.set_y(_v.Y());
return result;
}
+ /////////////////////////////////////////////
msgs::Quaternion Convert(const math::Quaternion &_q)
{
+ return Convert(_q.Ign());
+ }
+
+ /////////////////////////////////////////////
+ msgs::Quaternion Convert(const ignition::math::Quaterniond &_q)
+ {
msgs::Quaternion result;
- result.set_x(_q.x);
- result.set_y(_q.y);
- result.set_z(_q.z);
- result.set_w(_q.w);
+ result.set_x(_q.X());
+ result.set_y(_q.Y());
+ result.set_z(_q.Z());
+ result.set_w(_q.W());
return result;
}
+ /////////////////////////////////////////////
msgs::Pose Convert(const math::Pose &_p)
{
+ return Convert(_p.Ign());
+ }
+
+ /////////////////////////////////////////////
+ msgs::Pose Convert(const ignition::math::Pose3d &_p)
+ {
msgs::Pose result;
- result.mutable_position()->CopyFrom(Convert(_p.pos));
- result.mutable_orientation()->CopyFrom(Convert(_p.rot));
+ result.mutable_position()->CopyFrom(Convert(_p.Pos()));
+ result.mutable_orientation()->CopyFrom(Convert(_p.Rot()));
return result;
}
+ /////////////////////////////////////////////
msgs::Color Convert(const common::Color &_c)
{
msgs::Color result;
@@ -266,6 +348,7 @@ namespace gazebo
return result;
}
+ /////////////////////////////////////////////
msgs::Time Convert(const common::Time &_t)
{
msgs::Time result;
@@ -277,13 +360,25 @@ namespace gazebo
msgs::PlaneGeom Convert(const math::Plane &_p)
{
msgs::PlaneGeom result;
- result.mutable_normal()->CopyFrom(Convert(_p.normal));
+ result.mutable_normal()->CopyFrom(Convert(_p.normal.Ign()));
result.mutable_size()->set_x(_p.size.x);
result.mutable_size()->set_y(_p.size.y);
result.set_d(_p.d);
return result;
}
+ /////////////////////////////////////////////
+ msgs::PlaneGeom Convert(const ignition::math::Planed &_p)
+ {
+ msgs::PlaneGeom result;
+ result.mutable_normal()->CopyFrom(Convert(_p.Normal()));
+ result.mutable_size()->set_x(_p.Size().X());
+ result.mutable_size()->set_y(_p.Size().Y());
+ result.set_d(_p.Offset());
+ return result;
+ }
+
+ /////////////////////////////////////////////
msgs::Joint::Type ConvertJointType(const std::string &_str)
{
msgs::Joint::Type result = msgs::Joint::REVOLUTE;
@@ -315,10 +410,22 @@ namespace gazebo
{
result = msgs::Joint::GEARBOX;
}
+ else if (_str == "fixed")
+ {
+ result = msgs::Joint::FIXED;
+ }
+ else
+ {
+ gzerr << "Unrecognized JointType ["
+ << _str
+ << "], returning REVOLUTE"
+ << std::endl;
+ }
return result;
}
- std::string ConvertJointType(const msgs::Joint::Type _type)
+ /////////////////////////////////////////////
+ std::string ConvertJointType(const msgs::Joint::Type &_type)
{
std::string result;
switch (_type)
@@ -358,50 +465,197 @@ namespace gazebo
result = "gearbox";
break;
}
+ case msgs::Joint::FIXED:
+ {
+ result = "fixed";
+ break;
+ }
default:
{
result = "unknown";
+ gzerr << "Unrecognized JointType [" << _type << "]"
+ << std::endl;
break;
}
}
return result;
}
+ /////////////////////////////////////////////////
+ msgs::Geometry::Type ConvertGeometryType(const std::string &_str)
+ {
+ msgs::Geometry::Type result = msgs::Geometry::BOX;
+ if (_str == "box")
+ {
+ result = msgs::Geometry::BOX;
+ }
+ else if (_str == "cylinder")
+ {
+ result = msgs::Geometry::CYLINDER;
+ }
+ else if (_str == "sphere")
+ {
+ result = msgs::Geometry::SPHERE;
+ }
+ else if (_str == "plane")
+ {
+ result = msgs::Geometry::PLANE;
+ }
+ else if (_str == "image")
+ {
+ result = msgs::Geometry::IMAGE;
+ }
+ else if (_str == "heightmap")
+ {
+ result = msgs::Geometry::HEIGHTMAP;
+ }
+ else if (_str == "mesh")
+ {
+ result = msgs::Geometry::MESH;
+ }
+ else if (_str == "polyline")
+ {
+ result = msgs::Geometry::POLYLINE;
+ }
+ else
+ {
+ gzwarn << "Geometry: '" << _str << "' is not recognized, "
+ << " returning type as msgs::Geometry::BOX." << std::endl;
+ }
+
+ return result;
+ }
+
+ /////////////////////////////////////////////////
+ std::string ConvertGeometryType(const msgs::Geometry::Type _type)
+ {
+ std::string result;
+ switch (_type)
+ {
+ case msgs::Geometry::BOX:
+ {
+ result = "box";
+ break;
+ }
+ case msgs::Geometry::CYLINDER:
+ {
+ result = "cylinder";
+ break;
+ }
+ case msgs::Geometry::SPHERE:
+ {
+ result = "sphere";
+ break;
+ }
+ case msgs::Geometry::PLANE:
+ {
+ result = "plane";
+ break;
+ }
+ case msgs::Geometry::IMAGE:
+ {
+ result = "image";
+ break;
+ }
+ case msgs::Geometry::HEIGHTMAP:
+ {
+ result = "heightmap";
+ break;
+ }
+ case msgs::Geometry::MESH:
+ {
+ result = "mesh";
+ break;
+ }
+ case msgs::Geometry::POLYLINE:
+ {
+ result = "polyline";
+ break;
+ }
+ default:
+ {
+ result = "unknown";
+ break;
+ }
+ }
+ return result;
+ }
+
+ /////////////////////////////////////////////
math::Vector3 Convert(const msgs::Vector3d &_v)
{
return math::Vector3(_v.x(), _v.y(), _v.z());
}
+ /////////////////////////////////////////////
+ ignition::math::Vector3d ConvertIgn(const msgs::Vector3d &_v)
+ {
+ return ignition::math::Vector3d(_v.x(), _v.y(), _v.z());
+ }
+
+ /////////////////////////////////////////////
math::Vector2d Convert(const msgs::Vector2d &_v)
{
return math::Vector2d(_v.x(), _v.y());
}
+ /////////////////////////////////////////////
+ ignition::math::Vector2d ConvertIgn(const msgs::Vector2d &_v)
+ {
+ return ignition::math::Vector2d(_v.x(), _v.y());
+ }
+
+ /////////////////////////////////////////////
math::Quaternion Convert(const msgs::Quaternion &_q)
{
return math::Quaternion(_q.w(), _q.x(), _q.y(), _q.z());
}
+ /////////////////////////////////////////////
+ ignition::math::Quaterniond ConvertIgn(const msgs::Quaternion &_q)
+ {
+ return ignition::math::Quaterniond(_q.w(), _q.x(), _q.y(), _q.z());
+ }
+
+ /////////////////////////////////////////////
math::Pose Convert(const msgs::Pose &_p)
{
- return math::Pose(Convert(_p.position()),
- Convert(_p.orientation()));
+ return math::Pose(
+ ConvertIgn(_p.position()), ConvertIgn(_p.orientation()));
}
+ /////////////////////////////////////////////
+ ignition::math::Pose3d ConvertIgn(const msgs::Pose &_p)
+ {
+ return ignition::math::Pose3d(ConvertIgn(_p.position()),
+ ConvertIgn(_p.orientation()));
+ }
+
+ /////////////////////////////////////////////
common::Color Convert(const msgs::Color &_c)
{
return common::Color(_c.r(), _c.g(), _c.b(), _c.a());
}
+ /////////////////////////////////////////////
common::Time Convert(const msgs::Time &_t)
{
return common::Time(_t.sec(), _t.nsec());
}
+ /////////////////////////////////////////////
math::Plane Convert(const msgs::PlaneGeom &_p)
{
- return math::Plane(Convert(_p.normal()),
- math::Vector2d(_p.size().x(), _p.size().y()),
+ return math::Plane(ConvertIgn(_p.normal()),
+ ignition::math::Vector2d(_p.size().x(), _p.size().y()),
+ _p.d());
+ }
+
+ /////////////////////////////////////////////
+ ignition::math::Planed ConvertIgn(const msgs::PlaneGeom &_p)
+ {
+ return ignition::math::Planed(ConvertIgn(_p.normal()),
+ ignition::math::Vector2d(_p.size().x(), _p.size().y()),
_p.d());
}
@@ -442,7 +696,8 @@ namespace gazebo
if (camSDF->HasElement("pose"))
{
- msgs::Set(guiCam->mutable_pose(), camSDF->Get<math::Pose>("pose"));
+ msgs::Set(guiCam->mutable_pose(),
+ camSDF->Get<ignition::math::Pose3d>("pose"));
}
if (camSDF->HasElement("view_controller"))
@@ -451,6 +706,12 @@ namespace gazebo
camSDF->Get<std::string>("view_controller"));
}
+ if (camSDF->HasElement("projection_type"))
+ {
+ guiCam->set_projection_type(
+ camSDF->Get<std::string>("projection_type"));
+ }
+
if (camSDF->HasElement("track_visual"))
{
guiCam->mutable_track()->CopyFrom(
@@ -499,7 +760,8 @@ namespace gazebo
if (_sdf->HasElement("pose"))
{
- result.mutable_pose()->CopyFrom(Convert(_sdf->Get<math::Pose>("pose")));
+ result.mutable_pose()->CopyFrom(
+ Convert(_sdf->Get<ignition::math::Pose3d>("pose")));
}
if (_sdf->HasElement("diffuse"))
@@ -526,7 +788,7 @@ namespace gazebo
if (_sdf->HasElement("direction"))
{
result.mutable_direction()->CopyFrom(
- Convert(_sdf->Get<math::Vector3>("direction")));
+ Convert(_sdf->Get<ignition::math::Vector3d>("direction")));
}
if (_sdf->HasElement("spot"))
@@ -552,7 +814,8 @@ namespace gazebo
return result;
}
- msgs::Set(result.mutable_scale(), _sdf->Get<math::Vector3>("scale"));
+ msgs::Set(result.mutable_scale(),
+ _sdf->Get<ignition::math::Vector3d>("scale"));
result.set_filename(_sdf->Get<std::string>("uri"));
@@ -572,7 +835,6 @@ namespace gazebo
return result;
}
-
/////////////////////////////////////////////////
msgs::Geometry GeometryFromSDF(sdf::ElementPtr _sdf)
{
@@ -594,7 +856,7 @@ namespace gazebo
{
result.set_type(msgs::Geometry::BOX);
msgs::Set(result.mutable_box()->mutable_size(),
- geomElem->Get<math::Vector3>("size"));
+ geomElem->Get<ignition::math::Vector3d>("size"));
}
else if (geomElem->GetName() == "cylinder")
{
@@ -614,21 +876,28 @@ namespace gazebo
{
result.set_type(msgs::Geometry::PLANE);
msgs::Set(result.mutable_plane()->mutable_normal(),
- geomElem->Get<math::Vector3>("normal"));
+ geomElem->Get<ignition::math::Vector3d>("normal"));
msgs::Set(result.mutable_plane()->mutable_size(),
- geomElem->Get<math::Vector2d>("size"));
+ geomElem->Get<ignition::math::Vector2d>("size"));
}
else if (geomElem->GetName() == "polyline")
{
+ sdf::ElementPtr polylineElem = geomElem;
result.set_type(msgs::Geometry::POLYLINE);
- result.mutable_polyline()->set_height(geomElem->Get<double>("height"));
- sdf::ElementPtr pointElem = geomElem->GetElement("point");
- while (pointElem)
+ while (polylineElem)
{
- math::Vector2d point = pointElem->Get<math::Vector2d>();
- pointElem = pointElem->GetNextElement("point");
- msgs::Vector2d *ptMsg = result.mutable_polyline()->add_point();
- msgs::Set(ptMsg, point);
+ msgs::Polyline *polylineMsg = result.add_polyline();
+ polylineMsg->set_height(polylineElem->Get<double>("height"));
+ sdf::ElementPtr pointElem = polylineElem->GetElement("point");
+ while (pointElem)
+ {
+ ignition::math::Vector2d point =
+ pointElem->Get<ignition::math::Vector2d>();
+ pointElem = pointElem->GetNextElement("point");
+ msgs::Vector2d *ptMsg = polylineMsg->add_point();
+ msgs::Set(ptMsg, point);
+ }
+ polylineElem = polylineElem->GetNextElement("polyline");
}
}
else if (geomElem->GetName() == "image")
@@ -645,9 +914,9 @@ namespace gazebo
{
result.set_type(msgs::Geometry::HEIGHTMAP);
msgs::Set(result.mutable_heightmap()->mutable_size(),
- geomElem->Get<math::Vector3>("size"));
+ geomElem->Get<ignition::math::Vector3d>("size"));
msgs::Set(result.mutable_heightmap()->mutable_origin(),
- geomElem->Get<math::Vector3>("pos"));
+ geomElem->Get<ignition::math::Vector3d>("pos"));
sdf::ElementPtr textureElem = geomElem->GetElement("texture");
while (textureElem)
@@ -707,10 +976,19 @@ namespace gazebo
if (_sdf->HasElement("laser_retro"))
result.set_laser_retro(_sdf->Get<double>("laser_retro"));
+ // Set the meta information
+ if (_sdf->HasElement("meta"))
+ {
+ auto metaElem = _sdf->GetElement("meta");
+ auto meta = result.mutable_meta();
+ if (metaElem->HasElement("layer"))
+ meta->set_layer(metaElem->Get<int32_t>("layer"));
+ }
+
// Load the geometry
if (_sdf->HasElement("geometry"))
{
- msgs::Geometry *geomMsg = result.mutable_geometry();
+ auto geomMsg = result.mutable_geometry();
geomMsg->CopyFrom(GeometryFromSDF(_sdf->GetElement("geometry")));
}
@@ -718,7 +996,7 @@ namespace gazebo
if (_sdf->HasElement("material"))
{
sdf::ElementPtr elem = _sdf->GetElement("material");
- msgs::Material *matMsg = result.mutable_material();
+ auto matMsg = result.mutable_material();
if (elem->HasElement("script"))
{
@@ -779,7 +1057,8 @@ namespace gazebo
// Set the origin of the visual
if (_sdf->HasElement("pose"))
{
- msgs::Set(result.mutable_pose(), _sdf->Get<math::Pose>("pose"));
+ msgs::Set(result.mutable_pose(),
+ _sdf->Get<ignition::math::Pose3d>("pose"));
}
// Set plugins of the visual
@@ -806,6 +1085,312 @@ namespace gazebo
}
/////////////////////////////////////////////////
+ msgs::Axis AxisFromSDF(sdf::ElementPtr _sdf)
+ {
+ msgs::Axis result;
+
+ sdf::ElementPtr limitElem = _sdf->GetElement("limit");
+ result.set_limit_lower(limitElem->Get<double>("lower"));
+ result.set_limit_upper(limitElem->Get<double>("upper"));
+ result.set_limit_effort(limitElem->Get<double>("effort"));
+ result.set_limit_velocity(limitElem->Get<double>("velocity"));
+
+ result.set_use_parent_model_frame(
+ _sdf->Get<bool>("use_parent_model_frame"));
+
+ sdf::ElementPtr dynamicsElem = _sdf->GetElement("dynamics");
+ result.set_damping(dynamicsElem->Get<double>("damping"));
+ result.set_friction(dynamicsElem->Get<double>("friction"));
+
+ msgs::Set(result.mutable_xyz(),
+ _sdf->Get<ignition::math::Vector3d>("xyz"));
+
+ return result;
+ }
+
+ /////////////////////////////////////////////////
+ msgs::Joint JointFromSDF(sdf::ElementPtr _sdf)
+ {
+ msgs::Joint result;
+
+ // Name
+ result.set_name(_sdf->Get<std::string>("name"));
+
+ // parent
+ if (_sdf->HasElement("parent"))
+ result.set_parent(_sdf->Get<std::string>("parent"));
+
+ // child
+ if (_sdf->HasElement("child"))
+ result.set_child(_sdf->Get<std::string>("child"));
+
+ // Pose
+ ignition::math::Pose3d jointPose;
+ if (_sdf->HasElement("pose"))
+ {
+ msgs::Set(result.mutable_pose(),
+ _sdf->Get<ignition::math::Pose3d>("pose"));
+ }
+
+ // Type
+ std::string type = _sdf->Get<std::string>("type");
+ result.set_type(msgs::ConvertJointType(type));
+
+ // axis1
+ if (_sdf->HasElement("axis"))
+ {
+ msgs::Axis *axis = result.mutable_axis1();
+ axis->CopyFrom(AxisFromSDF(_sdf->GetElement("axis")));
+ result.add_angle(0);
+ }
+
+ // axis2
+ if (_sdf->HasElement("axis2"))
+ {
+ msgs::Axis *axis = result.mutable_axis2();
+ axis->CopyFrom(AxisFromSDF(_sdf->GetElement("axis2")));
+ result.add_angle(0);
+ }
+
+ // physics
+ if (_sdf->HasElement("physics"))
+ {
+ sdf::ElementPtr physicsElem = _sdf->GetElement("physics");
+ if (physicsElem->HasElement("ode"))
+ {
+ sdf::ElementPtr odeElem = physicsElem->GetElement("ode");
+ if (odeElem->HasElement("cfm"))
+ result.set_cfm(odeElem->Get<double>("cfm"));
+ if (odeElem->HasElement("bounce"))
+ result.set_bounce(odeElem->Get<double>("bounce"));
+ if (odeElem->HasElement("velocity"))
+ result.set_velocity(odeElem->Get<double>("velocity"));
+ if (odeElem->HasElement("fudge_factor"))
+ result.set_fudge_factor(odeElem->Get<double>("fudge_factor"));
+
+ if (odeElem->HasElement("limit"))
+ {
+ sdf::ElementPtr odeLimitElem = odeElem->GetElement("limit");
+ if (odeLimitElem->HasElement("cfm"))
+ result.set_limit_cfm(odeLimitElem->Get<double>("cfm"));
+ if (odeLimitElem->HasElement("erp"))
+ result.set_limit_erp(odeLimitElem->Get<double>("erp"));
+ }
+ if (odeElem->HasElement("suspension"))
+ {
+ sdf::ElementPtr odeSuspensionElem =
+ odeElem->GetElement("suspension");
+ if (odeSuspensionElem->HasElement("cfm"))
+ {
+ result.set_suspension_cfm(
+ odeSuspensionElem->Get<double>("cfm"));
+ }
+ if (odeSuspensionElem->HasElement("erp"))
+ {
+ result.set_suspension_erp(
+ odeSuspensionElem->Get<double>("erp"));
+ }
+ }
+ }
+ }
+ return result;
+ }
+
+ /////////////////////////////////////////////////
+ sdf::ElementPtr VisualToSDF(const msgs::Visual &_msg,
+ sdf::ElementPtr _sdf)
+ {
+ sdf::ElementPtr visualSDF;
+
+ if (_sdf)
+ {
+ visualSDF = _sdf;
+ }
+ else
+ {
+ visualSDF.reset(new sdf::Element);
+ sdf::initFile("visual.sdf", visualSDF);
+ }
+
+ // Set the meta information
+ if (_msg.has_meta())
+ {
+ if (_msg.meta().has_layer())
+ {
+ visualSDF->GetElement("meta")->GetElement("layer")->Set(
+ _msg.meta().layer());
+ }
+ }
+
+ if (_msg.has_name())
+ visualSDF->GetAttribute("name")->Set(_msg.name());
+
+ if (_msg.has_cast_shadows())
+ visualSDF->GetElement("cast_shadows")->Set(_msg.cast_shadows());
+
+ if (_msg.has_transparency())
+ visualSDF->GetElement("transparency")->Set(_msg.transparency());
+
+ if (_msg.has_laser_retro())
+ visualSDF->GetElement("laser_retro")->Set(_msg.laser_retro());
+
+ if (_msg.has_pose())
+ visualSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
+
+ // Load the geometry
+ if (_msg.has_geometry())
+ {
+ sdf::ElementPtr geomElem = visualSDF->GetElement("geometry");
+ geomElem = GeometryToSDF(_msg.geometry(), geomElem);
+ }
+
+ /// Load the material
+ if (_msg.has_material())
+ {
+ sdf::ElementPtr materialElem = visualSDF->GetElement("material");
+ materialElem = MaterialToSDF(_msg.material(), materialElem);
+ }
+
+ // Set plugins of the visual
+ if (_msg.has_plugin())
+ {
+ sdf::ElementPtr pluginElem = visualSDF->GetElement("plugin");
+ pluginElem = PluginToSDF(_msg.plugin(), pluginElem);
+ }
+
+ return visualSDF;
+ }
+
+ /////////////////////////////////////////////////
+ sdf::ElementPtr MaterialToSDF(const msgs::Material &_msg,
+ sdf::ElementPtr _sdf)
+ {
+ sdf::ElementPtr materialSDF;
+
+ if (_sdf)
+ {
+ materialSDF = _sdf;
+ }
+ else
+ {
+ materialSDF.reset(new sdf::Element);
+ sdf::initFile("material.sdf", materialSDF);
+ }
+
+ if (_msg.has_script())
+ {
+ sdf::ElementPtr scriptElem = materialSDF->GetElement("script");
+ msgs::Material::Script script = _msg.script();
+
+ if (script.has_name())
+ scriptElem->GetElement("name")->Set(script.name());
+
+ if (script.uri_size() > 0)
+ while (scriptElem->HasElement("uri"))
+ scriptElem->GetElement("uri")->RemoveFromParent();
+ for (int i = 0; i < script.uri_size(); ++i)
+ {
+ sdf::ElementPtr uriElem = scriptElem->AddElement("uri");
+ uriElem->Set(script.uri(i));
+ }
+ }
+
+ if (_msg.has_shader_type())
+ {
+ sdf::ElementPtr shaderElem = materialSDF->GetElement("shader");
+ shaderElem->GetAttribute("type")->Set(
+ ConvertShaderType(_msg.shader_type()));
+ }
+
+ if (_msg.has_normal_map())
+ {
+ sdf::ElementPtr shaderElem = materialSDF->GetElement("shader");
+ shaderElem->GetElement("normal_map")->Set(_msg.normal_map());
+ }
+
+ if (_msg.has_lighting())
+ materialSDF->GetElement("lighting")->Set(_msg.lighting());
+
+ if (_msg.has_ambient())
+ materialSDF->GetElement("ambient")->Set(Convert(_msg.ambient()));
+ if (_msg.has_diffuse())
+ materialSDF->GetElement("diffuse")->Set(Convert(_msg.diffuse()));
+ if (_msg.has_emissive())
+ materialSDF->GetElement("emissive")->Set(Convert(_msg.emissive()));
+ if (_msg.has_specular())
+ materialSDF->GetElement("specular")->Set(Convert(_msg.specular()));
+
+ return materialSDF;
+ }
+
+ /////////////////////////////////////////////////
+ msgs::Material::ShaderType ConvertShaderType(const std::string &_str)
+ {
+ auto result = msgs::Material::VERTEX;
+ if (_str == "vertex")
+ {
+ result = msgs::Material::VERTEX;
+ }
+ else if (_str == "pixel")
+ {
+ result = msgs::Material::PIXEL;
+ }
+ else if (_str == "normal_map_object_space")
+ {
+ result = msgs::Material::NORMAL_MAP_OBJECT_SPACE;
+ }
+ else if (_str == "normal_map_tangent_space")
+ {
+ result = msgs::Material::NORMAL_MAP_TANGENT_SPACE;
+ }
+ else
+ {
+ gzerr << "Unrecognized ShaderType ["
+ << _str
+ << "], returning VERTEX"
+ << std::endl;
+ }
+ return result;
+ }
+
+ /////////////////////////////////////////////////
+ std::string ConvertShaderType(const msgs::Material::ShaderType &_type)
+ {
+ std::string result;
+ switch (_type)
+ {
+ case msgs::Material::VERTEX:
+ {
+ result = "vertex";
+ break;
+ }
+ case msgs::Material::PIXEL:
+ {
+ result = "pixel";
+ break;
+ }
+ case msgs::Material::NORMAL_MAP_OBJECT_SPACE:
+ {
+ result = "normal_map_object_space";
+ break;
+ }
+ case msgs::Material::NORMAL_MAP_TANGENT_SPACE:
+ {
+ result = "normal_map_tangent_space";
+ break;
+ }
+ default:
+ {
+ result = "unknown";
+ gzerr << "Unrecognized ShaderType [" << _type << "]"
+ << std::endl;
+ break;
+ }
+ }
+ return result;
+ }
+
+ /////////////////////////////////////////////////
msgs::Fog FogFromSDF(sdf::ElementPtr _sdf)
{
msgs::Fog result;
@@ -831,6 +1416,7 @@ namespace gazebo
return result;
}
+ /////////////////////////////////////////////////
msgs::Scene SceneFromSDF(sdf::ElementPtr _sdf)
{
msgs::Scene result;
@@ -842,6 +1428,11 @@ namespace gazebo
else
result.set_grid(true);
+ if (_sdf->HasElement("origin_visual"))
+ result.set_origin_visual(_sdf->Get<bool>("origin_visual"));
+ else
+ result.set_origin_visual(true);
+
if (_sdf->HasElement("ambient"))
result.mutable_ambient()->CopyFrom(
Convert(_sdf->Get<common::Color>("ambient")));
@@ -907,7 +1498,7 @@ namespace gazebo
if (_msg.has_pose())
{
- lightSDF->GetElement("pose")->Set(msgs::Convert(_msg.pose()));
+ lightSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
}
if (_msg.has_diffuse())
@@ -922,7 +1513,7 @@ namespace gazebo
if (_msg.has_direction())
{
- lightSDF->GetElement("direction")->Set(msgs::Convert(_msg.direction()));
+ lightSDF->GetElement("direction")->Set(ConvertIgn(_msg.direction()));
}
if (_msg.has_attenuation_constant())
@@ -990,8 +1581,7 @@ namespace gazebo
if (_msg.has_horizontal_fov())
{
- cameraSDF->GetElement("horizontal_fov")->Set(
- _msg.horizontal_fov());
+ cameraSDF->GetElement("horizontal_fov")->Set(_msg.horizontal_fov());
}
if (_msg.has_image_size())
{
@@ -1022,7 +1612,7 @@ namespace gazebo
if (distortionMsg.has_center())
{
distortionElem->GetElement("center")->Set(
- msgs::Convert(distortionMsg.center()));
+ ConvertIgn(distortionMsg.center()));
}
if (distortionMsg.has_k1())
{
@@ -1072,7 +1662,7 @@ namespace gazebo
if (_msg.has_max_contacts())
collisionSDF->GetElement("max_contacts")->Set(_msg.max_contacts());
if (_msg.has_pose())
- collisionSDF->GetElement("pose")->Set(msgs::Convert(_msg.pose()));
+ collisionSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
if (_msg.has_geometry())
{
sdf::ElementPtr geomElem = collisionSDF->GetElement("geometry");
@@ -1112,22 +1702,30 @@ namespace gazebo
if (_msg.has_kinematic())
linkSDF->GetElement("kinematic")->Set(_msg.kinematic());
if (_msg.has_pose())
- linkSDF->GetElement("pose")->Set(msgs::Convert(_msg.pose()));
+ linkSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
if (_msg.has_inertial())
{
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
inertialElem = InertialToSDF(_msg.inertial(), inertialElem);
}
- while (linkSDF->HasElement("collision"))
- linkSDF->GetElement("collision")->RemoveFromParent();
+ if (_msg.collision_size() > 0)
+ while (linkSDF->HasElement("collision"))
+ linkSDF->GetElement("collision")->RemoveFromParent();
for (int i = 0; i < _msg.collision_size(); ++i)
{
sdf::ElementPtr collisionElem = linkSDF->AddElement("collision");
collisionElem = CollisionToSDF(_msg.collision(i), collisionElem);
}
+ if (_msg.visual_size() > 0)
+ while (linkSDF->HasElement("visual"))
+ linkSDF->GetElement("visual")->RemoveFromParent();
+ for (int i = 0; i < _msg.visual_size(); ++i)
+ {
+ sdf::ElementPtr visualElem = linkSDF->AddElement("visual");
+ visualElem = VisualToSDF(_msg.visual(i), visualElem);
+ }
- gzwarn << "msgs::LinkToSDF currently does not convert visual,"
- << " sensor, and projector data";
+ /// \todo LinkToSDF currently does not convert sensor and projector data
return linkSDF;
}
@@ -1151,7 +1749,7 @@ namespace gazebo
if (_msg.has_mass())
inertialSDF->GetElement("mass")->Set(_msg.mass());
if (_msg.has_pose())
- inertialSDF->GetElement("pose")->Set(msgs::Convert(_msg.pose()));
+ inertialSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
sdf::ElementPtr inertiaSDF = inertialSDF->GetElement("inertia");
if (_msg.has_ixx())
@@ -1198,7 +1796,7 @@ namespace gazebo
if (friction.has_fdir1())
{
physicsEngElem->GetElement("fdir1")->Set(
- msgs::Convert(friction.fdir1()));
+ ConvertIgn(friction.fdir1()));
}
if (friction.has_slip1())
physicsEngElem->GetElement("slip1")->Set(friction.slip1());
@@ -1229,20 +1827,42 @@ namespace gazebo
contactElem->GetElement("collide_without_contact_bitmask")->Set(
_msg.collide_without_contact_bitmask());
}
+ if (_msg.has_collide_bitmask())
+ {
+ contactElem->GetElement("collide_bitmask")->Set(
+ _msg.collide_bitmask());
+ }
- sdf::ElementPtr physicsEngElem = contactElem->GetElement("ode");
+ sdf::ElementPtr odeElem = contactElem->GetElement("ode");
+ sdf::ElementPtr bulletElem = contactElem->GetElement("bullet");
if (_msg.has_soft_cfm())
- physicsEngElem->GetElement("soft_cfm")->Set(_msg.soft_cfm());
+ {
+ odeElem->GetElement("soft_cfm")->Set(_msg.soft_cfm());
+ bulletElem->GetElement("soft_cfm")->Set(_msg.soft_cfm());
+ }
if (_msg.has_soft_erp())
- physicsEngElem->GetElement("soft_erp")->Set(_msg.soft_erp());
+ {
+ odeElem->GetElement("soft_erp")->Set(_msg.soft_erp());
+ bulletElem->GetElement("soft_erp")->Set(_msg.soft_erp());
+ }
if (_msg.has_kp())
- physicsEngElem->GetElement("kp")->Set(_msg.kp());
+ {
+ odeElem->GetElement("kp")->Set(_msg.kp());
+ bulletElem->GetElement("kp")->Set(_msg.kp());
+ }
if (_msg.has_kd())
- physicsEngElem->GetElement("kd")->Set(_msg.kd());
+ {
+ odeElem->GetElement("kd")->Set(_msg.kd());
+ bulletElem->GetElement("kd")->Set(_msg.kd());
+ }
if (_msg.has_max_vel())
- physicsEngElem->GetElement("max_vel")->Set(_msg.max_vel());
+ {
+ odeElem->GetElement("max_vel")->Set(_msg.max_vel());
+ }
if (_msg.has_min_depth())
- physicsEngElem->GetElement("min_depth")->Set(_msg.min_depth());
+ {
+ odeElem->GetElement("min_depth")->Set(_msg.min_depth());
+ }
return surfaceSDF;
}
@@ -1266,14 +1886,16 @@ namespace gazebo
if (!_msg.has_type())
return geometrySDF;
- if (_msg.type() == msgs::Geometry::BOX)
+ if (_msg.type() == msgs::Geometry::BOX &&
+ _msg.has_box())
{
sdf::ElementPtr geom = geometrySDF->GetElement("box");
msgs::BoxGeom boxGeom = _msg.box();
if (boxGeom.has_size())
- geom->GetElement("size")->Set(msgs::Convert(boxGeom.size()));
+ geom->GetElement("size")->Set(ConvertIgn(boxGeom.size()));
}
- else if (_msg.type() == msgs::Geometry::CYLINDER)
+ else if (_msg.type() == msgs::Geometry::CYLINDER &&
+ _msg.has_cylinder())
{
sdf::ElementPtr geom = geometrySDF->GetElement("cylinder");
msgs::CylinderGeom cylinderGeom = _msg.cylinder();
@@ -1282,26 +1904,30 @@ namespace gazebo
if (cylinderGeom.has_length())
geom->GetElement("length")->Set(cylinderGeom.length());
}
- if (_msg.type() == msgs::Geometry::SPHERE)
+ else if (_msg.type() == msgs::Geometry::SPHERE &&
+ _msg.has_sphere())
{
sdf::ElementPtr geom = geometrySDF->GetElement("sphere");
msgs::SphereGeom sphereGeom = _msg.sphere();
if (sphereGeom.has_radius())
geom->GetElement("radius")->Set(sphereGeom.radius());
}
- if (_msg.type() == msgs::Geometry::PLANE)
+ else if (_msg.type() == msgs::Geometry::PLANE &&
+ _msg.has_plane())
{
sdf::ElementPtr geom = geometrySDF->GetElement("plane");
msgs::PlaneGeom planeGeom = _msg.plane();
if (planeGeom.has_normal())
{
- geom->GetElement("normal")->Set(
- msgs::Convert(planeGeom.normal()));
+ geom->GetElement("normal")->Set(ConvertIgn(planeGeom.normal()));
}
if (planeGeom.has_size())
- geom->GetElement("size")->Set(msgs::Convert(planeGeom.size()));
+ geom->GetElement("size")->Set(ConvertIgn(planeGeom.size()));
+ if (planeGeom.has_d())
+ gzerr << "sdformat doesn't have Plane.d variable" << std::endl;
}
- if (_msg.type() == msgs::Geometry::IMAGE)
+ else if (_msg.type() == msgs::Geometry::IMAGE &&
+ _msg.has_image())
{
sdf::ElementPtr geom = geometrySDF->GetElement("image");
msgs::ImageGeom imageGeom = _msg.image();
@@ -1316,27 +1942,27 @@ namespace gazebo
if (imageGeom.has_granularity())
geom->GetElement("granularity")->Set(imageGeom.granularity());
}
- if (_msg.type() == msgs::Geometry::HEIGHTMAP)
+ else if (_msg.type() == msgs::Geometry::HEIGHTMAP &&
+ _msg.has_heightmap())
{
sdf::ElementPtr geom = geometrySDF->GetElement("heightmap");
msgs::HeightmapGeom heightmapGeom = _msg.heightmap();
if (heightmapGeom.has_size())
{
- geom->GetElement("size")->Set(
- msgs::Convert(heightmapGeom.size()));
+ geom->GetElement("size")->Set(ConvertIgn(heightmapGeom.size()));
}
if (heightmapGeom.has_origin())
{
- geom->GetElement("pos")->Set(
- msgs::Convert(heightmapGeom.origin()));
+ geom->GetElement("pos")->Set(ConvertIgn(heightmapGeom.origin()));
}
if (heightmapGeom.has_use_terrain_paging())
{
geom->GetElement("use_terrain_paging")->Set(
heightmapGeom.use_terrain_paging());
}
- while (geom->HasElement("texture"))
- geom->GetElement("texture")->RemoveFromParent();
+ if (heightmapGeom.texture_size() > 0)
+ while (geom->HasElement("texture"))
+ geom->GetElement("texture")->RemoveFromParent();
for (int i = 0; i < heightmapGeom.texture_size(); ++i)
{
gazebo::msgs::HeightmapGeom_Texture textureMsg =
@@ -1346,8 +1972,9 @@ namespace gazebo
textureElem->GetElement("normal")->Set(textureMsg.normal());
textureElem->GetElement("size")->Set(textureMsg.size());
}
- while (geom->HasElement("blend"))
- geom->GetElement("blend")->RemoveFromParent();
+ if (heightmapGeom.blend_size() > 0)
+ while (geom->HasElement("blend"))
+ geom->GetElement("blend")->RemoveFromParent();
for (int i = 0; i < heightmapGeom.blend_size(); ++i)
{
gazebo::msgs::HeightmapGeom_Blend blendMsg =
@@ -1359,27 +1986,36 @@ namespace gazebo
if (heightmapGeom.has_filename())
geom->GetElement("uri")->Set(heightmapGeom.filename());
}
- if (_msg.type() == msgs::Geometry::MESH)
+ else if (_msg.type() == msgs::Geometry::MESH &&
+ _msg.has_mesh())
{
sdf::ElementPtr geom = geometrySDF->GetElement("mesh");
msgs::MeshGeom meshGeom = _msg.mesh();
geom = msgs::MeshToSDF(meshGeom, geom);
}
- if (_msg.type() == msgs::Geometry::POLYLINE)
+ else if (_msg.type() == msgs::Geometry::POLYLINE &&
+ _msg.polyline_size() > 0)
{
- sdf::ElementPtr geom = geometrySDF->GetElement("polyline");
- gazebo::msgs::Polyline polylineGeom = _msg.polyline();
- if (polylineGeom.has_height())
- geom->GetElement("height")->Set(polylineGeom.height());
- while (geom->HasElement("point"))
- geom->GetElement("point")->RemoveFromParent();
+ if (_msg.polyline_size() > 0)
+ while (geometrySDF->HasElement("polyline"))
+ geometrySDF->GetElement("polyline")->RemoveFromParent();
- for (int i = 0; i < polylineGeom.point_size(); ++i)
+ for (int j = 0; j < _msg.polyline_size(); ++j)
{
- sdf::ElementPtr pointElem = geom->AddElement("point");
- pointElem->Set(msgs::Convert(polylineGeom.point(i)));
+ sdf::ElementPtr polylineElem = geometrySDF->AddElement("polyline");
+ if (_msg.polyline(j).has_height())
+ polylineElem->GetElement("height")->Set(_msg.polyline(j).height());
+ for (int i = 0; i < _msg.polyline(j).point_size(); ++i)
+ {
+ sdf::ElementPtr pointElem = polylineElem->AddElement("point");
+ pointElem->Set(ConvertIgn(_msg.polyline(j).point(i)));
+ }
}
}
+ else
+ {
+ gzerr << "Unrecognized geometry type" << std::endl;
+ }
return geometrySDF;
}
@@ -1401,16 +2037,17 @@ namespace gazebo
if (_msg.has_filename())
meshSDF->GetElement("uri")->Set(_msg.filename());
- sdf::ElementPtr submeshElem = meshSDF->GetElement("submesh");
if (_msg.has_submesh())
- submeshElem->GetElement("name")->Set(_msg.submesh());
- if (_msg.has_center_submesh())
- submeshElem->GetElement("center")->Set(_msg.center_submesh());
- if (_msg.has_scale())
{
- meshSDF->GetElement("scale")->Set(msgs::Convert(_msg.scale()));
+ sdf::ElementPtr submeshElem = meshSDF->GetElement("submesh");
+ submeshElem->GetElement("name")->Set(_msg.submesh());
+ if (_msg.has_center_submesh())
+ submeshElem->GetElement("center")->Set(_msg.center_submesh());
+ if (_msg.has_scale())
+ {
+ meshSDF->GetElement("scale")->Set(ConvertIgn(_msg.scale()));
+ }
}
-
return meshSDF;
}
@@ -1440,5 +2077,274 @@ namespace gazebo
return pluginSDF;
}
+
+ ////////////////////////////////////////////////////////
+ void AddLinkGeom(Model &_model, const Geometry &_geom)
+ {
+ _model.add_link();
+ int linkCount = _model.link_size();
+ auto link = _model.mutable_link(linkCount-1);
+ {
+ std::ostringstream linkName;
+ linkName << "link_" << linkCount;
+ link->set_name(linkName.str());
+ }
+
+ {
+ link->add_collision();
+ auto collision = link->mutable_collision(0);
+ collision->set_name("collision");
+ *(collision->mutable_geometry()) = _geom;
+ }
+
+ {
+ link->add_visual();
+ auto visual = link->mutable_visual(0);
+ visual->set_name("visual");
+ *(visual->mutable_geometry()) = _geom;
+
+ auto script = visual->mutable_material()->mutable_script();
+ script->add_uri();
+ script->set_uri(0, "file://media/materials/scripts/gazebo.material");
+ script->set_name("Gazebo/Grey");
+ }
+ }
+
+ ////////////////////////////////////////////////////////
+ void AddBoxLink(Model &_model, const double _mass,
+ const math::Vector3 &_size)
+ {
+ AddBoxLink(_model, _mass, _size.Ign());
+ }
+
+ ////////////////////////////////////////////////////////
+ void AddBoxLink(Model &_model, const double _mass,
+ const ignition::math::Vector3d &_size)
+ {
+ Geometry geometry;
+ geometry.set_type(Geometry_Type_BOX);
+ Set(geometry.mutable_box()->mutable_size(), _size);
+
+ AddLinkGeom(_model, geometry);
+ int linkCount = _model.link_size();
+ auto link = _model.mutable_link(linkCount-1);
+
+ auto inertial = link->mutable_inertial();
+ inertial->set_mass(_mass);
+ {
+ double dx = _size.X();
+ double dy = _size.Y();
+ double dz = _size.Z();
+ double ixx = _mass/12.0 * (dy*dy + dz*dz);
+ double iyy = _mass/12.0 * (dz*dz + dx*dx);
+ double izz = _mass/12.0 * (dx*dx + dy*dy);
+ inertial->set_ixx(ixx);
+ inertial->set_iyy(iyy);
+ inertial->set_izz(izz);
+ inertial->set_ixy(0.0);
+ inertial->set_ixz(0.0);
+ inertial->set_iyz(0.0);
+ }
+ }
+
+ ////////////////////////////////////////////////////////
+ void AddCylinderLink(Model &_model,
+ const double _mass,
+ const double _radius,
+ const double _length)
+ {
+ Geometry geometry;
+ geometry.set_type(Geometry_Type_CYLINDER);
+ geometry.mutable_cylinder()->set_radius(_radius);
+ geometry.mutable_cylinder()->set_length(_length);
+
+ AddLinkGeom(_model, geometry);
+ int linkCount = _model.link_size();
+ auto link = _model.mutable_link(linkCount-1);
+
+ auto inertial = link->mutable_inertial();
+ inertial->set_mass(_mass);
+ const double r2 = _radius * _radius;
+ const double ixx = _mass * (0.25 * r2 + _length*_length / 12.0);
+ const double izz = _mass * 0.5 * r2;
+ inertial->set_ixx(ixx);
+ inertial->set_iyy(ixx);
+ inertial->set_izz(izz);
+ inertial->set_ixy(0.0);
+ inertial->set_ixz(0.0);
+ inertial->set_iyz(0.0);
+ }
+
+ ////////////////////////////////////////////////////////
+ void AddSphereLink(Model &_model, const double _mass,
+ const double _radius)
+ {
+ Geometry geometry;
+ geometry.set_type(Geometry_Type_SPHERE);
+ geometry.mutable_sphere()->set_radius(_radius);
+
+ AddLinkGeom(_model, geometry);
+ int linkCount = _model.link_size();
+ auto link = _model.mutable_link(linkCount-1);
+
+ auto inertial = link->mutable_inertial();
+ inertial->set_mass(_mass);
+ const double ixx = _mass * 0.4 * _radius * _radius;
+ inertial->set_ixx(ixx);
+ inertial->set_iyy(ixx);
+ inertial->set_izz(ixx);
+ inertial->set_ixy(0.0);
+ inertial->set_ixz(0.0);
+ inertial->set_iyz(0.0);
+ }
+
+ ////////////////////////////////////////////////////////
+ sdf::ElementPtr ModelToSDF(const msgs::Model &_msg, sdf::ElementPtr _sdf)
+ {
+ sdf::ElementPtr modelSDF;
+
+ if (_sdf)
+ {
+ modelSDF = _sdf;
+ }
+ else
+ {
+ modelSDF.reset(new sdf::Element);
+ sdf::initFile("model.sdf", modelSDF);
+ }
+
+ if (_msg.has_name())
+ modelSDF->GetAttribute("name")->Set(_msg.name());
+ // ignore the id field, since it's not used in sdformat
+ if (_msg.has_is_static())
+ modelSDF->GetElement("static")->Set(_msg.is_static());
+ if (_msg.has_pose())
+ modelSDF->GetElement("pose")->Set(msgs::ConvertIgn(_msg.pose()));
+
+ if (_msg.joint_size() > 0)
+ while (modelSDF->HasElement("joint"))
+ modelSDF->GetElement("joint")->RemoveFromParent();
+ for (int i = 0; i < _msg.joint_size(); ++i)
+ {
+ sdf::ElementPtr jointElem = modelSDF->AddElement("joint");
+ jointElem = JointToSDF(_msg.joint(i), jointElem);
+ }
+
+ if (_msg.link_size())
+ while (modelSDF->HasElement("link"))
+ modelSDF->GetElement("link")->RemoveFromParent();
+ for (int i = 0; i < _msg.link_size(); ++i)
+ {
+ sdf::ElementPtr linkElem = modelSDF->AddElement("link");
+ linkElem = LinkToSDF(_msg.link(i), linkElem);
+ }
+
+ // ignore the deleted field, since it's not used in sdformat
+ if (_msg.visual_size() > 0)
+ {
+ // model element in SDF cannot store visuals,
+ // so ignore them for now
+ gzerr << "Model visuals not yet parsed" << std::endl;
+ }
+ // ignore the scale field, since it's not used in sdformat
+
+ return modelSDF;
+ }
+
+ ////////////////////////////////////////////////////////
+ sdf::ElementPtr JointToSDF(const msgs::Joint &_msg, sdf::ElementPtr _sdf)
+ {
+ sdf::ElementPtr jointSDF;
+
+ if (_sdf)
+ {
+ jointSDF = _sdf;
+ }
+ else
+ {
+ jointSDF.reset(new sdf::Element);
+ sdf::initFile("joint.sdf", jointSDF);
+ }
+
+ if (_msg.has_name())
+ jointSDF->GetAttribute("name")->Set(_msg.name());
+ if (_msg.has_type())
+ jointSDF->GetAttribute("type")->Set(ConvertJointType(_msg.type()));
+ // ignore the id field, since it's not used in sdformat
+ // ignore the parent_id field, since it's not used in sdformat
+ // ignore the child_id field, since it's not used in sdformat
+ // ignore the angle field, since it's not used in sdformat
+ if (_msg.has_parent())
+ jointSDF->GetElement("parent")->Set(_msg.parent());
+ if (_msg.has_child())
+ jointSDF->GetElement("child")->Set(_msg.child());
+ if (_msg.has_pose())
+ jointSDF->GetElement("pose")->Set(ConvertIgn(_msg.pose()));
+ if (_msg.has_axis1())
+ AxisToSDF(_msg.axis1(), jointSDF->GetElement("axis"));
+ if (_msg.has_axis2())
+ AxisToSDF(_msg.axis2(), jointSDF->GetElement("axis2"));
+
+ auto odePhysicsElem = jointSDF->GetElement("physics")->GetElement("ode");
+ if (_msg.has_cfm())
+ odePhysicsElem->GetElement("cfm")->Set(_msg.cfm());
+ if (_msg.has_bounce())
+ odePhysicsElem->GetElement("bounce")->Set(_msg.bounce());
+ if (_msg.has_velocity())
+ odePhysicsElem->GetElement("velocity")->Set(_msg.velocity());
+ if (_msg.has_fudge_factor())
+ odePhysicsElem->GetElement("fudge_factor")->Set(_msg.fudge_factor());
+
+ {
+ auto limitElem = odePhysicsElem->GetElement("limit");
+ if (_msg.has_limit_cfm())
+ limitElem->GetElement("cfm")->Set(_msg.limit_cfm());
+ if (_msg.has_limit_erp())
+ limitElem->GetElement("erp")->Set(_msg.limit_erp());
+ }
+
+ {
+ auto suspensionElem = odePhysicsElem->GetElement("suspension");
+ if (_msg.has_suspension_cfm())
+ suspensionElem->GetElement("cfm")->Set(_msg.suspension_cfm());
+ if (_msg.has_suspension_erp())
+ suspensionElem->GetElement("erp")->Set(_msg.suspension_erp());
+ }
+ /// \todo JointToSDF currently does not convert sensor data
+
+ return jointSDF;
+ }
+
+ ////////////////////////////////////////////////////////
+ void AxisToSDF(const msgs::Axis &_msg, sdf::ElementPtr _sdf)
+ {
+ if (_msg.has_xyz())
+ _sdf->GetElement("xyz")->Set(ConvertIgn(_msg.xyz()));
+ if (_msg.has_use_parent_model_frame())
+ {
+ _sdf->GetElement("use_parent_model_frame")->Set(
+ _msg.use_parent_model_frame());
+ }
+
+ {
+ auto dynamicsElem = _sdf->GetElement("dynamics");
+ if (_msg.has_damping())
+ dynamicsElem->GetElement("damping")->Set(_msg.damping());
+ if (_msg.has_friction())
+ dynamicsElem->GetElement("friction")->Set(_msg.friction());
+ }
+
+ {
+ auto limitElem = _sdf->GetElement("limit");
+ if (_msg.has_limit_lower())
+ limitElem->GetElement("lower")->Set(_msg.limit_lower());
+ if (_msg.has_limit_upper())
+ limitElem->GetElement("upper")->Set(_msg.limit_upper());
+ if (_msg.has_limit_effort())
+ limitElem->GetElement("effort")->Set(_msg.limit_effort());
+ if (_msg.has_limit_velocity())
+ limitElem->GetElement("velocity")->Set(_msg.limit_velocity());
+ }
+ }
}
}
diff --git a/gazebo/msgs/msgs.hh b/gazebo/msgs/msgs.hh
index def88e4..52cd5e0 100644
--- a/gazebo/msgs/msgs.hh
+++ b/gazebo/msgs/msgs.hh
@@ -14,20 +14,25 @@
* limitations under the License.
*
*/
-#ifndef MESSAGES_UTILITY_H
-#define MESSAGES_UTILITY_H
+#ifndef _GAZEBO_MESSAGES_UTILITY_HH_
+#define _GAZEBO_MESSAGES_UTILITY_HH_
#include <string>
#include <sdf/sdf.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Plane.hh>
+
+#include "gazebo/math/Quaternion.hh"
+
#include "gazebo/msgs/MessageTypes.hh"
#include "gazebo/math/MathTypes.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/math/Plane.hh"
-#include "gazebo/math/Box.hh"
#include "gazebo/common/SphericalCoordinates.hh"
#include "gazebo/common/Color.hh"
@@ -77,80 +82,161 @@ namespace gazebo
/// \brief Convert a math::Vector3 to a msgs::Vector3d
/// \param[in] _v The vector to convert
/// \return A msgs::Vector3d object
+ /// \deprecated See function that accepts an ignition::math object.
GAZEBO_VISIBLE
- msgs::Vector3d Convert(const math::Vector3 &_v);
+ msgs::Vector3d Convert(const math::Vector3 &_v) GAZEBO_DEPRECATED(6.0);
/// \brief Convert a math::Vector2d to a msgs::Vector2d
/// \param[in] _v The vector to convert
/// \return A msgs::Vector2d object
+ /// \deprecated See function that accepts an ignition::math object.
GAZEBO_VISIBLE
- msgs::Vector2d Convert(const math::Vector2d &_v);
+ msgs::Vector2d Convert(const math::Vector2d &_v) GAZEBO_DEPRECATED(6.0);
/// \brief Convert a math::Quaternion to a msgs::Quaternion
/// \param[in] _q The quaternion to convert
/// \return A msgs::Quaternion object
+ /// \deprecated See function that accepts an ignition::math object.
GAZEBO_VISIBLE
- msgs::Quaternion Convert(const math::Quaternion &_q);
+ msgs::Quaternion Convert(const math::Quaternion &_q) GAZEBO_DEPRECATED(6.0);
/// \brief Convert a math::Pose to a msgs::Pose
/// \param[in] _p The pose to convert
/// \return A msgs::Pose object
+ /// \deprecated See function that accepts an ignition::math object.
+ GAZEBO_VISIBLE
+ msgs::Pose Convert(const math::Pose &_p) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Convert a ignition::math::Vector3 to a msgs::Vector3d
+ /// \param[in] _v The vector to convert
+ /// \return A msgs::Vector3d object
+ GAZEBO_VISIBLE
+ msgs::Vector3d Convert(const ignition::math::Vector3d &_v);
+
+ /// \brief Convert a ignition::math::Vector2d to a msgs::Vector2d
+ /// \param[in] _v The vector to convert
+ /// \return A msgs::Vector2d object
+ GAZEBO_VISIBLE
+ msgs::Vector2d Convert(const ignition::math::Vector2d &_v);
+
+ /// \brief Convert a ignition::math::Quaternion to a msgs::Quaternion
+ /// \param[in] _q The quaternion to convert
+ /// \return A msgs::Quaternion object
+ GAZEBO_VISIBLE
+ msgs::Quaternion Convert(const ignition::math::Quaterniond &_q);
+
+ /// \brief Convert a ignition::math::Pose to a msgs::Pose
+ /// \param[in] _p The pose to convert
+ /// \return A msgs::Pose object
GAZEBO_VISIBLE
- msgs::Pose Convert(const math::Pose &_p);
+ msgs::Pose Convert(const ignition::math::Pose3d &_p);
/// \brief Convert a common::Color to a msgs::Color
/// \param[in] _c The color to convert
/// \return A msgs::Color object
GAZEBO_VISIBLE
- msgs::Color Convert(const common::Color &_c);
+ msgs::Color Convert(const common::Color &_c);
/// \brief Convert a common::Time to a msgs::Time
/// \param[in] _t The time to convert
/// \return A msgs::Time object
GAZEBO_VISIBLE
- msgs::Time Convert(const common::Time &_t);
+ msgs::Time Convert(const common::Time &_t);
/// \brief Convert a math::Plane to a msgs::PlaneGeom
/// \param[in] _p The plane to convert
/// \return A msgs::PlaneGeom object
+ /// \deprecated See function that accepts an ignition::math object.
+ GAZEBO_VISIBLE
+ msgs::PlaneGeom Convert(const math::Plane &_p) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Convert a ignition::math::Planed to a msgs::PlaneGeom
+ /// \param[in] _p The plane to convert
+ /// \return A msgs::PlaneGeom object
GAZEBO_VISIBLE
- msgs::PlaneGeom Convert(const math::Plane &_p);
+ msgs::PlaneGeom Convert(const ignition::math::Planed &_p);
/// \brief Convert a string to a msgs::Joint::Type enum.
/// \param[in] _str Joint type string.
- /// \return A msgs::Joint::Type enum.
+ /// \return A msgs::Joint::Type enum. Defaults to REVOLUTE
+ /// if _str is unrecognized.
GAZEBO_VISIBLE
msgs::Joint::Type ConvertJointType(const std::string &_str);
/// \brief Convert a msgs::Joint::Type to a string.
/// \param[in] _type A msgs::Joint::Type enum.
- /// \return Joint type string.
+ /// \return Joint type string. Returns "unknown" if
+ /// _type is unrecognized.
GAZEBO_VISIBLE
- std::string ConvertJointType(const msgs::Joint::Type _type);
+ std::string ConvertJointType(const msgs::Joint::Type &_type);
+
+ /// \brief Convert a string to a msgs::Geometry::Type enum.
+ /// \param[in] _str Geometry type string.
+ /// \return A msgs::Geometry::Type enum.
+ GAZEBO_VISIBLE
+ msgs::Geometry::Type ConvertGeometryType(const std::string &_str);
+
+ /// \brief Convert a msgs::Geometry::Type to a string.
+ /// \param[in] _type A msgs::Geometry::Type enum.
+ /// \return Geometry type string.
+ GAZEBO_VISIBLE
+ std::string ConvertGeometryType(const msgs::Geometry::Type _type);
/// \brief Convert a msgs::Vector3d to a math::Vector
/// \param[in] _v The plane to convert
/// \return A math::Vector3 object
+ /// \deprecated See ConvertIgn() function that returns an ignition::math
+ /// object.
GAZEBO_VISIBLE
- math::Vector3 Convert(const msgs::Vector3d &_v);
+ math::Vector3 Convert(const msgs::Vector3d &_v) GAZEBO_DEPRECATED(6.0);
/// \brief Convert a msgs::Vector2d to a math::Vector2d
/// \param[in] _v The vector2 to convert
/// \return A math::Vector2d object
+ /// \deprecated See ConvertIgn() function that returns an ignition::math
+ /// object.
GAZEBO_VISIBLE
- math::Vector2d Convert(const msgs::Vector2d &_v);
+ math::Vector2d Convert(const msgs::Vector2d &_v) GAZEBO_DEPRECATED(6.0);
/// \brief Convert a msgs::Quaternion to a math::Quaternion
/// \param[in] _q The quaternion to convert
/// \return A math::Quaternion object
+ /// \deprecated See ConvertIgn() function that returns an ignition::math
+ /// object.
GAZEBO_VISIBLE
- math::Quaternion Convert(const msgs::Quaternion &_q);
+ math::Quaternion Convert(const msgs::Quaternion &_q) GAZEBO_DEPRECATED(6.0);
/// \brief Convert a msgs::Pose to a math::Pose
/// \param[in] _q The pose to convert
/// \return A math::Pose object
+ /// \deprecated See ConvertIgn() function that returns an ignition::math
+ /// object.
GAZEBO_VISIBLE
- math::Pose Convert(const msgs::Pose &_p);
+ math::Pose Convert(const msgs::Pose &_p) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Convert a msgs::Vector3d to an ignition::math::Vector
+ /// \param[in] _v The plane to convert
+ /// \return An ignition::math::Vector3 object
+ GAZEBO_VISIBLE
+ ignition::math::Vector3d ConvertIgn(const msgs::Vector3d &_v);
+
+ /// \brief Convert a msgs::Vector2d to an ignition::math::Vector2d
+ /// \param[in] _v The vector2 to convert
+ /// \return An ignition::math::Vector2d object
+ GAZEBO_VISIBLE
+ ignition::math::Vector2d ConvertIgn(const msgs::Vector2d &_v);
+
+ /// \brief Convert a msgs::Quaternion to an ignition::math::Quaternion
+ /// \param[in] _q The quaternion to convert
+ /// \return An ignition::math::Quaternion object
+ GAZEBO_VISIBLE
+ ignition::math::Quaterniond ConvertIgn(const msgs::Quaternion &_q);
+
+ /// \brief Convert a msgs::Pose to an ignition::math::Pose
+ /// \param[in] _q The pose to convert
+ /// \return An ignition::math::Pose object
+ GAZEBO_VISIBLE
+ ignition::math::Pose3d ConvertIgn(const msgs::Pose &_p);
/// \brief Convert a msgs::Image to a common::Image
/// \param[out] _img The common::Image container
@@ -162,19 +248,27 @@ namespace gazebo
/// \param[in] _c The color to convert
/// \return A common::Color object
GAZEBO_VISIBLE
- common::Color Convert(const msgs::Color &_c);
+ common::Color Convert(const msgs::Color &_c);
/// \brief Convert a msgs::Time to a common::Time
/// \param[in] _t The time to convert
/// \return A common::Time object
GAZEBO_VISIBLE
- common::Time Convert(const msgs::Time &_t);
+ common::Time Convert(const msgs::Time &_t);
- /// \brief Convert a msgs::PlaneGeom to a common::Plane
+ /// \brief Convert a msgs::PlaneGeom to a math::Plane
/// \param[in] _p The plane to convert
- /// \return A common::Plane object
+ /// \return A math::Plane object
+ /// \deprecated See ConvertIgn() function that returns an ignition::math
+ /// object.
GAZEBO_VISIBLE
- math::Plane Convert(const msgs::PlaneGeom &_p);
+ math::Plane Convert(const msgs::PlaneGeom &_p) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Convert a msgs::PlaneGeom to an ignition::math::Planed
+ /// \param[in] _p The plane to convert
+ /// \return An ignition::math::Planed object
+ GAZEBO_VISIBLE
+ ignition::math::Planed ConvertIgn(const msgs::PlaneGeom &_p);
/// \brief Set a msgs::Image from a common::Image
/// \param[out] _msg A msgs::Image pointer
@@ -185,26 +279,61 @@ namespace gazebo
/// \brief Set a msgs::Vector3d from a math::Vector3
/// \param[out] _pt A msgs::Vector3d pointer
/// \param[in] _v A math::Vector3 reference
+ /// \deprecated See Set() function that accepts an
+ /// ignition::math::Vector3d object.
GAZEBO_VISIBLE
- void Set(msgs::Vector3d *_pt, const math::Vector3 &_v);
+ void Set(msgs::Vector3d *_pt, const math::Vector3 &_v)
+ GAZEBO_DEPRECATED(6.0);
/// \brief Set a msgs::Vector2d from a math::Vector3
/// \param[out] _pt A msgs::Vector2d pointer
/// \param[in] _v A math::Vector2d reference
+ /// \deprecated See Set() function that accepts an
+ /// ignition::math::Vector2d object.
GAZEBO_VISIBLE
- void Set(msgs::Vector2d *_pt, const math::Vector2d &_v);
+ void Set(msgs::Vector2d *_pt, const math::Vector2d &_v)
+ GAZEBO_DEPRECATED(6.0);
/// \brief Set a msgs::Quaternion from a math::Quaternion
/// \param[out] _q A msgs::Quaternion pointer
/// \param[in] _v A math::Quaternion reference
+ /// \deprecated See Set() function that accepts an
+ /// ignition::math::Quaterniond object.
GAZEBO_VISIBLE
- void Set(msgs::Quaternion *_q, const math::Quaternion &_v);
+ void Set(msgs::Quaternion *_q, const math::Quaternion &_v)
+ GAZEBO_DEPRECATED(6.0);
/// \brief Set a msgs::Pose from a math::Pose
/// \param[out] _p A msgs::Pose pointer
/// \param[in] _v A math::Pose reference
+ /// \deprecated See Set() function that accepts an
+ /// ignition::math::Pose3d object.
GAZEBO_VISIBLE
- void Set(msgs::Pose *_p, const math::Pose &_v);
+ void Set(msgs::Pose *_p, const math::Pose &_v) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set a msgs::Vector3d from an ignition::math::Vector3d
+ /// \param[out] _pt A msgs::Vector3d pointer
+ /// \param[in] _v An ignition::math::Vector3d reference
+ GAZEBO_VISIBLE
+ void Set(msgs::Vector3d *_pt, const ignition::math::Vector3d &_v);
+
+ /// \brief Set a msgs::Vector2d from an ignition::math::Vector2d
+ /// \param[out] _pt A msgs::Vector2d pointer
+ /// \param[in] _v An ignition::math::Vector2d reference
+ GAZEBO_VISIBLE
+ void Set(msgs::Vector2d *_pt, const ignition::math::Vector2d &_v);
+
+ /// \brief Set a msgs::Quaternion from an ignition::math::Quaterniond
+ /// \param[out] _q A msgs::Quaternion pointer
+ /// \param[in] _v An ignition::math::Quaterniond reference
+ GAZEBO_VISIBLE
+ void Set(msgs::Quaternion *_q, const ignition::math::Quaterniond &_v);
+
+ /// \brief Set a msgs::Pose from an ignition::math::Pose3d
+ /// \param[out] _p A msgs::Pose pointer
+ /// \param[in] _v An ignition::math::Pose3d reference
+ GAZEBO_VISIBLE
+ void Set(msgs::Pose *_p, const ignition::math::Pose3d &_v);
/// \brief Set a msgs::Color from a common::Color
/// \param[out] _p A msgs::Color pointer
@@ -229,8 +358,16 @@ namespace gazebo
/// \brief Set a msgs::Plane from a math::Plane
/// \param[out] _p A msgs::Plane pointer
/// \param[in] _v A math::Plane reference
+ /// \deprecated See Set() function that accepts an
+ /// ignition::math::Planed object.
GAZEBO_VISIBLE
- void Set(msgs::PlaneGeom *_p, const math::Plane &_v);
+ void Set(msgs::PlaneGeom *_p, const math::Plane &_v) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set a msgs::Plane from an ignition::math::Planed
+ /// \param[out] _p A msgs::Plane pointer
+ /// \param[in] _v An ignition::math::Planed reference
+ GAZEBO_VISIBLE
+ void Set(msgs::PlaneGeom *_p, const ignition::math::Planed &_v);
/// \brief Create a msgs::TrackVisual from a track visual SDF element
/// \param[in] _sdf The sdf element
@@ -268,6 +405,52 @@ namespace gazebo
GAZEBO_VISIBLE
msgs::Visual VisualFromSDF(sdf::ElementPtr _sdf);
+ /// \brief Create a msgs::Axis from an axis SDF element
+ /// \param[in] _sdf The sdf element
+ /// \return The new msgs::Axis object
+ GAZEBO_VISIBLE
+ msgs::Axis AxisFromSDF(sdf::ElementPtr _sdf);
+
+ /// \brief Create a msgs::Joint from a joint SDF element
+ /// \param[in] _sdf The sdf element
+ /// \return The new msgs::Joint object
+ GAZEBO_VISIBLE
+ msgs::Joint JointFromSDF(sdf::ElementPtr _sdf);
+
+ /// \brief Create or update an SDF element from a msgs::Visual
+ /// \param[in] _msg Visual messsage
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
+ /// creating a new sdf element.
+ /// \return The new SDF element.
+ GAZEBO_VISIBLE
+ sdf::ElementPtr VisualToSDF(const msgs::Visual &_msg,
+ sdf::ElementPtr _sdf = sdf::ElementPtr());
+
+ /// \brief Create or update an SDF element from a msgs::Material
+ /// If _sdf is supplied and _msg has script uri's
+ /// the <uri> elements will be removed from _sdf.
+ /// \param[in] _msg Material messsage
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
+ /// creating a new sdf element.
+ /// \return The new SDF element.
+ GAZEBO_VISIBLE
+ sdf::ElementPtr MaterialToSDF(const msgs::Material &_msg,
+ sdf::ElementPtr _sdf = sdf::ElementPtr());
+
+ /// \brief Convert a string to a msgs::Material::ShaderType enum.
+ /// \param[in] _str Shader type string.
+ /// \return A msgs::Material::ShaderType enum. Defaults to VERTEX
+ /// if _str is unrecognized.
+ GAZEBO_VISIBLE
+ msgs::Material::ShaderType ConvertShaderType(const std::string &_str);
+
+ /// \brief Convert a msgs::ShaderType to a string.
+ /// \param[in] _type A msgs::ShaderType enum.
+ /// \return Shader type string. Returns "unknown" if
+ /// _type is unrecognized.
+ GAZEBO_VISIBLE
+ std::string ConvertShaderType(const msgs::Material::ShaderType &_type);
+
/// \brief Create a msgs::Fog from a fog SDF element
/// \param[in] _sdf The sdf element
/// \return The new msgs::Fog object
@@ -280,88 +463,168 @@ namespace gazebo
GAZEBO_VISIBLE
msgs::Scene SceneFromSDF(sdf::ElementPtr _sdf);
- /// \brief Create an SDF element from a msgs::Light
+ /// \brief Create or update an SDF element from a msgs::Light
/// \param[in] _msg Light messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr LightToSDF(const msgs::Light &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::CameraSensor
+ /// \brief Create or update an SDF element from a msgs::CameraSensor
/// \param[in] _msg CameraSensor messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr CameraSensorToSDF(const msgs::CameraSensor &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Plugin
+ /// \brief Create or update an SDF element from a msgs::Plugin
/// \param[in] _msg Plugin messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr PluginToSDF(const msgs::Plugin &_plugin,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Collision
+ /// \brief Create or update an SDF element from a msgs::Collision
/// \param[in] _msg Collision messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr CollisionToSDF(const msgs::Collision &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \internal
- /// \brief Create an SDF element from a msgs::Link.
+ /// \brief Create or update an SDF element from a msgs::Link.
+ /// If _sdf is supplied and _msg has any collisions or visuals,
+ /// the <collision> and <visual> elements will be removed from _sdf.
/// \param[in] _msg Link messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr LinkToSDF(const msgs::Link &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Inertial
+ /// \brief Create or update an SDF element from a msgs::Inertial
/// \param[in] _msg Inertial messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr InertialToSDF(const msgs::Inertial &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Surface
+ /// \brief Create or update an SDF element from a msgs::Surface
/// \param[in] _msg Surface messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr SurfaceToSDF(const msgs::Surface &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Geometry
+ /// \brief Create or update an SDF element from a msgs::Geometry
+ /// If _sdf is supplied and the _msg has non-empty repeated elements,
+ /// any existing sdf elements will be removed from _sdf prior to adding
+ /// the new elements from _msg.
/// \param[in] _msg Geometry messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr GeometryToSDF(const msgs::Geometry &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
- /// \brief Create an SDF element from a msgs::Mesh
+ /// \brief Create or update an SDF element from a msgs::Mesh
/// \param[in] _msg Mesh messsage
- /// \param[in] _sdf if supplied, performs an update from _msg intead of
+ /// \param[in] _sdf if supplied, performs an update from _msg instead of
/// creating a new sdf element.
/// \return The new SDF element.
GAZEBO_VISIBLE
sdf::ElementPtr MeshToSDF(const msgs::MeshGeom &_msg,
sdf::ElementPtr _sdf = sdf::ElementPtr());
+ /// \brief Add a simple box link to a Model message.
+ /// The size and mass of the box are specified, and a
+ /// single collision is added, along with an inertial
+ /// block corresponding to a box of uniform density.
+ /// \param[out] _model The msgs::Model to which the link is added.
+ /// \param[in] _mass Mass of the box.
+ /// \param[in] _size Size of the box.
+ /// \deprecated See AddBoxLink() function that accepts an
+ /// ignition::math::Vector3d object.
+ GAZEBO_VISIBLE
+ void AddBoxLink(msgs::Model &_model, const double _mass,
+ const math::Vector3 &_size) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Add a simple box link to a Model message.
+ /// The size and mass of the box are specified, and a
+ /// single collision is added, along with an inertial
+ /// block corresponding to a box of uniform density.
+ /// \param[out] _model The msgs::Model to which the link is added.
+ /// \param[in] _mass Mass of the box.
+ /// \param[in] _size Size of the box.
+ GAZEBO_VISIBLE
+ void AddBoxLink(msgs::Model &_model, const double _mass,
+ const ignition::math::Vector3d &_size);
+
+ /// \brief Add a simple cylinder link to a Model message.
+ /// The radius, length, and mass of the cylinder are specified, and a
+ /// single collision is added, along with an inertial
+ /// block corresponding to a cylinder of uniform density
+ /// with an axis of symmetry along the Z axis.
+ /// \param[out] _model The msgs::Model to which the link is added.
+ /// \param[in] _mass Mass of the cylinder.
+ /// \param[in] _radius Radius of the cylinder.
+ /// \param[in] _length Length of the cylinder.
+ GAZEBO_VISIBLE
+ void AddCylinderLink(msgs::Model &_model, const double _mass,
+ const double _radius, const double _length);
+
+ /// \brief Add a simple sphere link to a Model message.
+ /// The size and mass of the sphere are specified, and a
+ /// single collision is added, along with an inertial
+ /// block corresponding to a sphere of uniform density.
+ /// \param[out] _model The msgs::Model to which the link is added.
+ /// \param[in] _mass Mass of the sphere.
+ /// \param[in] _radius Radius of the sphere.
+ GAZEBO_VISIBLE
+ void AddSphereLink(msgs::Model &_model, const double _mass,
+ const double _radius);
+
+ /// \brief Add a link with a collision and visual
+ /// of specified geometry to a model message.
+ /// It does not set any inertial values.
+ /// \param[out] _model The msgs::Model object to receive a new link.
+ /// \param[in] _geom Geometry to be added to collision and visual.
+ GAZEBO_VISIBLE
+ void AddLinkGeom(Model &_msg, const Geometry &_geom);
+
+ /// \brief Create or update an SDF element from msgs::Model.
+ /// If _sdf is supplied and _msg has any links or joints,
+ /// the <link> and <joint> elements will be removed from _sdf.
+ /// \param[in] _msg The msgs::Model object.
+ /// \param[in] _sdf if supplied, performs an update from _sdf instead of
+ /// creating a new sdf element.
+ /// \return The new SDF element.
+ GAZEBO_VISIBLE
+ sdf::ElementPtr ModelToSDF(const msgs::Model &_msg,
+ sdf::ElementPtr _sdf = sdf::ElementPtr());
+
+ /// \brief Create or update an SDF element from msgs::Joint.
+ /// \param[in] _msg The msgs::Joint object.
+ /// \param[in] _sdf if supplied, performs an update from _sdf instead of
+ /// creating a new sdf element.
+ /// \return The new SDF element.
+ GAZEBO_VISIBLE
+ sdf::ElementPtr JointToSDF(const msgs::Joint &_msg,
+ sdf::ElementPtr _sdf = sdf::ElementPtr());
+
/// \cond
GAZEBO_VISIBLE
const google::protobuf::FieldDescriptor *GetFD(
diff --git a/gazebo/msgs/msgs_TEST.cc b/gazebo/msgs/msgs_TEST.cc
index 1e2cae8..854bad2 100644
--- a/gazebo/msgs/msgs_TEST.cc
+++ b/gazebo/msgs/msgs_TEST.cc
@@ -104,7 +104,7 @@ TEST_F(MsgsTest, BadPackage)
TEST_F(MsgsTest, CovertMathVector3ToMsgs)
{
- msgs::Vector3d msg = msgs::Convert(math::Vector3(1, 2, 3));
+ msgs::Vector3d msg = msgs::Convert(ignition::math::Vector3d(1, 2, 3));
EXPECT_DOUBLE_EQ(1, msg.x());
EXPECT_DOUBLE_EQ(2, msg.y());
EXPECT_DOUBLE_EQ(3, msg.z());
@@ -112,17 +112,17 @@ TEST_F(MsgsTest, CovertMathVector3ToMsgs)
TEST_F(MsgsTest, ConvertMsgsVector3dToMath)
{
- msgs::Vector3d msg = msgs::Convert(math::Vector3(1, 2, 3));
- math::Vector3 v = msgs::Convert(msg);
- EXPECT_DOUBLE_EQ(1, v.x);
- EXPECT_DOUBLE_EQ(2, v.y);
- EXPECT_DOUBLE_EQ(3, v.z);
+ msgs::Vector3d msg = msgs::Convert(ignition::math::Vector3d(1, 2, 3));
+ ignition::math::Vector3d v = msgs::ConvertIgn(msg);
+ EXPECT_DOUBLE_EQ(1, v.X());
+ EXPECT_DOUBLE_EQ(2, v.Y());
+ EXPECT_DOUBLE_EQ(3, v.Z());
}
TEST_F(MsgsTest, ConvertMathQuaterionToMsgs)
{
msgs::Quaternion msg =
- msgs::Convert(math::Quaternion(M_PI * 0.25, M_PI * 0.5, M_PI));
+ msgs::Convert(ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI));
EXPECT_TRUE(math::equal(msg.x(), -0.65328148243818818));
EXPECT_TRUE(math::equal(msg.y(), 0.27059805007309856));
@@ -133,20 +133,21 @@ TEST_F(MsgsTest, ConvertMathQuaterionToMsgs)
TEST_F(MsgsTest, ConvertMsgsQuaterionToMath)
{
msgs::Quaternion msg =
- msgs::Convert(math::Quaternion(M_PI * 0.25, M_PI * 0.5, M_PI));
- math::Quaternion v = msgs::Convert(msg);
+ msgs::Convert(ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI));
+ ignition::math::Quaterniond v = msgs::ConvertIgn(msg);
// TODO: to real unit test move math::equal to EXPECT_DOUBLE_EQ
- EXPECT_TRUE(math::equal(v.x, -0.65328148243818818));
- EXPECT_TRUE(math::equal(v.y, 0.27059805007309856));
- EXPECT_TRUE(math::equal(v.z, 0.65328148243818829));
- EXPECT_TRUE(math::equal(v.w, 0.27059805007309851));
+ EXPECT_TRUE(math::equal(v.X(), -0.65328148243818818));
+ EXPECT_TRUE(math::equal(v.Y(), 0.27059805007309856));
+ EXPECT_TRUE(math::equal(v.Z(), 0.65328148243818829));
+ EXPECT_TRUE(math::equal(v.W(), 0.27059805007309851));
}
TEST_F(MsgsTest, ConvertPoseMathToMsgs)
{
- msgs::Pose msg = msgs::Convert(math::Pose(math::Vector3(1, 2, 3),
- math::Quaternion(M_PI * 0.25, M_PI * 0.5, M_PI)));
+ msgs::Pose msg = msgs::Convert(ignition::math::Pose3d(
+ ignition::math::Vector3d(1, 2, 3),
+ ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI)));
EXPECT_DOUBLE_EQ(1, msg.position().x());
EXPECT_DOUBLE_EQ(2, msg.position().y());
@@ -160,17 +161,18 @@ TEST_F(MsgsTest, ConvertPoseMathToMsgs)
TEST_F(MsgsTest, ConvertMsgPoseToMath)
{
- msgs::Pose msg = msgs::Convert(math::Pose(math::Vector3(1, 2, 3),
- math::Quaternion(M_PI * 0.25, M_PI * 0.5, M_PI)));
- math::Pose v = msgs::Convert(msg);
-
- EXPECT_DOUBLE_EQ(1, v.pos.x);
- EXPECT_DOUBLE_EQ(2, v.pos.y);
- EXPECT_DOUBLE_EQ(3, v.pos.z);
- EXPECT_TRUE(math::equal(v.rot.x, -0.65328148243818818));
- EXPECT_TRUE(math::equal(v.rot.y, 0.27059805007309856));
- EXPECT_TRUE(math::equal(v.rot.z, 0.65328148243818829));
- EXPECT_TRUE(math::equal(v.rot.w, 0.27059805007309851));
+ msgs::Pose msg = msgs::Convert(
+ ignition::math::Pose3d(ignition::math::Vector3d(1, 2, 3),
+ ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI)));
+ ignition::math::Pose3d v = msgs::ConvertIgn(msg);
+
+ EXPECT_DOUBLE_EQ(1, v.Pos().X());
+ EXPECT_DOUBLE_EQ(2, v.Pos().Y());
+ EXPECT_DOUBLE_EQ(3, v.Pos().Z());
+ EXPECT_TRUE(math::equal(v.Rot().X(), -0.65328148243818818));
+ EXPECT_TRUE(math::equal(v.Rot().Y(), 0.27059805007309856));
+ EXPECT_TRUE(math::equal(v.Rot().Z(), 0.65328148243818829));
+ EXPECT_TRUE(math::equal(v.Rot().W(), 0.27059805007309851));
}
TEST_F(MsgsTest, ConvertCommonColorToMsgs)
@@ -207,8 +209,9 @@ TEST_F(MsgsTest, ConvertCommonTimeToMsgs)
TEST_F(MsgsTest, ConvertMathPlaneToMsgs)
{
- msgs::PlaneGeom msg = msgs::Convert(math::Plane(math::Vector3(0, 0, 1),
- math::Vector2d(123, 456), 1.0));
+ msgs::PlaneGeom msg = msgs::Convert(
+ ignition::math::Planed(ignition::math::Vector3d(0, 0, 1),
+ ignition::math::Vector2d(123, 456), 1.0));
EXPECT_DOUBLE_EQ(0, msg.normal().x());
EXPECT_DOUBLE_EQ(0, msg.normal().y());
@@ -220,25 +223,43 @@ TEST_F(MsgsTest, ConvertMathPlaneToMsgs)
TEST_F(MsgsTest, ConvertMsgsPlaneToMath)
{
- msgs::PlaneGeom msg = msgs::Convert(math::Plane(math::Vector3(0, 0, 1),
- math::Vector2d(123, 456), 1.0));
- math::Plane v = msgs::Convert(msg);
+ msgs::PlaneGeom msg = msgs::Convert(
+ ignition::math::Planed(ignition::math::Vector3d(0, 0, 1),
+ ignition::math::Vector2d(123, 456), 1.0));
+ ignition::math::Planed v = msgs::ConvertIgn(msg);
- EXPECT_DOUBLE_EQ(0, v.normal.x);
- EXPECT_DOUBLE_EQ(0, v.normal.y);
- EXPECT_DOUBLE_EQ(1, v.normal.z);
+ EXPECT_DOUBLE_EQ(0, v.Normal().X());
+ EXPECT_DOUBLE_EQ(0, v.Normal().Y());
+ EXPECT_DOUBLE_EQ(1, v.Normal().Z());
- EXPECT_DOUBLE_EQ(123, v.size.x);
- EXPECT_DOUBLE_EQ(456, v.size.y);
+ EXPECT_DOUBLE_EQ(123, v.Size().X());
+ EXPECT_DOUBLE_EQ(456, v.Size().Y());
- EXPECT_TRUE(math::equal(1.0, v.d));
+ EXPECT_TRUE(math::equal(1.0, v.Offset()));
}
+//////////////////////////////////////////////////
+void CompareMsgsShaderTypeToString(const msgs::Material::ShaderType _type)
+{
+ EXPECT_EQ(_type, msgs::ConvertShaderType(msgs::ConvertShaderType(_type)));
+}
+
+//////////////////////////////////////////////////
+TEST_F(MsgsTest, ConvertMsgsShaderTypeToString)
+{
+ CompareMsgsShaderTypeToString(msgs::Material::NORMAL_MAP_OBJECT_SPACE);
+ CompareMsgsShaderTypeToString(msgs::Material::NORMAL_MAP_TANGENT_SPACE);
+ CompareMsgsShaderTypeToString(msgs::Material::PIXEL);
+ CompareMsgsShaderTypeToString(msgs::Material::VERTEX);
+}
+
+//////////////////////////////////////////////////
void CompareMsgsJointTypeToString(const msgs::Joint::Type _type)
{
EXPECT_EQ(_type, msgs::ConvertJointType(msgs::ConvertJointType(_type)));
}
+//////////////////////////////////////////////////
TEST_F(MsgsTest, ConvertMsgsJointTypeToString)
{
CompareMsgsJointTypeToString(msgs::Joint::REVOLUTE);
@@ -248,12 +269,50 @@ TEST_F(MsgsTest, ConvertMsgsJointTypeToString)
CompareMsgsJointTypeToString(msgs::Joint::BALL);
CompareMsgsJointTypeToString(msgs::Joint::SCREW);
CompareMsgsJointTypeToString(msgs::Joint::GEARBOX);
+ CompareMsgsJointTypeToString(msgs::Joint::FIXED);
+}
+
+//////////////////////////////////////////////////
+void CompareMsgsGeometryTypeToString(const msgs::Geometry::Type _type)
+{
+ EXPECT_EQ(_type, msgs::ConvertGeometryType(msgs::ConvertGeometryType(_type)));
+}
+
+//////////////////////////////////////////////////
+TEST_F(MsgsTest, ConvertMsgsGeometryTypeToString)
+{
+ CompareMsgsGeometryTypeToString(msgs::Geometry::BOX);
+ CompareMsgsGeometryTypeToString(msgs::Geometry::SPHERE);
+ CompareMsgsGeometryTypeToString(msgs::Geometry::CYLINDER);
+ CompareMsgsGeometryTypeToString(msgs::Geometry::PLANE);
+ CompareMsgsGeometryTypeToString(msgs::Geometry::IMAGE);
+ CompareMsgsGeometryTypeToString(msgs::Geometry::HEIGHTMAP);
+ CompareMsgsGeometryTypeToString(msgs::Geometry::MESH);
+ CompareMsgsGeometryTypeToString(msgs::Geometry::POLYLINE);
+
+ EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::BOX), "box");
+ EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::SPHERE), "sphere");
+ EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::CYLINDER), "cylinder");
+ EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::PLANE), "plane");
+ EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::IMAGE), "image");
+ EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::HEIGHTMAP), "heightmap");
+ EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::MESH), "mesh");
+ EXPECT_EQ(msgs::ConvertGeometryType(msgs::Geometry::POLYLINE), "polyline");
+
+ EXPECT_EQ(msgs::ConvertGeometryType("box"), msgs::Geometry::BOX);
+ EXPECT_EQ(msgs::ConvertGeometryType("sphere"), msgs::Geometry::SPHERE);
+ EXPECT_EQ(msgs::ConvertGeometryType("cylinder"), msgs::Geometry::CYLINDER);
+ EXPECT_EQ(msgs::ConvertGeometryType("plane"), msgs::Geometry::PLANE);
+ EXPECT_EQ(msgs::ConvertGeometryType("image"), msgs::Geometry::IMAGE);
+ EXPECT_EQ(msgs::ConvertGeometryType("heightmap"), msgs::Geometry::HEIGHTMAP);
+ EXPECT_EQ(msgs::ConvertGeometryType("mesh"), msgs::Geometry::MESH);
+ EXPECT_EQ(msgs::ConvertGeometryType("polyline"), msgs::Geometry::POLYLINE);
}
TEST_F(MsgsTest, SetVector3)
{
msgs::Vector3d msg;
- msgs::Set(&msg, math::Vector3(1, 2, 3));
+ msgs::Set(&msg, ignition::math::Vector3d(1, 2, 3));
EXPECT_DOUBLE_EQ(1, msg.x());
EXPECT_DOUBLE_EQ(2, msg.y());
EXPECT_DOUBLE_EQ(3, msg.z());
@@ -262,7 +321,7 @@ TEST_F(MsgsTest, SetVector3)
TEST_F(MsgsTest, SetVector2d)
{
msgs::Vector2d msg;
- msgs::Set(&msg, math::Vector2d(1, 2));
+ msgs::Set(&msg, ignition::math::Vector2d(1, 2));
EXPECT_DOUBLE_EQ(1, msg.x());
EXPECT_DOUBLE_EQ(2, msg.y());
}
@@ -270,7 +329,7 @@ TEST_F(MsgsTest, SetVector2d)
TEST_F(MsgsTest, SetQuaternion)
{
msgs::Quaternion msg;
- msgs::Set(&msg, math::Quaternion(M_PI * 0.25, M_PI * 0.5, M_PI));
+ msgs::Set(&msg, ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI));
EXPECT_TRUE(math::equal(msg.x(), -0.65328148243818818));
EXPECT_TRUE(math::equal(msg.y(), 0.27059805007309856));
EXPECT_TRUE(math::equal(msg.z(), 0.65328148243818829));
@@ -280,8 +339,8 @@ TEST_F(MsgsTest, SetQuaternion)
TEST_F(MsgsTest, SetPose)
{
msgs::Pose msg;
- msgs::Set(&msg, math::Pose(math::Vector3(1, 2, 3),
- math::Quaternion(M_PI * 0.25, M_PI * 0.5, M_PI)));
+ msgs::Set(&msg, ignition::math::Pose3d(ignition::math::Vector3d(1, 2, 3),
+ ignition::math::Quaterniond(M_PI * 0.25, M_PI * 0.5, M_PI)));
EXPECT_DOUBLE_EQ(1, msg.position().x());
EXPECT_DOUBLE_EQ(2, msg.position().y());
@@ -314,8 +373,9 @@ TEST_F(MsgsTest, SetTime)
TEST_F(MsgsTest, SetPlane)
{
msgs::PlaneGeom msg;
- msgs::Set(&msg, math::Plane(math::Vector3(0, 0, 1),
- math::Vector2d(123, 456), 1.0));
+ msgs::Set(&msg, ignition::math::Planed(
+ ignition::math::Vector3d(0, 0, 1),
+ ignition::math::Vector2d(123, 456), 1.0));
EXPECT_DOUBLE_EQ(0, msg.normal().x());
EXPECT_DOUBLE_EQ(0, msg.normal().y());
@@ -351,7 +411,7 @@ TEST_F(MsgsTest, GUIFromSDF)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("gui.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<gui fullscreen='true'>\
<camera name='camera'>\
@@ -364,7 +424,7 @@ TEST_F(MsgsTest, GUIFromSDF)
</track_visual>\
</camera>\
</gui>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::GUI msg = msgs::GUIFromSDF(sdf);
}
@@ -372,7 +432,7 @@ TEST_F(MsgsTest, GUIFromSDF_EmptyTrackVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("gui.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<gui fullscreen='true'>\
<camera name='camera'>\
@@ -385,7 +445,7 @@ TEST_F(MsgsTest, GUIFromSDF_EmptyTrackVisual)
</track_visual>\
</camera>\
</gui>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::GUI msg = msgs::GUIFromSDF(sdf);
}
@@ -393,13 +453,13 @@ TEST_F(MsgsTest, GUIFromSDF_WithEmptyCamera)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("gui.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<gui fullscreen='true'>\
<camera name='camera'>\
</camera>\
</gui>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::GUI msg = msgs::GUIFromSDF(sdf);
}
@@ -407,11 +467,11 @@ TEST_F(MsgsTest, GUIFromSDF_WithoutCamera)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("gui.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<gui fullscreen='true'>\
</gui>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::GUI msg = msgs::GUIFromSDF(sdf);
}
@@ -419,7 +479,7 @@ TEST_F(MsgsTest, LightFromSDF_ListDirectional)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("light.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<light type='directional' name='sun'>\
<cast_shadows>true</cast_shadows>\
@@ -429,12 +489,12 @@ TEST_F(MsgsTest, LightFromSDF_ListDirectional)
<attenuation>\
<range>20</range>\
<constant>0.8</constant>\
- <linear>0.01</liner>\
+ <linear>0.01</linear>\
<quadratic>0.0</quadratic>\
</attenuation>\
<direction>1.0 1.0 -1.0</direction>\
</light>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Light msg = msgs::LightFromSDF(sdf);
}
@@ -442,7 +502,7 @@ TEST_F(MsgsTest, LightFromSDF_LightSpot)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("light.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<light type='spot' name='lamp'>\
<pose>0 0 10 0 0 0</pose>\
@@ -461,7 +521,7 @@ TEST_F(MsgsTest, LightFromSDF_LightSpot)
</attenuation>\
<direction>1.0 1.0 -1.0</direction>\
</light>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Light msg = msgs::LightFromSDF(sdf);
}
@@ -469,7 +529,7 @@ TEST_F(MsgsTest, LightFromSDF_LightPoint)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("light.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<light type='point' name='lamp'>\
<pose>0 0 10 0 0 0</pose>\
@@ -482,7 +542,7 @@ TEST_F(MsgsTest, LightFromSDF_LightPoint)
<quadratic>0.0</quadratic>\
</attenuation>\
</light>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Light msg = msgs::LightFromSDF(sdf);
}
@@ -490,11 +550,11 @@ TEST_F(MsgsTest, LightFromSDF_LighBadType)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("light.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<light type='_bad_' name='lamp'>\
</light>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Light msg = msgs::LightFromSDF(sdf);
}
@@ -503,7 +563,7 @@ TEST_F(MsgsTest, VisualFromSDF_PlaneVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
@@ -512,7 +572,7 @@ TEST_F(MsgsTest, VisualFromSDF_PlaneVisual)
</geometry>\
<material><script>Gazebo/Grey</script></material>\
</visual>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
@@ -520,7 +580,7 @@ TEST_F(MsgsTest, VisualFromSDF_BoxVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
@@ -529,7 +589,7 @@ TEST_F(MsgsTest, VisualFromSDF_BoxVisual)
</geometry>\
<material><script>Gazebo/Grey'</script></material>\
</visual>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
@@ -537,7 +597,7 @@ TEST_F(MsgsTest, VisualFromSDF_SphereVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
@@ -550,7 +610,7 @@ TEST_F(MsgsTest, VisualFromSDF_SphereVisual)
</shader>\
</material>\
</visual>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
@@ -558,18 +618,18 @@ TEST_F(MsgsTest, VisualFromSDF_CylinderVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
<geometry>\
- <cylinder><radius>1</radius><length>1.0</length></cylinder\
+ <cylinder><radius>1</radius><length>1.0</length></cylinder>\
</geometry>\
<material><script>Gazebo/Grey</script>\
<shader type='normal_map_object_space'/>\
</material>\
</visual>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
@@ -577,7 +637,7 @@ TEST_F(MsgsTest, VisualFromSDF_MeshVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
@@ -588,7 +648,7 @@ TEST_F(MsgsTest, VisualFromSDF_MeshVisual)
<shader type='vertex'/>\
</material>\
</visual>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
@@ -596,7 +656,7 @@ TEST_F(MsgsTest, VisualFromSDF_ImageVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
@@ -606,9 +666,9 @@ TEST_F(MsgsTest, VisualFromSDF_ImageVisual)
<scale>1</scale>\
<height>1</height>\
<threshold>255</threshold>\
- <granularity>10</granularit>\
+ <granularity>10</granularity>\
<uri>test2.mesh</uri>\
- <image>\
+ </image>\
</geometry>\
<material>\
<script>Gazebo/Grey</script>\
@@ -619,7 +679,7 @@ TEST_F(MsgsTest, VisualFromSDF_ImageVisual)
<emissive>.1 .2 .3 1</emissive>\
</material>\
</visual>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
@@ -627,7 +687,7 @@ TEST_F(MsgsTest, VisualFromSDF_HeigthmapVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<cast_shadows>false</cast_shadows>\
@@ -643,7 +703,7 @@ TEST_F(MsgsTest, VisualFromSDF_HeigthmapVisual)
<shader type='pixel'/>\
</material>\
</visual>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Visual msg = msgs::VisualFromSDF(sdf);
}
@@ -651,11 +711,11 @@ TEST_F(MsgsTest, VisualFromSDF_NoGeometry)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
</visual>\
- </gazebo>", sdf);
+ </sdf>", sdf));
EXPECT_THROW(msgs::Visual msg = msgs::VisualFromSDF(sdf),
common::Exception);
}
@@ -664,7 +724,7 @@ TEST_F(MsgsTest, VisualFromSDF_ShaderTypeThrow)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<pose>1 1 1 1 2 3</pose>\
@@ -675,28 +735,30 @@ TEST_F(MsgsTest, VisualFromSDF_ShaderTypeThrow)
<pos>0 0 0</pos>\
</heightmap>\
</geometry>\
- <shader type='throw'/>\
+ <material><script>Gazebo/Grey</script>\
+ <shader type='throw'/>\
</material>\
</visual>\
- </gazebo>", sdf);
- msgs::Visual msg = msgs::VisualFromSDF(sdf);
+ </sdf>", sdf));
+ EXPECT_THROW(msgs::Visual msg = msgs::VisualFromSDF(sdf),
+ common::Exception);
}
TEST_F(MsgsTest, VisualFromSDF_BadGeometryVisual)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<pose>1 1 1 1 2 3</pose>\
<geometry>\
</geometry>\
<material><script>Gazebo/Grey</script>\
- <shader type='pixel'/>\
+ <shader type='pixel'/>\
</material>\
</visual>\
- </gazebo>", sdf);
+ </sdf>", sdf));
EXPECT_THROW(msgs::Visual msg = msgs::VisualFromSDF(sdf),
common::Exception);
}
@@ -705,7 +767,10 @@ TEST_F(MsgsTest, VisualFromSDF_BadGeometryType)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("visual.sdf", sdf);
- EXPECT_FALSE(sdf::readString(
+ // As of sdformat pull request 148 (released in version 2.3.1),
+ // unknown elements are now ignored with a warning message
+ // rather than causing an error.
+ EXPECT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
<pose>1 1 1 1 2 3</pose>\
@@ -713,16 +778,16 @@ TEST_F(MsgsTest, VisualFromSDF_BadGeometryType)
<bad_type/>\
</geometry>\
<material><script>Gazebo/Grey</script>\
- <shader type='pixel'/>\
+ <shader type='pixel'/>\
</material>\
</visual>\
- </gazebo>", sdf));
+ </sdf>", sdf));
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<visual name='visual'>\
</visual>\
- </gazebo>", sdf);
+ </sdf>", sdf));
sdf::ElementPtr badElement(new sdf::Element());
badElement->SetName("bad_type");
@@ -735,17 +800,22 @@ TEST_F(MsgsTest, VisualFromSDF_BadFogType)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
<ambient>0.1 0.1 0.1 1</ambient>\
<background>0 0 0 1</background>\
<shadows>true</shadows>\
- <fog><color>1 1 1 1</color> <type>throw</type>\
- <start>0</start> <end>10</end> <density>1</density> </fog>\
+ <fog>\
+ <color>1 1 1 1</color>\
+ <type>throw</type>\
+ <start>0</start>\
+ <end>10</end>\
+ <density>1</density>\
+ </fog>\
<grid>false</grid>\
</scene>\
- </gazebo>", sdf);
+ </sdf>", sdf));
EXPECT_THROW(msgs::Scene msg = msgs::SceneFromSDF(sdf), common::Exception);
}
@@ -753,16 +823,22 @@ TEST_F(MsgsTest, VisualSceneFromSDF_A)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
<ambient>0.1 0.1 0.1 1</ambient>\
<background>0 0 0 1</background>\
<shadows>true</shadows>\
- <fog>1 1 1 1' type='linear' start='0' end='10' density='1'/>\
+ <fog>\
+ <color>1 1 1 1</color>\
+ <type>linear</type>\
+ <start>0</start>\
+ <end>10</end>\
+ <density>1</density>\
+ </fog>\
<grid>false</grid>\
</scene>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Scene msg = msgs::SceneFromSDF(sdf);
}
@@ -770,16 +846,21 @@ TEST_F(MsgsTest, VisualSceneFromSDF_B)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
<ambient>0.1 0.1 0.1 1</ambient>\
<background>0 0 0 1</background>\
<shadows>false</shadows>\
- <fog><color>1 1 1 1</color><type>exp</type><start>0</start>\
- <end>10</end><density>1<density/>\
+ <fog>\
+ <color>1 1 1 1</color>\
+ <type>exp</type>\
+ <start>0</start>\
+ <end>10</end>\
+ <density>1</density>\
+ </fog>\
</scene>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Scene msg = msgs::SceneFromSDF(sdf);
}
@@ -787,18 +868,22 @@ TEST_F(MsgsTest, VisualSceneFromSDF_C)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
<ambient>0.1 0.1 0.1 1</ambient>\
<background>0 0 0 1</background>\
<shadows>false</shadows>\
- <fog><color>1 1 1 1</color>\
- <type>exp2</type><start>0</start><end>10</end>\
- <density>1</density>\
+ <fog>\
+ <color>1 1 1 1</color>\
+ <type>exp2</type>\
+ <start>0</start>\
+ <end>10</end>\
+ <density>1</density>\
+ </fog>\
<grid>true</grid>\
</scene>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Scene msg = msgs::SceneFromSDF(sdf);
}
@@ -806,11 +891,11 @@ TEST_F(MsgsTest, VisualSceneFromSDF_CEmpty)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
</scene>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Scene msg = msgs::SceneFromSDF(sdf);
}
@@ -818,12 +903,12 @@ TEST_F(MsgsTest, VisualSceneFromSDF_CEmptyNoSky)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("scene.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<scene>\
<background>0 0 0 1</background>\
</scene>\
- </gazebo>", sdf);
+ </sdf>", sdf));
msgs::Scene msg = msgs::SceneFromSDF(sdf);
}
@@ -832,7 +917,7 @@ TEST_F(MsgsTest, MeshFromSDF)
{
sdf::ElementPtr sdf(new sdf::Element());
sdf::initFile("geometry.sdf", sdf);
- sdf::readString(
+ ASSERT_TRUE(sdf::readString(
"<sdf version='" SDF_VERSION "'>\
<geometry>\
<mesh>\
@@ -844,8 +929,7 @@ TEST_F(MsgsTest, MeshFromSDF)
</submesh>\
</mesh>\
</geometry>\
- </visual>\
- </sdf>", sdf);
+ </sdf>", sdf));
msgs::MeshGeom msg = msgs::MeshFromSDF(sdf->GetElement("mesh"));
EXPECT_TRUE(msg.has_filename());
@@ -864,43 +948,203 @@ TEST_F(MsgsTest, MeshFromSDF)
}
/////////////////////////////////////////////////
+TEST_F(MsgsTest, AxisFromSDF)
+{
+ sdf::ElementPtr sdf(new sdf::Element());
+ sdf::initFile("joint.sdf", sdf);
+ ASSERT_TRUE(sdf::readString(
+ "<sdf version='" SDF_VERSION "'>\
+ <joint name='arm' type='revolute'>\
+ <parent>arm_base</parent>\
+ <child>arm_shoulder</child>\
+ <axis>\
+ <xyz>0 0 1</xyz>\
+ <limit>\
+ <lower>0.01</lower>\
+ <upper>9</upper>\
+ <effort>2.2</effort>\
+ <velocity>0.1</velocity>\
+ </limit>\
+ <use_parent_model_frame>false</use_parent_model_frame>\
+ <dynamics>\
+ <damping>0.1</damping>\
+ <friction>0.2</friction>\
+ </dynamics>\
+ </axis>\
+ </joint>\
+ </sdf>", sdf));
+ msgs::Axis msg = msgs::AxisFromSDF(sdf->GetElement("axis"));
+
+ EXPECT_TRUE(msg.has_xyz());
+ EXPECT_EQ(msgs::ConvertIgn(msg.xyz()), ignition::math::Vector3d(0, 0, 1));
+ EXPECT_TRUE(msg.has_limit_lower());
+ EXPECT_NEAR(msg.limit_lower(), 0.01, 1e-6);
+ EXPECT_TRUE(msg.has_limit_upper());
+ EXPECT_NEAR(msg.limit_upper(), 9, 1e-6);
+ EXPECT_TRUE(msg.has_limit_effort());
+ EXPECT_NEAR(msg.limit_effort(), 2.2, 1e-6);
+ EXPECT_TRUE(msg.has_limit_velocity());
+ EXPECT_NEAR(msg.limit_velocity(), 0.1, 1e-6);
+ EXPECT_TRUE(msg.has_use_parent_model_frame());
+ EXPECT_EQ(msg.use_parent_model_frame(), false);
+ EXPECT_TRUE(msg.has_damping());
+ EXPECT_NEAR(msg.damping(), 0.1, 1e-6);
+ EXPECT_TRUE(msg.has_friction());
+ EXPECT_NEAR(msg.friction(), 0.2, 1e-6);
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, JointFromSDF)
+{
+ sdf::ElementPtr sdf(new sdf::Element());
+ sdf::initFile("joint.sdf", sdf);
+ ASSERT_TRUE(sdf::readString(
+ "<sdf version='" SDF_VERSION "'>\
+ <joint name='arm' type='revolute'>\
+ <pose>1 2 3 0 1.57 0</pose>\
+ <parent>arm_base</parent>\
+ <child>arm_shoulder</child>\
+ <axis>\
+ <xyz>1 0 0</xyz>\
+ <limit>\
+ <lower>0.1</lower>\
+ <upper>3.14</upper>\
+ <effort>2.4</effort>\
+ <velocity>0.4</velocity>\
+ </limit>\
+ <use_parent_model_frame>true</use_parent_model_frame>\
+ <dynamics>\
+ <damping>1.0</damping>\
+ <friction>0.1</friction>\
+ </dynamics>\
+ </axis>\
+ <physics>\
+ <ode>\
+ <cfm>0.2</cfm>\
+ <bounce>0.1</bounce>\
+ <velocity>1.1</velocity>\
+ <fudge_factor>0.4</fudge_factor>\
+ <limit>\
+ <cfm>0.0</cfm>\
+ <erp>0.9</erp>\
+ </limit>\
+ <suspension>\
+ <cfm>0.1</cfm>\
+ <erp>0.3</erp>\
+ </suspension>\
+ </ode>\
+ </physics>\
+ </joint>\
+ </sdf>", sdf));
+ msgs::Joint msg = msgs::JointFromSDF(sdf);
+
+ EXPECT_TRUE(msg.has_name());
+ EXPECT_EQ(msg.name(), "arm");
+ EXPECT_TRUE(msg.has_type());
+ EXPECT_EQ(msgs::ConvertJointType(msg.type()), "revolute");
+ EXPECT_TRUE(msg.has_pose());
+ EXPECT_EQ(msgs::ConvertIgn(msg.pose()),
+ ignition::math::Pose3d(1, 2, 3, 0, 1.57, 0));
+ EXPECT_TRUE(msg.has_parent());
+ EXPECT_EQ(msg.parent(), "arm_base");
+ EXPECT_TRUE(msg.has_child());
+ EXPECT_EQ(msg.child(), "arm_shoulder");
+ EXPECT_TRUE(msg.has_cfm());
+ EXPECT_NEAR(msg.cfm(), 0.2, 1e-6);
+ EXPECT_TRUE(msg.has_bounce());
+ EXPECT_NEAR(msg.bounce(), 0.1, 1e-6);
+ EXPECT_TRUE(msg.has_velocity());
+ EXPECT_NEAR(msg.velocity(), 1.1, 1e-6);
+ EXPECT_TRUE(msg.has_fudge_factor());
+ EXPECT_NEAR(msg.fudge_factor(), 0.4, 1e-6);
+ EXPECT_TRUE(msg.has_limit_cfm());
+ EXPECT_NEAR(msg.limit_cfm(), 0.0, 1e-6);
+ EXPECT_TRUE(msg.has_limit_erp());
+ EXPECT_NEAR(msg.limit_erp(), 0.9, 1e-6);
+ EXPECT_TRUE(msg.has_suspension_cfm());
+ EXPECT_NEAR(msg.suspension_cfm(), 0.1, 1e-6);
+ EXPECT_TRUE(msg.has_suspension_erp());
+ EXPECT_NEAR(msg.suspension_erp(), 0.3, 1e-6);
+
+ EXPECT_TRUE(msg.has_axis1());
+ EXPECT_TRUE(!msg.has_axis2());
+ const msgs::Axis axisMsg = msg.axis1();
+ EXPECT_TRUE(axisMsg.has_xyz());
+ EXPECT_EQ(msgs::ConvertIgn(axisMsg.xyz()), ignition::math::Vector3d(1, 0, 0));
+ EXPECT_TRUE(axisMsg.has_limit_lower());
+ EXPECT_NEAR(axisMsg.limit_lower(), 0.1, 1e-6);
+ EXPECT_TRUE(axisMsg.has_limit_upper());
+ EXPECT_NEAR(axisMsg.limit_upper(), 3.14, 1e-6);
+ EXPECT_TRUE(axisMsg.has_limit_effort());
+ EXPECT_NEAR(axisMsg.limit_effort(), 2.4, 1e-6);
+ EXPECT_TRUE(axisMsg.has_limit_velocity());
+ EXPECT_NEAR(axisMsg.limit_velocity(), 0.4, 1e-6);
+ EXPECT_TRUE(axisMsg.has_use_parent_model_frame());
+ EXPECT_EQ(axisMsg.use_parent_model_frame(), true);
+ EXPECT_TRUE(axisMsg.has_damping());
+ EXPECT_NEAR(axisMsg.damping(), 1.0, 1e-6);
+ EXPECT_TRUE(axisMsg.has_friction());
+ EXPECT_NEAR(axisMsg.friction(), 0.1, 1e-6);
+}
+
+/////////////////////////////////////////////////
TEST_F(MsgsTest, LinkToSDF)
{
+ const std::string name("test_link");
+ const math::Pose pose(math::Vector3(3, 2, 1),
+ math::Quaternion(0.5, -0.5, -0.5, 0.5));
+
msgs::Link linkMsg;
- linkMsg.set_name("test_link");
+ linkMsg.set_name(name);
linkMsg.set_self_collide(false);
linkMsg.set_gravity(true);
linkMsg.set_kinematic(false);
- msgs::Set(linkMsg.mutable_pose(), math::Pose(math::Vector3(3, 2, 1),
- math::Quaternion(0.5, -0.5, -0.5, 0.5)));
+ msgs::Set(linkMsg.mutable_pose(), pose.Ign());
+
+ const double laserRetro1 = 0.4;
+ const double laserRetro2 = 0.5;
// collision - see CollisionToSDF for a more detailed test
- msgs::Collision *collisionMsg1 = linkMsg.add_collision();
- collisionMsg1->set_laser_retro(0.4);
+ auto collisionMsg1 = linkMsg.add_collision();
+ collisionMsg1->set_laser_retro(laserRetro1);
collisionMsg1->set_max_contacts(100);
- msgs::Collision *collisionMsg2 = linkMsg.add_collision();
- collisionMsg2->set_laser_retro(0.5);
+ auto collisionMsg2 = linkMsg.add_collision();
+ collisionMsg2->set_laser_retro(laserRetro2);
collisionMsg2->set_max_contacts(300);
+ // visual - see VisualToSDF for a more detailed test
+ auto visualMsg1 = linkMsg.add_visual();
+ visualMsg1->set_laser_retro(laserRetro1);
+
+ auto visualMsg2 = linkMsg.add_visual();
+ visualMsg2->set_laser_retro(laserRetro2);
+
// inertial - see InertialToSDF for a more detailed test
- msgs::Inertial *inertialMsg = linkMsg.mutable_inertial();
+ auto inertialMsg = linkMsg.mutable_inertial();
inertialMsg->set_mass(3.5);
sdf::ElementPtr linkSDF = msgs::LinkToSDF(linkMsg);
- EXPECT_STREQ(linkSDF->Get<std::string>("name").c_str(), "test_link");
+ EXPECT_EQ(linkSDF->Get<std::string>("name"), name);
EXPECT_FALSE(linkSDF->Get<bool>("self_collide"));
EXPECT_TRUE(linkSDF->Get<bool>("gravity"));
EXPECT_FALSE(linkSDF->Get<bool>("kinematic"));
+ EXPECT_EQ(pose, linkSDF->Get<math::Pose>("pose"));
sdf::ElementPtr collisionElem1 = linkSDF->GetElement("collision");
- EXPECT_DOUBLE_EQ(collisionElem1->Get<double>("laser_retro"), 0.4);
+ EXPECT_DOUBLE_EQ(collisionElem1->Get<double>("laser_retro"), laserRetro1);
EXPECT_DOUBLE_EQ(collisionElem1->Get<double>("max_contacts"), 100);
sdf::ElementPtr collisionElem2 = collisionElem1->GetNextElement("collision");
- EXPECT_DOUBLE_EQ(collisionElem2->Get<double>("laser_retro"), 0.5);
+ EXPECT_DOUBLE_EQ(collisionElem2->Get<double>("laser_retro"), laserRetro2);
EXPECT_DOUBLE_EQ(collisionElem2->Get<double>("max_contacts"), 300);
+ sdf::ElementPtr visualElem1 = linkSDF->GetElement("visual");
+ EXPECT_DOUBLE_EQ(visualElem1->Get<double>("laser_retro"), laserRetro1);
+
+ sdf::ElementPtr visualElem2 = visualElem1->GetNextElement("visual");
+ EXPECT_DOUBLE_EQ(visualElem2->Get<double>("laser_retro"), laserRetro2);
+
sdf::ElementPtr inertialElem = linkSDF->GetElement("inertial");
EXPECT_DOUBLE_EQ(inertialElem->Get<double>("mass"), 3.5);
}
@@ -908,11 +1152,15 @@ TEST_F(MsgsTest, LinkToSDF)
/////////////////////////////////////////////////
TEST_F(MsgsTest, CollisionToSDF)
{
+ const std::string name("collision");
+
msgs::Collision collisionMsg;
+ collisionMsg.set_name(name);
collisionMsg.set_laser_retro(0.2);
collisionMsg.set_max_contacts(5);
- msgs::Set(collisionMsg.mutable_pose(), math::Pose(math::Vector3(1, 2, 3),
- math::Quaternion(0, 0, 1, 0)));
+ msgs::Set(collisionMsg.mutable_pose(),
+ ignition::math::Pose3d(ignition::math::Vector3d(1, 2, 3),
+ ignition::math::Quaterniond(0, 0, 1, 0)));
// geometry - see GeometryToSDF for a more detailed test
msgs::Geometry *geomMsg = collisionMsg.mutable_geometry();
@@ -927,6 +1175,10 @@ TEST_F(MsgsTest, CollisionToSDF)
surfaceMsg->set_bounce_threshold(1300);
sdf::ElementPtr collisionSDF = msgs::CollisionToSDF(collisionMsg);
+
+ EXPECT_TRUE(collisionSDF->HasAttribute("name"));
+ EXPECT_EQ(name, collisionSDF->Get<std::string>("name"));
+
EXPECT_DOUBLE_EQ(collisionSDF->Get<double>("laser_retro"), 0.2);
EXPECT_DOUBLE_EQ(collisionSDF->Get<double>("max_contacts"), 5);
@@ -945,13 +1197,82 @@ TEST_F(MsgsTest, CollisionToSDF)
}
/////////////////////////////////////////////////
+TEST_F(MsgsTest, VisualToSDF)
+{
+ const std::string name("visual");
+ const double laserRetro = 0.2;
+ const math::Pose pose(math::Vector3(1, 2, 3), math::Quaternion(0, 0, 1, 0));
+ const double radius = 3.3;
+ const std::string materialName("Gazebo/Grey");
+ const std::string uri("pretend_this_is_a_URI");
+
+ msgs::Visual visualMsg;
+ visualMsg.set_name(name);
+ visualMsg.set_laser_retro(laserRetro);
+ msgs::Set(visualMsg.mutable_pose(), pose.Ign());
+
+ // geometry - see GeometryToSDF for a more detailed test
+ auto geomMsg = visualMsg.mutable_geometry();
+ geomMsg->set_type(msgs::Geometry::SPHERE);
+ geomMsg->mutable_sphere()->set_radius(radius);
+
+ // material - see MaterialToSDF for a more detailed test
+ auto scriptMsg = visualMsg.mutable_material()->mutable_script();
+ scriptMsg->set_name(materialName);
+ scriptMsg->add_uri();
+ scriptMsg->set_uri(0, uri);
+
+ sdf::ElementPtr visualSDF = msgs::VisualToSDF(visualMsg);
+
+ EXPECT_TRUE(visualSDF->HasAttribute("name"));
+ EXPECT_EQ(name, visualSDF->Get<std::string>("name"));
+
+ EXPECT_DOUBLE_EQ(visualSDF->Get<double>("laser_retro"), laserRetro);
+
+ EXPECT_EQ(pose, visualSDF->Get<math::Pose>("pose"));
+
+ ASSERT_TRUE(visualSDF->HasElement("geometry"));
+ sdf::ElementPtr geomElem = visualSDF->GetElement("geometry");
+ EXPECT_TRUE(geomElem->HasElement("sphere"));
+ sdf::ElementPtr sphereElem = geomElem->GetElement("sphere");
+ EXPECT_TRUE(sphereElem->HasElement("radius"));
+ EXPECT_DOUBLE_EQ(sphereElem->Get<double>("radius"), radius);
+
+ ASSERT_TRUE(visualSDF->HasElement("material"));
+ sdf::ElementPtr materialElem = visualSDF->GetElement("material");
+ EXPECT_TRUE(materialElem->HasElement("script"));
+ sdf::ElementPtr scriptElem = materialElem->GetElement("script");
+ EXPECT_TRUE(scriptElem->HasElement("name"));
+ EXPECT_EQ(materialName, scriptElem->Get<std::string>("name"));
+ EXPECT_TRUE(scriptElem->HasElement("uri"));
+ EXPECT_EQ(uri, scriptElem->Get<std::string>("uri"));
+
+ // Test the meta.layer property
+ {
+ msgs::Visual msg;
+ auto metaMsg = msg.mutable_meta();
+ metaMsg->set_layer(1);
+
+ sdf::ElementPtr visSdf = msgs::VisualToSDF(msg);
+ EXPECT_TRUE(visSdf->HasElement("meta"));
+ EXPECT_TRUE(visSdf->GetElement("meta")->HasElement("layer"));
+ EXPECT_EQ(visSdf->GetElement("meta")->Get<int32_t>("layer"), 1);
+
+ msgs::Visual msg2 = msgs::VisualFromSDF(visSdf);
+ EXPECT_TRUE(msg2.has_meta());
+ EXPECT_TRUE(msg2.meta().has_layer());
+ EXPECT_EQ(msg2.meta().layer(), 1);
+ }
+}
+
+/////////////////////////////////////////////////
TEST_F(MsgsTest, GeometryToSDF)
{
// box
msgs::Geometry boxMsg;
boxMsg.set_type(msgs::Geometry::BOX);
msgs::BoxGeom *boxGeom = boxMsg.mutable_box();
- msgs::Set(boxGeom->mutable_size(), math::Vector3(0.5, 0.75, 1.0));
+ msgs::Set(boxGeom->mutable_size(), ignition::math::Vector3d(0.5, 0.75, 1.0));
sdf::ElementPtr boxSDF = msgs::GeometryToSDF(boxMsg);
sdf::ElementPtr boxElem = boxSDF->GetElement("box");
@@ -984,8 +1305,8 @@ TEST_F(MsgsTest, GeometryToSDF)
msgs::Geometry planeMsg;
planeMsg.set_type(msgs::Geometry::PLANE);
msgs::PlaneGeom *planeGeom = planeMsg.mutable_plane();
- msgs::Set(planeGeom->mutable_normal(), math::Vector3(0, 0, 1.0));
- msgs::Set(planeGeom->mutable_size(), math::Vector2d(0.5, 0.8));
+ msgs::Set(planeGeom->mutable_normal(), ignition::math::Vector3d(0, 0, 1.0));
+ msgs::Set(planeGeom->mutable_size(), ignition::math::Vector2d(0.5, 0.8));
sdf::ElementPtr planeSDF = msgs::GeometryToSDF(planeMsg);
sdf::ElementPtr planeElem = planeSDF->GetElement("plane");
@@ -1017,8 +1338,10 @@ TEST_F(MsgsTest, GeometryToSDF)
heightmapMsg.set_type(msgs::Geometry::HEIGHTMAP);
msgs::HeightmapGeom *heightmapGeom = heightmapMsg.mutable_heightmap();
heightmapGeom->set_filename("test_heightmap_filename");
- msgs::Set(heightmapGeom->mutable_size(), math::Vector3(100, 200, 30));
- msgs::Set(heightmapGeom->mutable_origin(), math::Vector3(50, 100, 15));
+ msgs::Set(heightmapGeom->mutable_size(),
+ ignition::math::Vector3d(100, 200, 30));
+ msgs::Set(heightmapGeom->mutable_origin(),
+ ignition::math::Vector3d(50, 100, 15));
heightmapGeom->set_use_terrain_paging(true);
msgs::HeightmapGeom_Texture *texture1 = heightmapGeom->add_texture();
@@ -1067,7 +1390,7 @@ TEST_F(MsgsTest, GeometryToSDF)
meshMsg.set_type(msgs::Geometry::MESH);
msgs::MeshGeom *meshGeom = meshMsg.mutable_mesh();
meshGeom->set_filename("test_mesh_filename");
- msgs::Set(meshGeom->mutable_scale(), math::Vector3(2.3, 1.2, 2.9));
+ msgs::Set(meshGeom->mutable_scale(), ignition::math::Vector3d(2.3, 1.2, 2.9));
meshGeom->set_submesh("test_mesh_submesh");
meshGeom->set_center_submesh(false);
@@ -1085,11 +1408,11 @@ TEST_F(MsgsTest, GeometryToSDF)
// polyline
msgs::Geometry polylineMsg;
polylineMsg.set_type(msgs::Geometry::POLYLINE);
- msgs::Polyline *polylineGeom = polylineMsg.mutable_polyline();
+ msgs::Polyline *polylineGeom = polylineMsg.add_polyline();
polylineGeom->set_height(2.33);
- msgs::Set(polylineGeom->add_point(), math::Vector2d(0.5, 0.7));
- msgs::Set(polylineGeom->add_point(), math::Vector2d(3.5, 4.7));
- msgs::Set(polylineGeom->add_point(), math::Vector2d(1000, 2000));
+ msgs::Set(polylineGeom->add_point(), ignition::math::Vector2d(0.5, 0.7));
+ msgs::Set(polylineGeom->add_point(), ignition::math::Vector2d(3.5, 4.7));
+ msgs::Set(polylineGeom->add_point(), ignition::math::Vector2d(1000, 2000));
sdf::ElementPtr polylineSDF = msgs::GeometryToSDF(polylineMsg);
sdf::ElementPtr polylineElem = polylineSDF->GetElement("polyline");
@@ -1108,7 +1431,7 @@ TEST_F(MsgsTest, MeshToSDF)
{
msgs::MeshGeom msg;
msg.set_filename("test_filename");
- msgs::Set(msg.mutable_scale(), math::Vector3(0.1, 0.2, 0.3));
+ msgs::Set(msg.mutable_scale(), ignition::math::Vector3d(0.1, 0.2, 0.3));
msg.set_submesh("test_submesh");
msg.set_center_submesh(true);
@@ -1128,33 +1451,113 @@ TEST_F(MsgsTest, MeshToSDF)
/////////////////////////////////////////////////
TEST_F(MsgsTest, InertialToSDF)
{
+ const double mass = 3.4;
+ const math::Pose pose = math::Pose(math::Vector3(1.2, 3.4, 5.6),
+ math::Quaternion(0.7071, 0.0, 0.7071, 0.0));
+ const double ixx = 0.0133;
+ const double ixy = -0.0003;
+ const double ixz = -0.0004;
+ const double iyy = 0.0116;
+ const double iyz = 0.0008;
+ const double izz = 0.0038;
+
msgs::Inertial msg;
- msg.set_mass(3.4);
- msgs::Set(msg.mutable_pose(), math::Pose(math::Vector3(1.2, 3.4, 5.6),
- math::Quaternion(0.7071, 0.0, 0.7071, 0.0)));
- msg.set_ixx(0.0133);
- msg.set_ixy(-0.0003);
- msg.set_ixz(-0.0004);
- msg.set_iyy(0.0116);
- msg.set_iyz(0.0008);
- msg.set_izz(0.0038);
+ msg.set_mass(mass);
+ msgs::Set(msg.mutable_pose(), pose.Ign());
+ msg.set_ixx(ixx);
+ msg.set_ixy(ixy);
+ msg.set_ixz(ixz);
+ msg.set_iyy(iyy);
+ msg.set_iyz(iyz);
+ msg.set_izz(izz);
sdf::ElementPtr inertialSDF = msgs::InertialToSDF(msg);
- EXPECT_DOUBLE_EQ(inertialSDF->Get<double>("mass"), 3.4);
+ EXPECT_TRUE(inertialSDF->HasElement("mass"));
+ EXPECT_DOUBLE_EQ(inertialSDF->Get<double>("mass"), mass);
+
+ EXPECT_TRUE(inertialSDF->HasElement("pose"));
+ EXPECT_EQ(inertialSDF->Get<math::Pose>("pose"), pose);
+
+ {
+ ASSERT_TRUE(inertialSDF->HasElement("inertia"));
+ sdf::ElementPtr inertiaElem = inertialSDF->GetElement("inertia");
+
+ EXPECT_TRUE(inertiaElem->HasElement("ixx"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixx"), ixx);
+
+ EXPECT_TRUE(inertiaElem->HasElement("ixy"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixy"), ixy);
+
+ EXPECT_TRUE(inertiaElem->HasElement("ixz"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixz"), ixz);
+
+ EXPECT_TRUE(inertiaElem->HasElement("iyy"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("iyy"), iyy);
+
+ EXPECT_TRUE(inertiaElem->HasElement("iyz"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("iyz"), iyz);
+
+ EXPECT_TRUE(inertiaElem->HasElement("izz"));
+ EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("izz"), izz);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, MaterialToSDF)
+{
+ msgs::Material msg;
+
+ const std::string name("Gazebo/Grey");
+ const std::string uri("file://media/materials/scripts/gazebo.material");
+ const msgs::Material::ShaderType type = msgs::Material::VERTEX;
+ const std::string normalMap("normalMap");
+ const bool lighting = true;
+ const common::Color ambient(.1, .2, .3, 1.0);
+ const common::Color diffuse(.4, .5, .6, 1.0);
+ const common::Color emissive(.5, .5, .5, 0.5);
+ const common::Color specular(.7, .8, .9, 1.0);
+
+ msg.mutable_script()->set_name(name);
+ msg.mutable_script()->add_uri();
+ msg.mutable_script()->set_uri(0, uri);
+ msg.set_shader_type(type);
+ msg.set_normal_map(normalMap);
+ msg.set_lighting(lighting);
+ msgs::Set(msg.mutable_ambient(), ambient);
+ msgs::Set(msg.mutable_diffuse(), diffuse);
+ msgs::Set(msg.mutable_emissive(), emissive);
+ msgs::Set(msg.mutable_specular(), specular);
+
+ sdf::ElementPtr materialSDF = msgs::MaterialToSDF(msg);
+
+ {
+ ASSERT_TRUE(materialSDF->HasElement("script"));
+ sdf::ElementPtr scriptElem = materialSDF->GetElement("script");
+ EXPECT_TRUE(scriptElem->HasElement("name"));
+ EXPECT_EQ(name, scriptElem->Get<std::string>("name"));
+ EXPECT_TRUE(scriptElem->HasElement("uri"));
+ EXPECT_EQ(uri, scriptElem->Get<std::string>("uri"));
+ }
- EXPECT_TRUE(inertialSDF->Get<math::Pose>("pose") ==
- math::Pose(math::Vector3(1.2, 3.4, 5.6),
- math::Quaternion(0.7071, 0.0, 0.7071, 0.0)));
+ {
+ ASSERT_TRUE(materialSDF->HasElement("shader"));
+ sdf::ElementPtr shaderElem = materialSDF->GetElement("shader");
+ EXPECT_TRUE(shaderElem->HasAttribute("type"));
+ EXPECT_EQ(msgs::ConvertShaderType(type),
+ shaderElem->Get<std::string>("type"));
+ EXPECT_TRUE(shaderElem->HasElement("normal_map"));
+ EXPECT_EQ(normalMap, shaderElem->Get<std::string>("normal_map"));
+ }
- // inertia
- sdf::ElementPtr inertiaElem = inertialSDF->GetElement("inertia");
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixx"), 0.0133);
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixy"), -0.0003);
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("ixz"), -0.0004);
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("iyy"), 0.0116);
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("iyz"), 0.0008);
- EXPECT_DOUBLE_EQ(inertiaElem->Get<double>("izz"), 0.0038);
+ EXPECT_TRUE(materialSDF->HasElement("ambient"));
+ EXPECT_EQ(ambient, materialSDF->Get<common::Color>("ambient"));
+ EXPECT_TRUE(materialSDF->HasElement("diffuse"));
+ EXPECT_EQ(diffuse, materialSDF->Get<common::Color>("diffuse"));
+ EXPECT_TRUE(materialSDF->HasElement("emissive"));
+ EXPECT_EQ(emissive, materialSDF->Get<common::Color>("emissive"));
+ EXPECT_TRUE(materialSDF->HasElement("specular"));
+ EXPECT_EQ(specular, materialSDF->Get<common::Color>("specular"));
}
/////////////////////////////////////////////////
@@ -1163,12 +1566,18 @@ TEST_F(MsgsTest, SurfaceToSDF)
msgs::Surface msg;
// friction
+ const double mu = 0.1;
+ const double mu2 = 0.2;
+ const math::Vector3 fdir1(0.3, 0.4, 0.5);
+ const double slip1 = 0.6;
+ const double slip2 = 0.7;
+
msgs::Friction *friction = msg.mutable_friction();
- friction->set_mu(0.1);
- friction->set_mu2(0.2);
- msgs::Set(friction->mutable_fdir1(), math::Vector3(0.3, 0.4, 0.5));
- friction->set_slip1(0.6);
- friction->set_slip2(0.7);
+ friction->set_mu(mu);
+ friction->set_mu2(mu2);
+ msgs::Set(friction->mutable_fdir1(), fdir1.Ign());
+ friction->set_slip1(slip1);
+ friction->set_slip2(slip2);
// bounce
msg.set_restitution_coefficient(1.1);
@@ -1183,16 +1592,16 @@ TEST_F(MsgsTest, SurfaceToSDF)
msg.set_min_depth(0.0001);
msg.set_collide_without_contact(true);
msg.set_collide_without_contact_bitmask(0x0004);
+ msg.set_collide_bitmask(0x01);
sdf::ElementPtr surfaceSDF = msgs::SurfaceToSDF(msg);
sdf::ElementPtr frictionElem = surfaceSDF->GetElement("friction");
sdf::ElementPtr frictionPhysicsElem = frictionElem->GetElement("ode");
- EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("mu"), 0.1);
- EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("mu2"), 0.2);
- EXPECT_TRUE(frictionPhysicsElem->Get<math::Vector3>("fdir1") ==
- math::Vector3(0.3, 0.4, 0.5));
- EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("slip1"), 0.6);
- EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("slip2"), 0.7);
+ EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("mu"), mu);
+ EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("mu2"), mu2);
+ EXPECT_TRUE(frictionPhysicsElem->Get<math::Vector3>("fdir1") == fdir1);
+ EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("slip1"), slip1);
+ EXPECT_DOUBLE_EQ(frictionPhysicsElem->Get<double>("slip2"), slip2);
sdf::ElementPtr bounceElem = surfaceSDF->GetElement("bounce");
EXPECT_DOUBLE_EQ(bounceElem->Get<double>("restitution_coefficient"), 1.1);
@@ -1210,4 +1619,482 @@ TEST_F(MsgsTest, SurfaceToSDF)
EXPECT_TRUE(contactElem->Get<bool>("collide_without_contact"));
EXPECT_EQ(contactElem->Get<unsigned int>("collide_without_contact_bitmask"),
static_cast<unsigned int>(0x0004));
+ EXPECT_EQ(contactElem->Get<unsigned int>("collide_bitmask"),
+ static_cast<unsigned int>(0x01));
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, JointToSDF)
+{
+ const std::string name("test_joint");
+ const msgs::Joint::Type type = msgs::Joint::UNIVERSAL;
+ const math::Pose pose(math::Vector3(9, 1, 1), math::Quaternion(0, 1, 0, 0));
+ const std::string parent("parent_link");
+ const std::string child("child_link");
+
+ const double cfm = 0.1;
+ const double bounce = 0.2;
+ const double velocity = 0.6;
+ const double fudge_factor = 0.7;
+ const double limit_cfm = 0.3;
+ const double limit_erp = 0.4;
+ const double suspension_cfm = 0.8;
+ const double suspension_erp = 0.9;
+ const math::Vector3 xyz1(0.6, 0.8, 0.0);
+ const math::Vector3 xyz2(0.0, 0.0, 1.0);
+ const double limit_lower1 = -2.0;
+ const double limit_lower2 = -4.0;
+ const double limit_upper1 = 12.0;
+ const double limit_upper2 = 24.0;
+ const double limit_effort1 = 1e3;
+ const double limit_effort2 = 1e4;
+ const double limit_velocity1 = 33;
+ const double limit_velocity2 = 44;
+ const double damping1 = 1e-2;
+ const double damping2 = 3e-2;
+ const double friction1 = 1e2;
+ const double friction2 = 3e2;
+ const bool useParentModelFrame1 = true;
+ // don't set use_parent_model_frame for axis2
+ // expect it to match sdformat default (false)
+
+ msgs::Joint jointMsg;
+ jointMsg.set_name(name);
+ jointMsg.set_type(type);
+ jointMsg.set_parent(parent);
+ jointMsg.set_child(child);
+ msgs::Set(jointMsg.mutable_pose(), pose.Ign());
+ jointMsg.set_cfm(cfm);
+ jointMsg.set_bounce(bounce);
+ jointMsg.set_velocity(velocity);
+ jointMsg.set_fudge_factor(fudge_factor);
+ jointMsg.set_limit_cfm(limit_cfm);
+ jointMsg.set_limit_erp(limit_erp);
+ jointMsg.set_suspension_cfm(suspension_cfm);
+ jointMsg.set_suspension_erp(suspension_erp);
+ {
+ auto axis1 = jointMsg.mutable_axis1();
+ msgs::Set(axis1->mutable_xyz(), xyz1.Ign());
+ axis1->set_limit_lower(limit_lower1);
+ axis1->set_limit_upper(limit_upper1);
+ axis1->set_limit_effort(limit_effort1);
+ axis1->set_limit_velocity(limit_velocity1);
+ axis1->set_damping(damping1);
+ axis1->set_friction(friction1);
+ axis1->set_use_parent_model_frame(useParentModelFrame1);
+ }
+ {
+ auto axis2 = jointMsg.mutable_axis2();
+ msgs::Set(axis2->mutable_xyz(), xyz2.Ign());
+ axis2->set_limit_lower(limit_lower2);
+ axis2->set_limit_upper(limit_upper2);
+ axis2->set_limit_effort(limit_effort2);
+ axis2->set_limit_velocity(limit_velocity2);
+ axis2->set_damping(damping2);
+ axis2->set_friction(friction2);
+ }
+
+ sdf::ElementPtr jointSDF = msgs::JointToSDF(jointMsg);
+ EXPECT_TRUE(jointSDF->HasAttribute("name"));
+ EXPECT_EQ(jointSDF->Get<std::string>("name"), name);
+ EXPECT_TRUE(jointSDF->HasAttribute("type"));
+ EXPECT_STREQ(jointSDF->Get<std::string>("type").c_str(), "universal");
+ EXPECT_TRUE(jointSDF->HasElement("parent"));
+ EXPECT_EQ(jointSDF->Get<std::string>("parent"), parent);
+ EXPECT_TRUE(jointSDF->HasElement("child"));
+ EXPECT_EQ(jointSDF->Get<std::string>("child"), child);
+ EXPECT_TRUE(jointSDF->HasElement("pose"));
+ EXPECT_EQ(pose, jointSDF->Get<math::Pose>("pose"));
+
+ EXPECT_TRUE(jointSDF->HasElement("axis"));
+ {
+ auto axisElem = jointSDF->GetElement("axis");
+ EXPECT_TRUE(axisElem->HasElement("xyz"));
+ EXPECT_EQ(xyz1, axisElem->Get<math::Vector3>("xyz"));
+ EXPECT_TRUE(axisElem->HasElement("use_parent_model_frame"));
+ EXPECT_EQ(useParentModelFrame1,
+ axisElem->Get<bool>("use_parent_model_frame"));
+
+ EXPECT_TRUE(axisElem->HasElement("dynamics"));
+ auto axisDynamics = axisElem->GetElement("dynamics");
+ EXPECT_TRUE(axisDynamics->HasElement("damping"));
+ EXPECT_DOUBLE_EQ(damping1, axisDynamics->Get<double>("damping"));
+ EXPECT_TRUE(axisDynamics->HasElement("friction"));
+ EXPECT_DOUBLE_EQ(friction1, axisDynamics->Get<double>("friction"));
+
+ EXPECT_TRUE(axisElem->HasElement("limit"));
+ auto axisLimit = axisElem->GetElement("limit");
+ EXPECT_TRUE(axisLimit->HasElement("lower"));
+ EXPECT_DOUBLE_EQ(limit_lower1, axisLimit->Get<double>("lower"));
+ EXPECT_TRUE(axisLimit->HasElement("upper"));
+ EXPECT_DOUBLE_EQ(limit_upper1, axisLimit->Get<double>("upper"));
+ EXPECT_TRUE(axisLimit->HasElement("effort"));
+ EXPECT_DOUBLE_EQ(limit_effort1, axisLimit->Get<double>("effort"));
+ EXPECT_TRUE(axisLimit->HasElement("velocity"));
+ EXPECT_DOUBLE_EQ(limit_velocity1, axisLimit->Get<double>("velocity"));
+ }
+
+ EXPECT_TRUE(jointSDF->HasElement("axis2"));
+ {
+ auto axisElem = jointSDF->GetElement("axis2");
+ EXPECT_TRUE(axisElem->HasElement("xyz"));
+ EXPECT_EQ(xyz2, axisElem->Get<math::Vector3>("xyz"));
+ // use_parent_model_frame is required in axis.proto
+ // so expect to to exist even if we don't set it
+ EXPECT_TRUE(axisElem->HasElement("use_parent_model_frame"));
+ // expect false (default sdformat value)
+ EXPECT_FALSE(axisElem->Get<bool>("use_parent_model_frame"));
+
+ EXPECT_TRUE(axisElem->HasElement("dynamics"));
+ auto axisDynamics = axisElem->GetElement("dynamics");
+ EXPECT_TRUE(axisDynamics->HasElement("damping"));
+ EXPECT_DOUBLE_EQ(damping2, axisDynamics->Get<double>("damping"));
+ EXPECT_TRUE(axisDynamics->HasElement("friction"));
+ EXPECT_DOUBLE_EQ(friction2, axisDynamics->Get<double>("friction"));
+
+ EXPECT_TRUE(axisElem->HasElement("limit"));
+ auto axisLimit = axisElem->GetElement("limit");
+ EXPECT_TRUE(axisLimit->HasElement("lower"));
+ EXPECT_DOUBLE_EQ(limit_lower2, axisLimit->Get<double>("lower"));
+ EXPECT_TRUE(axisLimit->HasElement("upper"));
+ EXPECT_DOUBLE_EQ(limit_upper2, axisLimit->Get<double>("upper"));
+ EXPECT_TRUE(axisLimit->HasElement("effort"));
+ EXPECT_DOUBLE_EQ(limit_effort2, axisLimit->Get<double>("effort"));
+ EXPECT_TRUE(axisLimit->HasElement("velocity"));
+ EXPECT_DOUBLE_EQ(limit_velocity2, axisLimit->Get<double>("velocity"));
+ }
+
+ EXPECT_TRUE(jointSDF->HasElement("physics"));
+ auto physicsElem = jointSDF->GetElement("physics");
+ EXPECT_TRUE(physicsElem->HasElement("ode"));
+ auto odePhysics = physicsElem->GetElement("ode");
+ EXPECT_TRUE(odePhysics->HasElement("cfm"));
+ EXPECT_DOUBLE_EQ(odePhysics->Get<double>("cfm"), cfm);
+ EXPECT_TRUE(odePhysics->HasElement("bounce"));
+ EXPECT_DOUBLE_EQ(odePhysics->Get<double>("bounce"), bounce);
+ EXPECT_TRUE(odePhysics->HasElement("velocity"));
+ EXPECT_DOUBLE_EQ(odePhysics->Get<double>("velocity"), velocity);
+ EXPECT_TRUE(odePhysics->HasElement("fudge_factor"));
+ EXPECT_DOUBLE_EQ(odePhysics->Get<double>("fudge_factor"), fudge_factor);
+
+ EXPECT_TRUE(odePhysics->HasElement("limit"));
+ auto limitElem = odePhysics->GetElement("limit");
+ EXPECT_TRUE(limitElem->HasElement("cfm"));
+ EXPECT_DOUBLE_EQ(limitElem->Get<double>("cfm"), limit_cfm);
+ EXPECT_TRUE(limitElem->HasElement("erp"));
+ EXPECT_DOUBLE_EQ(limitElem->Get<double>("erp"), limit_erp);
+
+ EXPECT_TRUE(odePhysics->HasElement("suspension"));
+ auto suspensionElem = odePhysics->GetElement("suspension");
+ EXPECT_TRUE(suspensionElem->HasElement("cfm"));
+ EXPECT_DOUBLE_EQ(suspensionElem->Get<double>("cfm"), suspension_cfm);
+ EXPECT_TRUE(suspensionElem->HasElement("erp"));
+ EXPECT_DOUBLE_EQ(suspensionElem->Get<double>("erp"), suspension_erp);
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, AddBoxLink)
+{
+ msgs::Model model;
+ EXPECT_EQ(model.link_size(), 0);
+
+ const double mass = 1.0;
+ const math::Vector3 size(1, 1, 1);
+ msgs::AddBoxLink(model, mass, size.Ign());
+ EXPECT_EQ(model.link_size(), 1);
+ {
+ auto link = model.link(0);
+ EXPECT_EQ(link.name(), std::string("link_1"));
+
+ auto inertial = link.inertial();
+ EXPECT_DOUBLE_EQ(inertial.mass(), mass);
+ double ixx = inertial.ixx();
+ double iyy = inertial.iyy();
+ double izz = inertial.izz();
+ double ixy = inertial.ixy();
+ double ixz = inertial.ixz();
+ double iyz = inertial.iyz();
+ EXPECT_GT(ixx, 0.0);
+ EXPECT_GT(iyy, 0.0);
+ EXPECT_GT(izz, 0.0);
+ EXPECT_DOUBLE_EQ(ixy, 0.0);
+ EXPECT_DOUBLE_EQ(ixz, 0.0);
+ EXPECT_DOUBLE_EQ(iyz, 0.0);
+ // triangle inequality
+ EXPECT_GT(ixx + iyy, izz);
+ EXPECT_GT(iyy + izz, ixx);
+ EXPECT_GT(izz + ixx, iyy);
+
+ EXPECT_EQ(link.collision_size(), 1);
+ {
+ auto collision = link.collision(0);
+ auto geometry = collision.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_BOX);
+ EXPECT_EQ(msgs::ConvertIgn(geometry.box().size()), size.Ign());
+ }
+
+ EXPECT_EQ(link.visual_size(), 1);
+ {
+ auto visual = link.visual(0);
+ auto geometry = visual.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_BOX);
+ EXPECT_EQ(msgs::ConvertIgn(geometry.box().size()), size.Ign());
+ }
+ }
+
+ const double massRatio = 2.0;
+ msgs::AddBoxLink(model, mass*massRatio, size.Ign());
+ EXPECT_EQ(model.link_size(), 2);
+ {
+ auto link1 = model.link(0);
+ auto link2 = model.link(1);
+ EXPECT_EQ(link2.name(), std::string("link_2"));
+
+ auto inertial1 = link1.inertial();
+ auto inertial2 = link2.inertial();
+
+ EXPECT_NEAR(massRatio * inertial1.mass(), inertial2.mass(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.ixx(), inertial2.ixx(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.iyy(), inertial2.iyy(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.izz(), inertial2.izz(), 1e-6);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, AddCylinderLink)
+{
+ msgs::Model model;
+ EXPECT_EQ(model.link_size(), 0);
+
+ const double mass = 1.0;
+ const double radius = 0.5;
+ const double length = 2.5;
+ msgs::AddCylinderLink(model, mass, radius, length);
+ EXPECT_EQ(model.link_size(), 1);
+ {
+ auto link = model.link(0);
+ EXPECT_EQ(link.name(), std::string("link_1"));
+
+ auto inertial = link.inertial();
+ EXPECT_DOUBLE_EQ(inertial.mass(), mass);
+ double ixx = inertial.ixx();
+ double iyy = inertial.iyy();
+ double izz = inertial.izz();
+ double ixy = inertial.ixy();
+ double ixz = inertial.ixz();
+ double iyz = inertial.iyz();
+ EXPECT_GT(ixx, 0.0);
+ EXPECT_GT(iyy, 0.0);
+ EXPECT_GT(izz, 0.0);
+ EXPECT_DOUBLE_EQ(ixy, 0.0);
+ EXPECT_DOUBLE_EQ(ixz, 0.0);
+ EXPECT_DOUBLE_EQ(iyz, 0.0);
+ // triangle inequality
+ EXPECT_GT(ixx + iyy, izz);
+ EXPECT_GT(iyy + izz, ixx);
+ EXPECT_GT(izz + ixx, iyy);
+
+ EXPECT_EQ(link.collision_size(), 1);
+ {
+ auto collision = link.collision(0);
+ auto geometry = collision.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_CYLINDER);
+ EXPECT_DOUBLE_EQ(geometry.cylinder().radius(), radius);
+ EXPECT_DOUBLE_EQ(geometry.cylinder().length(), length);
+ }
+
+ EXPECT_EQ(link.visual_size(), 1);
+ {
+ auto visual = link.visual(0);
+ auto geometry = visual.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_CYLINDER);
+ EXPECT_DOUBLE_EQ(geometry.cylinder().radius(), radius);
+ EXPECT_DOUBLE_EQ(geometry.cylinder().length(), length);
+ }
+ }
+
+ const double massRatio = 2.0;
+ msgs::AddCylinderLink(model, mass*massRatio, radius, length);
+ EXPECT_EQ(model.link_size(), 2);
+ {
+ auto link1 = model.link(0);
+ auto link2 = model.link(1);
+ EXPECT_EQ(link2.name(), std::string("link_2"));
+
+ auto inertial1 = link1.inertial();
+ auto inertial2 = link2.inertial();
+
+ EXPECT_NEAR(massRatio * inertial1.mass(), inertial2.mass(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.ixx(), inertial2.ixx(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.iyy(), inertial2.iyy(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.izz(), inertial2.izz(), 1e-6);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, AddSphereLink)
+{
+ msgs::Model model;
+ EXPECT_EQ(model.link_size(), 0);
+
+ const double mass = 1.0;
+ const double radius = 0.5;
+ msgs::AddSphereLink(model, mass, radius);
+ EXPECT_EQ(model.link_size(), 1);
+ {
+ auto link = model.link(0);
+ EXPECT_EQ(link.name(), std::string("link_1"));
+
+ auto inertial = link.inertial();
+ EXPECT_DOUBLE_EQ(inertial.mass(), mass);
+ double ixx = inertial.ixx();
+ double iyy = inertial.iyy();
+ double izz = inertial.izz();
+ double ixy = inertial.ixy();
+ double ixz = inertial.ixz();
+ double iyz = inertial.iyz();
+ EXPECT_GT(ixx, 0.0);
+ EXPECT_GT(iyy, 0.0);
+ EXPECT_GT(izz, 0.0);
+ EXPECT_DOUBLE_EQ(ixy, 0.0);
+ EXPECT_DOUBLE_EQ(ixz, 0.0);
+ EXPECT_DOUBLE_EQ(iyz, 0.0);
+ // triangle inequality
+ EXPECT_GT(ixx + iyy, izz);
+ EXPECT_GT(iyy + izz, ixx);
+ EXPECT_GT(izz + ixx, iyy);
+
+ EXPECT_EQ(link.collision_size(), 1);
+ {
+ auto collision = link.collision(0);
+ auto geometry = collision.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_SPHERE);
+ EXPECT_DOUBLE_EQ(geometry.sphere().radius(), radius);
+ }
+
+ EXPECT_EQ(link.visual_size(), 1);
+ {
+ auto visual = link.visual(0);
+ auto geometry = visual.geometry();
+ EXPECT_EQ(geometry.type(), msgs::Geometry_Type_SPHERE);
+ EXPECT_DOUBLE_EQ(geometry.sphere().radius(), radius);
+ }
+ }
+
+ const double massRatio = 2.0;
+ msgs::AddSphereLink(model, mass*massRatio, radius);
+ EXPECT_EQ(model.link_size(), 2);
+ {
+ auto link1 = model.link(0);
+ auto link2 = model.link(1);
+ EXPECT_EQ(link2.name(), std::string("link_2"));
+
+ auto inertial1 = link1.inertial();
+ auto inertial2 = link2.inertial();
+
+ EXPECT_NEAR(massRatio * inertial1.mass(), inertial2.mass(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.ixx(), inertial2.ixx(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.iyy(), inertial2.iyy(), 1e-6);
+ EXPECT_NEAR(massRatio * inertial1.izz(), inertial2.izz(), 1e-6);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(MsgsTest, ModelToSDF)
+{
+ const std::string name("test_bicycle");
+ const math::Pose pose(math::Vector3(6, 1, 7),
+ math::Quaternion(0.5, 0.5, 0.5, 0.5));
+
+ msgs::Model model;
+ model.set_name(name);
+ model.set_is_static(false);
+ msgs::Set(model.mutable_pose(), pose.Ign());
+ EXPECT_EQ(model.link_size(), 0);
+ EXPECT_EQ(model.joint_size(), 0);
+
+ // This will be a bicycle with two wheels.
+ // The frame is a box.
+ const double length = 1.5;
+ const double height = 0.9;
+ const double width = 0.1;
+ const ignition::math::Vector3d boxSize(length, width, height);
+ const double boxMass = 4.0;
+ AddBoxLink(model, boxMass, boxSize);
+ ASSERT_EQ(model.link_size(), 1);
+ EXPECT_EQ(model.joint_size(), 0);
+ model.mutable_link(0)->set_name("frame");
+
+ // The rear wheel is a cylinder.
+ const double radius = height / 2;
+ AddCylinderLink(model, 0.5, radius, radius);
+ ASSERT_EQ(model.link_size(), 2);
+ EXPECT_EQ(model.joint_size(), 0);
+ const math::Pose cylinderPose(-length/2, 0, -height/2, M_PI/2, 0, 0);
+ {
+ auto link = model.mutable_link(1);
+ msgs::Set(link->mutable_pose(), cylinderPose.Ign());
+ link->set_name("rear_wheel");
+ }
+
+ // The front wheel is a sphere.
+ AddSphereLink(model, 0.5, radius);
+ ASSERT_EQ(model.link_size(), 3);
+ EXPECT_EQ(model.joint_size(), 0);
+ const math::Pose spherePose(length/2, 0, -height/2, 0, 0, 0);
+ {
+ auto link = model.mutable_link(2);
+ msgs::Set(link->mutable_pose(), spherePose.Ign());
+ link->set_name("front_wheel");
+ }
+
+ // Add revolute joints for the wheels.
+ // Front wheel joint
+ model.add_joint();
+ ASSERT_EQ(model.joint_size(), 1);
+ auto frontJoint = model.mutable_joint(0);
+ frontJoint->set_name("front_hinge");
+ frontJoint->set_type(msgs::ConvertJointType("revolute"));
+ frontJoint->set_parent("frame");
+ frontJoint->set_child("front_wheel");
+ const ignition::math::Vector3d frontAxis(0, 1, 0);
+ msgs::Set(frontJoint->mutable_axis1()->mutable_xyz(), frontAxis);
+
+ // Rear wheel joint
+ model.add_joint();
+ ASSERT_EQ(model.joint_size(), 2);
+ auto rearJoint = model.mutable_joint(1);
+ rearJoint->set_name("rear_hinge");
+ rearJoint->set_type(msgs::ConvertJointType("revolute"));
+ rearJoint->set_parent("frame");
+ rearJoint->set_child("rear_wheel");
+ const ignition::math::Vector3d rearAxis(0, 0, 1);
+ msgs::Set(rearJoint->mutable_axis1()->mutable_xyz(), rearAxis);
+
+ sdf::ElementPtr modelSDF = msgs::ModelToSDF(model);
+ EXPECT_EQ(modelSDF->Get<std::string>("name"), name);
+ EXPECT_FALSE(modelSDF->Get<bool>("static"));
+ EXPECT_EQ(pose, modelSDF->Get<math::Pose>("pose"));
+
+ sdf::ElementPtr linkElem1 = modelSDF->GetElement("link");
+ EXPECT_EQ(linkElem1->Get<std::string>("name"), "frame");
+ EXPECT_EQ(linkElem1->Get<math::Pose>("pose"), math::Pose());
+
+ sdf::ElementPtr linkElem2 = linkElem1->GetNextElement("link");
+ EXPECT_EQ(linkElem2->Get<std::string>("name"), "rear_wheel");
+ EXPECT_EQ(linkElem2->Get<math::Pose>("pose"), cylinderPose);
+
+ sdf::ElementPtr linkElem3 = linkElem2->GetNextElement("link");
+ EXPECT_EQ(linkElem3->Get<std::string>("name"), "front_wheel");
+ EXPECT_EQ(linkElem3->Get<math::Pose>("pose"), spherePose);
+
+ sdf::ElementPtr jointElem1 = modelSDF->GetElement("joint");
+ EXPECT_EQ(jointElem1->Get<std::string>("name"), "front_hinge");
+ EXPECT_EQ(jointElem1->Get<std::string>("type"), "revolute");
+ EXPECT_EQ(jointElem1->Get<math::Pose>("pose"), math::Pose());
+
+ sdf::ElementPtr jointElem2 = jointElem1->GetNextElement("joint");
+ EXPECT_EQ(jointElem2->Get<std::string>("name"), "rear_hinge");
+ EXPECT_EQ(jointElem2->Get<std::string>("type"), "revolute");
+ EXPECT_EQ(jointElem2->Get<math::Pose>("pose"), math::Pose());
}
diff --git a/gazebo/msgs/physics.proto b/gazebo/msgs/physics.proto
index 706bf0d..6dff0df 100644
--- a/gazebo/msgs/physics.proto
+++ b/gazebo/msgs/physics.proto
@@ -32,4 +32,9 @@ message Physics
optional double real_time_factor = 13;
optional double real_time_update_rate = 14;
optional double max_step_size = 15;
+ // The name of this physics profile (not to be confused with type)
+ optional string profile_name = 16;
+
+ /// \brief Magnetic field
+ optional Vector3d magnetic_field = 17;
}
diff --git a/gazebo/msgs/rest_error.proto b/gazebo/msgs/rest_error.proto
new file mode 100644
index 0000000..eab2245
--- /dev/null
+++ b/gazebo/msgs/rest_error.proto
@@ -0,0 +1,12 @@
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface RestError
+/// \brief A message for reporting to gzclient an error when accessing
+/// a REST service
+
+message RestError
+{
+ required string type = 1;
+ optional string msg = 2;
+}
diff --git a/gazebo/msgs/rest_login.proto b/gazebo/msgs/rest_login.proto
new file mode 100644
index 0000000..4df4cbc
--- /dev/null
+++ b/gazebo/msgs/rest_login.proto
@@ -0,0 +1,12 @@
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface RestLogin
+/// \brief A message to login to a REST service
+
+message RestLogin
+{
+ required string url = 1;
+ optional string username = 2;
+ optional string password = 3;
+}
diff --git a/gazebo/msgs/rest_logout.proto b/gazebo/msgs/rest_logout.proto
new file mode 100644
index 0000000..d1facf1
--- /dev/null
+++ b/gazebo/msgs/rest_logout.proto
@@ -0,0 +1,11 @@
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface RestLogout
+/// \brief Message for login out of a REST service.
+
+message RestLogout
+{
+ /// \brief the web service url
+ optional string url = 1;
+}
diff --git a/gazebo/msgs/rest_post.proto b/gazebo/msgs/rest_post.proto
new file mode 100644
index 0000000..650a52d
--- /dev/null
+++ b/gazebo/msgs/rest_post.proto
@@ -0,0 +1,12 @@
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface RestPost
+/// \brief A message to POST data on a REST service. The route (ex /news) and
+/// the content of the message in JSON format are specified.
+
+message RestPost
+{
+ required string route = 1;
+ required string json = 2;
+}
diff --git a/gazebo/msgs/scene.proto b/gazebo/msgs/scene.proto
index f2baf59..da1f7e6 100644
--- a/gazebo/msgs/scene.proto
+++ b/gazebo/msgs/scene.proto
@@ -23,8 +23,11 @@ message Scene
optional bool grid = 7;
repeated Model model = 8;
- repeated Light light = 9;
+ repeated Light light = 9;
repeated Joint joint = 10;
+
+ /// \brief Show/hide world origin indicator.
+ optional bool origin_visual = 11;
}
diff --git a/gazebo/msgs/sim_event.proto b/gazebo/msgs/sim_event.proto
index c64d51a..a7caa7f 100644
--- a/gazebo/msgs/sim_event.proto
+++ b/gazebo/msgs/sim_event.proto
@@ -7,9 +7,8 @@ import "world_stats.proto";
message SimEvent
{
- required string type = 1;
- required string name = 2;
- required WorldStatistics world_statistics =3;
-
- optional string data = 4;
+ required string type = 1;
+ required string name = 2;
+ required WorldStatistics world_statistics = 3;
+ optional string data = 4;
}
diff --git a/gazebo/msgs/surface.proto b/gazebo/msgs/surface.proto
index d2a1ba5..bbf8829 100644
--- a/gazebo/msgs/surface.proto
+++ b/gazebo/msgs/surface.proto
@@ -20,4 +20,5 @@ message Surface
optional double min_depth = 9;
optional bool collide_without_contact = 10;
optional uint32 collide_without_contact_bitmask = 11;
+ optional uint32 collide_bitmask = 12;
}
diff --git a/gazebo/msgs/visual.proto b/gazebo/msgs/visual.proto
index 7dbe06b..99364da 100644
--- a/gazebo/msgs/visual.proto
+++ b/gazebo/msgs/visual.proto
@@ -13,8 +13,39 @@ import "vector3d.proto";
message Visual
{
+ /// \brief Optional meta information for the visual. The information
+ /// contained within this element should be used to provide additional
+ /// feedback to an end user.
+ message Meta
+ {
+ /// \brief The layer in which this visual is displayed. The layer number
+ /// is useful for programs, such as Gazebo, that put visuals in different
+ /// layers for enhanced visualization.
+ optional int32 layer = 1;
+ }
+
+ enum Type
+ {
+ /// \brief Entity visual
+ ENTITY = 0;
+ /// \brief Model visual
+ MODEL = 1;
+ /// \brief Link visual
+ LINK = 2;
+ /// \brief Visual visual
+ VISUAL = 3;
+ /// \brief Collision visual
+ COLLISION = 4;
+ /// \brief Sensor visual
+ SENSOR = 5;
+ /// \brief GUI visual
+ GUI = 6;
+ /// \brief Physics data visual
+ PHYSICS = 7;
+ }
+
required string name = 1;
- optional uint32 id = 2;
+ optional uint32 id = 2;
required string parent_name = 3;
optional uint32 parent_id = 4;
optional bool cast_shadows = 5;
@@ -29,4 +60,10 @@ message Visual
optional bool is_static = 13;
optional Plugin plugin = 14;
optional Vector3d scale = 15;
+
+ /// \brief Option meta information associated with this visual.
+ optional Meta meta = 16;
+
+ /// \brief Type of visual.
+ optional Type type = 17;
}
diff --git a/gazebo/msgs/world_stats.proto b/gazebo/msgs/world_stats.proto
index 70fa1cb..c372d72 100644
--- a/gazebo/msgs/world_stats.proto
+++ b/gazebo/msgs/world_stats.proto
@@ -4,15 +4,16 @@ package gazebo.msgs;
/// \interface WorldStatistics
/// \brief A message statiscs about a world
-
+import "log_playback_stats.proto";
import "time.proto";
message WorldStatistics
{
- required Time sim_time = 2;
- required Time pause_time = 3;
- required Time real_time = 4;
- required bool paused = 5;
- required uint64 iterations = 6;
- optional int32 model_count = 7;
+ required Time sim_time = 2;
+ required Time pause_time = 3;
+ required Time real_time = 4;
+ required bool paused = 5;
+ required uint64 iterations = 6;
+ optional int32 model_count = 7;
+ optional LogPlaybackStatistics log_playback_stats = 8;
}
diff --git a/gazebo/msgs/wrench.proto b/gazebo/msgs/wrench.proto
index 3629962..a7640da 100644
--- a/gazebo/msgs/wrench.proto
+++ b/gazebo/msgs/wrench.proto
@@ -1,7 +1,7 @@
package gazebo.msgs;
/// \ingroup gazebo_msgs
-/// \interface Wrench
+/// \interface Wrench
/// \brief Message for a wrench value
@@ -9,6 +9,7 @@ import "vector3d.proto";
message Wrench
{
- required Vector3d force = 1;
- required Vector3d torque = 2;
+ required Vector3d force = 1;
+ required Vector3d torque = 2;
+ optional Vector3d force_offset = 3;
}
diff --git a/gazebo/physics/Actor.cc b/gazebo/physics/Actor.cc
index fa68b07..c3e62bb 100644
--- a/gazebo/physics/Actor.cc
+++ b/gazebo/physics/Actor.cc
@@ -14,6 +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/thread/recursive_mutex.hpp>
#include <sstream>
#include <limits>
@@ -126,10 +134,10 @@ void Actor::Load(sdf::ElementPtr _sdf)
linkSdf->GetAttribute("name")->Set(bone->GetName());
linkSdf->GetElement("gravity")->Set(false);
linkPose = linkSdf->GetElement("pose");
- math::Pose pose(bone->GetModelTransform().GetTranslation(),
- bone->GetModelTransform().GetRotation());
+ ignition::math::Pose3d pose(bone->ModelTransform().Translation(),
+ bone->ModelTransform().Rotation());
if (bone->IsRootNode())
- pose = math::Pose();
+ pose = ignition::math::Pose3d();
linkPose->Set(pose);
/// FIXME hardcoded inertia of a sphere with mass 1.0 and radius 0.01
@@ -161,20 +169,22 @@ void Actor::Load(sdf::ElementPtr _sdf)
{
SkeletonNode *curChild = bone->GetChild(i);
- math::Vector3 dir = curChild->GetModelTransform().GetTranslation() -
- bone->GetModelTransform().GetTranslation();
+ math::Vector3 dir = curChild->ModelTransform().Translation() -
+ bone->ModelTransform().Translation();
double length = dir.GetLength();
if (!math::equal(length, 0.0))
{
- math::Vector3 r = curChild->GetTransform().GetTranslation();
- math::Vector3 linkPos = math::Vector3(r.x / 2.0,
- r.y / 2.0, r.z / 2.0);
+ ignition::math::Vector3d r =
+ curChild->Transform().Translation();
+ ignition::math::Vector3d linkPos =
+ ignition::math::Vector3d(r.X() / 2.0, r.Y() / 2.0, r.Z() / 2.0);
double theta = atan2(dir.y, dir.x);
double phi = acos(dir.z / length);
- math::Pose bonePose(linkPos, math::Quaternion(0.0, phi, theta));
- bonePose.rot = pose.rot.GetInverse() * bonePose.rot;
+ ignition::math::Pose3d bonePose(linkPos,
+ ignition::math::Quaterniond(0.0, phi, theta));
+ bonePose.Rot() = pose.Rot().Inverse() * bonePose.Rot();
this->AddBoxVisual(linkSdf, bone->GetName() + "_" +
curChild->GetName() + "__SKELETON_VISUAL__", bonePose,
@@ -276,12 +286,12 @@ void Actor::LoadScript(sdf::ElementPtr _sdf)
if (pIter == points.begin() && !math::equal(pIter->first, 0.0))
{
key = anim->CreateKeyFrame(0.0);
- key->SetTranslation(pIter->second.pos);
- key->SetRotation(pIter->second.rot);
+ key->Translation(pIter->second.pos.Ign());
+ key->Rotation(pIter->second.rot.Ign());
}
key = anim->CreateKeyFrame(pIter->first);
- key->SetTranslation(pIter->second.pos);
- key->SetRotation(pIter->second.rot);
+ key->Translation(pIter->second.pos.Ign());
+ key->Rotation(pIter->second.rot.Ign());
}
this->trajectories[this->trajInfo[idx].id] = anim;
@@ -483,51 +493,54 @@ void Actor::Update()
SkeletonAnimation *skelAnim = this->skelAnimation[tinfo.type];
std::map<std::string, std::string> skelMap = this->skelNodesMap[tinfo.type];
- math::Pose modelPose;
- std::map<std::string, math::Matrix4> frame;
+ ignition::math::Pose3d modelPose;
+ std::map<std::string, ignition::math::Matrix4d> frame;
if (this->trajectories.find(tinfo.id) != this->trajectories.end())
{
common::PoseKeyFrame posFrame(0.0);
this->trajectories[tinfo.id]->SetTime(scriptTime);
this->trajectories[tinfo.id]->GetInterpolatedKeyFrame(posFrame);
- modelPose.pos = posFrame.GetTranslation();
- modelPose.rot = posFrame.GetRotation();
+
+ modelPose.Pos() = posFrame.Translation();
+ modelPose.Rot() = posFrame.Rotation();
if (this->lastTraj == tinfo.id)
- this->pathLength += fabs(this->lastPos.Distance(modelPose.pos));
+ this->pathLength += fabs(this->lastPos.Distance(modelPose.Pos()));
else
{
common::PoseKeyFrame *frame0 = dynamic_cast<common::PoseKeyFrame*>
(this->trajectories[tinfo.id]->GetKeyFrame(0));
- this->pathLength = fabs(modelPose.pos.Distance(frame0->GetTranslation()));
+ ignition::math::Vector3d vector3Ign;
+ vector3Ign = frame0->Translation();
+ this->pathLength = fabs(modelPose.Pos().Distance(vector3Ign));
}
- this->lastPos = modelPose.pos;
+ this->lastPos = modelPose.Pos();
}
if (this->interpolateX[tinfo.type] &&
this->trajectories.find(tinfo.id) != this->trajectories.end())
{
- frame = skelAnim->GetPoseAtX(this->pathLength,
+ frame = skelAnim->PoseAtX(this->pathLength,
skelMap[this->skeleton->GetRootNode()->GetName()]);
}
else
- frame = skelAnim->GetPoseAt(scriptTime);
+ frame = skelAnim->PoseAt(scriptTime);
this->lastTraj = tinfo.id;
- math::Matrix4 rootTrans =
- frame[skelMap[this->skeleton->GetRootNode()->GetName()]];
+ ignition::math::Matrix4d rootTrans =
+ frame[skelMap[this->skeleton->GetRootNode()->GetName()]];
- math::Vector3 rootPos = rootTrans.GetTranslation();
- math::Quaternion rootRot = rootTrans.GetRotation();
+ ignition::math::Vector3d rootPos = rootTrans.Translation();
+ ignition::math::Quaterniond rootRot = rootTrans.Rotation();
if (tinfo.translated)
- rootPos.x = 0.0;
- math::Pose actorPose;
- actorPose.pos = modelPose.pos + modelPose.rot.RotateVector(rootPos);
- actorPose.rot = modelPose.rot *rootRot;
+ rootPos.X() = 0.0;
+ ignition::math::Pose3d actorPose;
+ actorPose.Pos() = modelPose.Pos() + modelPose.Rot().RotateVector(rootPos);
+ actorPose.Rot() = modelPose.Rot() * rootRot;
- math::Matrix4 rootM(actorPose.rot.GetAsMatrix4());
- rootM.SetTranslate(actorPose.pos);
+ ignition::math::Matrix4d rootM(actorPose.Rot());
+ rootM.Translate(actorPose.Pos());
frame[skelMap[this->skeleton->GetRootNode()->GetName()]] = rootM;
@@ -537,6 +550,83 @@ void Actor::Update()
}
//////////////////////////////////////////////////
+void Actor::SetPose(std::map<std::string, ignition::math::Matrix4d> _frame,
+ std::map<std::string, std::string> _skelMap, double _time)
+{
+ msgs::PoseAnimation msg;
+ msg.set_model_name(this->visualName);
+ msg.set_model_id(this->visualId);
+
+ ignition::math::Matrix4d modelTrans(ignition::math::Matrix4d::Identity);
+ ignition::math::Pose3d mainLinkPose;
+
+ for (unsigned int i = 0; i < this->skeleton->GetNumNodes(); i++)
+ {
+ SkeletonNode *bone = this->skeleton->GetNodeByHandle(i);
+ SkeletonNode *parentBone = bone->GetParent();
+ ignition::math::Matrix4d transform(ignition::math::Matrix4d::Identity);
+ if (_frame.find(_skelMap[bone->GetName()]) != _frame.end())
+ transform = _frame[_skelMap[bone->GetName()]];
+ else
+ transform = bone->Transform();
+
+ LinkPtr currentLink = this->GetChildLink(bone->GetName());
+ ignition::math::Pose3d bonePose = transform.Pose();
+
+ if (!bonePose.IsFinite())
+ {
+ std::cerr << "ACTOR: " << _time << " " << bone->GetName()
+ << " " << bonePose << "\n";
+ bonePose.Correct();
+ }
+
+ msgs::Pose *bone_pose = msg.add_pose();
+ bone_pose->set_name(bone->GetName());
+
+ if (!parentBone)
+ {
+ bone_pose->mutable_position()->CopyFrom(
+ msgs::Convert(ignition::math::Vector3d()));
+ bone_pose->mutable_orientation()->CopyFrom(msgs::Convert(
+ ignition::math::Quaterniond()));
+ mainLinkPose = bonePose;
+ }
+ else
+ {
+ bone_pose->mutable_position()->CopyFrom(msgs::Convert(bonePose.Pos()));
+ bone_pose->mutable_orientation()->CopyFrom(msgs::Convert(bonePose.Rot()));
+ LinkPtr parentLink = this->GetChildLink(parentBone->GetName());
+ math::Pose parentPose = parentLink->GetWorldPose();
+ math::Matrix4 parentTrans(parentPose.rot.GetAsMatrix4());
+ parentTrans.SetTranslate(parentPose.pos);
+ transform = (parentTrans * transform).Ign();
+ }
+
+ msgs::Pose *link_pose = msg.add_pose();
+ link_pose->set_name(currentLink->GetScopedName());
+ link_pose->set_id(currentLink->GetId());
+ ignition::math::Pose3d linkPose = transform.Pose() - mainLinkPose;
+ link_pose->mutable_position()->CopyFrom(msgs::Convert(linkPose.Pos()));
+ link_pose->mutable_orientation()->CopyFrom(msgs::Convert(linkPose.Rot()));
+ currentLink->SetWorldPose(transform.Pose(), true, false);
+ }
+
+ msgs::Time *stamp = msg.add_time();
+ stamp->CopyFrom(msgs::Convert(_time));
+
+ msgs::Pose *model_pose = msg.add_pose();
+ model_pose->set_name(this->GetScopedName());
+ model_pose->set_id(this->GetId());
+ model_pose->mutable_position()->CopyFrom(msgs::Convert(mainLinkPose.Pos()));
+ model_pose->mutable_orientation()->CopyFrom(
+ msgs::Convert(mainLinkPose.Rot()));
+
+ if (this->bonePosePub && this->bonePosePub->HasConnections())
+ this->bonePosePub->Publish(msg);
+ this->SetWorldPose(mainLinkPose, true, false);
+}
+
+//////////////////////////////////////////////////
void Actor::SetPose(std::map<std::string, math::Matrix4> _frame,
std::map<std::string, std::string> _skelMap, double _time)
{
@@ -555,7 +645,7 @@ void Actor::SetPose(std::map<std::string, math::Matrix4> _frame,
if (_frame.find(_skelMap[bone->GetName()]) != _frame.end())
transform = _frame[_skelMap[bone->GetName()]];
else
- transform = bone->GetTransform();
+ transform = bone->Transform();
LinkPtr currentLink = this->GetChildLink(bone->GetName());
math::Pose bonePose = transform.GetAsPose();
@@ -572,15 +662,18 @@ void Actor::SetPose(std::map<std::string, math::Matrix4> _frame,
if (!parentBone)
{
- bone_pose->mutable_position()->CopyFrom(msgs::Convert(math::Vector3()));
+ bone_pose->mutable_position()->CopyFrom(
+ msgs::Convert(ignition::math::Vector3d()));
bone_pose->mutable_orientation()->CopyFrom(msgs::Convert(
- math::Quaternion()));
+ ignition::math::Quaterniond()));
mainLinkPose = bonePose;
}
else
{
- bone_pose->mutable_position()->CopyFrom(msgs::Convert(bonePose.pos));
- bone_pose->mutable_orientation()->CopyFrom(msgs::Convert(bonePose.rot));
+ bone_pose->mutable_position()->CopyFrom(
+ msgs::Convert(bonePose.pos.Ign()));
+ bone_pose->mutable_orientation()->CopyFrom(
+ msgs::Convert(bonePose.rot.Ign()));
LinkPtr parentLink = this->GetChildLink(parentBone->GetName());
math::Pose parentPose = parentLink->GetWorldPose();
math::Matrix4 parentTrans(parentPose.rot.GetAsMatrix4());
@@ -592,8 +685,10 @@ void Actor::SetPose(std::map<std::string, math::Matrix4> _frame,
link_pose->set_name(currentLink->GetScopedName());
link_pose->set_id(currentLink->GetId());
math::Pose linkPose = transform.GetAsPose() - mainLinkPose;
- link_pose->mutable_position()->CopyFrom(msgs::Convert(linkPose.pos));
- link_pose->mutable_orientation()->CopyFrom(msgs::Convert(linkPose.rot));
+ link_pose->mutable_position()->CopyFrom(
+ msgs::Convert(linkPose.pos.Ign()));
+ link_pose->mutable_orientation()->CopyFrom(
+ msgs::Convert(linkPose.rot.Ign()));
currentLink->SetWorldPose(transform.GetAsPose(), true, false);
}
@@ -603,8 +698,10 @@ void Actor::SetPose(std::map<std::string, math::Matrix4> _frame,
msgs::Pose *model_pose = msg.add_pose();
model_pose->set_name(this->GetScopedName());
model_pose->set_id(this->GetId());
- model_pose->mutable_position()->CopyFrom(msgs::Convert(mainLinkPose.pos));
- model_pose->mutable_orientation()->CopyFrom(msgs::Convert(mainLinkPose.rot));
+ model_pose->mutable_position()->CopyFrom(
+ msgs::Convert(mainLinkPose.pos.Ign()));
+ model_pose->mutable_orientation()->CopyFrom(
+ msgs::Convert(mainLinkPose.rot.Ign()));
if (this->bonePosePub && this->bonePosePub->HasConnections())
this->bonePosePub->Publish(msg);
diff --git a/gazebo/physics/Actor.hh b/gazebo/physics/Actor.hh
index c85c2ba..84360fb 100644
--- a/gazebo/physics/Actor.hh
+++ b/gazebo/physics/Actor.hh
@@ -38,7 +38,7 @@ namespace gazebo
namespace physics
{
/// \brief Information about a trajectory for an Actor.
- class GAZEBO_VISIBLE TrajectoryInfo
+ class GZ_PHYSICS_VISIBLE TrajectoryInfo
{
/// \brief Constructor.
public: TrajectoryInfo();
@@ -68,7 +68,7 @@ namespace gazebo
/// \class Actor Actor.hh physics/physics.hh
/// \brief Actor class enables GPU based mesh model / skeleton
/// scriptable animation.
- class GAZEBO_VISIBLE Actor : public Model
+ class GZ_PHYSICS_VISIBLE Actor : public Model
{
/// \brief Constructor
/// \param[in] _parent Parent object
@@ -174,8 +174,19 @@ namespace gazebo
/// \param[in] _frame Each frame name and transform.
/// \param[in] _skelMap Map of bone relationships.
/// \param[in] _time Time over which to animate the set pose.
+ /// \deprecated See SetPose function that accepts an
+ /// ignition::math::Matrix4d object.
private: void SetPose(std::map<std::string, math::Matrix4> _frame,
- std::map<std::string, std::string> _skelMap, double _time);
+ std::map<std::string, std::string> _skelMap, double _time)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set the actor's pose.
+ /// \param[in] _frame Each frame name and transform.
+ /// \param[in] _skelMap Map of bone relationships.
+ /// \param[in] _time Time over which to animate the set pose.
+ private: void SetPose(
+ std::map<std::string, ignition::math::Matrix4d> _frame,
+ std::map<std::string, std::string> _skelMap, double _time);
/// \brief Pointer to the actor's mesh.
protected: const common::Mesh *mesh;
diff --git a/gazebo/physics/BallJoint.hh b/gazebo/physics/BallJoint.hh
index 21ea7f6..57c471f 100644
--- a/gazebo/physics/BallJoint.hh
+++ b/gazebo/physics/BallJoint.hh
@@ -37,7 +37,7 @@ namespace gazebo
///
/// Each physics engine should implement this class.
template< class T>
- class GAZEBO_VISIBLE BallJoint : public T
+ class GZ_PHYSICS_VISIBLE BallJoint : public T
{
/// \brief Constructor
/// \param[in] _parent Pointer to the parent link.
diff --git a/gazebo/physics/Base.cc b/gazebo/physics/Base.cc
index 736bd82..3dc4c2c 100644
--- a/gazebo/physics/Base.cc
+++ b/gazebo/physics/Base.cc
@@ -15,6 +15,14 @@
*
*/
+// We also include this winsock2 trick in Base.hh but it is used last,
+// so we need it again here.
+#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 "gazebo/common/Assert.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Exception.hh"
@@ -385,5 +393,3 @@ const sdf::ElementPtr Base::GetSDF()
this->sdf->Update();
return this->sdf;
}
-
-
diff --git a/gazebo/physics/Base.hh b/gazebo/physics/Base.hh
index 05a3ac1..65fea70 100644
--- a/gazebo/physics/Base.hh
+++ b/gazebo/physics/Base.hh
@@ -23,6 +23,12 @@
#ifndef _GAZEBO_PHYSICS_BASE_HH_
#define _GAZEBO_PHYSICS_BASE_HH_
+#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/enable_shared_from_this.hpp>
#include <string>
@@ -71,7 +77,7 @@ namespace gazebo
/// \class Base Base.hh physics/physics.hh
/// \brief Base class for most physics classes
- class GAZEBO_VISIBLE Base : public boost::enable_shared_from_this<Base>
+ class GZ_PHYSICS_VISIBLE Base : public boost::enable_shared_from_this<Base>
{
/// \enum EntityType
/// \brief Unique identifiers for all entity types.
@@ -109,6 +115,8 @@ namespace gazebo
UNIVERSAL_JOINT = 0x00001000,
/// \brief GearboxJoint type
GEARBOX_JOINT = 0x00002000,
+ /// \brief FixedJoint type
+ FIXED_JOINT = 0x00004000,
/// \brief Shape type
SHAPE = 0x00010000,
diff --git a/gazebo/physics/BoxShape.cc b/gazebo/physics/BoxShape.cc
index 684a153..545a55f 100644
--- a/gazebo/physics/BoxShape.cc
+++ b/gazebo/physics/BoxShape.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/math/Vector3.hh"
#include "gazebo/physics/BoxShape.hh"
@@ -68,11 +74,18 @@ void BoxShape::SetScale(const math::Vector3 &_scale)
void BoxShape::FillMsg(msgs::Geometry &_msg)
{
_msg.set_type(msgs::Geometry::BOX);
- msgs::Set(_msg.mutable_box()->mutable_size(), this->GetSize());
+ msgs::Set(_msg.mutable_box()->mutable_size(), this->GetSize().Ign());
}
//////////////////////////////////////////////////
void BoxShape::ProcessMsg(const msgs::Geometry &_msg)
{
- this->SetSize(msgs::Convert(_msg.box().size()));
+ this->SetSize(msgs::ConvertIgn(_msg.box().size()));
+}
+
+//////////////////////////////////////////////////
+double BoxShape::ComputeVolume() const
+{
+ math::Vector3 size = this->GetSize();
+ return size.x * size.y * size.z;
}
diff --git a/gazebo/physics/BoxShape.hh b/gazebo/physics/BoxShape.hh
index c57278a..93d6b17 100644
--- a/gazebo/physics/BoxShape.hh
+++ b/gazebo/physics/BoxShape.hh
@@ -34,7 +34,7 @@ namespace gazebo
/// \class BoxShape BoxShape.hh physics/physcs.hh
/// \brief Box geometry primitive.
- class GAZEBO_VISIBLE BoxShape : public Shape
+ class GZ_PHYSICS_VISIBLE BoxShape : public Shape
{
/// \brief Constructor.
/// \param[in] _parent Parent Collision.
@@ -65,6 +65,9 @@ namespace gazebo
/// \brief Process a geometry message.
/// \param[in] _msg The message to set values from.
public: virtual void ProcessMsg(const msgs::Geometry &_msg);
+
+ /// Documentation inherited
+ public: virtual double ComputeVolume() const;
};
/// \}
}
diff --git a/gazebo/physics/BoxShape_TEST.cc b/gazebo/physics/BoxShape_TEST.cc
index 9b52e80..53240f1 100644
--- a/gazebo/physics/BoxShape_TEST.cc
+++ b/gazebo/physics/BoxShape_TEST.cc
@@ -43,7 +43,7 @@ TEST_F(BoxShapeTest, Scale)
boxSDF->SetFromString(boxStr.str());
physics::BoxShapePtr box(new physics::BoxShape(physics::CollisionPtr()));
- sdf::ElementPtr elem = boxSDF->root;
+ sdf::ElementPtr elem = boxSDF->Root();
ASSERT_TRUE(elem != NULL);
elem = elem->GetElement("model");
ASSERT_TRUE(elem != NULL);
@@ -131,6 +131,53 @@ TEST_F(BoxShapeTest, Scale)
EXPECT_DOUBLE_EQ(size.z, 2.5);
}
+TEST_F(BoxShapeTest, Volume)
+{
+ std::ostringstream boxStr;
+ boxStr << "<sdf version ='" << SDF_VERSION << "'>"
+ << "<model name='model'>"
+ << "<link name ='link'>"
+ << "<collision name ='collision'>"
+ << "<geometry>"
+ << "<box>"
+ << "<size>1.0 1.0 1.0</size>"
+ << "</box>"
+ << "</geometry>"
+ << "</collision>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr boxSDF(new sdf::SDF);
+ boxSDF->SetFromString(boxStr.str());
+
+ physics::BoxShapePtr box(new physics::BoxShape(physics::CollisionPtr()));
+ sdf::ElementPtr elem = boxSDF->Root();
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("model");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("link");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("collision");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("geometry");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("box");
+ ASSERT_TRUE(elem != NULL);
+ box->Load(elem);
+
+ // A 1x1x1 box has volume 1
+ EXPECT_DOUBLE_EQ(box->ComputeVolume(), 1.0);
+
+ box->SetSize(math::Vector3(2.0, 3.0, 4.0));
+
+ EXPECT_DOUBLE_EQ(box->ComputeVolume(), 24.0);
+
+ // The bounding box approximation should be 0 because the Shape has no
+ // Collision parent
+ EXPECT_DOUBLE_EQ(box->Shape::ComputeVolume(), 0);
+}
+
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
diff --git a/gazebo/physics/CMakeLists.txt b/gazebo/physics/CMakeLists.txt
index 356fadb..ef79380 100644
--- a/gazebo/physics/CMakeLists.txt
+++ b/gazebo/physics/CMakeLists.txt
@@ -16,6 +16,16 @@ if (HAVE_GDAL)
include_directories(${GDAL_INCLUDE_DIR})
endif()
+include_directories(${TBB_INCLUDEDIR})
+
+# Be sure to locate this definition *after* add_subdirectories
+# in order to get the proper visibility when building libs
+if (WIN32)
+ add_definitions(-DBUILDING_DLL_GZ_PHYSICS)
+ include_directories(${libdl_include_dir})
+endif()
+
+
set (sources
Actor.cc
Base.cc
@@ -45,6 +55,7 @@ set (sources
PlaneShape.cc
PolylineShape.cc
Population.cc
+ PresetManager.cc
RayShape.cc
Road.cc
Shape.cc
@@ -90,6 +101,7 @@ set (headers
PlaneShape.hh
PolylineShape.hh
Population.hh
+ PresetManager.hh
RayShape.hh
Road.hh
Shape.hh
@@ -121,6 +133,7 @@ target_link_libraries(gazebo_physics
if (HAVE_BULLET)
target_link_libraries(gazebo_physics gazebo_physics_bullet)
endif()
+
if (HAVE_DART)
target_link_libraries(gazebo_physics gazebo_physics_dart)
endif()
@@ -144,6 +157,7 @@ set (gtest_sources
Inertial_TEST.cc
JointController_TEST.cc
PhysicsEngine_TEST.cc
+ PresetManager_TEST.cc
Road_TEST.cc
SphereShape_TEST.cc
)
diff --git a/gazebo/physics/Collision.cc b/gazebo/physics/Collision.cc
index 90c290b..5779cf1 100644
--- a/gazebo/physics/Collision.cc
+++ b/gazebo/physics/Collision.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/msgs/msgs.hh"
@@ -94,12 +100,6 @@ void Collision::Load(sdf::ElementPtr _sdf)
this->shape->Load(this->sdf->GetElement("geometry")->GetFirstElement());
else
gzwarn << "No shape has been specified. Error!!!\n";
-
- if (!this->shape->HasType(Base::MULTIRAY_SHAPE) &&
- !this->shape->HasType(Base::RAY_SHAPE))
- {
- this->visPub->Publish(this->CreateCollisionVisual());
- }
}
//////////////////////////////////////////////////
@@ -266,7 +266,7 @@ void Collision::UpdateParameters(sdf::ElementPtr _sdf)
//////////////////////////////////////////////////
void Collision::FillMsg(msgs::Collision &_msg)
{
- msgs::Set(_msg.mutable_pose(), this->GetRelativePose());
+ msgs::Set(_msg.mutable_pose(), this->GetRelativePose().Ign());
_msg.set_id(this->GetId());
_msg.set_name(this->GetScopedName());
_msg.set_laser_retro(this->GetLaserRetro());
@@ -274,11 +274,13 @@ void Collision::FillMsg(msgs::Collision &_msg)
this->shape->FillMsg(*_msg.mutable_geometry());
this->surface->FillMsg(*_msg.mutable_surface());
- msgs::Set(this->visualMsg->mutable_pose(), this->GetRelativePose());
+ msgs::Set(this->visualMsg->mutable_pose(), this->GetRelativePose().Ign());
if (!this->HasType(physics::Base::SENSOR_COLLISION))
{
_msg.add_visual()->CopyFrom(*this->visualMsg);
+ // TODO remove the need to create the special collision visual msg and
+ // let the gui handle this.
_msg.add_visual()->CopyFrom(this->CreateCollisionVisual());
}
}
@@ -299,7 +301,7 @@ void Collision::ProcessMsg(const msgs::Collision &_msg)
if (_msg.has_pose())
{
this->link->SetEnabled(true);
- this->SetRelativePose(msgs::Convert(_msg.pose()));
+ this->SetRelativePose(msgs::ConvertIgn(_msg.pose()));
}
if (_msg.has_geometry())
@@ -327,7 +329,8 @@ msgs::Visual Collision::CreateCollisionVisual()
msg.set_parent_id(this->parent->GetId());
msg.set_is_static(this->IsStatic());
msg.set_cast_shadows(false);
- msgs::Set(msg.mutable_pose(), this->GetRelativePose());
+ msg.set_type(msgs::Visual::COLLISION);
+ msgs::Set(msg.mutable_pose(), this->GetRelativePose().Ign());
msg.mutable_material()->mutable_script()->add_uri(
"file://media/materials/scripts/gazebo.material");
msg.mutable_material()->mutable_script()->set_name(
diff --git a/gazebo/physics/Collision.hh b/gazebo/physics/Collision.hh
index 62ab5f7..f795bdd 100644
--- a/gazebo/physics/Collision.hh
+++ b/gazebo/physics/Collision.hh
@@ -41,7 +41,7 @@ namespace gazebo
/// \{
/// \brief Base class for all collision entities
- class GAZEBO_VISIBLE Collision : public Entity
+ class GZ_PHYSICS_VISIBLE Collision : public Entity
{
/// \brief Constructor.
/// \param[in] _link Link that contains this collision object.
diff --git a/gazebo/physics/CollisionState.cc b/gazebo/physics/CollisionState.cc
index daf1471..a8a7a94 100644
--- a/gazebo/physics/CollisionState.cc
+++ b/gazebo/physics/CollisionState.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/physics/Collision.hh"
#include "gazebo/physics/World.hh"
#include "gazebo/physics/CollisionState.hh"
@@ -31,7 +37,8 @@ CollisionState::CollisionState()
/////////////////////////////////////////////////
CollisionState::CollisionState(const CollisionPtr _collision)
: State(_collision->GetName(), _collision->GetWorld()->GetRealTime(),
- _collision->GetWorld()->GetSimTime())
+ _collision->GetWorld()->GetSimTime(),
+ _collision->GetWorld()->GetIterations())
{
this->pose = _collision->GetRelativePose();
}
diff --git a/gazebo/physics/CollisionState.hh b/gazebo/physics/CollisionState.hh
index 4a1369d..a66c0d7 100644
--- a/gazebo/physics/CollisionState.hh
+++ b/gazebo/physics/CollisionState.hh
@@ -39,7 +39,7 @@ namespace gazebo
/// specific time during a simulation run.
///
/// State of a Collision is its Pose.
- class GAZEBO_VISIBLE CollisionState : public State
+ class GZ_PHYSICS_VISIBLE CollisionState : public State
{
/// \brief Default constructor
public: CollisionState();
diff --git a/gazebo/physics/Contact.cc b/gazebo/physics/Contact.cc
index 0b1d596..a95d647 100644
--- a/gazebo/physics/Contact.cc
+++ b/gazebo/physics/Contact.cc
@@ -19,6 +19,12 @@
* Date: 10 Nov 2009
*/
+#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 "gazebo/physics/physics.hh"
#include "gazebo/physics/Collision.hh"
#include "gazebo/physics/Contact.hh"
@@ -87,23 +93,23 @@ Contact &Contact::operator =(const msgs::Contact &_contact)
for (int j = 0; j < _contact.position_size(); ++j)
{
- this->positions[j] = msgs::Convert(_contact.position(j));
+ this->positions[j] = msgs::ConvertIgn(_contact.position(j));
- this->normals[j] = msgs::Convert(_contact.normal(j));
+ this->normals[j] = msgs::ConvertIgn(_contact.normal(j));
this->depths[j] = _contact.depth(j);
this->wrench[j].body1Force =
- msgs::Convert(_contact.wrench(j).body_1_wrench().force());
+ msgs::ConvertIgn(_contact.wrench(j).body_1_wrench().force());
this->wrench[j].body2Force =
- msgs::Convert(_contact.wrench(j).body_2_wrench().force());
+ msgs::ConvertIgn(_contact.wrench(j).body_2_wrench().force());
this->wrench[j].body1Torque =
- msgs::Convert(_contact.wrench(j).body_1_wrench().torque());
+ msgs::ConvertIgn(_contact.wrench(j).body_1_wrench().torque());
this->wrench[j].body2Torque =
- msgs::Convert(_contact.wrench(j).body_2_wrench().torque());
+ msgs::ConvertIgn(_contact.wrench(j).body_2_wrench().torque());
this->count++;
}
@@ -157,8 +163,8 @@ void Contact::FillMsg(msgs::Contact &_msg) const
{
_msg.add_depth(this->depths[j]);
- msgs::Set(_msg.add_position(), this->positions[j]);
- msgs::Set(_msg.add_normal(), this->normals[j]);
+ msgs::Set(_msg.add_position(), this->positions[j].Ign());
+ msgs::Set(_msg.add_normal(), this->normals[j].Ign());
msgs::JointWrench *jntWrench = _msg.add_wrench();
jntWrench->set_body_1_name(this->collision1->GetScopedName());
@@ -167,11 +173,11 @@ void Contact::FillMsg(msgs::Contact &_msg) const
jntWrench->set_body_2_id(this->collision2->GetId());
msgs::Wrench *wrenchMsg = jntWrench->mutable_body_1_wrench();
- msgs::Set(wrenchMsg->mutable_force(), this->wrench[j].body1Force);
- msgs::Set(wrenchMsg->mutable_torque(), this->wrench[j].body1Torque);
+ msgs::Set(wrenchMsg->mutable_force(), this->wrench[j].body1Force.Ign());
+ msgs::Set(wrenchMsg->mutable_torque(), this->wrench[j].body1Torque.Ign());
wrenchMsg = jntWrench->mutable_body_2_wrench();
- msgs::Set(wrenchMsg->mutable_force(), this->wrench[j].body2Force);
- msgs::Set(wrenchMsg->mutable_torque(), this->wrench[j].body2Torque);
+ msgs::Set(wrenchMsg->mutable_force(), this->wrench[j].body2Force.Ign());
+ msgs::Set(wrenchMsg->mutable_torque(), this->wrench[j].body2Torque.Ign());
}
}
diff --git a/gazebo/physics/Contact.hh b/gazebo/physics/Contact.hh
index 6f62669..2ad55d1 100644
--- a/gazebo/physics/Contact.hh
+++ b/gazebo/physics/Contact.hh
@@ -38,7 +38,7 @@
// for proper contact dynamics.
// MAX_CONTACT_JOINTS truncates <max_contacts> specified in SDF
#define MAX_COLLIDE_RETURNS 250
-#define MAX_CONTACT_JOINTS 32
+#define MAX_CONTACT_JOINTS 250
namespace gazebo
{
@@ -51,7 +51,7 @@ namespace gazebo
/// \class Contact Contact.hh physics/physics.hh
/// \brief A contact between two collisions. Each contact can consist of
/// a number of contact points
- class GAZEBO_VISIBLE Contact
+ class GZ_PHYSICS_VISIBLE Contact
{
/// \brief Constructor.
public: Contact();
@@ -91,6 +91,7 @@ namespace gazebo
public: Collision *collision2;
/// \brief Array of forces for the contact.
+ /// All forces and torques are in the world frame.
/// All forces and torques are relative to the center of mass of the
/// respective links that the collision elments are attached to.
public: JointWrench wrench[MAX_CONTACT_JOINTS];
diff --git a/gazebo/physics/ContactManager.cc b/gazebo/physics/ContactManager.cc
index cf617c9..4861f98 100644
--- a/gazebo/physics/ContactManager.cc
+++ b/gazebo/physics/ContactManager.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/transport/Node.hh"
#include "gazebo/transport/Publisher.hh"
#include "gazebo/transport/TransportIface.hh"
diff --git a/gazebo/physics/ContactManager.hh b/gazebo/physics/ContactManager.hh
index fad98e3..69b9fcb 100644
--- a/gazebo/physics/ContactManager.hh
+++ b/gazebo/physics/ContactManager.hh
@@ -38,7 +38,7 @@ namespace gazebo
{
/// \brief A custom contact publisher created for each contact filter
/// in the Contact Manager.
- class GAZEBO_VISIBLE ContactPublisher
+ class GZ_PHYSICS_VISIBLE ContactPublisher
{
/// \brief Contact message publisher
public: transport::PublisherPtr publisher;
@@ -62,7 +62,7 @@ namespace gazebo
/// \class ContactManager ContactManager.hh physics/physics.hh
/// \brief Aggregates all the contact information generated by the
/// collision detection engine.
- class GAZEBO_VISIBLE ContactManager
+ class GZ_PHYSICS_VISIBLE ContactManager
{
/// \brief Constructor.
public: ContactManager();
diff --git a/gazebo/physics/ContactManager_TEST.cc b/gazebo/physics/ContactManager_TEST.cc
index 5f5650f..c236a97 100644
--- a/gazebo/physics/ContactManager_TEST.cc
+++ b/gazebo/physics/ContactManager_TEST.cc
@@ -16,7 +16,7 @@
*/
#include "gazebo/physics/ContactManager.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
@@ -107,6 +107,8 @@ TEST_F(ContactManagerTest, RemoveFilter)
ss << name << i;
std::map<std::string, physics::CollisionPtr> collisions;
collisionMap["collision"] = physics::CollisionPtr();
+ ASSERT_TRUE(collisionMap["collision"] == NULL);
+
manager->CreateFilter(ss.str(), collisions);
EXPECT_TRUE(manager->HasFilter(ss.str()));
EXPECT_EQ(manager->GetFilterCount(), i+1);
diff --git a/gazebo/physics/CylinderShape.cc b/gazebo/physics/CylinderShape.cc
index a49e40a..554e222 100644
--- a/gazebo/physics/CylinderShape.cc
+++ b/gazebo/physics/CylinderShape.cc
@@ -111,3 +111,9 @@ void CylinderShape::ProcessMsg(const msgs::Geometry &_msg)
{
this->SetSize(_msg.cylinder().radius(), _msg.cylinder().length());
}
+
+/////////////////////////////////////////////////
+double CylinderShape::ComputeVolume() const
+{
+ return this->GetLength()*M_PI*pow(this->GetRadius(), 2);
+}
diff --git a/gazebo/physics/CylinderShape.hh b/gazebo/physics/CylinderShape.hh
index ccef124..a54cbcf 100644
--- a/gazebo/physics/CylinderShape.hh
+++ b/gazebo/physics/CylinderShape.hh
@@ -22,6 +22,12 @@
#ifndef _CYLINDERSHAPE_HH_
#define _CYLINDERSHAPE_HH_
+#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 "gazebo/physics/Shape.hh"
#include "gazebo/util/system.hh"
@@ -34,7 +40,7 @@ namespace gazebo
/// \class CylinderShape CylinderShape.hh physics/physics.hh
/// \brief Cylinder collision
- class GAZEBO_VISIBLE CylinderShape : public Shape
+ class GZ_PHYSICS_VISIBLE CylinderShape : public Shape
{
/// \brief Constructor.
/// \param[in] _parent Parent of the shape.
@@ -78,6 +84,9 @@ namespace gazebo
/// \brief Update values based on a message.
/// \param[in] _msg Message to update from.
public: virtual void ProcessMsg(const msgs::Geometry &_msg);
+
+ /// Documentation inherited
+ public: virtual double ComputeVolume() const;
};
/// \}
}
diff --git a/gazebo/physics/CylinderShape_TEST.cc b/gazebo/physics/CylinderShape_TEST.cc
index 950ee31..0231eb4 100644
--- a/gazebo/physics/CylinderShape_TEST.cc
+++ b/gazebo/physics/CylinderShape_TEST.cc
@@ -45,7 +45,7 @@ TEST_F(CylinderShapeTest, Scale)
physics::CylinderShapePtr cylinder(
new physics::CylinderShape(physics::CollisionPtr()));
- sdf::ElementPtr elem = cylinderSDF->root;
+ sdf::ElementPtr elem = cylinderSDF->Root();
ASSERT_TRUE(elem != NULL);
elem = elem->GetElement("model");
ASSERT_TRUE(elem != NULL);
@@ -136,6 +136,53 @@ TEST_F(CylinderShapeTest, Scale)
EXPECT_DOUBLE_EQ(length, 2.5);
}
+TEST_F(CylinderShapeTest, Volume)
+{
+ std::ostringstream cylinderStr;
+ cylinderStr << "<sdf version ='" << SDF_VERSION << "'>"
+ << "<model name='model'>"
+ << "<link name ='link'>"
+ << "<collision name ='collision'>"
+ << "<geometry>"
+ << "<cylinder>"
+ << "<radius>1.0</radius>"
+ << "<length>1.0</length>"
+ << "</cylinder>"
+ << "</geometry>"
+ << "</collision>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr cylinderSDF(new sdf::SDF);
+ cylinderSDF->SetFromString(cylinderStr.str());
+
+ physics::CylinderShapePtr cylinder(
+ new physics::CylinderShape(physics::CollisionPtr()));
+ sdf::ElementPtr elem = cylinderSDF->Root();
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("model");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("link");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("collision");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("geometry");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("cylinder");
+ ASSERT_TRUE(elem != NULL);
+ cylinder->Load(elem);
+
+ EXPECT_FLOAT_EQ(cylinder->ComputeVolume(), M_PI);
+ cylinder->SetLength(3);
+ cylinder->SetRadius(2);
+ EXPECT_FLOAT_EQ(cylinder->ComputeVolume(), M_PI*4*3);
+
+ // The bounding box approximation should be 0 because the Shape has no
+ // Collision parent
+ EXPECT_DOUBLE_EQ(cylinder->Shape::ComputeVolume(), 0);
+}
+
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
diff --git a/gazebo/physics/Entity.cc b/gazebo/physics/Entity.cc
index 70757e7..d6eedbc 100644
--- a/gazebo/physics/Entity.cc
+++ b/gazebo/physics/Entity.cc
@@ -19,6 +19,12 @@
* Date: 03 Apr 2007
*/
+#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/thread/recursive_mutex.hpp>
#include "gazebo/msgs/msgs.hh"
@@ -123,7 +129,14 @@ void Entity::Load(sdf::ElementPtr _sdf)
this->visualMsg->set_parent_name(this->world->GetName());
this->visualMsg->set_parent_id(0);
}
- msgs::Set(this->visualMsg->mutable_pose(), this->GetRelativePose());
+ msgs::Set(this->visualMsg->mutable_pose(), this->GetRelativePose().Ign());
+
+ if (this->HasType(Base::MODEL))
+ this->visualMsg->set_type(msgs::Visual::MODEL);
+ if (this->HasType(Base::LINK))
+ this->visualMsg->set_type(msgs::Visual::LINK);
+ if (this->HasType(Base::COLLISION))
+ this->visualMsg->set_type(msgs::Visual::COLLISION);
this->visPub->Publish(*this->visualMsg);
@@ -532,7 +545,7 @@ void Entity::OnPoseMsg(ConstPosePtr &_msg)
{
if (_msg->name() == this->GetScopedName())
{
- math::Pose p = msgs::Convert(*_msg);
+ ignition::math::Pose3d p = msgs::ConvertIgn(*_msg);
this->SetWorldPose(p);
}
}
@@ -588,8 +601,8 @@ void Entity::UpdateAnimation(const common::UpdateInfo &_info)
this->animation->GetInterpolatedKeyFrame(kf);
math::Pose offset;
- offset.pos = kf.GetTranslation();
- offset.rot = kf.GetRotation();
+ offset.pos = kf.Translation();
+ offset.rot = kf.Rotation();
this->SetWorldPose(offset);
this->prevAnimationTime = _info.simTime;
diff --git a/gazebo/physics/Entity.hh b/gazebo/physics/Entity.hh
index 903166f..e7f06ef 100644
--- a/gazebo/physics/Entity.hh
+++ b/gazebo/physics/Entity.hh
@@ -53,7 +53,7 @@ namespace gazebo
/// \class Entity Entity.hh physics/physics.hh
/// \brief Base class for all physics objects in Gazebo.
- class GAZEBO_VISIBLE Entity : public Base
+ class GZ_PHYSICS_VISIBLE Entity : public Base
{
/// \brief Constructor.
/// \param[in] _parent Parent of the entity.
diff --git a/gazebo/physics/FixedJoint.hh b/gazebo/physics/FixedJoint.hh
new file mode 100644
index 0000000..2096cc3
--- /dev/null
+++ b/gazebo/physics/FixedJoint.hh
@@ -0,0 +1,64 @@
+/*
+ * 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_FIXEDJOINT_HH_
+#define _GAZEBO_FIXEDJOINT_HH_
+
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \addtogroup gazebo_physics
+ /// \{
+
+ /// \class FixedJoint FixedJoint.hh physics/physics.hh
+ /// \brief A fixed joint rigidly connecting two bodies
+ template<class T>
+ class GZ_PHYSICS_VISIBLE FixedJoint : public T
+ {
+ /// \brief Constructor
+ /// \param[in] _parent Parent link
+ public: FixedJoint(BasePtr _parent) : T(_parent)
+ { this->AddType(Base::FIXED_JOINT); }
+ /// \brief Destructor
+ public: virtual ~FixedJoint()
+ { }
+
+ // Documentation inherited.
+ public: virtual unsigned int GetAngleCount() const
+ {return 0;}
+
+ /// \brief Load joint
+ /// \param[in] _sdf Pointer to SDF element
+ public: virtual void Load(sdf::ElementPtr _sdf)
+ {
+ T::Load(_sdf);
+ }
+
+ /// \brief Initialize joint
+ protected: virtual void Init()
+ {
+ T::Init();
+ }
+ };
+ /// \}
+ }
+}
+#endif
+
diff --git a/gazebo/physics/GearboxJoint.hh b/gazebo/physics/GearboxJoint.hh
index d0f95a4..b641159 100644
--- a/gazebo/physics/GearboxJoint.hh
+++ b/gazebo/physics/GearboxJoint.hh
@@ -18,6 +18,12 @@
#ifndef _GEARBOXJOINT_HH_
#define _GEARBOXJOINT_HH_
+#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 "gazebo/math/Angle.hh"
@@ -34,7 +40,7 @@ namespace gazebo
/// \class GearboxJoint GearboxJoint.hh physics/physics.hh
/// \brief A double axis gearbox joint
template<class T>
- class GAZEBO_VISIBLE GearboxJoint : public T
+ class GZ_PHYSICS_VISIBLE GearboxJoint : public T
{
/// \brief Constructor
/// \param[in] _parent Parent link
diff --git a/gazebo/physics/Gripper.cc b/gazebo/physics/Gripper.cc
index aae42ae..f95525a 100644
--- a/gazebo/physics/Gripper.cc
+++ b/gazebo/physics/Gripper.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/common/Events.hh"
#include "gazebo/transport/Node.hh"
diff --git a/gazebo/physics/Gripper.hh b/gazebo/physics/Gripper.hh
index af7501c..ab88969 100644
--- a/gazebo/physics/Gripper.hh
+++ b/gazebo/physics/Gripper.hh
@@ -41,7 +41,7 @@ namespace gazebo
/// will intelligently generate fixed joints between the gripper and an
/// object within the gripper. This allows the object to be manipulated
/// without falling or behaving poorly.
- class GAZEBO_VISIBLE Gripper
+ class GZ_PHYSICS_VISIBLE Gripper
{
/// \brief Constructor
/// \param[in] _model The model which contains the Gripper.
diff --git a/gazebo/physics/HeightmapShape.cc b/gazebo/physics/HeightmapShape.cc
index 8cf21a4..e92c056 100644
--- a/gazebo/physics/HeightmapShape.cc
+++ b/gazebo/physics/HeightmapShape.cc
@@ -19,11 +19,20 @@
* Date: 8 May 2003
*/
+#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 <gazebo/gazebo_config.h>
#ifdef HAVE_GDAL
-# include <gdal/gdalwarper.h>
-# include <gdal/gdal_priv.h>
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wfloat-equal"
+# include <gdalwarper.h>
+# include <gdal_priv.h>
+# pragma GCC diagnostic pop
#endif
#include <algorithm>
@@ -124,7 +133,7 @@ int HeightmapShape::LoadDEMAsTerrain(const std::string &_filename)
if (sphericalCoordinates)
{
- math::Angle latitude, longitude;
+ ignition::math::Angle latitude, longitude;
double elevation;
this->dem.GetGeoReferenceOrigin(latitude, longitude);
@@ -240,7 +249,7 @@ void HeightmapShape::Init()
// Step 1: Construct the heightmap lookup table
this->heightmapData->FillHeightMap(this->subSampling, this->vertSize,
- this->GetSize(), this->scale, this->flipY, this->heights);
+ this->GetSize().Ign(), this->scale.Ign(), this->flipY, this->heights);
}
//////////////////////////////////////////////////
@@ -287,8 +296,8 @@ void HeightmapShape::FillMsg(msgs::Geometry &_msg)
}
}
- msgs::Set(_msg.mutable_heightmap()->mutable_size(), this->GetSize());
- msgs::Set(_msg.mutable_heightmap()->mutable_origin(), this->GetPos());
+ msgs::Set(_msg.mutable_heightmap()->mutable_size(), this->GetSize().Ign());
+ msgs::Set(_msg.mutable_heightmap()->mutable_origin(), this->GetPos().Ign());
_msg.mutable_heightmap()->set_filename(this->img.GetFilename());
}
@@ -384,3 +393,9 @@ common::Image HeightmapShape::GetImage() const
delete [] imageData;
return result;
}
+
+//////////////////////////////////////////////////
+double HeightmapShape::ComputeVolume() const
+{
+ return 0;
+}
diff --git a/gazebo/physics/HeightmapShape.hh b/gazebo/physics/HeightmapShape.hh
index b87167a..8042904 100644
--- a/gazebo/physics/HeightmapShape.hh
+++ b/gazebo/physics/HeightmapShape.hh
@@ -45,7 +45,7 @@ namespace gazebo
/// \brief HeightmapShape collision shape builds a heightmap from
/// an image. The supplied image must be square with
/// N*N+1 pixels per side, where N is an integer.
- class GAZEBO_VISIBLE HeightmapShape : public Shape
+ class GZ_PHYSICS_VISIBLE HeightmapShape : public Shape
{
/// \brief Constructor.
/// \param[in] _parent Parent Collision object.
@@ -97,6 +97,9 @@ namespace gazebo
/// \param[in] _msg Message to update from.
public: virtual void ProcessMsg(const msgs::Geometry &_msg);
+ /// Documentation inherited
+ public: virtual double ComputeVolume() const;
+
/// \brief Get the maximum height.
/// \return The maximum height.
public: float GetMaxHeight() const;
diff --git a/gazebo/physics/Hinge2Joint.hh b/gazebo/physics/Hinge2Joint.hh
index 2ef5a57..4a828aa 100644
--- a/gazebo/physics/Hinge2Joint.hh
+++ b/gazebo/physics/Hinge2Joint.hh
@@ -39,7 +39,7 @@ namespace gazebo
/// \class Hinge2Joint Hinge2Joint.hh physics/physics.hh
/// \brief A two axis hinge joint
template< class T>
- class GAZEBO_VISIBLE Hinge2Joint : public T
+ class GZ_PHYSICS_VISIBLE Hinge2Joint : public T
{
/// \brief Constructor.
/// \param[in] _parent Parent link.
diff --git a/gazebo/physics/HingeJoint.hh b/gazebo/physics/HingeJoint.hh
index e7165aa..965d992 100644
--- a/gazebo/physics/HingeJoint.hh
+++ b/gazebo/physics/HingeJoint.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \class HingeJoint HingeJoint.hh physics/physics.hh
/// \brief A single axis hinge joint
template<class T>
- class GAZEBO_VISIBLE HingeJoint : public T
+ class GZ_PHYSICS_VISIBLE HingeJoint : public T
{
/// \brief Constructor
/// \param[in] _parent Parent link
diff --git a/gazebo/physics/Inertial.cc b/gazebo/physics/Inertial.cc
index 7ce3293..015f1f1 100644
--- a/gazebo/physics/Inertial.cc
+++ b/gazebo/physics/Inertial.cc
@@ -14,7 +14,7 @@
* limitations under the License.
*
*/
-#include "Inertial.hh"
+#include "gazebo/physics/Inertial.hh"
using namespace gazebo;
using namespace physics;
diff --git a/gazebo/physics/Inertial.hh b/gazebo/physics/Inertial.hh
index e4af92a..16fcfcf 100644
--- a/gazebo/physics/Inertial.hh
+++ b/gazebo/physics/Inertial.hh
@@ -17,6 +17,12 @@
#ifndef _INERTIAL_HH_
#define _INERTIAL_HH_
+#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 <sdf/sdf.hh>
@@ -36,7 +42,7 @@ namespace gazebo
/// \class Inertial Inertial.hh physics/physics.hh
/// \brief A class for inertial information about a link
- class GAZEBO_VISIBLE Inertial
+ class GZ_PHYSICS_VISIBLE Inertial
{
/// \brief Default Constructor
public: Inertial();
diff --git a/gazebo/physics/Inertial_TEST.cc b/gazebo/physics/Inertial_TEST.cc
index b53775f..92dd99e 100644
--- a/gazebo/physics/Inertial_TEST.cc
+++ b/gazebo/physics/Inertial_TEST.cc
@@ -17,7 +17,7 @@
#include <gtest/gtest.h>
#include "gazebo/physics/physics.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/Inertial.hh"
#define TOL 1e-6
diff --git a/gazebo/physics/Joint.cc b/gazebo/physics/Joint.cc
index 7fe37b3..6123669 100644
--- a/gazebo/physics/Joint.cc
+++ b/gazebo/physics/Joint.cc
@@ -14,6 +14,13 @@
* 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 "gazebo/transport/TransportIface.hh"
#include "gazebo/transport/Publisher.hh"
@@ -420,7 +427,10 @@ void Joint::UpdateParameters(sdf::ElementPtr _sdf)
//////////////////////////////////////////////////
void Joint::Reset()
{
- this->SetVelocity(0, 0);
+ for (unsigned int i = 0; i < this->GetAngleCount(); ++i)
+ {
+ this->SetVelocity(i, 0.0);
+ }
this->staticAngle.SetFromRadian(0);
}
@@ -448,10 +458,19 @@ void Joint::SetModel(ModelPtr _model)
}
//////////////////////////////////////////////////
-double Joint::GetParam(const std::string &/*_key*/,
- unsigned int /*_index*/)
+double Joint::GetParam(const std::string &_key, unsigned int _index)
{
- gzerr << "GetParam not yet implemented"
+ if (_key == "hi_stop")
+ {
+ return this->GetHighStop(_index).Radian();
+ }
+ else if (_key == "lo_stop")
+ {
+ return this->GetLowStop(_index).Radian();
+ }
+ gzerr << "GetParam unrecognized parameter ["
+ << _key
+ << "]"
<< std::endl;
return 0;
}
@@ -499,6 +518,10 @@ msgs::Joint::Type Joint::GetMsgType() const
{
return msgs::Joint::UNIVERSAL;
}
+ else if (this->HasType(Base::FIXED_JOINT))
+ {
+ return msgs::Joint::FIXED;
+ }
gzerr << "No joint recognized in type ["
<< this->GetType()
@@ -513,7 +536,7 @@ void Joint::FillMsg(msgs::Joint &_msg)
_msg.set_name(this->GetScopedName());
_msg.set_id(this->GetId());
- msgs::Set(_msg.mutable_pose(), this->anchorPose);
+ msgs::Set(_msg.mutable_pose(), this->anchorPose.Ign());
_msg.set_type(this->GetMsgType());
for (unsigned int i = 0; i < this->GetAngleCount(); ++i)
@@ -527,7 +550,7 @@ void Joint::FillMsg(msgs::Joint &_msg)
else
break;
- msgs::Set(axis->mutable_xyz(), this->GetLocalAxis(i));
+ msgs::Set(axis->mutable_xyz(), this->GetLocalAxis(i).Ign());
axis->set_limit_lower(this->GetLowStop(i).Radian());
axis->set_limit_upper(this->GetHighStop(i).Radian());
axis->set_limit_effort(this->GetEffortLimit(i));
@@ -836,9 +859,11 @@ bool Joint::SetVelocityMaximal(unsigned int _index, double _velocity)
//////////////////////////////////////////////////
void Joint::SetState(const JointState &_state)
{
- this->SetVelocity(0, 0);
for (unsigned int i = 0; i < _state.GetAngleCount(); ++i)
+ {
+ this->SetVelocity(i, 0.0);
this->SetPosition(i, _state.GetAngle(i).Radian());
+ }
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/Joint.hh b/gazebo/physics/Joint.hh
index cf357d5..cf4ff2d 100644
--- a/gazebo/physics/Joint.hh
+++ b/gazebo/physics/Joint.hh
@@ -47,7 +47,7 @@ namespace gazebo
/// \class Joint Joint.hh physics/physics.hh
/// \brief Base class for all joints
- class GAZEBO_VISIBLE Joint : public Base
+ class GZ_PHYSICS_VISIBLE Joint : public Base
{
/// \enum Attribute
/// \brief Joint attribute types.
diff --git a/gazebo/physics/JointController.cc b/gazebo/physics/JointController.cc
index d676509..7891121 100644
--- a/gazebo/physics/JointController.cc
+++ b/gazebo/physics/JointController.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/transport/Node.hh"
#include "gazebo/transport/Subscriber.hh"
#include "gazebo/physics/Model.hh"
diff --git a/gazebo/physics/JointController.hh b/gazebo/physics/JointController.hh
index 05f19ea..d94acf9 100644
--- a/gazebo/physics/JointController.hh
+++ b/gazebo/physics/JointController.hh
@@ -40,7 +40,7 @@ namespace gazebo
/// \class JointController JointController.hh physics/physics.hh
/// \brief A class for manipulating physics::Joint
- class GAZEBO_VISIBLE JointController
+ class GZ_PHYSICS_VISIBLE JointController
{
/// \brief Constructor
/// \param[in] _model Model that uses this joint controller.
diff --git a/gazebo/physics/JointState.cc b/gazebo/physics/JointState.cc
index e5bfdc8..67cf9c5 100644
--- a/gazebo/physics/JointState.cc
+++ b/gazebo/physics/JointState.cc
@@ -32,8 +32,8 @@ JointState::JointState()
/////////////////////////////////////////////////
JointState::JointState(JointPtr _joint, const common::Time &_realTime,
- const common::Time &_simTime)
-: State(_joint->GetName(), _realTime, _simTime)
+ const common::Time &_simTime, const uint64_t _iterations)
+: State(_joint->GetName(), _realTime, _simTime, _iterations)
{
// Set the joint angles.
for (unsigned int i = 0; i < _joint->GetAngleCount(); ++i)
@@ -43,7 +43,7 @@ JointState::JointState(JointPtr _joint, const common::Time &_realTime,
/////////////////////////////////////////////////
JointState::JointState(JointPtr _joint)
: State(_joint->GetName(), _joint->GetWorld()->GetRealTime(),
- _joint->GetWorld()->GetSimTime())
+ _joint->GetWorld()->GetSimTime(), _joint->GetWorld()->GetIterations())
{
// Set the joint angles.
for (unsigned int i = 0; i < _joint->GetAngleCount(); ++i)
diff --git a/gazebo/physics/JointState.hh b/gazebo/physics/JointState.hh
index bfb4e4f..7cba7dc 100644
--- a/gazebo/physics/JointState.hh
+++ b/gazebo/physics/JointState.hh
@@ -21,6 +21,12 @@
#ifndef _JOINTSTATE_HH_
#define _JOINTSTATE_HH_
+#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 <vector>
#include <string>
@@ -37,7 +43,7 @@ namespace gazebo
/// \class JointState JointState.hh physics/physics.hh
/// \brief keeps track of state of a physics::Joint
- class GAZEBO_VISIBLE JointState : public State
+ class GZ_PHYSICS_VISIBLE JointState : public State
{
/// \brief Default constructor.
public: JointState();
@@ -46,8 +52,9 @@ namespace gazebo
/// \param[in] _joint Joint to get the state of.
/// \param[in] _realTime Real time stamp.
/// \param[in] _simTime Sim time stamp.
+ /// \param[in] _iterations Simulation iterations.
public: JointState(JointPtr _joint, const common::Time &_realTime,
- const common::Time &_simTime);
+ const common::Time &_simTime, const uint64_t _iterations);
/// \brief Constructor.
/// \param[in] _joint Joint to get the state of.
diff --git a/gazebo/physics/JointWrench.hh b/gazebo/physics/JointWrench.hh
index 9f506ba..d8c685a 100644
--- a/gazebo/physics/JointWrench.hh
+++ b/gazebo/physics/JointWrench.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \brief Wrench information from a joint. These are
/// forces and torques on parent and child Links, relative to
/// the Joint frame immediately after rotation.
- class GAZEBO_VISIBLE JointWrench
+ class GZ_PHYSICS_VISIBLE JointWrench
{
/// \brief Operator =
/// \param[in] _wrench Joint wrench to set from.
diff --git a/gazebo/physics/Link.cc b/gazebo/physics/Link.cc
index 7ec37a3..0fa01f0 100644
--- a/gazebo/physics/Link.cc
+++ b/gazebo/physics/Link.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/msgs/msgs.hh"
@@ -56,7 +62,6 @@ Link::Link(EntityPtr _parent)
this->publishDataMutex = new boost::recursive_mutex();
}
-
//////////////////////////////////////////////////
Link::~Link()
{
@@ -104,6 +109,7 @@ Link::~Link()
this->requestPub.reset();
this->dataPub.reset();
+ this->wrenchSub.reset();
this->connections.clear();
delete this->publishDataMutex;
@@ -115,13 +121,18 @@ Link::~Link()
//////////////////////////////////////////////////
void Link::Load(sdf::ElementPtr _sdf)
{
- bool needUpdate = false;
-
Entity::Load(_sdf);
- // before loading child collsion, we have to figure out of selfCollide is true
- // and modify parent class Entity so this body has its own spaceId
- this->SetSelfCollide(this->sdf->Get<bool>("self_collide"));
+ // before loading child collision, we have to figure out if selfCollide is
+ // true and modify parent class Entity so this body has its own spaceId
+ if (this->sdf->HasElement("self_collide"))
+ {
+ this->SetSelfCollide(this->sdf->Get<bool>("self_collide"));
+ }
+ else
+ {
+ this->SetSelfCollide(this->GetModel()->GetSelfCollide());
+ }
this->sdf->GetElement("self_collide")->GetValue()->SetUpdateFunc(
boost::bind(&Link::GetSelfCollide, this));
@@ -201,21 +212,21 @@ void Link::Load(sdf::ElementPtr _sdf)
this->audioContactsSub = this->node->Subscribe(topic,
&Link::OnCollision, this);
}
-
- needUpdate = true;
}
if (_sdf->HasElement("audio_sink"))
{
- needUpdate = true;
this->audioSink = util::OpenAL::Instance()->CreateSink(
_sdf->GetElement("audio_sink"));
}
#endif
- if (needUpdate)
- this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
- boost::bind(&Link::Update, this, _1)));
+ this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&Link::Update, this, _1)));
+
+ std::string topicName = "~/" + this->GetScopedName() + "/wrench";
+ boost::replace_all(topicName, "::", "/");
+ this->wrenchSub = this->node->Subscribe(topicName, &Link::OnWrenchMsg, this);
}
//////////////////////////////////////////////////
@@ -324,7 +335,7 @@ void Link::UpdateParameters(sdf::ElementPtr _sdf)
if (this->sdf->Get<bool>("gravity") != this->GetGravityMode())
this->SetGravityMode(this->sdf->Get<bool>("gravity"));
- // before loading child collsiion, we have to figure out if
+ // before loading child collision, we have to figure out if
// selfCollide is true and modify parent class Entity so this
// body has its own spaceId
this->SetSelfCollide(this->sdf->Get<bool>("self_collide"));
@@ -341,6 +352,7 @@ void Link::UpdateParameters(sdf::ElementPtr _sdf)
msg.set_name(this->GetScopedName() + "::" + msg.name());
msg.set_parent_name(this->GetScopedName());
msg.set_is_static(this->IsStatic());
+ msg.set_type(msgs::Visual::VISUAL);
this->visPub->Publish(msg);
@@ -437,16 +449,16 @@ void Link::Update(const common::UpdateInfo & /*_info*/)
#ifdef HAVE_OPENAL
if (this->audioSink)
{
- this->audioSink->SetPose(this->GetWorldPose());
- this->audioSink->SetVelocity(this->GetWorldLinearVel());
+ this->audioSink->SetPose(this->GetWorldPose().Ign());
+ this->audioSink->SetVelocity(this->GetWorldLinearVel().Ign());
}
// Update all the audio sources
for (std::vector<util::OpenALSourcePtr>::iterator iter =
this->audioSources.begin(); iter != this->audioSources.end(); ++iter)
{
- (*iter)->SetPose(this->GetWorldPose());
- (*iter)->SetVelocity(this->GetWorldLinearVel());
+ (*iter)->SetPose(this->GetWorldPose().Ign());
+ (*iter)->SetVelocity(this->GetWorldLinearVel().Ign());
}
#endif
@@ -456,6 +468,21 @@ void Link::Update(const common::UpdateInfo & /*_info*/)
this->enabled = this->GetEnabled();
this->enabledSignal(this->enabled);
}*/
+
+ if (!this->wrenchMsgs.empty())
+ {
+ std::vector<msgs::Wrench> messages;
+ {
+ boost::mutex::scoped_lock lock(this->wrenchMsgMutex);
+ messages = this->wrenchMsgs;
+ this->wrenchMsgs.clear();
+ }
+
+ for (auto it : messages)
+ {
+ this->ProcessWrenchMsg(it);
+ }
+ }
}
/////////////////////////////////////////////////
@@ -609,13 +636,26 @@ math::Vector3 Link::GetWorldLinearAccel() const
//////////////////////////////////////////////////
math::Vector3 Link::GetRelativeAngularAccel() const
{
- return this->GetRelativeTorque() / this->inertial->GetMass();
+ return this->GetWorldPose().rot.RotateVectorReverse(
+ this->GetWorldAngularAccel());
}
//////////////////////////////////////////////////
math::Vector3 Link::GetWorldAngularAccel() const
{
- return this->GetWorldTorque() / this->inertial->GetMass();
+ // I: inertia matrix in world frame
+ // T: sum of external torques in world frame
+ // L: angular momentum of CoG in world frame
+ // w: angular velocity in world frame
+ // return I^-1 * (T - w x L)
+ return this->GetWorldInertiaMatrix().Inverse() * (this->GetWorldTorque()
+ - this->GetWorldAngularVel().Cross(this->GetWorldAngularMomentum()));
+}
+
+//////////////////////////////////////////////////
+math::Vector3 Link::GetWorldAngularMomentum() const
+{
+ return this->GetWorldInertiaMatrix() * this->GetWorldAngularVel();
}
//////////////////////////////////////////////////
@@ -747,9 +787,9 @@ void Link::FillMsg(msgs::Link &_msg)
_msg.set_gravity(this->GetGravityMode());
_msg.set_kinematic(this->GetKinematic());
_msg.set_enabled(this->GetEnabled());
- msgs::Set(_msg.mutable_pose(), relPose);
+ msgs::Set(_msg.mutable_pose(), relPose.Ign());
- msgs::Set(this->visualMsg->mutable_pose(), relPose);
+ msgs::Set(this->visualMsg->mutable_pose(), relPose.Ign());
_msg.add_visual()->CopyFrom(*this->visualMsg);
_msg.mutable_inertial()->set_mass(this->inertial->GetMass());
@@ -759,12 +799,16 @@ void Link::FillMsg(msgs::Link &_msg)
_msg.mutable_inertial()->set_iyy(this->inertial->GetIYY());
_msg.mutable_inertial()->set_iyz(this->inertial->GetIYZ());
_msg.mutable_inertial()->set_izz(this->inertial->GetIZZ());
- msgs::Set(_msg.mutable_inertial()->mutable_pose(), this->inertial->GetPose());
+ msgs::Set(_msg.mutable_inertial()->mutable_pose(),
+ this->inertial->GetPose().Ign());
- for (Collision_V::iterator iter = this->collisions.begin();
- iter != this->collisions.end(); ++iter)
+ for (auto &child : this->children)
{
- (*iter)->FillMsg(*_msg.add_collision());
+ if (child->HasType(Base::COLLISION))
+ {
+ CollisionPtr collision = boost::static_pointer_cast<Collision>(child);
+ collision->FillMsg(*_msg.add_collision());
+ }
}
for (std::vector<std::string>::iterator iter = this->sensors.begin();
@@ -775,9 +819,10 @@ void Link::FillMsg(msgs::Link &_msg)
sensor->FillMsg(*_msg.add_sensor());
}
- // Parse visuals from SDF
if (this->visuals.empty())
this->ParseVisuals();
+ else
+ this->UpdateVisualMsg();
for (Visuals_M::iterator iter = this->visuals.begin();
iter != this->visuals.end(); ++iter)
@@ -797,7 +842,7 @@ void Link::FillMsg(msgs::Link &_msg)
proj->set_fov(elem->Get<double>("fov"));
proj->set_near_clip(elem->Get<double>("near_clip"));
proj->set_far_clip(elem->Get<double>("far_clip"));
- msgs::Set(proj->mutable_pose(), elem->Get<math::Pose>("pose"));
+ msgs::Set(proj->mutable_pose(), elem->Get<ignition::math::Pose3d>("pose"));
}
if (this->IsCanonicalLink())
@@ -836,7 +881,7 @@ void Link::ProcessMsg(const msgs::Link &_msg)
if (_msg.has_pose())
{
this->SetEnabled(true);
- this->SetRelativePose(msgs::Convert(_msg.pose()));
+ this->SetRelativePose(msgs::ConvertIgn(_msg.pose()));
}
for (int i = 0; i < _msg.collision_size(); i++)
@@ -985,9 +1030,9 @@ void Link::PublishData()
msgs::Set(this->linkDataMsg.mutable_time(), this->world->GetSimTime());
linkDataMsg.set_name(this->GetScopedName());
msgs::Set(this->linkDataMsg.mutable_linear_velocity(),
- this->GetWorldLinearVel());
+ this->GetWorldLinearVel().Ign());
msgs::Set(this->linkDataMsg.mutable_angular_velocity(),
- this->GetWorldAngularVel());
+ this->GetWorldAngularVel().Ign());
this->dataPub->Publish(this->linkDataMsg);
}
}
@@ -1027,32 +1072,10 @@ void Link::OnCollision(ConstContactsPtr &_msg)
/////////////////////////////////////////////////
void Link::ParseVisuals()
{
- // TODO: this shouldn't be in the physics sim
- if (this->sdf->HasElement("visual"))
- {
- sdf::ElementPtr visualElem = this->sdf->GetElement("visual");
- while (visualElem)
- {
- msgs::Visual msg = msgs::VisualFromSDF(visualElem);
-
- std::string visName = this->GetScopedName() + "::" + msg.name();
- msg.set_name(visName);
- msg.set_id(physics::getUniqueId());
- msg.set_parent_name(this->GetScopedName());
- msg.set_parent_id(this->GetId());
- msg.set_is_static(this->IsStatic());
-
- this->visPub->Publish(msg);
-
- Visuals_M::iterator iter = this->visuals.find(msg.id());
- if (iter != this->visuals.end())
- gzthrow(std::string("Duplicate visual name[")+msg.name()+"]\n");
-
- this->visuals[msg.id()] = msg;
+ this->UpdateVisualMsg();
- visualElem = visualElem->GetNextElement("visual");
- }
- }
+ for (auto const it : this->visuals)
+ this->visPub->Publish(it.second);
}
/////////////////////////////////////////////////
@@ -1094,14 +1117,14 @@ void Link::SetScale(const math::Vector3 &_scale)
}
}
- this->scale = _scale;
+ // update the visual sdf to ensure cloning and saving has the correct values.
+ this->UpdateVisualGeomSDF(_scale);
- // update the visual sdf to ensure cloning gets the correct values.
- this->UpdateVisualSDF();
+ this->scale = _scale;
}
//////////////////////////////////////////////////
-void Link::UpdateVisualSDF()
+void Link::UpdateVisualGeomSDF(const math::Vector3 &_scale)
{
// TODO: this shouldn't be in the physics sim
if (this->sdf->HasElement("visual"))
@@ -1113,22 +1136,85 @@ void Link::UpdateVisualSDF()
if (geomElem->HasElement("box"))
{
- geomElem->GetElement("box")->GetElement("size")->Set(this->scale);
+ math::Vector3 size =
+ geomElem->GetElement("box")->Get<math::Vector3>("size");
+ geomElem->GetElement("box")->GetElement("size")->Set(
+ _scale/this->scale*size);
}
else if (geomElem->HasElement("sphere"))
{
+ // update radius the same way as collision shapes
+ double radius = geomElem->GetElement("sphere")->Get<double>("radius");
+ double newRadius = std::max(_scale.z, std::max(_scale.x, _scale.y));
+ double oldRadius = std::max(this->scale.z,
+ std::max(this->scale.x, this->scale.y));
geomElem->GetElement("sphere")->GetElement("radius")->Set(
- this->scale.x/2.0);
+ newRadius/oldRadius*radius);
}
else if (geomElem->HasElement("cylinder"))
{
- geomElem->GetElement("cylinder")->GetElement("radius")
- ->Set(this->scale.x/2.0);
+ // update radius the same way as collision shapes
+ double radius = geomElem->GetElement("cylinder")->Get<double>("radius");
+ double newRadius = std::max(_scale.x, _scale.y);
+ double oldRadius = std::max(this->scale.x, this->scale.y);
+
+ double length = geomElem->GetElement("cylinder")->Get<double>("length");
+ geomElem->GetElement("cylinder")->GetElement("radius")->Set(
+ newRadius/oldRadius*radius);
geomElem->GetElement("cylinder")->GetElement("length")->Set(
- this->scale.z);
+ _scale.z/this->scale.z*length);
}
else if (geomElem->HasElement("mesh"))
- geomElem->GetElement("mesh")->GetElement("scale")->Set(this->scale);
+ geomElem->GetElement("mesh")->GetElement("scale")->Set(_scale);
+
+ visualElem = visualElem->GetNextElement("visual");
+ }
+ }
+}
+
+//////////////////////////////////////////////////
+void Link::UpdateVisualMsg()
+{
+ // TODO: this shouldn't be in the physics sim
+ if (this->sdf->HasElement("visual"))
+ {
+ sdf::ElementPtr visualElem = this->sdf->GetElement("visual");
+ while (visualElem)
+ {
+ msgs::Visual msg = msgs::VisualFromSDF(visualElem);
+
+ bool newVis = true;
+ std::string linkName = this->GetScopedName();
+
+ // update visual msg if it exists
+ for (auto &iter : this->visuals)
+ {
+ std::string visName = linkName + "::" +
+ visualElem->Get<std::string>("name");
+ if (iter.second.name() == visName)
+ {
+ iter.second.mutable_geometry()->CopyFrom(msg.geometry());
+ newVis = false;
+ break;
+ }
+ }
+
+ // add to visual msgs if not found.
+ if (newVis)
+ {
+ std::string visName = this->GetScopedName() + "::" + msg.name();
+ msg.set_name(visName);
+ msg.set_id(physics::getUniqueId());
+ msg.set_parent_name(this->GetScopedName());
+ msg.set_parent_id(this->GetId());
+ msg.set_is_static(this->IsStatic());
+ msg.set_type(msgs::Visual::VISUAL);
+
+ auto iter = this->visuals.find(msg.id());
+ if (iter != this->visuals.end())
+ gzthrow(std::string("Duplicate visual name[")+msg.name()+"]\n");
+ this->visuals[msg.id()] = msg;
+ }
visualElem = visualElem->GetNextElement("visual");
}
@@ -1326,3 +1412,26 @@ msgs::Visual Link::GetVisualMessage(const std::string &_name) const
return result;
}
+
+//////////////////////////////////////////////////
+void Link::OnWrenchMsg(ConstWrenchPtr &_msg)
+{
+ boost::mutex::scoped_lock lock(this->wrenchMsgMutex);
+ this->wrenchMsgs.push_back(*_msg);
+}
+
+//////////////////////////////////////////////////
+void Link::ProcessWrenchMsg(const msgs::Wrench &_msg)
+{
+ math::Vector3 pos = math::Vector3::Zero;
+ if (_msg.has_force_offset())
+ {
+ pos = msgs::ConvertIgn(_msg.force_offset());
+ }
+
+ const ignition::math::Vector3d force = msgs::ConvertIgn(_msg.force());
+ this->AddLinkForce(force, pos);
+
+ const ignition::math::Vector3d torque = msgs::ConvertIgn(_msg.torque());
+ this->AddRelativeTorque(torque);
+}
diff --git a/gazebo/physics/Link.hh b/gazebo/physics/Link.hh
index 66d30de..9fde3e8 100644
--- a/gazebo/physics/Link.hh
+++ b/gazebo/physics/Link.hh
@@ -21,6 +21,12 @@
#ifndef _LINK_HH_
#define _LINK_HH_
+#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 <map>
#include <vector>
#include <string>
@@ -58,7 +64,7 @@ namespace gazebo
/// \brief Link class defines a rigid body entity, containing
/// information on inertia, visual and collision properties of
/// a rigid body.
- class GAZEBO_VISIBLE Link : public Entity
+ class GZ_PHYSICS_VISIBLE Link : public Entity
{
/// \brief Constructor
/// \param[in] _parent Parent of this link.
@@ -119,7 +125,8 @@ namespace gazebo
/// \brief Set whether this body will collide with others in the
/// model.
- /// \param[in] _collid True to enable collisions.
+ /// \sa GetSelfCollide
+ /// \param[in] _collide True to enable collisions.
public: virtual void SetSelfCollide(bool _collide) = 0;
/// \brief Set the collide mode of the body.
@@ -132,8 +139,12 @@ namespace gazebo
/// ghost: collides with everything else but other ghost
public: void SetCollideMode(const std::string &_mode);
- /// \brief Get Self-Collision Flag, if this is true, this body will
- /// collide with other bodies even if they share the same parent.
+ /// \brief Get Self-Collision Flag.
+ /// Two links within the same model will not collide if both have
+ /// self_collide == false. \n
+ /// link 1 and link2 collide = link1.self_collide || link2.self_collide
+ /// Bodies connected by a joint are exempt from this, and will
+ /// never collide.
/// \return True if self collision is enabled.
public: bool GetSelfCollide() const;
@@ -188,6 +199,15 @@ namespace gazebo
const math::Vector3 &_force,
const math::Vector3 &_relPos) = 0;
+ /// \brief Add a force expressed in the link frame.
+ /// \param[in] _force Direction vector expressed in the link frame. Each
+ /// component corresponds to the force which will be added in that axis
+ /// and the vector's magnitude corresponds to the total force.
+ /// \param[in] _offset Offset position expressed in the link frame. It
+ /// defaults to the link origin.
+ public: virtual void AddLinkForce(const math::Vector3 &_force,
+ const math::Vector3 &_offset = math::Vector3::Zero) = 0;
+
/// \brief Add a torque to the body.
/// \param[in] _torque Torque value to add to the link.
public: virtual void AddTorque(const math::Vector3 &_torque) = 0;
@@ -256,8 +276,19 @@ namespace gazebo
/// \return Angular acceleration of the body.
public: math::Vector3 GetRelativeAngularAccel() const;
- /// \brief Get the angular acceleration of the body in the world
- /// frame.
+ /// \brief Get the angular momentum of the body CoG in the world frame,
+ /// which is computed as (I * w), where
+ /// I: inertia matrix in world frame
+ /// w: angular velocity in world frame
+ /// \return Angular momentum of the body.
+ public: math::Vector3 GetWorldAngularMomentum() const;
+
+ /// \brief Get the angular acceleration of the body in the world frame,
+ /// which is computed as (I^-1 * (T - w x L)), where
+ /// I: inertia matrix in world frame
+ /// T: sum of external torques in world frame
+ /// L: angular momentum of CoG in world frame
+ /// w: angular velocity in world frame
/// \return Angular acceleration of the body in the world frame.
public: math::Vector3 GetWorldAngularAccel() const;
@@ -547,8 +578,21 @@ namespace gazebo
/// \return true if value is in vector.
private: bool ContainsLink(const Link_V &_vector, const LinkPtr &_value);
- /// \brief Update visual SDFs.
- private: void UpdateVisualSDF();
+ /// \brief Update visual SDF's geometry size with the new scale.
+ /// \param[in] _scale New scale applied to the visual
+ private: void UpdateVisualGeomSDF(const math::Vector3 &_scale);
+
+ /// \brief Update visual msgs.
+ private: void UpdateVisualMsg();
+
+ /// \brief Called when a new wrench message arrives. The wrench's force,
+ /// torque and force offset are described in the link frame,
+ /// \param[in] _msg The wrench message.
+ private: void OnWrenchMsg(ConstWrenchPtr &_msg);
+
+ /// \brief Process the message and add force and torque.
+ /// \param[in] _msg The message to set the wrench from.
+ private: void ProcessWrenchMsg(const msgs::Wrench &_msg);
/// \brief Inertial properties.
protected: InertialPtr inertial;
@@ -608,6 +652,15 @@ namespace gazebo
/// \brief Cached list of collisions. This is here for performance.
private: Collision_V collisions;
+ /// \brief Wrench subscriber.
+ private: transport::SubscriberPtr wrenchSub;
+
+ /// \brief Vector of wrench messages to be processed.
+ private: std::vector<msgs::Wrench> wrenchMsgs;
+
+ /// \brief Mutex to protect the wrenchMsgs variable.
+ private: boost::mutex wrenchMsgMutex;
+
#ifdef HAVE_OPENAL
/// \brief All the audio sources
private: std::vector<util::OpenALSourcePtr> audioSources;
diff --git a/gazebo/physics/LinkState.cc b/gazebo/physics/LinkState.cc
index 7bb747c..2350f80 100644
--- a/gazebo/physics/LinkState.cc
+++ b/gazebo/physics/LinkState.cc
@@ -32,8 +32,8 @@ LinkState::LinkState()
/////////////////////////////////////////////////
LinkState::LinkState(const LinkPtr _link, const common::Time &_realTime,
- const common::Time &_simTime)
- : State(_link->GetName(), _realTime, _simTime)
+ const common::Time &_simTime, const uint64_t _iterations)
+ : State(_link->GetName(), _realTime, _simTime, _iterations)
{
this->pose = _link->GetWorldPose();
this->velocity = math::Pose(_link->GetWorldLinearVel(),
@@ -46,7 +46,7 @@ LinkState::LinkState(const LinkPtr _link, const common::Time &_realTime,
/////////////////////////////////////////////////
LinkState::LinkState(const LinkPtr _link)
: State(_link->GetName(), _link->GetWorld()->GetRealTime(),
- _link->GetWorld()->GetSimTime())
+ _link->GetWorld()->GetSimTime(), _link->GetWorld()->GetIterations())
{
this->pose = _link->GetWorldPose();
this->velocity = math::Pose(_link->GetWorldLinearVel(),
@@ -81,12 +81,13 @@ LinkState::~LinkState()
/////////////////////////////////////////////////
void LinkState::Load(const LinkPtr _link, const common::Time &_realTime,
- const common::Time &_simTime)
+ const common::Time &_simTime, const uint64_t _iterations)
{
this->name = _link->GetName();
this->wallTime = common::Time::GetWallTime();
this->realTime = _realTime;
this->simTime = _simTime;
+ this->iterations = _iterations;
this->pose = _link->GetWorldPose();
this->velocity.Set(_link->GetWorldLinearVel(),
@@ -353,3 +354,12 @@ void LinkState::SetSimTime(const common::Time &_time)
(*iter).SetSimTime(_time);
}
}
+
+/////////////////////////////////////////////////
+void LinkState::SetIterations(const uint64_t _iterations)
+{
+ State::SetIterations(_iterations);
+
+ for (auto &collisionState : this->collisionStates)
+ collisionState.SetIterations(_iterations);
+}
diff --git a/gazebo/physics/LinkState.hh b/gazebo/physics/LinkState.hh
index ac4176a..317256a 100644
--- a/gazebo/physics/LinkState.hh
+++ b/gazebo/physics/LinkState.hh
@@ -46,7 +46,7 @@ namespace gazebo
///
/// State of a Link includes the state of itself all its child Collision
/// entities.
- class GAZEBO_VISIBLE LinkState : public State
+ class GZ_PHYSICS_VISIBLE LinkState : public State
{
/// \brief Default constructor
public: LinkState();
@@ -58,8 +58,9 @@ namespace gazebo
/// info.
/// \param[in] _realTime Real time stamp.
/// \param[in] _simTime Sim time stamp
+ /// \param[in] _iterations Simulation iterations.
public: LinkState(const LinkPtr _link, const common::Time &_realTime,
- const common::Time &_simTime);
+ const common::Time &_simTime, const uint64_t _iterations);
/// \brief Constructor
///
@@ -83,9 +84,10 @@ namespace gazebo
/// \param[in] _model Pointer to the Link from which to gather state
/// info.
/// \param[in] _realTime Real time stamp.
- /// \param[in] _simTime Sim time stamp
+ /// \param[in] _simTime Sim time stamp.
+ /// \param[in] _iterations Simulation iterations.
public: void Load(const LinkPtr _link, const common::Time &_realTime,
- const common::Time &_simTime);
+ const common::Time &_simTime, const uint64_t _iterations);
/// \brief Load state from SDF element.
///
@@ -159,6 +161,11 @@ namespace gazebo
/// \param[in] _time Simulation time when the data was recorded.
public: virtual void SetSimTime(const common::Time &_time);
+ /// \brief Set the simulation iterations when this state was generated
+ /// \param[in] _iterations Simulation iterations when the data was
+ /// recorded.
+ public: virtual void SetIterations(const uint64_t _iterations);
+
/// \brief Assignment operator
/// \param[in] _state State value
/// \return this
diff --git a/gazebo/physics/MapShape.cc b/gazebo/physics/MapShape.cc
index b51ee45..4fbbd80 100644
--- a/gazebo/physics/MapShape.cc
+++ b/gazebo/physics/MapShape.cc
@@ -18,6 +18,13 @@
* 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/thread/recursive_mutex.hpp>
#include <string.h>
#include <math.h>
diff --git a/gazebo/physics/MapShape.hh b/gazebo/physics/MapShape.hh
index d3c98c8..99fb707 100644
--- a/gazebo/physics/MapShape.hh
+++ b/gazebo/physics/MapShape.hh
@@ -43,7 +43,7 @@ namespace gazebo
/// \class MapShape MapShape.hh physics/physics.hh
/// \brief Creates box extrusions based on an image.
/// This function is not yet complete, to be implemented.
- class GAZEBO_VISIBLE MapShape : public Shape
+ class GZ_PHYSICS_VISIBLE MapShape : public Shape
{
/// \brief Constructor.
/// \param[in] _parent Parent collision object.
@@ -149,7 +149,7 @@ namespace gazebo
/// \class QuadNode MapShape.hh physics/physics.hh
/// \cond
- class GAZEBO_VISIBLE QuadNode
+ class GZ_PHYSICS_VISIBLE QuadNode
{
/// \brief Constructor
/// \param[in] _parent Parent quad tree node.
diff --git a/gazebo/physics/MeshShape.cc b/gazebo/physics/MeshShape.cc
index b741009..587e826 100644
--- a/gazebo/physics/MeshShape.cc
+++ b/gazebo/physics/MeshShape.cc
@@ -15,6 +15,13 @@
*
*/
+#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/thread/recursive_mutex.hpp>
#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/MeshManager.hh"
@@ -72,18 +79,26 @@ void MeshShape::Init()
if (this->sdf->HasElement("submesh"))
{
sdf::ElementPtr submeshElem = this->sdf->GetElement("submesh");
- this->submesh = new common::SubMesh(
- this->mesh->GetSubMesh(submeshElem->Get<std::string>("name")));
-
- if (!this->submesh)
- gzthrow("Unable to get submesh with name[" +
- submeshElem->Get<std::string>("name") + "]");
-
- // Center the submesh if specified in SDF.
- if (submeshElem->HasElement("center") &&
- submeshElem->Get<bool>("center"))
+ std::string submeshName = submeshElem->Get<std::string>("name");
+ if (submeshName != "__default__" && !submeshName.empty())
{
- this->submesh->Center();
+ const common::SubMesh *smesh = this->mesh->GetSubMesh(submeshName);
+ if (smesh)
+ {
+ this->submesh = new common::SubMesh(
+ this->mesh->GetSubMesh(submeshName));
+
+ if (!this->submesh)
+ gzthrow("Unable to get submesh with name[" +
+ submeshElem->Get<std::string>("name") + "]");
+
+ // Center the submesh if specified in SDF.
+ if (submeshElem->HasElement("center") &&
+ submeshElem->Get<bool>("center"))
+ {
+ this->submesh->Center(ignition::math::Vector3d::Zero);
+ }
+ }
}
}
}
@@ -132,7 +147,7 @@ void MeshShape::FillMsg(msgs::Geometry &_msg)
//////////////////////////////////////////////////
void MeshShape::ProcessMsg(const msgs::Geometry &_msg)
{
- this->SetScale(msgs::Convert(_msg.mesh().scale()));
+ this->SetScale(msgs::ConvertIgn(_msg.mesh().scale()));
this->SetMesh(_msg.mesh().filename(),
_msg.mesh().has_submesh() ? _msg.mesh().submesh() : std::string(),
_msg.mesh().has_center_submesh() ? _msg.mesh().center_submesh() : false);
diff --git a/gazebo/physics/MeshShape.hh b/gazebo/physics/MeshShape.hh
index 5050759..0e88e0a 100644
--- a/gazebo/physics/MeshShape.hh
+++ b/gazebo/physics/MeshShape.hh
@@ -38,7 +38,7 @@ namespace gazebo
/// \class MeshShape MeshShape.hh physics/physics.hh
/// \brief Triangle mesh collision shape
- class GAZEBO_VISIBLE MeshShape : public Shape
+ class GZ_PHYSICS_VISIBLE MeshShape : public Shape
{
/// \brief Constructor.
/// \param[in] _parent Parent collision.
diff --git a/gazebo/physics/Model.cc b/gazebo/physics/Model.cc
index 491915a..b18068b 100644
--- a/gazebo/physics/Model.cc
+++ b/gazebo/physics/Model.cc
@@ -15,6 +15,12 @@
*
*/
+#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 <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
#include <float.h>
@@ -73,8 +79,14 @@ void Model::Load(sdf::ElementPtr _sdf)
boost::bind(&Entity::IsStatic, this));
}
+ if (this->sdf->HasElement("self_collide"))
+ {
+ this->SetSelfCollide(this->sdf->Get<bool>("self_collide"));
+ }
+
if (this->sdf->HasElement("allow_auto_disable"))
this->SetAutoDisable(this->sdf->Get<bool>("allow_auto_disable"));
+
this->LoadLinks();
// Load the joints if the world is already loaded. Otherwise, the World
@@ -322,9 +334,9 @@ void Model::Fini()
{
Entity::Fini();
+ this->plugins.clear();
this->attachedModels.clear();
this->joints.clear();
- this->plugins.clear();
this->links.clear();
this->canonicalLink.reset();
}
@@ -858,11 +870,12 @@ void Model::FillMsg(msgs::Model &_msg)
{
_msg.set_name(this->GetScopedName());
_msg.set_is_static(this->IsStatic());
- msgs::Set(_msg.mutable_pose(), this->GetWorldPose());
+ _msg.set_self_collide(this->GetSelfCollide());
+ msgs::Set(_msg.mutable_pose(), this->GetWorldPose().Ign());
_msg.set_id(this->GetId());
- msgs::Set(_msg.mutable_scale(), this->scale);
+ msgs::Set(_msg.mutable_scale(), this->scale.Ign());
- msgs::Set(this->visualMsg->mutable_pose(), this->GetWorldPose());
+ msgs::Set(this->visualMsg->mutable_pose(), this->GetWorldPose().Ign());
_msg.add_visual()->CopyFrom(*this->visualMsg);
for (Link_V::iterator iter = this->links.begin(); iter != this->links.end();
@@ -896,7 +909,7 @@ void Model::ProcessMsg(const msgs::Model &_msg)
this->SetName(this->world->StripWorldName(_msg.name()));
if (_msg.has_pose())
- this->SetWorldPose(msgs::Convert(_msg.pose()));
+ this->SetWorldPose(msgs::ConvertIgn(_msg.pose()));
for (int i = 0; i < _msg.link_size(); i++)
{
LinkPtr link = this->GetLinkById(_msg.link(i).id());
@@ -908,7 +921,7 @@ void Model::ProcessMsg(const msgs::Model &_msg)
this->SetStatic(_msg.is_static());
if (_msg.has_scale())
- this->SetScale(msgs::Convert(_msg.scale()));
+ this->SetScale(msgs::ConvertIgn(_msg.scale()));
}
//////////////////////////////////////////////////
@@ -1074,6 +1087,18 @@ bool Model::GetAutoDisable() const
}
/////////////////////////////////////////////////
+void Model::SetSelfCollide(bool _self_collide)
+{
+ this->sdf->GetElement("self_collide")->Set(_self_collide);
+}
+
+/////////////////////////////////////////////////
+bool Model::GetSelfCollide() const
+{
+ return this->sdf->Get<bool>("self_collide");
+}
+
+/////////////////////////////////////////////////
void Model::RemoveLink(const std::string &_name)
{
for (Link_V::iterator iter = this->links.begin();
diff --git a/gazebo/physics/Model.hh b/gazebo/physics/Model.hh
index 93e70e0..98e327c 100644
--- a/gazebo/physics/Model.hh
+++ b/gazebo/physics/Model.hh
@@ -49,7 +49,7 @@ namespace gazebo
/// \class Model Model.hh physics/physics.hh
/// \brief A model is a collection of links, joints, and plugins.
- class GAZEBO_VISIBLE Model : public Entity
+ class GZ_PHYSICS_VISIBLE Model : public Entity
{
/// \brief Constructor.
/// \param[in] _parent Parent object.
@@ -178,6 +178,20 @@ namespace gazebo
/// \return Pointer to the link, NULL if the name is invalid.
public: LinkPtr GetLink(const std::string &_name ="canonical") const;
+ /// \brief If true, all links within the model will collide by default.
+ /// Two links within the same model will not collide if both have
+ /// link.self_collide == false.
+ /// link 1 and link2 collide = link1.self_collide || link2.self_collide
+ /// Bodies connected by a joint are exempt from this, and will
+ /// never collide.
+ /// \return True if self-collide enabled for this model, false otherwise.
+ public: bool GetSelfCollide() const;
+
+ /// \brief Set this model's self_collide property
+ /// \sa GetSelfCollide
+ /// \param[in] _self_collide True if self-collisions enabled by default.
+ public: void SetSelfCollide(bool _self_collide);
+
/// \brief Set the gravity mode of the model.
/// \param[in] _value False to turn gravity on for the model.
public: void SetGravityMode(const bool &_value);
diff --git a/gazebo/physics/ModelState.cc b/gazebo/physics/ModelState.cc
index b547d38..c45cccd 100644
--- a/gazebo/physics/ModelState.cc
+++ b/gazebo/physics/ModelState.cc
@@ -32,8 +32,8 @@ ModelState::ModelState()
/////////////////////////////////////////////////
ModelState::ModelState(const ModelPtr _model, const common::Time &_realTime,
- const common::Time &_simTime)
-: State(_model->GetName(), _realTime, _simTime)
+ const common::Time &_simTime, const uint64_t _iterations)
+: State(_model->GetName(), _realTime, _simTime, _iterations)
{
this->pose = _model->GetWorldPose();
@@ -42,7 +42,7 @@ ModelState::ModelState(const ModelPtr _model, const common::Time &_realTime,
for (Link_V::const_iterator iter = links.begin(); iter != links.end(); ++iter)
{
this->linkStates.insert(std::make_pair((*iter)->GetName(),
- LinkState(*iter, _realTime, _simTime)));
+ LinkState(*iter, _realTime, _simTime, _iterations)));
}
// Copy all the joints
@@ -51,14 +51,14 @@ ModelState::ModelState(const ModelPtr _model, const common::Time &_realTime,
iter != joints.end(); ++iter)
{
this->jointStates.insert(std::make_pair((*iter)->GetName(),
- JointState(*iter, _realTime, _simTime)));
+ JointState(*iter, _realTime, _simTime, _iterations)));
}*/
}
/////////////////////////////////////////////////
ModelState::ModelState(const ModelPtr _model)
: State(_model->GetName(), _model->GetWorld()->GetRealTime(),
- _model->GetWorld()->GetSimTime())
+ _model->GetWorld()->GetSimTime(), _model->GetWorld()->GetIterations())
{
this->pose = _model->GetWorldPose();
@@ -94,19 +94,21 @@ ModelState::~ModelState()
/////////////////////////////////////////////////
void ModelState::Load(const ModelPtr _model, const common::Time &_realTime,
- const common::Time &_simTime)
+ const common::Time &_simTime, const uint64_t _iterations)
{
this->name = _model->GetName();
this->wallTime = common::Time::GetWallTime();
this->realTime = _realTime;
this->simTime = _simTime;
+ this->iterations = _iterations;
this->pose = _model->GetWorldPose();
// Load all the links
const Link_V links = _model->GetLinks();
for (Link_V::const_iterator iter = links.begin(); iter != links.end(); ++iter)
{
- this->linkStates[(*iter)->GetName()].Load(*iter, _realTime, _simTime);
+ this->linkStates[(*iter)->GetName()].Load(*iter, _realTime, _simTime,
+ _iterations);
}
// Remove links that no longer exist. We determine this by check the time
@@ -125,7 +127,8 @@ void ModelState::Load(const ModelPtr _model, const common::Time &_realTime,
for (Joint_V::const_iterator iter = joints.begin();
iter != joints.end(); ++iter)
{
- this->jointStates[(*iter)->GetName()].Load(*iter, _realTime, _simTime);
+ this->jointStates[(*iter)->GetName()].Load(*iter, _realTime, _simTime,
+ _iterations);
}
// Remove joints that no longer exist. We determine this by check the time
@@ -533,3 +536,15 @@ void ModelState::SetSimTime(const common::Time &_time)
iter->second.SetSimTime(_time);
}
}
+
+/////////////////////////////////////////////////
+void ModelState::SetIterations(const uint64_t _iterations)
+{
+ State::SetIterations(_iterations);
+
+ for (auto &linkState : this->linkStates)
+ linkState.second.SetIterations(_iterations);
+
+ for (auto &jointState : this->jointStates)
+ jointState.second.SetIterations(_iterations);
+}
diff --git a/gazebo/physics/ModelState.hh b/gazebo/physics/ModelState.hh
index 4fc3018..68dd41f 100644
--- a/gazebo/physics/ModelState.hh
+++ b/gazebo/physics/ModelState.hh
@@ -47,7 +47,7 @@ namespace gazebo
///
/// State of a Model includes the state of all its child Links and
/// Joints.
- class GAZEBO_VISIBLE ModelState : public State
+ class GZ_PHYSICS_VISIBLE ModelState : public State
{
/// \brief Default constructor.
public: ModelState();
@@ -59,8 +59,9 @@ namespace gazebo
/// info.
/// \param[in] _realTime Real time stamp.
/// \param[in] _simTime Sim time stamp.
+ /// \param[in] _iterations Simulation iterations.
public: ModelState(const ModelPtr _model, const common::Time &_realTime,
- const common::Time &_simTime);
+ const common::Time &_simTime, const uint64_t _iterations);
/// \brief Constructor.
///
@@ -85,8 +86,9 @@ namespace gazebo
/// info.
/// \param[in] _realTime Real time stamp.
/// \param[in] _simTime Sim time stamp.
+ /// \param[in] _iterations Simulation iterations.
public: void Load(const ModelPtr _model, const common::Time &_realTime,
- const common::Time &_simTime);
+ const common::Time &_simTime, const uint64_t _iterations);
/// \brief Load state from SDF element.
///
@@ -188,6 +190,11 @@ namespace gazebo
/// \param[in] _time Simulation time when the data was recorded.
public: virtual void SetSimTime(const common::Time &_time);
+ /// \brief Set the simulation iterations when this state was generated
+ /// \param[in] _iterations Simulation iterations when the data was
+ /// recorded.
+ public: virtual void SetIterations(const uint64_t _iterations);
+
/// \brief Assignment operator
/// \param[in] _state State value
/// \return this
diff --git a/gazebo/physics/MultiRayShape.cc b/gazebo/physics/MultiRayShape.cc
index 7afd00c..ef1ce38 100644
--- a/gazebo/physics/MultiRayShape.cc
+++ b/gazebo/physics/MultiRayShape.cc
@@ -14,6 +14,13 @@
* 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 "gazebo/common/Exception.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/MultiRayShape.hh"
@@ -284,3 +291,9 @@ void MultiRayShape::FillMsg(msgs::Geometry &/*_msg*/)
void MultiRayShape::ProcessMsg(const msgs::Geometry &/*_msg*/)
{
}
+
+//////////////////////////////////////////////////
+double MultiRayShape::ComputeVolume() const
+{
+ return 0;
+}
diff --git a/gazebo/physics/MultiRayShape.hh b/gazebo/physics/MultiRayShape.hh
index 8c95e36..1d066c1 100644
--- a/gazebo/physics/MultiRayShape.hh
+++ b/gazebo/physics/MultiRayShape.hh
@@ -38,7 +38,7 @@ namespace gazebo
/// \class MultiRayShape MultiRayShape.hh physics/physics.hh
/// \brief Laser collision contains a set of ray-collisions,
/// structured to simulate a laser range scanner.
- class GAZEBO_VISIBLE MultiRayShape : public Shape
+ class GZ_PHYSICS_VISIBLE MultiRayShape : public Shape
{
/// \brief Constructor.
/// \param[in] _parent Parent collision shape.
@@ -126,6 +126,9 @@ namespace gazebo
/// \param[in] _msg Message to update from.
public: virtual void ProcessMsg(const msgs::Geometry &_msg);
+ /// Documentation inherited
+ public: virtual double ComputeVolume() const;
+
/// \brief Connect a to the new laser scan signal.
/// \param[in] _subscriber Callback function.
/// \return The connection, which must be kept in scope.
diff --git a/gazebo/physics/PhysicsEngine.cc b/gazebo/physics/PhysicsEngine.cc
index 9296a0e..be717ff 100644
--- a/gazebo/physics/PhysicsEngine.cc
+++ b/gazebo/physics/PhysicsEngine.cc
@@ -14,6 +14,13 @@
* 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 <sdf/sdf.hh>
#include "gazebo/msgs/msgs.hh"
@@ -31,6 +38,7 @@
#include "gazebo/physics/Model.hh"
#include "gazebo/physics/World.hh"
#include "gazebo/physics/PhysicsEngine.hh"
+#include "gazebo/physics/PresetManager.hh"
using namespace gazebo;
using namespace physics;
@@ -105,6 +113,15 @@ math::Vector3 PhysicsEngine::GetGravity() const
}
//////////////////////////////////////////////////
+ignition::math::Vector3d PhysicsEngine::MagneticField() const
+{
+ if (this->sdf->HasElement("magnetic_field"))
+ return this->sdf->Get<ignition::math::Vector3d>("magnetic_field");
+ else
+ return ignition::math::Vector3d::Zero;
+}
+
+//////////////////////////////////////////////////
CollisionPtr PhysicsEngine::CreateCollision(const std::string &_shapeType,
const std::string &_linkName)
{
@@ -177,60 +194,145 @@ void PhysicsEngine::SetMaxStepSize(double _stepSize)
}
//////////////////////////////////////////////////
-void PhysicsEngine::SetWorldCFM(double /*_cfm*/)
-{
-}
-
-//////////////////////////////////////////////////
-void PhysicsEngine::SetWorldERP(double /*_erp*/)
-{
-}
-
-//////////////////////////////////////////////////
void PhysicsEngine::SetAutoDisableFlag(bool /*_autoDisable*/)
{
}
//////////////////////////////////////////////////
-void PhysicsEngine::SetContactMaxCorrectingVel(double /*_vel*/)
+void PhysicsEngine::SetMaxContacts(unsigned int /*_maxContacts*/)
{
}
//////////////////////////////////////////////////
-void PhysicsEngine::SetMaxContacts(unsigned int /*_maxContacts*/)
+void PhysicsEngine::OnRequest(ConstRequestPtr &/*_msg*/)
{
}
//////////////////////////////////////////////////
-void PhysicsEngine::OnRequest(ConstRequestPtr &/*_msg*/)
+void PhysicsEngine::OnPhysicsMsg(ConstPhysicsPtr &_msg)
{
+ this->world->GetPresetManager()->CurrentProfile(_msg->profile_name());
}
//////////////////////////////////////////////////
-void PhysicsEngine::OnPhysicsMsg(ConstPhysicsPtr &/*_msg*/)
+bool PhysicsEngine::SetParam(const std::string &_key,
+ const boost::any &_value)
{
+ try
+ {
+ if (_key == "type")
+ {
+ // Cannot set physics engine type from SetParam
+ return false;
+ }
+ if (_key == "max_step_size")
+ this->SetMaxStepSize(boost::any_cast<double>(_value));
+ else if (_key == "real_time_update_rate")
+ this->SetRealTimeUpdateRate(boost::any_cast<double>(_value));
+ else if (_key == "real_time_factor")
+ this->SetTargetRealTimeFactor(boost::any_cast<double>(_value));
+ else if (_key == "gravity")
+ {
+ boost::any copy = _value;
+#ifndef _WIN32
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+ if (_value.type() == typeid(sdf::Vector3))
+ {
+ copy = boost::lexical_cast<ignition::math::Vector3d>
+ (boost::any_cast<sdf::Vector3>(_value));
+ }
+ else if (_value.type() == typeid(math::Vector3))
+ {
+ copy = boost::lexical_cast<ignition::math::Vector3d>
+ (boost::any_cast<math::Vector3>(_value));
+ }
+ this->SetGravity(boost::any_cast<ignition::math::Vector3d>(copy));
+ }
+ else if (_key == "magnetic_field")
+ {
+ boost::any copy = _value;
+ if (_value.type() == typeid(sdf::Vector3))
+ {
+ copy = boost::lexical_cast<ignition::math::Vector3d>
+ (boost::any_cast<sdf::Vector3>(_value));
+ }
+ else if (_value.type() == typeid(math::Vector3))
+ {
+ copy = boost::lexical_cast<ignition::math::Vector3d>
+ (boost::any_cast<math::Vector3>(_value));
+ }
+#ifndef _WIN32
+#pragma GCC diagnostic pop
+#endif
+ this->sdf->GetElement("magnetic_field")->
+ Set(boost::any_cast<ignition::math::Vector3d>(copy));
+ }
+ else
+ {
+ gzwarn << "SetParam failed for [" << _key << "] in physics engine "
+ << this->GetType() << std::endl;
+ return false;
+ }
+ }
+ catch(boost::bad_any_cast &_e)
+ {
+ gzerr << "Caught bad any_cast in PhysicsEngine::SetParam: " << _e.what()
+ << std::endl;
+ return false;
+ }
+ catch(boost::bad_lexical_cast &_e)
+ {
+ gzerr << "Caught bad lexical_cast in PhysicsEngine::SetParam: " << _e.what()
+ << std::endl;
+ return false;
+ }
+ return true;
}
//////////////////////////////////////////////////
-void PhysicsEngine::SetContactSurfaceLayer(double /*_layerDepth*/)
+boost::any PhysicsEngine::GetParam(const std::string &_key) const
{
+ boost::any value;
+ this->PhysicsEngine::GetParam(_key, value);
+ return value;
}
//////////////////////////////////////////////////
-bool PhysicsEngine::SetParam(const std::string &/*_key*/,
- const boost::any &/*_value*/)
+bool PhysicsEngine::GetParam(const std::string &_key,
+ boost::any &_value) const
{
+ if (_key == "type")
+ _value = this->GetType();
+ else if (_key == "max_step_size")
+ _value = this->GetMaxStepSize();
+ else if (_key == "real_time_update_rate")
+ _value = this->GetRealTimeUpdateRate();
+ else if (_key == "real_time_factor")
+ _value = this->GetTargetRealTimeFactor();
+ else if (_key == "gravity")
+ _value = this->GetGravity();
+ else if (_key == "magnetic_field")
+ _value = this->sdf->Get<math::Vector3>("magnetic_field");
+ else
+ {
+ gzwarn << "GetParam failed for [" << _key << "] in physics engine "
+ << this->GetType() << std::endl;
+ return false;
+ }
+
return true;
}
//////////////////////////////////////////////////
-boost::any PhysicsEngine::GetParam(const std::string &/*_key*/) const
+ContactManager *PhysicsEngine::GetContactManager() const
{
- return 0;
+ return this->contactManager;
}
//////////////////////////////////////////////////
-ContactManager *PhysicsEngine::GetContactManager() const
+sdf::ElementPtr PhysicsEngine::GetSDF() const
{
- return this->contactManager;
+ return this->sdf;
}
diff --git a/gazebo/physics/PhysicsEngine.hh b/gazebo/physics/PhysicsEngine.hh
index a28d421..55fbbdb 100644
--- a/gazebo/physics/PhysicsEngine.hh
+++ b/gazebo/physics/PhysicsEngine.hh
@@ -37,7 +37,7 @@ namespace gazebo
/// \class PhysicsEngine PhysicsEngine.hh physics/physics.hh
/// \brief Base class for a physics engine.
- class GAZEBO_VISIBLE PhysicsEngine
+ class GZ_PHYSICS_VISIBLE PhysicsEngine
{
/// \brief Default constructor.
/// \param[in] _world Pointer to the world.
@@ -136,26 +136,32 @@ namespace gazebo
public: virtual JointPtr CreateJoint(const std::string &_type,
ModelPtr _parent = ModelPtr()) = 0;
- /// \brief Return the gavity vector.
- /// \return The gavity vector.
+ /// \brief Return the gravity vector.
+ /// \return The gravity vector.
public: virtual math::Vector3 GetGravity() const;
- /// \brief Set the gavity vector.
+ /// \brief Set the gravity vector.
/// \param[in] _gravity New gravity vector.
public: virtual void SetGravity(
const gazebo::math::Vector3 &_gravity) = 0;
+ /// \brief Return the magnetic field vector.
+ /// \return The magnetic field vector.
+ public: virtual ignition::math::Vector3d MagneticField() const;
+
/// \TODO: Remove this function, and replace it with a more generic
/// property map
/// \brief Access functions to set ODE parameters.
/// \param[in] _cfm Constraint force mixing.
- public: virtual void SetWorldCFM(double _cfm);
+ public: virtual void SetWorldCFM(double /*_cfm*/)
+ GAZEBO_DEPRECATED(6.0) {}
/// \TODO: Remove this function, and replace it with a more generic
/// property map
/// \brief Access functions to set ODE parameters.
/// \param[in] _erp Error reduction parameter.
- public: virtual void SetWorldERP(double _erp);
+ public: virtual void SetWorldERP(double /*_erp*/)
+ GAZEBO_DEPRECATED(6.0) {}
/// \TODO: Remove this function, and replace it with a more generic
/// property map
@@ -167,13 +173,15 @@ namespace gazebo
/// property map
/// \brief Access functions to set ODE parameters.
/// \param[in] _vel Max correcting velocity.
- public: virtual void SetContactMaxCorrectingVel(double _vel);
+ public: virtual void SetContactMaxCorrectingVel(double /*_vel*/)
+ GAZEBO_DEPRECATED(6.0) {}
/// \TODO: Remove this function, and replace it with a more generic
/// property map
/// \brief Access functions to set ODE parameters.
/// \param[in] _layerDepth Surface layer depth
- public: virtual void SetContactSurfaceLayer(double _layerDepth);
+ public: virtual void SetContactSurfaceLayer(double /*_layerDepth*/)
+ GAZEBO_DEPRECATED(6.0) {}
/// \TODO: Remove this function, and replace it with a more generic
/// property map
@@ -185,13 +193,13 @@ namespace gazebo
/// property map
/// \brief Get World CFM.
/// \return World CFM.
- public: virtual double GetWorldCFM() {return 0;}
+ public: virtual double GetWorldCFM() GAZEBO_DEPRECATED(6.0) {return 0;}
/// \TODO: Remove this function, and replace it with a more generic
/// property map
/// \brief Get World ERP.
/// \return World ERP.
- public: virtual double GetWorldERP() {return 0;}
+ public: virtual double GetWorldERP() GAZEBO_DEPRECATED(6.0) {return 0;}
/// \TODO: Remove this function, and replace it with a more generic
/// property map
@@ -203,13 +211,15 @@ namespace gazebo
/// property map.
/// \brief access functions to set ODE parameters.
/// \return Max correcting velocity.
- public: virtual double GetContactMaxCorrectingVel() {return 0;}
+ public: virtual double GetContactMaxCorrectingVel()
+ GAZEBO_DEPRECATED(6.0) {return 0;}
/// \TODO: Remove this function, and replace it with a more generic
/// property map.
/// \brief access functions to set ODE parameters.
/// \return Contact suerface layer depth.
- public: virtual double GetContactSurfaceLayer() {return 0;}
+ public: virtual double GetContactSurfaceLayer()
+ GAZEBO_DEPRECATED(6.0) {return 0;}
/// \TODO: Remove this function, and replace it with a more generic
/// property map.
@@ -225,20 +235,20 @@ namespace gazebo
/// "sequential_impulse' for Bullet, "quick" for ODE
/// "Featherstone and Lemkes" for DART and
/// "Spatial Algebra and Elastic Foundation" for Simbody.
- /// -# "cfm" (double) - global CFM
- /// -# "erp" (double) - global ERP
+ /// -# "cfm" (double) - global CFM (ODE/Bullet)
+ /// -# "erp" (double) - global ERP (ODE/Bullet)
/// -# "precon_iters" (bool) - precondition iterations
- /// (experimental).
+ /// (experimental). (ODE)
/// -# "iters" (int) - number of LCP PGS iterations. If
/// sor_lcp_tolerance is negative, full iteration count is
/// executed. Otherwise, PGS may stop iteration early if
/// sor_lcp_tolerance is satisfied by the total RMS residual.
/// -# "sor" (double) - relaxation parameter for Projected
- /// Gauss-Seidel (PGS) updates.
+ /// Gauss-Seidel (PGS) updates. (ODE/Bullet)
/// -# "contact_max_correcting_vel" (double) - truncates correction
- /// impulses from ERP by this value.
+ /// impulses from ERP by this value. (ODE)
/// -# "contact_surface_layer" (double) - ERP is 0 for
- /// interpenetration depths below this value.
+ /// interpenetration depths below this value. (ODE/Bullet)
/// -# "max_contacts" (int) - max number of contact constraints
/// between any pair of collision bodies.
/// -# "min_step_size" (double) - minimum internal step size.
@@ -257,6 +267,14 @@ namespace gazebo
/// \return The value of the parameter
public: virtual boost::any GetParam(const std::string &_key) const;
+ /// \brief Get a parameter from the physics engine with a boolean to
+ /// indicate success or failure
+ /// \param[in] _key Key of the accessed param
+ /// \param[out] _value Value of the accessed param
+ /// \return True if the parameter was successfully retrieved
+ public: virtual bool GetParam(const std::string &_key,
+ boost::any &_value) const;
+
/// \brief Debug print out of the physic engine state.
public: virtual void DebugPrint() const = 0;
@@ -269,6 +287,10 @@ namespace gazebo
public: boost::recursive_mutex *GetPhysicsUpdateMutex() const
{return this->physicsUpdateMutex;}
+ /// \brief Get a pointer to the SDF element for this physics engine.
+ /// \return Pointer to the physics SDF element.
+ public: sdf::ElementPtr GetSDF() const;
+
/// \brief virtual callback for gztopic "~/request".
/// \param[in] _msg Request message.
protected: virtual void OnRequest(ConstRequestPtr &_msg);
diff --git a/gazebo/physics/PhysicsEngine_TEST.cc b/gazebo/physics/PhysicsEngine_TEST.cc
index a62dc8f..a834c62 100644
--- a/gazebo/physics/PhysicsEngine_TEST.cc
+++ b/gazebo/physics/PhysicsEngine_TEST.cc
@@ -15,8 +15,8 @@
*
*/
-#include "test/ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#include "gazebo/msgs/msgs.hh"
using namespace gazebo;
@@ -26,6 +26,7 @@ class PhysicsEngineTest : public ServerFixture,
{
public: void OnPhysicsMsgResponse(ConstResponsePtr &_msg);
public: void PhysicsEngineParam(const std::string &_physicsEngine);
+ public: void PhysicsEngineGetParamBool(const std::string &_physicsEngine);
public: static msgs::Physics physicsPubMsg;
public: static msgs::Physics physicsResponseMsg;
};
@@ -68,6 +69,8 @@ void PhysicsEngineTest::PhysicsEngineParam(const std::string &_physicsEngine)
type = msgs::Physics::BULLET;
else if (_physicsEngine == "dart")
type = msgs::Physics::DART;
+ else if (_physicsEngine == "simbody")
+ type = msgs::Physics::SIMBODY;
else
type = msgs::Physics::ODE;
physicsPubMsg.set_type(type);
@@ -93,7 +96,7 @@ void PhysicsEngineTest::PhysicsEngineParam(const std::string &_physicsEngine)
EXPECT_DOUBLE_EQ(physicsResponseMsg.real_time_factor(),
physicsPubMsg.real_time_factor());
- // Test PhysicsEngine::GetParam()
+ // Test PhysicsEngine::[GS]etParam()
{
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
boost::any dt = physics->GetParam("max_step_size");
@@ -101,6 +104,65 @@ void PhysicsEngineTest::PhysicsEngineParam(const std::string &_physicsEngine)
physicsPubMsg.max_step_size());
EXPECT_NO_THROW(physics->GetParam("fake_param_name"));
+ EXPECT_NO_THROW(physics->SetParam("fake_param_name", 0));
+
+ // Try SetParam with wrong type
+ EXPECT_NO_THROW(physics->SetParam("iters", std::string("wrong")));
+ }
+
+ {
+ // Test SetParam for non-implementation-specific parameters
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ try
+ {
+ boost::any value;
+ double maxStepSize = 0.02;
+ double realTimeUpdateRate = 0.03;
+ double realTimeFactor = 0.04;
+ ignition::math::Vector3d gravity(0, 0, 0);
+ ignition::math::Vector3d magneticField(0.1, 0.1, 0.1);
+ gazebo::math::Vector3 gravity2(0.2, 0.2, 0.2);
+ gazebo::math::Vector3 magneticField2(0.3, 0.3, 0.3);
+ gzdbg << "Set and Get max_step_size" << std::endl;
+ EXPECT_TRUE(physics->SetParam("max_step_size", maxStepSize));
+ EXPECT_TRUE(physics->GetParam("max_step_size", value));
+ EXPECT_NEAR(boost::any_cast<double>(value), maxStepSize, 1e-6);
+ gzdbg << "Set and Get real_time_update_rate" << std::endl;
+ EXPECT_TRUE(physics->SetParam("real_time_update_rate",
+ realTimeUpdateRate));
+ EXPECT_TRUE(physics->GetParam("real_time_update_rate", value));
+ EXPECT_NEAR(boost::any_cast<double>(value), realTimeUpdateRate, 1e-6);
+ gzdbg << "Set and Get real_time_factor" << std::endl;
+ EXPECT_TRUE(physics->SetParam("real_time_factor",
+ realTimeFactor));
+ EXPECT_TRUE(physics->GetParam("real_time_factor", value));
+ EXPECT_NEAR(boost::any_cast<double>(value), realTimeFactor, 1e-6);
+ gzdbg << "Set gravity as ignition::math::Vector3d" << std::endl;
+ EXPECT_TRUE(physics->SetParam("gravity", gravity));
+ EXPECT_TRUE(physics->GetParam("gravity", value));
+ EXPECT_EQ(boost::any_cast<math::Vector3>(value), math::Vector3(gravity));
+ gzdbg << "Set gravity as gazebo::math::Vector3" << std::endl;
+ EXPECT_TRUE(physics->SetParam("gravity", math::Vector3(gravity2)));
+ EXPECT_TRUE(physics->GetParam("gravity", value));
+ EXPECT_EQ(boost::any_cast<math::Vector3>(value), math::Vector3(gravity2));
+ gzdbg << "Set magnetic_field as ignition::math::Vector3d" << std::endl;
+ EXPECT_TRUE(physics->SetParam("magnetic_field", magneticField));
+ EXPECT_TRUE(physics->GetParam("magnetic_field", value));
+ EXPECT_EQ(boost::any_cast<math::Vector3>(value),
+ math::Vector3(magneticField));
+ gzdbg << "Set magnetic_field as gazebo::math::Vector3" << std::endl;
+ EXPECT_TRUE(physics->SetParam("magnetic_field",
+ math::Vector3(magneticField2)));
+ EXPECT_TRUE(physics->GetParam("magnetic_field", value));
+ EXPECT_EQ(boost::any_cast<math::Vector3>(value),
+ math::Vector3(magneticField2));
+ }
+ catch(boost::bad_any_cast &_e)
+ {
+ std::cout << "Bad any_cast in PhysicsEngine::SetParam test: " << _e.what()
+ << std::endl;
+ FAIL();
+ }
}
physicsNode->Fini();
@@ -112,6 +174,56 @@ TEST_P(PhysicsEngineTest, PhysicsEngineParam)
PhysicsEngineParam(GetParam());
}
+/////////////////////////////////////////////////
+void PhysicsEngineTest::PhysicsEngineGetParamBool
+ (const std::string &_physicsEngine)
+{
+ Load("worlds/empty.world", false, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+
+ // Initialize to failure conditions
+ boost::any value;
+
+ // Test shared physics engine parameter(s)
+ EXPECT_TRUE(physics->GetParam("gravity", value));
+ EXPECT_EQ(boost::any_cast<math::Vector3>(value), math::Vector3(0, 0, -9.8));
+ EXPECT_TRUE(physics->GetParam("max_step_size", value));
+ EXPECT_NEAR(boost::any_cast<double>(value), 0.001, 1e-6);
+ EXPECT_TRUE(physics->GetParam("real_time_factor", value));
+ EXPECT_NEAR(boost::any_cast<double>(value), 1.0, 1e-6);
+ EXPECT_TRUE(physics->GetParam("real_time_update_rate", value));
+ EXPECT_NEAR(boost::any_cast<double>(value), 1000.0, 1e-6);
+ EXPECT_TRUE(physics->GetParam("type", value));
+ EXPECT_EQ(boost::any_cast<std::string>(value), _physicsEngine);
+
+ if (_physicsEngine == "ode" || _physicsEngine == "bullet")
+ {
+ EXPECT_TRUE(physics->GetParam("iters", value));
+ EXPECT_EQ(boost::any_cast<int>(value), 50);
+ }
+ else if (_physicsEngine == "dart")
+ {
+ gzwarn << "DARTPhysics::GetParam not yet implemented." << std::endl;
+ return;
+ }
+ else if (_physicsEngine == "simbody")
+ {
+ EXPECT_TRUE(physics->GetParam("accuracy", value));
+ EXPECT_NEAR(boost::any_cast<double>(value), 1e-3, 1e-6);
+ }
+
+ EXPECT_FALSE(physics->GetParam("param_does_not_exist", value));
+}
+
+/////////////////////////////////////////////////
+TEST_P(PhysicsEngineTest, PhysicsEngineGetParamBool)
+{
+ PhysicsEngineGetParamBool(GetParam());
+}
+
INSTANTIATE_TEST_CASE_P(PhysicsEngines, PhysicsEngineTest,
PHYSICS_ENGINE_VALUES);
diff --git a/gazebo/physics/PhysicsFactory.cc b/gazebo/physics/PhysicsFactory.cc
index 652ce0b..d4db04b 100644
--- a/gazebo/physics/PhysicsFactory.cc
+++ b/gazebo/physics/PhysicsFactory.cc
@@ -27,20 +27,21 @@
#include "gazebo/util/system.hh"
#include "gazebo/gazebo_config.h"
-GAZEBO_VISIBLE
+GZ_PHYSICS_VISIBLE
void RegisterODEPhysics();
#ifdef HAVE_SIMBODY
-GAZEBO_VISIBLE
-void RegisterSimbodyPhysics();
+ GZ_PHYSICS_VISIBLE
+ void RegisterSimbodyPhysics();
#endif
#ifdef HAVE_BULLET
-GAZEBO_VISIBLE
-void RegisterBulletPhysics();
+ GZ_PHYSICS_VISIBLE
+ void RegisterBulletPhysics();
#endif
#ifdef HAVE_DART
+ GZ_PHYSICS_VISIBLE
void RegisterDARTPhysics();
#endif
diff --git a/gazebo/physics/PhysicsFactory.hh b/gazebo/physics/PhysicsFactory.hh
index 4b1888f..4992868 100644
--- a/gazebo/physics/PhysicsFactory.hh
+++ b/gazebo/physics/PhysicsFactory.hh
@@ -42,7 +42,7 @@ namespace gazebo
/// \class PhysicsFactory PhysicsFactory.hh physics/physics.hh
/// \brief The physics factory instantiates different physics engines.
- class GAZEBO_VISIBLE PhysicsFactory
+ class GZ_PHYSICS_VISIBLE PhysicsFactory
{
/// \brief Register everything.
public: static void RegisterAll();
@@ -75,11 +75,11 @@ namespace gazebo
/// \param[in] name Physics type name, as it appears in the world file.
/// \param[in] classname C++ class name for the physics engine.
#define GZ_REGISTER_PHYSICS_ENGINE(name, classname) \
- GAZEBO_VISIBLE PhysicsEnginePtr New##classname(WorldPtr _world) \
+ GZ_PHYSICS_VISIBLE PhysicsEnginePtr New##classname(WorldPtr _world) \
{ \
return PhysicsEnginePtr(new gazebo::physics::classname(_world)); \
} \
- GAZEBO_VISIBLE \
+ GZ_PHYSICS_VISIBLE \
void Register##classname() \
{\
PhysicsFactory::RegisterPhysicsEngine(name, New##classname);\
diff --git a/gazebo/physics/PhysicsIface.cc b/gazebo/physics/PhysicsIface.cc
index 4c7783d..179ae4a 100644
--- a/gazebo/physics/PhysicsIface.cc
+++ b/gazebo/physics/PhysicsIface.cc
@@ -15,6 +15,12 @@
*
*/
+#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/thread/mutex.hpp>
#include "gazebo/common/Console.hh"
#include "gazebo/common/Exception.hh"
@@ -64,11 +70,10 @@ physics::WorldPtr physics::get_world(const std::string &_name)
}
else
{
- for (std::vector<WorldPtr>::iterator iter = g_worlds.begin();
- iter != g_worlds.end(); ++iter)
+ for (auto const &world : g_worlds)
{
- if ((*iter)->GetName() == _name)
- return (*iter);
+ if (world->GetName() == _name)
+ return world;
}
}
@@ -80,41 +85,36 @@ physics::WorldPtr physics::get_world(const std::string &_name)
/////////////////////////////////////////////////
void physics::load_worlds(sdf::ElementPtr _sdf)
{
- std::vector<WorldPtr>::iterator iter;
- for (iter = g_worlds.begin(); iter != g_worlds.end(); ++iter)
- (*iter)->Load(_sdf);
+ for (auto &world : g_worlds)
+ world->Load(_sdf);
}
/////////////////////////////////////////////////
void physics::init_worlds()
{
- std::vector<WorldPtr>::iterator iter;
- for (iter = g_worlds.begin(); iter != g_worlds.end(); ++iter)
- (*iter)->Init();
+ for (auto &world : g_worlds)
+ world->Init();
}
/////////////////////////////////////////////////
void physics::run_worlds(unsigned int _steps)
{
- std::vector<WorldPtr>::iterator iter;
- for (iter = g_worlds.begin(); iter != g_worlds.end(); ++iter)
- (*iter)->Run(_steps);
+ for (auto &world : g_worlds)
+ world->Run(_steps);
}
/////////////////////////////////////////////////
void physics::pause_worlds(bool _pause)
{
- std::vector<WorldPtr>::iterator iter;
- for (iter = g_worlds.begin(); iter != g_worlds.end(); ++iter)
- (*iter)->SetPaused(_pause);
+ for (auto &world : g_worlds)
+ world->SetPaused(_pause);
}
/////////////////////////////////////////////////
void physics::stop_worlds()
{
- std::vector<WorldPtr>::iterator iter;
- for (iter = g_worlds.begin(); iter != g_worlds.end(); ++iter)
- (*iter)->Stop();
+ for (auto &world : g_worlds)
+ world->Stop();
}
/////////////////////////////////////////////////
@@ -150,11 +150,10 @@ void physics::stop_world(WorldPtr _world)
/////////////////////////////////////////////////
void physics::remove_worlds()
{
- for (std::vector<WorldPtr>::iterator iter = g_worlds.begin();
- iter != g_worlds.end(); ++iter)
+ for (auto &world : g_worlds)
{
- (*iter)->Fini();
- (*iter).reset();
+ world->Fini();
+ world.reset();
}
g_worlds.clear();
@@ -163,10 +162,9 @@ void physics::remove_worlds()
/////////////////////////////////////////////////
bool physics::worlds_running()
{
- for (std::vector<WorldPtr>::const_iterator iter = g_worlds.begin();
- iter != g_worlds.end(); ++iter)
+ for (auto const &world : g_worlds)
{
- if ((*iter)->GetRunning())
+ if (world->GetRunning())
return true;
}
diff --git a/gazebo/physics/PhysicsIface.hh b/gazebo/physics/PhysicsIface.hh
index 754ecdd..f81ef85 100644
--- a/gazebo/physics/PhysicsIface.hh
+++ b/gazebo/physics/PhysicsIface.hh
@@ -33,95 +33,95 @@ namespace gazebo
/// \{
/// \brief Setup gazebo::SystemPlugin's and call gazebo::transport::init.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
bool load();
/// \brief Finalize transport by calling gazebo::transport::fini.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
bool fini();
/// \brief Create a world given a name.
/// \param[in] _name Name of the world to create.
/// \return Pointer to the new world.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
WorldPtr create_world(const std::string &_name ="");
/// \brief Returns a pointer to a world by name.
/// \param[in] _name Name of the world to get.
/// \return Pointer to the world.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
WorldPtr get_world(const std::string &_name = "");
/// \brief Load world from sdf::Element pointer.
/// \param[in] _world Pointer to a world.
/// \param[in] _sdf SDF values to load from.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
void load_world(WorldPtr _world, sdf::ElementPtr _sdf);
/// \brief Init world given a pointer to it.
/// \param[in] _world World to initialize.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
void init_world(WorldPtr _world);
/// \brief Run world by calling World::Run() given a pointer to it.
/// \param[in] _world World to run.
/// \param[in] _iterations Number of iterations for each world to take.
/// Zero indicates that each world should continue forever.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
void run_world(WorldPtr _world, unsigned int _iterations = 0);
/// \brief Stop world by calling World::Stop() given a pointer to it.
/// \param[in] _world World to stop.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
void stop_world(WorldPtr _world);
/// \brief Pause world by calling World::SetPaused.
/// \param[in] _world World to pause or unpause.
/// \param[in] _pause True to pause, False to unpause.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
void pause_world(WorldPtr _world, bool _pause);
/// \brief load multiple worlds from single sdf::Element pointer
/// \param[in] _sdf SDF values used to create worlds.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
void load_worlds(sdf::ElementPtr _sdf);
/// \brief initialize multiple worlds stored in static variable
/// gazebo::g_worlds
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
void init_worlds();
/// \brief Run multiple worlds stored in static variable
/// gazebo::g_worlds
/// \param[in] _iterations Number of iterations for each world to take.
/// Zero indicates that each world should continue forever.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
void run_worlds(unsigned int _iterations = 0);
/// \brief stop multiple worlds stored in static variable
/// gazebo::g_worlds
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
void stop_worlds();
/// \brief pause multiple worlds stored in static variable
/// gazebo::g_worlds
/// \param[in] _pause True to pause, False to unpause.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
void pause_worlds(bool pause);
/// \brief remove multiple worlds stored in static variable
/// gazebo::g_worlds
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
void remove_worlds();
/// \brief Return true if any world is running.
/// \return True if any world is running.
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
bool worlds_running();
/// \brief Get a unique ID
/// \return A unique integer
- GAZEBO_VISIBLE
+ GZ_PHYSICS_VISIBLE
uint32_t getUniqueId();
/// \}
}
diff --git a/gazebo/physics/PhysicsTypes.hh b/gazebo/physics/PhysicsTypes.hh
index 3a9fc76..d5b398c 100644
--- a/gazebo/physics/PhysicsTypes.hh
+++ b/gazebo/physics/PhysicsTypes.hh
@@ -38,10 +38,12 @@ namespace gazebo
class Actor;
class Link;
class Collision;
+ class FrictionPyramid;
class Gripper;
class Joint;
class JointController;
class Contact;
+ class PresetManager;
class PhysicsEngine;
class Mass;
class Road;
@@ -105,6 +107,10 @@ namespace gazebo
/// \brief Boost shared pointer to a PhysicsEngine object
typedef boost::shared_ptr<PhysicsEngine> PhysicsEnginePtr;
+ /// \def PresetManagerPtr
+ /// \brief Shared pointer to a PresetManager object
+ typedef boost::shared_ptr<PresetManager> PresetManagerPtr;
+
/// \def ShapePtr
/// \brief Boost shared pointer to a Shape object
typedef boost::shared_ptr<Shape> ShapePtr;
@@ -129,6 +135,10 @@ namespace gazebo
/// \brief Boost shared pointer to a Road object
typedef boost::shared_ptr<Road> RoadPtr;
+ /// \def FrictionPyramidPtr
+ /// \brief Boost shared pointer to a FrictionPyramid object
+ typedef boost::shared_ptr<FrictionPyramid> FrictionPyramidPtr;
+
/// \def SurfaceParamsPtr
/// \brief Boost shared pointer to a SurfaceParams object
typedef boost::shared_ptr<SurfaceParams> SurfaceParamsPtr;
diff --git a/gazebo/physics/PlaneShape.cc b/gazebo/physics/PlaneShape.cc
index e5ec8ea..97a0cd6 100644
--- a/gazebo/physics/PlaneShape.cc
+++ b/gazebo/physics/PlaneShape.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/physics/Collision.hh"
#include "gazebo/physics/PlaneShape.hh"
@@ -92,12 +98,18 @@ void PlaneShape::SetScale(const math::Vector3 &_scale)
void PlaneShape::FillMsg(msgs::Geometry &_msg)
{
_msg.set_type(msgs::Geometry::PLANE);
- msgs::Set(_msg.mutable_plane()->mutable_normal(), this->GetNormal());
- msgs::Set(_msg.mutable_plane()->mutable_size(), this->GetSize());
+ msgs::Set(_msg.mutable_plane()->mutable_normal(), this->GetNormal().Ign());
+ msgs::Set(_msg.mutable_plane()->mutable_size(), this->GetSize().Ign());
}
//////////////////////////////////////////////////
void PlaneShape::ProcessMsg(const msgs::Geometry &_msg)
{
- this->SetNormal(msgs::Convert(_msg.plane().normal()));
+ this->SetNormal(msgs::ConvertIgn(_msg.plane().normal()));
+}
+
+//////////////////////////////////////////////////
+double PlaneShape::ComputeVolume() const
+{
+ return 0;
}
diff --git a/gazebo/physics/PlaneShape.hh b/gazebo/physics/PlaneShape.hh
index 12a7aa2..49bd752 100644
--- a/gazebo/physics/PlaneShape.hh
+++ b/gazebo/physics/PlaneShape.hh
@@ -38,7 +38,7 @@ namespace gazebo
///
/// This collision is used primarily for ground planes. Note that while
/// the plane in infinite, only the part near the camera is drawn.
- class GAZEBO_VISIBLE PlaneShape : public Shape
+ class GZ_PHYSICS_VISIBLE PlaneShape : public Shape
{
/// \brief Constructor.
/// \param[in] _parent Link to which we are attached.
@@ -85,6 +85,9 @@ namespace gazebo
/// this object.
/// \param[in] _msg Message to update from.
public: virtual void ProcessMsg(const msgs::Geometry &_msg);
+
+ /// Documentation inherited
+ public: virtual double ComputeVolume() const;
};
/// \}
}
diff --git a/gazebo/physics/PolylineShape.cc b/gazebo/physics/PolylineShape.cc
index 7a82905..2451466 100644
--- a/gazebo/physics/PolylineShape.cc
+++ b/gazebo/physics/PolylineShape.cc
@@ -39,16 +39,14 @@ PolylineShape::~PolylineShape()
//////////////////////////////////////////////////
void PolylineShape::Init()
{
- this->SetPolylineShape(this->GetHeight(),
- this->GetVertices());
+ this->SetPolylineShape(this->GetHeight(), this->Vertices());
std::string meshName =
- boost::lexical_cast<std::string>(physics::getUniqueId()) +
- "_extruded_polyline";
+ boost::lexical_cast<std::string>(physics::getUniqueId()) +
+ "_extruded_polyline";
common::MeshManager::Instance()->CreateExtrudedPolyline(
- meshName, this->GetVertices(),
- this->GetHeight(), math::Vector2d(1, 1));
+ meshName, this->Vertices(), this->GetHeight());
this->mesh = common::MeshManager::Instance()->GetMesh(meshName);
@@ -57,23 +55,60 @@ void PolylineShape::Init()
}
//////////////////////////////////////////////////
-std::vector<math::Vector2d> PolylineShape::GetVertices() const
+std::vector<std::vector<math::Vector2d> > PolylineShape::GetVertices() const
{
- std::vector<math::Vector2d> vertices;
- sdf::ElementPtr pointElem = this->sdf->GetElement("point");
- while (pointElem)
+ std::vector<std::vector<math::Vector2d> > paths;
+
+ sdf::ElementPtr polylineElem = this->sdf;
+ while (polylineElem)
{
- math::Vector2d point = pointElem->Get<math::Vector2d>();
- pointElem = pointElem->GetNextElement("point");
- vertices.push_back(point);
+ sdf::ElementPtr pointElem = polylineElem->GetElement("point");
+ std::vector<math::Vector2d> vertices;
+ while (pointElem)
+ {
+ math::Vector2d point = pointElem->Get<math::Vector2d>();
+ pointElem = pointElem->GetNextElement("point");
+ vertices.push_back(point);
+ }
+ polylineElem = polylineElem->GetNextElement("polyline");
+ paths.push_back(vertices);
}
- return vertices;
+ return paths;
+}
+
+//////////////////////////////////////////////////
+std::vector<std::vector<ignition::math::Vector2d> >
+PolylineShape::Vertices() const
+{
+ std::vector<std::vector<ignition::math::Vector2d> > paths;
+
+ sdf::ElementPtr polylineElem = this->sdf;
+ while (polylineElem)
+ {
+ sdf::ElementPtr pointElem = polylineElem->GetElement("point");
+ std::vector<ignition::math::Vector2d> vertices;
+ while (pointElem)
+ {
+ ignition::math::Vector2d point =
+ pointElem->Get<ignition::math::Vector2d>();
+ pointElem = pointElem->GetNextElement("point");
+ vertices.push_back(point);
+ }
+ polylineElem = polylineElem->GetNextElement("polyline");
+ paths.push_back(vertices);
+ }
+ return paths;
}
//////////////////////////////////////////////////
void PolylineShape::SetHeight(const double &_height)
{
- this->sdf->GetElement("height")->Set(_height);
+ sdf::ElementPtr polylineElem = this->sdf;
+ while (polylineElem)
+ {
+ polylineElem->GetElement("height")->Set(_height);
+ polylineElem = polylineElem->GetNextElement("polyline");
+ }
}
//////////////////////////////////////////////////
@@ -97,63 +132,88 @@ void PolylineShape::SetScale(const math::Vector3 &_scale)
////////////////////////////////////////////////////
void PolylineShape::SetVertices(const msgs::Geometry &_msg)
{
- if (!_msg.has_polyline())
- {
- gzerr << "Unable to set vertices from message, no polyline shape.\n";
- return;
- }
+ sdf::ElementPtr geomSDF = msgs::GeometryToSDF(_msg);
+ this->sdf = geomSDF->GetElement("polyline");
+}
- sdf::ElementPtr pointElem = this->sdf->GetElement("point");
- for (int i = 0; i < _msg.polyline().point_size(); ++i)
+////////////////////////////////////////////////////
+void PolylineShape::SetVertices(
+ const std::vector<std::vector<ignition::math::Vector2d> > &_vertices)
+{
+ sdf::ElementPtr polylineElem = this->sdf;
+ for (unsigned int i = 0; i < _vertices.size(); ++i)
{
- math::Vector2d point(_msg.polyline().point(i).x(),
- _msg.polyline().point(i).y());
- pointElem->Set(point);
- pointElem = pointElem->GetNextElement("point");
+ std::vector<ignition::math::Vector2d> v = _vertices[i];
+
+ sdf::ElementPtr pointElem = polylineElem->GetElement("point");
+ for (unsigned int j = 0; j < v.size(); ++j)
+ {
+ pointElem->Set(v[j]);
+ pointElem = pointElem->GetNextElement("point");
+ }
+ polylineElem = polylineElem->GetNextElement("polyline");
}
}
////////////////////////////////////////////////////
-void PolylineShape::SetVertices(const std::vector<math::Vector2d> &_vertices)
+void PolylineShape::SetVertices(
+ const std::vector<std::vector<math::Vector2d> > &_vertices)
{
- unsigned int i;
- sdf::ElementPtr pointElem = this->sdf->GetElement("point");
- for (i = 0; i < _vertices.size(); ++i)
+ sdf::ElementPtr polylineElem = this->sdf;
+ for (unsigned int i = 0; i < _vertices.size(); ++i)
{
- pointElem->Set(_vertices[i]);
- pointElem = pointElem->GetNextElement("point");
+ std::vector<math::Vector2d> v = _vertices[i];
+
+ sdf::ElementPtr pointElem = polylineElem->GetElement("point");
+ for (unsigned int j = 0; j < v.size(); ++j)
+ {
+ pointElem->Set(v[j]);
+ pointElem = pointElem->GetNextElement("point");
+ }
+ polylineElem = polylineElem->GetNextElement("polyline");
}
}
//////////////////////////////////////////////////
void PolylineShape::SetPolylineShape(const double &_height,
- const std::vector<math::Vector2d> &_vertices)
+ const std::vector<std::vector<ignition::math::Vector2d> > &_vertices)
{
this->SetHeight(_height);
this->SetVertices(_vertices);
}
//////////////////////////////////////////////////
-void PolylineShape::FillMsg(msgs::Geometry &_msg)
+void PolylineShape::SetPolylineShape(const double &_height,
+ const std::vector<std::vector<math::Vector2d> > &_vertices)
{
- _msg.set_type(msgs::Geometry::POLYLINE);
- _msg.mutable_polyline()->set_height(this->GetHeight());
- sdf::ElementPtr pointElem = this->sdf->GetElement("point");
- while (pointElem)
+ this->SetHeight(_height);
+
+ std::vector<std::vector<ignition::math::Vector2d> > vertices;
+ for (auto const &v : _vertices)
{
- math::Vector2d point = pointElem->Get<math::Vector2d>();
- pointElem = pointElem->GetNextElement("point");
- msgs::Vector2d *ptMsg = _msg.mutable_polyline()->add_point();
- msgs::Set(ptMsg, point);
+ std::vector<ignition::math::Vector2d> vIgn;
+ for (auto const &v1 : v)
+ {
+ vIgn.push_back(v1.Ign());
+ }
+ vertices.push_back(vIgn);
}
+
+ this->SetVertices(vertices);
+}
+
+//////////////////////////////////////////////////
+void PolylineShape::FillMsg(msgs::Geometry &_msg)
+{
+ _msg = msgs::GeometryFromSDF(this->sdf->GetParent());
}
//////////////////////////////////////////////////
void PolylineShape::ProcessMsg(const msgs::Geometry &_msg)
{
- if (_msg.has_polyline())
+ if (_msg.polyline_size() > 0)
{
- this->SetHeight(_msg.polyline().height());
+ this->SetHeight(_msg.polyline(0).height());
this->SetVertices(_msg);
}
else
diff --git a/gazebo/physics/PolylineShape.hh b/gazebo/physics/PolylineShape.hh
index d42f029..efed7dd 100644
--- a/gazebo/physics/PolylineShape.hh
+++ b/gazebo/physics/PolylineShape.hh
@@ -17,6 +17,12 @@
#ifndef _GAZEBO_POLYLINESHAPE_HH_
#define _GAZEBO_POLYLINESHAPE_HH_
+#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 <vector>
#include "gazebo/physics/Shape.hh"
@@ -29,7 +35,7 @@ namespace gazebo
/// \class PolylineShape PolylineShape.hh physics/physcs.hh
/// \brief Polyline geometry primitive.
- class GAZEBO_VISIBLE PolylineShape : public Shape
+ class GZ_PHYSICS_VISIBLE PolylineShape : public Shape
{
/// \brief Constructor.
/// \param[in] _parent Parent Collision.
@@ -41,12 +47,26 @@ namespace gazebo
/// \brief Initialize the polyLine.
public: virtual void Init();
- /// \brief Get the vertices of the polyline
- /// \return The vertex information of the polyline
- public: std::vector<math::Vector2d> GetVertices() const;
+ /// \brief Get the vertices of the polylines
+ /// \return A multidimensional vector of polylines and their vertices.
+ /// Each element in the outer vector consists of a vector of vertices that
+ /// describe one polyline.
+ /// \sa SetVertices
+ /// \deprecated See Vertices function that returns an array of
+ /// ignition::math::Vector2d objects.
+ public: std::vector<std::vector<math::Vector2d> > GetVertices() const
+ GAZEBO_DEPRECATED(6.0);
- /// \brief Get the height of the polyLine.
- /// \return The height of each side of the polyLine.
+ /// \brief Get the vertices of the polylines
+ /// \return A multidimensional vector of polylines and their vertices.
+ /// Each element in the outer vector consists of a vector of vertices that
+ /// describe one polyline.
+ /// \sa SetVertices
+ public: std::vector<std::vector<ignition::math::Vector2d> >
+ Vertices() const;
+
+ /// \brief Get the height of the polylines.
+ /// \return The height of the polylines.
public: double GetHeight() const;
/// \brief Fill in the values for a geomertry message.
@@ -57,15 +77,33 @@ namespace gazebo
/// \param[in] _msg The message to set values from.
public: virtual void ProcessMsg(const msgs::Geometry &_msg);
- /// \brief Set the scale of the polyLine.
- /// \param[in] _scale Scale of the polyLine.
+ /// \brief Set the scale of the polyline.
+ /// \param[in] _scale Scale of the polyline.
private: virtual void SetScale(const math::Vector3 &_scale);
- /// \brief Set the vertices of the polyline
- /// \param[in] _vertices std::vector<math::Vector2d>
- /// containing the vertex information
+ /// \brief Set the vertices of the polylines. The polylines are assumed
+ /// to be closed and non-intersecting. If there is more than one polyline,
+ /// a ray casting algorithm will be used to identify the
+ /// exterior/interior edges and remove the holes in the shape.
+ /// \param[in] _vertices A multidimensional vector of polylines and their
+ /// vertices. Each element in the outer vector consists of a vector of
+ /// vertices that describe one polyline.
+ /// \deprecated See SetVertices function that accepts an
+ /// ignition::math::Vector2d object.
+ private: virtual void SetVertices(
+ const std::vector<std::vector<math::Vector2d> > &_vertices)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set the vertices of the polylines. The polylines are assumed
+ /// to be closed and non-intersecting. If there is more than one polyline,
+ /// a ray casting algorithm will be used to identify the
+ /// exterior/interior edges and remove the holes in the shape.
+ /// \param[in] _vertices A multidimensional vector of polylines and their
+ /// vertices. Each element in the outer vector consists of a vector of
+ /// vertices that describe one polyline.
private: virtual void SetVertices(
- const std::vector<math::Vector2d> &_vertices);
+ const std::vector<std::vector<ignition::math::Vector2d> >
+ &_vertices);
/// \brief Set the vertices of the polyline
/// \param[in] _msg geometry msg containing the vertex information
@@ -75,11 +113,23 @@ namespace gazebo
/// \param[in] _height Height of the polygon
/// \param[in] _vertices std::vector<math::Vector2d>
/// containing the vertex information
+ /// \deprecated See SetPolylineShape that accepts an
+ /// ignition::math::Vector2d object.
+ private: void SetPolylineShape(const double &_height,
+ const std::vector<std::vector<math::Vector2d> > &_vertices)
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set the parameters of polyline shape
+ /// \param[in] _height Height of the polygon
+ /// \param[in] _vertices std::vector<math::Vector2d>
+ /// containing the vertex information
private: void SetPolylineShape(const double &_height,
- const std::vector<math::Vector2d> &_vertices);
+ const std::vector<std::vector<ignition::math::Vector2d> >
+ &_vertices);
- /// \brief Set the height of the polyLine.
- /// \param[in] _height Height of the polyLine.
+ /// \brief Set the height of the polylines. The same height value is set
+ /// for all polylines.
+ /// \param[in] _height Height of the polylines.
private: virtual void SetHeight(const double &_height);
/// \brief Pointer to the mesh data.
diff --git a/gazebo/physics/Population.cc b/gazebo/physics/Population.cc
index a628b7c..66e5f76 100644
--- a/gazebo/physics/Population.cc
+++ b/gazebo/physics/Population.cc
@@ -15,6 +15,12 @@
*
*/
+#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 <vector>
#include <boost/shared_ptr.hpp>
diff --git a/gazebo/physics/Population.hh b/gazebo/physics/Population.hh
index 0474f36..63c70ce 100644
--- a/gazebo/physics/Population.hh
+++ b/gazebo/physics/Population.hh
@@ -39,7 +39,7 @@ namespace gazebo
class PopulationPrivate;
/// \brief Stores all the posible parameters that define a population.
- class GAZEBO_VISIBLE PopulationParams
+ class GZ_PHYSICS_VISIBLE PopulationParams
{
/// \brief The three side lengths of the box.
public: math::Vector3 size;
@@ -82,7 +82,7 @@ namespace gazebo
/// \brief Class that automatically populates an environment with multiple
/// objects based on several parameters to define the number of objects,
/// shape of the object distribution or type of distribution.
- class GAZEBO_VISIBLE Population
+ class GZ_PHYSICS_VISIBLE Population
{
/// \brief Constructor. Load an sdf file containing a population element.
/// \param[in] _sdf SDF parameters.
diff --git a/gazebo/physics/PresetManager.cc b/gazebo/physics/PresetManager.cc
new file mode 100644
index 0000000..9a558a0
--- /dev/null
+++ b/gazebo/physics/PresetManager.cc
@@ -0,0 +1,634 @@
+/*
+ * 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/common/Console.hh"
+#include "gazebo/common/Assert.hh"
+#include "gazebo/physics/PresetManagerPrivate.hh"
+#include "gazebo/physics/PresetManager.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+template<typename T> bool CastAnyValue(const boost::any &_value, T &_return)
+{
+ try
+ {
+ _return = boost::any_cast<T>(_value);
+ }
+ catch(boost::bad_any_cast &_e)
+ {
+ return false;
+ }
+ return true;
+}
+
+//////////////////////////////////////////////////
+Preset::Preset()
+ : dataPtr(new PresetPrivate)
+{
+ GZ_ASSERT(this->dataPtr != NULL, "Data ptr NULL for Preset!");
+}
+
+//////////////////////////////////////////////////
+Preset::Preset(const std::string &_name)
+ : dataPtr(new PresetPrivate(_name))
+{
+ GZ_ASSERT(this->dataPtr != NULL, "Data ptr NULL for Preset!");
+}
+
+//////////////////////////////////////////////////
+Preset::~Preset()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+//////////////////////////////////////////////////
+bool Preset::SetAllPhysicsParameters(PhysicsEnginePtr _physicsEngine) const
+{
+ bool result = true;
+
+ if (!_physicsEngine)
+ {
+ gzwarn << "Physics engine for PresetManager is NULL. PresetManager will "
+ << "have no effect on simulation!" << std::endl;
+ result = false;
+ }
+ else
+ {
+ for (auto const ¶m : this->dataPtr->parameterMap)
+ {
+ // disable params we know can't be set
+ if (param.first != "type" &&
+ !_physicsEngine->SetParam(param.first, param.second))
+ {
+ gzwarn << "Couldn't set parameter [" << param.first
+ << "] in physics engine" << std::endl;
+ result = false;
+ }
+ }
+ }
+
+ return result;
+}
+
+//////////////////////////////////////////////////
+bool Preset::SetAllParamsFromSDF(const sdf::ElementPtr _elem)
+{
+ return this->SetAllParamsHelper(_elem);
+}
+
+//////////////////////////////////////////////////
+bool Preset::SetAllParamsHelper(const sdf::ElementPtr _elem)
+{
+ bool result = true;
+
+ if (!_elem)
+ {
+ return result;
+ }
+
+ // Avoid setting parameters that do not match the physics engine type set in
+ // SDF
+ if (_elem->GetParent() && _elem->GetParent()->GetName() == "physics" &&
+ _elem->GetParent()->Get<std::string>("type") != _elem->GetName())
+ {
+ return result;
+ }
+
+ for (sdf::ElementPtr elem = _elem->GetFirstElement(); elem;
+ elem = elem->GetNextElement())
+ {
+ if (elem->GetValue())
+ {
+ result &= this->SetParam(elem->GetName(), elem->GetAny());
+ }
+ result &= this->SetAllParamsHelper(elem);
+ }
+
+ return result;
+}
+
+//////////////////////////////////////////////////
+std::string Preset::Name() const
+{
+ return this->dataPtr->name;
+}
+
+//////////////////////////////////////////////////
+bool Preset::HasParam(const std::string &_key) const
+{
+ return this->dataPtr->parameterMap.find(_key) !=
+ this->dataPtr->parameterMap.end();
+}
+
+//////////////////////////////////////////////////
+bool Preset::GetParam(const std::string &_key, boost::any &_value) const
+{
+ bool result = true;
+
+ if (!this->HasParam(_key))
+ {
+ gzwarn << "Parameter [" << _key << "] is not a member of profile ["
+ << this->Name() << "]" << std::endl;
+ result = false;
+ }
+ else
+ {
+ _value = this->dataPtr->parameterMap[_key];
+ }
+
+ return result;
+}
+
+//////////////////////////////////////////////////
+bool Preset::SetParam(const std::string &_key, const boost::any &_value)
+{
+ bool result = true;
+
+ if (_key.empty())
+ result = false;
+ else
+ this->dataPtr->parameterMap[_key] = _value;
+
+ return result;
+}
+
+//////////////////////////////////////////////////
+sdf::ElementPtr Preset::SDF() const
+{
+ return this->dataPtr->elementSDF;
+}
+
+//////////////////////////////////////////////////
+bool Preset::SDF(const sdf::ElementPtr _sdfElement)
+{
+ if (!_sdfElement)
+ {
+ gzwarn << "Can't add NULL SDF element to Preset" << std::endl;
+ return false;
+ }
+ if (_sdfElement->GetName() != "physics")
+ {
+ gzwarn << "Can't assign non-physics element to preset profile" << std::endl;
+ return false;
+ }
+ this->dataPtr->elementSDF = _sdfElement;
+ return true;
+}
+
+//////////////////////////////////////////////////
+PresetManager::PresetManager(PhysicsEnginePtr _physicsEngine,
+ const sdf::ElementPtr _sdf)
+ : dataPtr(new PresetManagerPrivate)
+{
+ GZ_ASSERT(this->dataPtr != NULL, "Data pointer NULL for PresetManager!");
+
+ this->dataPtr->physicsEngine = _physicsEngine;
+ if (!_physicsEngine || !_sdf)
+ {
+ gzerr << "Required arguments NULL for PresetManager" << std::endl;
+ return;
+ }
+
+ // Load SDF
+ if (!_sdf->HasElement("physics"))
+ {
+ gzerr << "PresetManager does not support preset profiles for SDF besides "
+ << "physics. No profiles will be made." << std::endl;
+ }
+
+ bool defaultSet = false;
+ for (sdf::ElementPtr physicsElem = _sdf->GetElement("physics"); physicsElem;
+ physicsElem = physicsElem->GetNextElement("physics"))
+ {
+ // Get name attribute
+ std::string name = this->CreateProfile(physicsElem);
+ if (name.empty())
+ {
+ gzlog << "Empty physics profile name specified in SDF. No profile made "
+ << "in PresetManager." << std::endl;
+ }
+ else
+ {
+ if (this->CurrentProfile().empty())
+ {
+ this->CurrentProfile(name);
+ }
+ if (physicsElem->HasAttribute("default"))
+ {
+ if (physicsElem->Get<bool>("default") && !defaultSet)
+ {
+ this->CurrentProfile(name);
+ defaultSet = true;
+ }
+ }
+ }
+ }
+}
+
+//////////////////////////////////////////////////
+PresetManager::~PresetManager()
+{
+ this->dataPtr->presetProfiles.clear();
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+//////////////////////////////////////////////////
+bool PresetManager::CurrentProfile(const std::string &_name)
+{
+ GZ_ASSERT(this->dataPtr->physicsEngine, "Physics engine was NULL");
+
+ if (_name.empty())
+ return false;
+
+ if (!this->HasProfile(_name))
+ {
+ gzwarn << "Profile [" << _name << "] not found." << std::endl;
+ return false;
+ }
+
+ {
+ std::lock_guard<std::mutex> lock(this->dataPtr->currentProfileMutex);
+
+ if (_name == this->CurrentProfile())
+ return true;
+
+ this->dataPtr->currentPreset = _name;
+
+ // For now, ignore the return value of this function, since not all
+ // parameters are supported
+ this->CurrentPreset()->SetAllPhysicsParameters(
+ this->dataPtr->physicsEngine);
+ }
+
+ return true;
+}
+
+//////////////////////////////////////////////////
+std::string PresetManager::CurrentProfile() const
+{
+ return this->dataPtr->currentPreset;
+}
+
+//////////////////////////////////////////////////
+std::vector<std::string> PresetManager::AllProfiles() const
+{
+ std::vector<std::string> ret;
+ for (auto const &profile : this->dataPtr->presetProfiles)
+ {
+ ret.push_back(profile.first);
+ }
+ return ret;
+}
+
+//////////////////////////////////////////////////
+bool PresetManager::SetProfileParam(const std::string &_profileName,
+ const std::string &_key, const boost::any &_value)
+{
+ if (_profileName == this->CurrentProfile())
+ {
+ return this->SetCurrentProfileParam(_key, _value);
+ }
+
+ auto iter = this->dataPtr->presetProfiles.find(_profileName);
+
+ if (iter == this->dataPtr->presetProfiles.end())
+ {
+ gzwarn << "Invalid profile name: [" << _profileName << "]" << std::endl;
+ return false;
+ }
+
+ if (!iter->second.HasParam(_key))
+ {
+ gzwarn << "Profile [" << _profileName << "] does not have key [" << _key
+ << "], so it was not set." << std::endl;
+ return false;
+ }
+
+ if (!iter->second.SetParam(_key, _value))
+ {
+ gzwarn << "Could not set key [" << _key
+ << "] for profile [" << _profileName << "]." << std::endl;
+ return false;
+ }
+ return true;
+}
+
+//////////////////////////////////////////////////
+bool PresetManager::GetProfileParam(const std::string &_name,
+ const std::string &_key, boost::any &_value) const
+{
+ if (!this->HasProfile(_name))
+ {
+ return false;
+ }
+ return this->dataPtr->presetProfiles[_name].GetParam(_key, _value);
+}
+
+//////////////////////////////////////////////////
+bool PresetManager::SetCurrentProfileParam(const std::string &_key,
+ const boost::any &_value)
+{
+ if (this->CurrentProfile().empty() || this->CurrentPreset() == NULL)
+ {
+ return false;
+ }
+
+ std::lock_guard<std::mutex> lock(this->dataPtr->currentProfileMutex);
+ if (!this->CurrentPreset()->HasParam(_key))
+ {
+ return false;
+ }
+ if (!this->CurrentPreset()->SetParam(_key, _value))
+ return false;
+ try
+ {
+ return this->dataPtr->physicsEngine->SetParam(_key, _value);
+ }
+ catch(const boost::bad_any_cast &e)
+ {
+ gzerr << "Couldn't set physics engine parameter[" << e.what()
+ << "]" << std::endl;
+ return false;
+ }
+ return true;
+}
+
+//////////////////////////////////////////////////
+bool PresetManager::GetCurrentProfileParam(const std::string &_key,
+ boost::any &_value)
+{
+ if (this->CurrentProfile().empty())
+ {
+ return false;
+ }
+ return this->CurrentPreset()->GetParam(_key, _value);
+}
+
+//////////////////////////////////////////////////
+bool PresetManager::CreateProfile(const std::string &_name)
+{
+ if (_name.empty())
+ {
+ gzwarn << "Specified profile name was empty. Aborting." << std::endl;
+ return false;
+ }
+ if (this->HasProfile(_name))
+ {
+ gzwarn << "Warning: profile [" << _name << "] already exists! Overwriting."
+ << std::endl;
+ }
+ this->dataPtr->presetProfiles.emplace(_name, _name);
+
+ if (!this->ProfileSDF(_name, this->dataPtr->physicsEngine->GetSDF()))
+ return false;
+
+ return true;
+}
+
+//////////////////////////////////////////////////
+std::string PresetManager::CreateProfile(const sdf::ElementPtr _elem)
+{
+ if (!_elem || !_elem->HasAttribute("name"))
+ {
+ return "";
+ }
+ const std::string name = _elem->Get<std::string>("name");
+ if (name.empty())
+ return "";
+
+ if (!this->CreateProfile(name))
+ return "";
+
+ // Make a copy of this SDF element.
+ this->ProfileSDF(name, _elem);
+ return name;
+}
+
+//////////////////////////////////////////////////
+void PresetManager::RemoveProfile(const std::string &_name)
+{
+ if (!this->HasProfile(_name))
+ {
+ gzwarn << "Cannot remove non-existent profile [" << _name << "]"
+ << std::endl;
+ return;
+ }
+
+ if (_name == this->CurrentProfile())
+ {
+ gzmsg << "deselecting current preset " << _name << std::endl;
+ this->dataPtr->currentPreset = "";
+ }
+
+ this->dataPtr->presetProfiles.erase(_name);
+}
+
+//////////////////////////////////////////////////
+bool PresetManager::HasProfile(const std::string &_name) const
+{
+ return this->dataPtr->presetProfiles.find(_name) !=
+ this->dataPtr->presetProfiles.end();
+}
+
+//////////////////////////////////////////////////
+sdf::ElementPtr PresetManager::ProfileSDF(const std::string &_name) const
+{
+ if (_name.empty() || !this->HasProfile(_name))
+ {
+ gzerr << "Profile [" << _name << "] does not exist. Returning NULL "
+ << "SDF pointer." << std::endl;
+ return NULL;
+ }
+
+ return this->dataPtr->presetProfiles[_name].SDF();
+}
+
+//////////////////////////////////////////////////
+bool PresetManager::ProfileSDF(const std::string &_name,
+ const sdf::ElementPtr _sdf)
+{
+ if (!_sdf)
+ {
+ gzwarn << "Received NULL SDF element pointer in ProfileSDF" << std::endl;
+ return false;
+ }
+
+ auto iter = this->dataPtr->presetProfiles.find(_name);
+
+ if (iter == this->dataPtr->presetProfiles.end() || !iter->second.SDF(_sdf))
+ return false;
+
+ this->GeneratePresetFromSDF(_sdf, iter->second);
+
+ if (_name == this->CurrentProfile())
+ {
+ std::lock_guard<std::mutex> lock(this->dataPtr->currentProfileMutex);
+ this->CurrentPreset()->SetAllPhysicsParameters(
+ this->dataPtr->physicsEngine);
+ }
+
+ iter->second.SetAllParamsFromSDF(_sdf);
+ return true;
+}
+
+//////////////////////////////////////////////////
+void PresetManager::GeneratePresetFromSDF(const sdf::ElementPtr _elem,
+ Preset &_preset) const
+{
+ GZ_ASSERT(_elem, "NULL elem in GeneratePresetFromSDF, should never happen");
+
+ // Check for physics engine type. If the type specified in SDF doesn't match
+ // the type of the physics engine, don't add the children to the parameter
+ // map. This will be changed in the future when switching the type of the
+ // physics engine is allowed.
+ GZ_ASSERT(this->dataPtr->physicsEngine, "No physics engine in PresetManager");
+ if (_elem->GetParent() && _elem->GetParent()->GetName() == "physics" &&
+ this->dataPtr->physicsEngine->GetType() != _elem->GetName())
+ {
+ return;
+ }
+
+ for (sdf::ElementPtr elem = _elem->GetFirstElement(); elem;
+ elem = elem->GetNextElement())
+ {
+ if (elem->GetValue() != NULL)
+ {
+ _preset.SetParam(elem->GetName(), elem->GetAny());
+ }
+ this->GeneratePresetFromSDF(elem, _preset);
+ }
+}
+
+//////////////////////////////////////////////////
+void PresetManager::GenerateSDFFromPreset(const std::string &_name,
+ sdf::ElementPtr &_elem) const
+{
+ auto iter = this->dataPtr->presetProfiles.find(_name);
+
+ if (iter == this->dataPtr->presetProfiles.end())
+ {
+ gzwarn << "Profile [" << _name << "] does not exist. No SDF will be "
+ << "generated." << std::endl;
+ return;
+ }
+ _elem.reset();
+
+ // Start with the elementSDF member variable of the preset
+ _elem = iter->second.SDF()->Clone();
+ GZ_ASSERT(_elem, "Null SDF pointer in preset");
+
+ this->GenerateSDFHelper(iter->second, _elem);
+ GZ_ASSERT(_elem, "Generated NULL SDF pointer");
+}
+
+//////////////////////////////////////////////////
+void PresetManager::GenerateSDFHelper(const Preset &_preset,
+ sdf::ElementPtr &_elem) const
+{
+ GZ_ASSERT(_elem, "NULL elem in GenerateSDFHelper, should never happen");
+
+ // For each element, enforce that its equivalent in Preset has the same value
+ for (sdf::ElementPtr elem = _elem->GetFirstElement(); elem;
+ elem = elem->GetNextElement())
+ {
+ boost::any value;
+ if (_preset.GetParam(elem->GetName(), value) && elem->GetValue())
+ {
+ // cast based on type in SDF
+ if (elem->GetValue()->IsType<int>())
+ {
+ int v;
+ if (CastAnyValue(value, v))
+ elem->Set(v);
+ else
+ gzerr << "SDF type did not give successful cast" << std::endl;
+ }
+ else if (elem->GetValue()->IsType<double>())
+ {
+ double v;
+ if (CastAnyValue(value, v))
+ elem->Set(v);
+ else
+ gzerr << "SDF type did not give successful cast" << std::endl;
+ }
+ else if (elem->GetValue()->IsType<float>())
+ {
+ float v;
+ if (CastAnyValue(value, v))
+ elem->Set(v);
+ else
+ gzerr << "SDF type did not give successful cast" << std::endl;
+ }
+ else if (elem->GetValue()->IsType<bool>())
+ {
+ bool v;
+ if (CastAnyValue(value, v))
+ elem->Set(v);
+ else
+ gzerr << "SDF type did not give successful cast" << std::endl;
+ }
+ else if (elem->GetValue()->IsType<std::string>())
+ {
+ std::string v;
+ if (CastAnyValue(value, v))
+ elem->Set(v);
+ else
+ gzerr << "SDF type did not give successful cast" << std::endl;
+ }
+ else if (elem->GetValue()->IsType<ignition::math::Vector3d>())
+ {
+ ignition::math::Vector3d v;
+ if (CastAnyValue(value, v))
+ {
+ gzdbg << "Vector3: " << v << std::endl;
+ elem->Set(v);
+ }
+ else
+ gzerr << "SDF type did not give successful cast" << std::endl;
+#ifndef _WIN32
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+ }
+ else if (elem->GetValue()->IsType<sdf::Vector3>())
+ {
+ math::Vector3 v;
+ if (CastAnyValue(value, v))
+ {
+ gzdbg << "Vector3: " << v << std::endl;
+ elem->Set(v);
+ }
+ else
+ gzerr << "SDF type did not give successful cast" << std::endl;
+#ifndef _WIN32
+#pragma GCC diagnostic pop
+#endif
+ }
+ }
+ this->GenerateSDFHelper(_preset, elem);
+ }
+}
+
+//////////////////////////////////////////////////
+Preset *PresetManager::CurrentPreset() const
+{
+ return &this->dataPtr->presetProfiles[this->CurrentProfile()];
+}
diff --git a/gazebo/physics/PresetManager.hh b/gazebo/physics/PresetManager.hh
new file mode 100644
index 0000000..2fe37d4
--- /dev/null
+++ b/gazebo/physics/PresetManager.hh
@@ -0,0 +1,221 @@
+/*
+ * 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_PHYSICS_PRESETMANAGER_HH_
+#define _GAZEBO_PHYSICS_PRESETMANAGER_HH_
+
+#include <boost/any.hpp>
+#include <string>
+#include <vector>
+#include <sdf/sdf.hh>
+
+#include "gazebo/physics/PhysicsTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ class PresetPrivate;
+
+ /// \class Preset PresetManager.hh gazebo/physics/PresetManager.hh
+ /// \brief Representation of a preset physics profile
+ class GAZEBO_VISIBLE Preset
+ {
+ /// \brief Constructor.
+ public: Preset();
+
+ /// \brief Constructor
+ /// \param[in] _name The name of the preset profile.
+ public: Preset(const std::string & _name);
+
+ /// \brief Destructor.
+ public: ~Preset();
+
+ /// \brief Get the profile name.
+ /// \return The name of the preset profile.
+ public: std::string Name() const;
+
+ /// \brief Get a profile parameter.
+ /// \param[in] _key The key of the parameter to retrieve.
+ /// \param[out] _value The parameter value at the input key.
+ /// \return True if the parameter exists in the map, false otherwise.
+ public: bool GetParam(const std::string &_key, boost::any &_value) const;
+
+ /// \brief Set a profile parameter.
+ /// \param[in] _key The key of the parameter to change.
+ /// \param[in] _value The new value of the parameter.
+ public: bool SetParam(const std::string &_key, const boost::any &_value);
+
+ /// \brief Check if profile parameter is set.
+ /// \param[in] _key The profile key to check.
+ /// \return True if the profile has parameter _key, false otherwise
+ public: bool HasParam(const std::string &_key) const;
+
+ /// \brief Set all parameters of this preset in the physics engine.
+ /// \param[in] _physicsEngine The physics engine in which to affect the
+ /// change.
+ /// \return True if setting all parameters was successful.
+ public: bool SetAllPhysicsParameters(PhysicsEnginePtr _physicsEngine)
+ const;
+
+ /// \brief Set all parameters of this preset based on the key/value pairs
+ /// in the given SDF element.
+ /// \param[in] _elem The physics SDF element from which to read values.
+ /// \return True if setting all parameters was successful.
+ public: bool SetAllParamsFromSDF(const sdf::ElementPtr _elem);
+
+ /// \brief Recursive helper for SetAllParamsFromSDF.
+ /// \param[in] _elem The physics SDF element from which to read values.
+ /// \return True if setting all parameters in one level of SDF was
+ /// successful.
+ private: bool SetAllParamsHelper(const sdf::ElementPtr _elem);
+
+ /// \brief Get this preset profile's SDF
+ /// \return An SDF element pointer representing a <physics> element
+ public: sdf::ElementPtr SDF() const;
+
+ /// \brief Set this preset profile's SDF
+ /// \param[in] _sdfElement Pointer to an SDF physics element.
+ /// \return True if setting the profile SDF was successful.
+ public: bool SDF(const sdf::ElementPtr _sdfElement);
+
+ /// \brief Private data pointer for PIMPL
+ private: PresetPrivate *dataPtr;
+ };
+
+ class PresetManagerPrivate;
+
+ /// \class PresetManager PresetManager.hh gazebo/physics/PresetManager.hh
+ /// \brief Class to manage preset physics profiles.
+ class GAZEBO_VISIBLE PresetManager
+ {
+ /// \brief Constructor
+ /// \param[in] _physicsEngine Pointer to the world physics engine.
+ /// \param[in] _sdf Pointer to the world SDF element.
+ public: PresetManager(PhysicsEnginePtr _physicsEngine,
+ const sdf::ElementPtr _sdf);
+
+ /// \brief Destructor
+ public: ~PresetManager();
+
+ /// \brief Set the current profile.
+ /// \param[in] _name The name of the new current profile.
+ /// \return True if the profile switch was successful.
+ public: bool CurrentProfile(const std::string &_name);
+
+ /// \brief Get the name of the current profile.
+ /// \return The name of the current profile.
+ public: std::string CurrentProfile() const;
+
+ /// \brief Get the name of all profiles.
+ /// \return A vector containing all profile names.
+ public: std::vector<std::string> AllProfiles() const;
+
+ /// \brief Set a parameter for a certain profile.
+ /// \param[in] _profileName The name of the profile to change.
+ /// \param[in] _key The key of the parameter to change.
+ /// \param[in] _value The value of the parameter to change.
+ /// \return True if setting the parameter was successful.
+ public: bool SetProfileParam(const std::string &_profileName,
+ const std::string &_key, const boost::any &_value);
+
+ /// \brief Get a parameter for a certain profile.
+ /// \param[in] _name The name of the accessed profile.
+ /// \param[in] _key The key of the accessed parameter.
+ /// \param[out] _value The value of the accessed parameter.
+ /// \return True if the parameter existed in profile "_name".
+ public: bool GetProfileParam(const std::string &_name,
+ const std::string &_key, boost::any &_value) const;
+
+ /// \brief Set a parameter for the current profile.
+ /// \param[in] _key The key of the parameter to be set.
+ /// \param[in] _value The value of the parameter to be set.
+ /// \return True if setting the parameter was successful.
+ public: bool SetCurrentProfileParam(const std::string &_key,
+ const boost::any &_value);
+
+ /// \brief Get a parameter for the current profile.
+ /// \param[in] _key The key of the accessed parameter.
+ /// \param[out] _value The value of the accessed parameter.
+ /// \return True if the parameter existed in profile "_name".
+ public: bool GetCurrentProfileParam(const std::string &_key,
+ boost::any &_value);
+
+ /// \brief Create a new profile. A profile created in this way will store
+ /// all of the current parameter values of the physics engine.
+ /// \param[in] _name The name of the new profile.
+ /// \return True if the profile was successfully created.
+ public: bool CreateProfile(const std::string &_name);
+
+ /// \brief Create a new profile from SDF. SDF determines the profile name
+ /// \param[in] _sdf Pointer to a physics SDF element.
+ /// \return The name of the new profile, read from SDF. If the profile
+ /// was not successfully created, return the empty string, which is an
+ /// invalid profile name.
+ public: std::string CreateProfile(const sdf::ElementPtr _sdf);
+
+ /// \brief Remove a profile.
+ /// \param[in] _name The name of the profile to remove.
+ public: void RemoveProfile(const std::string &_name);
+
+ /// \brief Determine if we have a profile.
+ /// \param[in] _name The name of the profile to find.
+ /// \return True if we have the profile, false otherwise
+ public: bool HasProfile(const std::string &_name) const;
+
+ /// \brief Get the SDF for a profile.
+ /// \param[in] _name The name of the profile to be accessed.
+ /// \return Pointer to the SDF physics element representing the profile.
+ /// Can be NULL if no profile was found.
+ public: sdf::ElementPtr ProfileSDF(const std::string &_name) const;
+
+ /// \brief Set the SDF for a profile.
+ /// \param[in] _name The name of the profile to set.
+ /// \param[in] _sdf The new SDF physics element for the profile.
+ /// \return True if setting the new SDF element was successful.
+ public: bool ProfileSDF(const std::string &_name,
+ const sdf::ElementPtr _sdf);
+
+ /// \brief Generate an SDF element from an Preset object.
+ /// \param[in] _name The name of the profile to copy.
+ /// \param[out] _elem The SDF physics element for the profile.
+ public: void GenerateSDFFromPreset(const std::string &_name,
+ sdf::ElementPtr &_elem) const;
+
+ /// \brief Recursive helper for traversing SDF elements.
+ /// \param[in] _preset The preset profile element.
+ /// \param[out] _elem The SDF physics element for the profile.
+ private: void GenerateSDFHelper(const Preset &_preset,
+ sdf::ElementPtr &_elem) const;
+
+ /// \brief Generate a Preset object from an SDF pointer
+ /// \param[in] _elem The SDF physics element for the profile.
+ /// \param[out] _preset The preset profile element.
+ private: void GeneratePresetFromSDF(const sdf::ElementPtr _elem,
+ Preset &_preset) const;
+
+ /// \brief Get a pointer to the current profile preset.
+ /// \return Pointer to the current profile preset object.
+ private: Preset *CurrentPreset() const;
+
+ /// \brief Private data pointer for PIMPL.
+ private: PresetManagerPrivate *dataPtr;
+ };
+ }
+}
+
+#endif
diff --git a/gazebo/physics/PresetManagerPrivate.hh b/gazebo/physics/PresetManagerPrivate.hh
new file mode 100644
index 0000000..c45812b
--- /dev/null
+++ b/gazebo/physics/PresetManagerPrivate.hh
@@ -0,0 +1,71 @@
+/*
+ * 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_PHYSICS_PRESETMANAGER_PRIVATE_HH_
+#define _GAZEBO_PHYSICS_PRESETMANAGER_PRIVATE_HH_
+
+#include <map>
+#include <string>
+#include <mutex>
+#include "gazebo/physics/PhysicsEngine.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data pointer for Preset class
+ class PresetPrivate
+ {
+ /// \brief Constructor
+ /// \param[in] _name The name of the preset profile.
+ public: PresetPrivate(const std::string &_name = "default name")
+ : name(_name)
+ {
+ }
+
+ /// \brief Name of this preset profile
+ public: std::string name;
+
+ /// \brief Map of key, parameter pairs
+ public: std::map<std::string, boost::any> parameterMap;
+
+ /// \brief SDF for the physics element represented by this object
+ public: sdf::ElementPtr elementSDF;
+ };
+
+ class Preset;
+
+ /// \internal
+ /// \brief Private data pointer for PresetManager class
+ class PresetManagerPrivate
+ {
+ /// \brief Name of the current preset
+ public: std::string currentPreset;
+
+ /// \brief Map of all known preset profile pairs keyed by name
+ public: std::map<std::string, Preset> presetProfiles;
+
+ /// \brief Physics engine instrumented by this PresetManager
+ public: PhysicsEnginePtr physicsEngine;
+
+ /// \brief Mutex to protect setting the current preset profile.
+ public: std::mutex currentProfileMutex;
+ };
+ }
+}
+
+#endif
diff --git a/gazebo/physics/PresetManager_TEST.cc b/gazebo/physics/PresetManager_TEST.cc
new file mode 100644
index 0000000..06ac179
--- /dev/null
+++ b/gazebo/physics/PresetManager_TEST.cc
@@ -0,0 +1,291 @@
+/*
+ * Copyright (C) 2012-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/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+#include "gazebo/physics/PresetManager.hh"
+#include "sdf/sdf.hh"
+
+using namespace gazebo;
+
+class PresetManagerTest : public ServerFixture
+{
+};
+
+/////////////////////////////////////////////////
+TEST_F(PresetManagerTest, GetSetPresetParam)
+{
+ physics::Preset preset("preset1");
+ EXPECT_EQ(preset.Name(), "preset1");
+ std::string foo = "foo";
+ boost::any value1 = foo;
+ EXPECT_FALSE(preset.HasParam("key1"));
+ EXPECT_TRUE(preset.SetParam("key1", value1));
+ EXPECT_FALSE(preset.SetParam("", value1));
+ EXPECT_TRUE(preset.HasParam("key1"));
+ EXPECT_FALSE(preset.HasParam(""));
+ boost::any value2;
+ EXPECT_FALSE(preset.GetParam("", value2));
+ EXPECT_FALSE(preset.GetParam("key_does_not_exist", value2));
+ EXPECT_TRUE(preset.GetParam("key1", value2));
+ try
+ {
+ EXPECT_EQ(boost::any_cast<std::string>(value1),
+ boost::any_cast<std::string>(value2));
+ }
+ catch(boost::bad_any_cast &_e)
+ {
+ gzerr << "Bad any cast in PresetManager_TEST" << std::endl;
+ FAIL();
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(PresetManagerTest, GetSetProfileParam)
+{
+ EXPECT_NO_THROW(physics::PresetManager(NULL, NULL));
+
+ // Load preset test world
+ Load("test/worlds/presets.world", true);
+
+ // Get a pointer to the world, make sure world loads
+ physics::WorldPtr world = physics::get_world("default");
+ physics::PresetManagerPtr presetManager = world->GetPresetManager();
+
+ EXPECT_FALSE(presetManager->CurrentProfile(""));
+ EXPECT_FALSE(presetManager->CurrentProfile("preset_does_not_exist"));
+ EXPECT_EQ(presetManager->CurrentProfile(), "preset_1");
+
+ // current
+ double max_step_size = 0.9;
+ EXPECT_FALSE(presetManager->SetCurrentProfileParam("param_does_not_exist",
+ max_step_size));
+ EXPECT_TRUE(presetManager->SetCurrentProfileParam("max_step_size",
+ 0.8));
+ EXPECT_TRUE(presetManager->SetProfileParam("preset_1", "max_step_size",
+ max_step_size));
+
+ boost::any value2;
+ EXPECT_FALSE(presetManager->GetCurrentProfileParam("param_does_not_exist",
+ value2));
+ EXPECT_TRUE(presetManager->GetCurrentProfileParam("max_step_size", value2));
+ EXPECT_TRUE(presetManager->GetProfileParam("preset_1", "max_step_size",
+ value2));
+
+ try
+ {
+ EXPECT_DOUBLE_EQ(max_step_size, boost::any_cast<double>(value2));
+ }
+ catch(boost::bad_any_cast &_e)
+ {
+ gzerr << "Bad any cast in PresetManager_TEST" << std::endl;
+ FAIL();
+ }
+
+ // Trying to set a preset profile that does not exist should return false.
+ EXPECT_FALSE(presetManager->SetProfileParam("preset_does_not_exist",
+ "max_step_size", 10));
+ EXPECT_FALSE(presetManager->SetProfileParam("preset_2",
+ "param_does_not_exist", 10));
+
+ EXPECT_TRUE(presetManager->SetProfileParam("preset_2", "max_step_size", 10));
+
+ // Trying to get from a nonexistent preset profile should return false.
+ EXPECT_FALSE(presetManager->GetProfileParam("preset_does_not_exist",
+ "max_step_size", value2));
+ // Trying to get a nonexistent param should return false.
+ EXPECT_FALSE(presetManager->GetProfileParam("preset_2",
+ "param_does_not_exist", value2));
+
+ EXPECT_TRUE(presetManager->GetProfileParam("preset_2", "max_step_size",
+ value2));
+}
+
+/////////////////////////////////////////////////
+TEST_F(PresetManagerTest, CreateRemoveProfile)
+{
+ // Load preset test world
+ Load("test/worlds/presets.world", true);
+
+ // Get a pointer to the world, make sure world loads
+ physics::WorldPtr world = physics::get_world("default");
+ physics::PresetManagerPtr presetManager = world->GetPresetManager();
+ boost::any value;
+
+ presetManager->RemoveProfile("preset_2");
+ EXPECT_FALSE(presetManager->HasProfile("preset_2"));
+
+ // Remove the current profile
+ presetManager->RemoveProfile("preset_1");
+ // Test setting and getting when there is no current profile
+ EXPECT_EQ(presetManager->CurrentProfile(), "");
+ // Even with valid keys, setting/getting from the current profile will fail
+ EXPECT_FALSE(presetManager->SetCurrentProfileParam("max_step_size", 1));
+ EXPECT_FALSE(presetManager->GetCurrentProfileParam("max_step_size", value));
+
+ // Even with valid keys, setting/getting from the removed profile will fail
+ EXPECT_FALSE(presetManager->SetProfileParam("preset_1", "max_step_size", 1));
+ EXPECT_FALSE(presetManager->GetProfileParam("preset_1", "max_step_size",
+ value));
+
+ EXPECT_FALSE(presetManager->HasProfile("preset_1"));
+
+ // Create a profile using (name)
+ EXPECT_FALSE(presetManager->CreateProfile(""));
+ EXPECT_TRUE(presetManager->CreateProfile("preset_1"));
+ EXPECT_TRUE(presetManager->SetProfileParam("preset_1", "max_step_size", 1.0));
+ EXPECT_TRUE(presetManager->GetProfileParam("preset_1", "max_step_size",
+ value));
+
+ try
+ {
+ EXPECT_DOUBLE_EQ(1.0, boost::any_cast<double>(value));
+ }
+ catch(boost::bad_any_cast &_e)
+ {
+ gzerr << "Bad any cast in PresetManager_TEST" << std::endl;
+ FAIL();
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(PresetManagerTest, SDF)
+{
+ // Load preset test world
+ Load("test/worlds/presets.world", true);
+
+ // Get a pointer to the world, make sure world loads
+ physics::WorldPtr world = physics::get_world("default");
+ physics::PresetManagerPtr presetManager = world->GetPresetManager();
+
+ EXPECT_TRUE(!presetManager->ProfileSDF("profile_does_not_exist"));
+
+ {
+ // Try to set using a bad SDF element
+ sdf::SDF worldSDF;
+ sdf::ElementPtr physicsSDF;
+
+ worldSDF.SetFromString(
+ "<sdf version = \"1.5\">\
+ <world name = \"default\">\
+ <physics name = \"preset_3\" type = \"ode\">\
+ <max_step_size>0.03</max_step_size>\
+ <bad_sdf_tag/>\
+ </physics>\
+ </world>\
+ </sdf>");
+ EXPECT_EQ(presetManager->CreateProfile(physicsSDF), "");
+ }
+
+ {
+ sdf::SDF worldSDF;
+ sdf::ElementPtr physicsSDF;
+ boost::any value;
+
+ worldSDF.SetFromString(
+ "<sdf version = \"1.5\">\
+ <world name = \"default\">\
+ <physics name = \"preset_3\" type = \"ode\">\
+ <max_step_size>0.03</max_step_size>\
+ <magnetic_field>0 0 0</magnetic_field>\
+ <ode>\
+ <solver>\
+ <min_step_size>0.003</min_step_size>\
+ <iters>150</iters>\
+ <sor>1.6</sor>\
+ </solver>\
+ <constraints>\
+ <cfm>0.03</cfm>\
+ <erp>0.7</erp>\
+ </constraints>\
+ </ode>\
+ </physics>\
+ </world>\
+ </sdf>");
+ physicsSDF = worldSDF.Root()->GetElement("world")->GetElement("physics");
+ EXPECT_EQ(presetManager->CreateProfile(physicsSDF), "preset_3");
+ EXPECT_TRUE(presetManager->HasProfile("preset_3"));
+
+ // Compare the SDF as strings
+ EXPECT_EQ(presetManager->ProfileSDF("preset_3")->ToString(""),
+ physicsSDF->ToString(""));
+ EXPECT_TRUE(presetManager->ProfileSDF("preset_3") != NULL);
+ EXPECT_TRUE(presetManager->CurrentProfile("preset_3"));
+ try
+ {
+ EXPECT_TRUE(presetManager->GetCurrentProfileParam("max_step_size",
+ value));
+ EXPECT_DOUBLE_EQ(boost::any_cast<double>(value), 0.03);
+ EXPECT_TRUE(presetManager->GetCurrentProfileParam("min_step_size",
+ value));
+ EXPECT_DOUBLE_EQ(boost::any_cast<double>(value), 0.003);
+ }
+ catch(boost::bad_any_cast &_e)
+ {
+ gzerr << "Bad any cast in PresetManager_TEST" << std::endl;
+ FAIL();
+ }
+
+ // GenerateSDFFromPreset
+ sdf::ElementPtr generatedPhysicsSDF = NULL;
+ presetManager->GenerateSDFFromPreset("this_preset_does_not_exist",
+ generatedPhysicsSDF);
+ // Call doesn't do anything
+ ASSERT_TRUE(generatedPhysicsSDF == NULL);
+
+ presetManager->GenerateSDFFromPreset("preset_3", generatedPhysicsSDF);
+ // Compare the SDF as strings
+ ASSERT_TRUE(generatedPhysicsSDF != NULL);
+
+ EXPECT_EQ(generatedPhysicsSDF->ToString(""), physicsSDF->ToString(""));
+ }
+
+ {
+ // Try to set a null pointer
+ sdf::ElementPtr nullElement = NULL;
+ presetManager->ProfileSDF("preset_3", nullElement);
+ // Should have no effect
+ EXPECT_TRUE(presetManager->ProfileSDF("preset_3") != NULL);
+ }
+
+ {
+ // Try to set a non-physics element
+ sdf::SDF worldSDF;
+ sdf::ElementPtr sceneSDF;
+
+ worldSDF.SetFromString(
+ "<sdf version = \"1.5\">\
+ <world name = \"default\">\
+ <scene>\
+ <ambient>0.1 0.1 0.1 1</ambient>\
+ <background>1 1 1 1</background>\
+ <shadows>false</shadows>\
+ <grid>false</grid>\
+ </scene>\
+ </world>\
+ </sdf>");
+ sceneSDF = worldSDF.Root()->GetElement("world")->GetElement("scene");
+ EXPECT_EQ(presetManager->CreateProfile(sceneSDF), "");
+ EXPECT_FALSE(presetManager->ProfileSDF("default_physics", sceneSDF));
+ }
+}
+
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/physics/RayShape.cc b/gazebo/physics/RayShape.cc
index 99d2368..76c6c8b 100644
--- a/gazebo/physics/RayShape.cc
+++ b/gazebo/physics/RayShape.cc
@@ -19,6 +19,12 @@
* Date: 14 Oct 2009
*/
+#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 <vector>
#include <list>
#include <string>
@@ -182,3 +188,9 @@ void RayShape::FillMsg(msgs::Geometry &/*_msg*/)
void RayShape::ProcessMsg(const msgs::Geometry &/*_msg*/)
{
}
+
+//////////////////////////////////////////////////
+double RayShape::ComputeVolume() const
+{
+ return 0;
+}
diff --git a/gazebo/physics/RayShape.hh b/gazebo/physics/RayShape.hh
index 2588ece..fae7a2e 100644
--- a/gazebo/physics/RayShape.hh
+++ b/gazebo/physics/RayShape.hh
@@ -37,7 +37,7 @@ namespace gazebo
/// \class RayShape RayShape.hh physics/physics.hh
/// \brief Base class for Ray collision geometry
- class GAZEBO_VISIBLE RayShape : public Shape
+ class GZ_PHYSICS_VISIBLE RayShape : public Shape
{
/// \brief Constructor for a global ray.
/// \param[in] _physicsEngine Pointer to the physics engine.
@@ -119,6 +119,9 @@ namespace gazebo
/// \TODO Implement this function.
public: virtual void ProcessMsg(const msgs::Geometry &_msg);
+ /// Documentation inherited
+ public: virtual double ComputeVolume() const;
+
// Contact information; this is filled out during collision
// detection.
/// \brief Length of the ray.
diff --git a/gazebo/physics/Road.cc b/gazebo/physics/Road.cc
index 044d84a..35aa828 100644
--- a/gazebo/physics/Road.cc
+++ b/gazebo/physics/Road.cc
@@ -15,6 +15,12 @@
*
*/
+#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 <algorithm>
#include <string>
#include <vector>
@@ -89,7 +95,7 @@ void Road::Init()
sdf::ElementPtr pointElem = this->sdf->GetElement("point");
while (pointElem)
{
- math::Vector3 point = pointElem->Get<math::Vector3>();
+ ignition::math::Vector3d point = pointElem->Get<ignition::math::Vector3d>();
pointElem = pointElem->GetNextElement("point");
msgs::Vector3d *ptMsg = msg.add_point();
diff --git a/gazebo/physics/Road.hh b/gazebo/physics/Road.hh
index 63e3cb5..1a958b1 100644
--- a/gazebo/physics/Road.hh
+++ b/gazebo/physics/Road.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \class Road Road.hh physics/physics.hh
/// \brief for building a Road from SDF
- class GAZEBO_VISIBLE Road : public Base
+ class GZ_PHYSICS_VISIBLE Road : public Base
{
/// \brief Constructor.
/// \param[in] _parent Parent of this road object.
diff --git a/gazebo/physics/Road_TEST.cc b/gazebo/physics/Road_TEST.cc
index eaf1383..13dc490 100644
--- a/gazebo/physics/Road_TEST.cc
+++ b/gazebo/physics/Road_TEST.cc
@@ -33,8 +33,8 @@ TEST_F(RoadTest, Texture)
<< "<material>"
<< "<script>"
<< "<name>primary</name>"
- << "</material>"
<< "</script>"
+ << "</material>"
<< "</road></sdf>";
sdf::ElementPtr roadSDF(new sdf::Element);
diff --git a/gazebo/physics/ScrewJoint.hh b/gazebo/physics/ScrewJoint.hh
index 13cfca7..d9e66f8 100644
--- a/gazebo/physics/ScrewJoint.hh
+++ b/gazebo/physics/ScrewJoint.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class ScrewJoint ScrewJoint.hh physics/physics.hh
/// \brief A screw joint, which has both prismatic and rotational DOFs
template<class T>
- class GAZEBO_VISIBLE ScrewJoint : public T
+ class GZ_PHYSICS_VISIBLE ScrewJoint : public T
{
/// \brief Constructor.
/// \param[in] _parent Parent of the joint.
diff --git a/gazebo/physics/Shape.cc b/gazebo/physics/Shape.cc
index fcb1cf1..3ee4a52 100644
--- a/gazebo/physics/Shape.cc
+++ b/gazebo/physics/Shape.cc
@@ -14,6 +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 "gazebo/common/Console.hh"
#include "gazebo/physics/Collision.hh"
#include "gazebo/physics/Shape.hh"
@@ -45,3 +53,18 @@ math::Vector3 Shape::GetScale() const
{
return this->scale;
}
+
+//////////////////////////////////////////////////
+double Shape::ComputeVolume() const
+{
+ if (!this->collisionParent)
+ {
+ gzerr << "Cannot discern shape type, returning 0 volume" << std::endl;
+ return 0;
+ }
+ gzwarn << "ComputeVolume not fully implemented for this shape type, returning"
+ << " bounding box approximation" << std::endl;
+
+ math::Vector3 size = this->collisionParent->GetBoundingBox().GetSize();
+ return size.x * size.y * size.z;
+}
diff --git a/gazebo/physics/Shape.hh b/gazebo/physics/Shape.hh
index 0199a35..9476fc7 100644
--- a/gazebo/physics/Shape.hh
+++ b/gazebo/physics/Shape.hh
@@ -17,6 +17,12 @@
#ifndef _SHAPE_HH_
#define _SHAPE_HH_
+#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 "gazebo/msgs/msgs.hh"
@@ -37,7 +43,7 @@ namespace gazebo
/// \class Shape Shape.hh physics/physics.hh
/// \brief Base class for all shapes.
- class GAZEBO_VISIBLE Shape : public Base
+ class GZ_PHYSICS_VISIBLE Shape : public Base
{
/// \brief Constructor.
/// \param[in] _parent Parent of the shape.
@@ -65,6 +71,11 @@ namespace gazebo
/// \param[in] _msg The message to set values from.
public: virtual void ProcessMsg(const msgs::Geometry &_msg) = 0;
+ /// \brief Get the volume of this shape. Implemented accurately for
+ /// simple shapes; an approximation is used for meshes, polylines, etc.
+ /// \return The shape volume in kg/m^3.
+ public: virtual double ComputeVolume() const;
+
/// \brief This shape's collision parent.
protected: CollisionPtr collisionParent;
diff --git a/gazebo/physics/SliderJoint.hh b/gazebo/physics/SliderJoint.hh
index b04bbc2..7f1b17a 100644
--- a/gazebo/physics/SliderJoint.hh
+++ b/gazebo/physics/SliderJoint.hh
@@ -35,7 +35,7 @@ namespace gazebo
/// \class SliderJoint SliderJoint.hh physics/physics.hh
/// \brief A slider joint
template<class T>
- class GAZEBO_VISIBLE SliderJoint : public T
+ class GZ_PHYSICS_VISIBLE SliderJoint : public T
{
/// \brief Constructor.
/// \param[in] _parent Parent of the joint.
diff --git a/gazebo/physics/SphereShape.cc b/gazebo/physics/SphereShape.cc
index 87c2070..99dbf99 100644
--- a/gazebo/physics/SphereShape.cc
+++ b/gazebo/physics/SphereShape.cc
@@ -83,3 +83,9 @@ void SphereShape::ProcessMsg(const msgs::Geometry &_msg)
{
this->SetRadius(_msg.sphere().radius());
}
+
+//////////////////////////////////////////////////
+double SphereShape::ComputeVolume() const
+{
+ return 4*M_PI*pow(this->GetRadius(), 3)/3;
+}
diff --git a/gazebo/physics/SphereShape.hh b/gazebo/physics/SphereShape.hh
index c89dea8..ddc058a 100644
--- a/gazebo/physics/SphereShape.hh
+++ b/gazebo/physics/SphereShape.hh
@@ -22,6 +22,12 @@
#ifndef _SPHERESHAPE_HH_
#define _SPHERESHAPE_HH_
+#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 "gazebo/physics/Shape.hh"
#include "gazebo/physics/PhysicsTypes.hh"
#include "gazebo/util/system.hh"
@@ -35,7 +41,7 @@ namespace gazebo
/// \class SphereShape SphereShape.hh physics/physics.hh
/// \brief Sphere collision shape.
- class GAZEBO_VISIBLE SphereShape : public Shape
+ class GZ_PHYSICS_VISIBLE SphereShape : public Shape
{
/// \brief Constructor.
/// \param[in] _parent Parent collision object.
@@ -66,6 +72,9 @@ namespace gazebo
/// \brief Process a geometry message.
/// \param[in] _msg The message to set values from.
public: virtual void ProcessMsg(const msgs::Geometry &_msg);
+
+ /// Documentation inherited
+ public: virtual double ComputeVolume() const;
};
/// \}
}
diff --git a/gazebo/physics/SphereShape_TEST.cc b/gazebo/physics/SphereShape_TEST.cc
index 6c36ccb..23e010d 100644
--- a/gazebo/physics/SphereShape_TEST.cc
+++ b/gazebo/physics/SphereShape_TEST.cc
@@ -44,7 +44,7 @@ TEST_F(SphereShapeTest, Scale)
physics::SphereShapePtr sphere(
new physics::SphereShape(physics::CollisionPtr()));
- sdf::ElementPtr elem = sphereSDF->root;
+ sdf::ElementPtr elem = sphereSDF->Root();
ASSERT_TRUE(elem != NULL);
elem = elem->GetElement("model");
ASSERT_TRUE(elem != NULL);
@@ -111,6 +111,53 @@ TEST_F(SphereShapeTest, Scale)
EXPECT_DOUBLE_EQ(radius, 1.25);
}
+TEST_F(SphereShapeTest, Volume)
+{
+ std::ostringstream sphereStr;
+ sphereStr << "<sdf version ='" << SDF_VERSION << "'>"
+ << "<model name='model'>"
+ << "<link name ='link'>"
+ << "<collision name ='collision'>"
+ << "<geometry>"
+ << "<sphere>"
+ << "<radius>1.0</radius>"
+ << "</sphere>"
+ << "</geometry>"
+ << "</collision>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ sdf::SDFPtr sphereSDF(new sdf::SDF);
+ sphereSDF->SetFromString(sphereStr.str());
+
+ physics::SphereShapePtr sphere(
+ new physics::SphereShape(physics::CollisionPtr()));
+ sdf::ElementPtr elem = sphereSDF->Root();
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("model");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("link");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("collision");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("geometry");
+ ASSERT_TRUE(elem != NULL);
+ elem = elem->GetElement("sphere");
+ ASSERT_TRUE(elem != NULL);
+ sphere->Load(elem);
+
+ EXPECT_DOUBLE_EQ(sphere->ComputeVolume(), 4*M_PI/3);
+
+ sphere->SetRadius(2);
+
+ EXPECT_DOUBLE_EQ(sphere->ComputeVolume(), 4*M_PI*8/3);
+
+ // The bounding box approximation should be 0 because the Shape has no
+ // Collision parent
+ EXPECT_DOUBLE_EQ(sphere->Shape::ComputeVolume(), 0);
+}
+
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
diff --git a/gazebo/physics/State.cc b/gazebo/physics/State.cc
index 0404bd6..b7a9a21 100644
--- a/gazebo/physics/State.cc
+++ b/gazebo/physics/State.cc
@@ -29,9 +29,9 @@ State::State()
/////////////////////////////////////////////////
State::State(const std::string &_name, const common::Time &_realTime,
- const common::Time &_simTime)
+ const common::Time &_simTime, const uint64_t _iterations)
: name(_name), wallTime(common::Time::GetWallTime()), realTime(_realTime),
- simTime(_simTime)
+ simTime(_simTime), iterations(_iterations)
{
}
@@ -76,12 +76,19 @@ common::Time State::GetSimTime() const
}
/////////////////////////////////////////////////
+uint64_t State::GetIterations() const
+{
+ return this->iterations;
+}
+
+/////////////////////////////////////////////////
State &State::operator=(const State &_state)
{
this->name = _state.name;
this->wallTime = _state.wallTime;
this->realTime = _state.realTime;
this->simTime = _state.simTime;
+ this->iterations = _state.iterations;
return *this;
}
@@ -97,7 +104,8 @@ State State::operator-(const State &_state) const
}
return State(this->name, this->realTime - _state.realTime,
- this->simTime - _state.simTime);
+ this->simTime - _state.simTime,
+ this->iterations - _state.iterations);
}
/////////////////////////////////////////////////
@@ -117,3 +125,9 @@ void State::SetSimTime(const common::Time &_time)
{
this->simTime = _time;
}
+
+/////////////////////////////////////////////////
+void State::SetIterations(const uint64_t _iterations)
+{
+ this->iterations = _iterations;
+}
diff --git a/gazebo/physics/State.hh b/gazebo/physics/State.hh
index 3571c8a..043f953 100644
--- a/gazebo/physics/State.hh
+++ b/gazebo/physics/State.hh
@@ -21,6 +21,12 @@
#ifndef _STATE_HH_
#define _STATE_HH_
+#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 <sdf/sdf.hh>
@@ -40,7 +46,7 @@ namespace gazebo
/// \brief State of an entity.
///
/// This is the base class for all State information.
- class GAZEBO_VISIBLE State
+ class GZ_PHYSICS_VISIBLE State
{
/// \brief Default constructor
public: State();
@@ -50,11 +56,13 @@ namespace gazebo
/// Construct a State object using some basic information.
/// \param _name Name associated with the State information. This is
/// typically the name of an Entity.
- /// \pararm _realTime Clock time since simulation started.
+ /// \param _realTime Clock time since simulation started.
/// \param _simTime Simulation time associated with this State info.
+ /// \param _iterations Simulation iterations since simulation started.
public: State(const std::string &_name,
const common::Time &_realTime,
- const common::Time &_simTime);
+ const common::Time &_simTime,
+ const uint64_t _iterations);
/// \brief Destructor
public: virtual ~State();
@@ -97,6 +105,10 @@ namespace gazebo
/// \return Simulation time when the data was recorded.
public: common::Time GetSimTime() const;
+ /// \brief Get the iterations when this state was generated
+ /// \return Iterations when the data was recorded
+ public: uint64_t GetIterations() const;
+
/// \brief Set the wall time when this state was generated
/// \param[in] _time The absolute clock time when the State
/// data was recorded.
@@ -110,11 +122,19 @@ namespace gazebo
/// \param[in] _time Simulation time when the data was recorded.
public: virtual void SetSimTime(const common::Time &_time);
- /// Name associated with this State
+ /// \brief Set the iterations when this state was generated
+ /// \param[in] _iterations Iterations when the data was recorded.
+ public: virtual void SetIterations(const uint64_t _iterations);
+
+ /// \brief Name associated with this State
protected: std::string name;
- /// Times for the state data
+ /// \brief Times for the state data
protected: common::Time wallTime, realTime, simTime;
+
+ /// \brief The number of simulation iterations when this state was
+ /// generated.
+ protected: uint64_t iterations;
};
/// \}
}
diff --git a/gazebo/physics/SurfaceParams.cc b/gazebo/physics/SurfaceParams.cc
index 7d9f008..cef65d9 100644
--- a/gazebo/physics/SurfaceParams.cc
+++ b/gazebo/physics/SurfaceParams.cc
@@ -83,7 +83,8 @@ void FrictionPyramid::SetMu(unsigned int _index, double _mu)
//////////////////////////////////////////////////
SurfaceParams::SurfaceParams()
: collideWithoutContact(false),
- collideWithoutContactBitmask(1)
+ collideWithoutContactBitmask(1),
+ collideBitmask(65535)
{
}
@@ -108,6 +109,12 @@ void SurfaceParams::Load(sdf::ElementPtr _sdf)
contactElem->Get<bool>("collide_without_contact");
this->collideWithoutContactBitmask =
contactElem->Get<unsigned int>("collide_without_contact_bitmask");
+
+ if (contactElem->HasElement("collide_bitmask"))
+ {
+ this->collideBitmask =
+ contactElem->Get<unsigned int>("collide_bitmask");
+ }
}
}
}
@@ -117,13 +124,22 @@ void SurfaceParams::FillMsg(msgs::Surface &_msg)
{
_msg.set_collide_without_contact(this->collideWithoutContact);
_msg.set_collide_without_contact_bitmask(this->collideWithoutContactBitmask);
+ _msg.set_collide_bitmask(this->collideBitmask);
}
-
+/////////////////////////////////////////////////
void SurfaceParams::ProcessMsg(const msgs::Surface &_msg)
{
if (_msg.has_collide_without_contact())
this->collideWithoutContact = _msg.collide_without_contact();
if (_msg.has_collide_without_contact_bitmask())
this->collideWithoutContactBitmask = _msg.collide_without_contact_bitmask();
+ if (_msg.has_collide_bitmask())
+ this->collideBitmask = _msg.collide_bitmask();
+}
+
+/////////////////////////////////////////////////
+FrictionPyramidPtr SurfaceParams::GetFrictionPyramid() const
+{
+ return FrictionPyramidPtr();
}
diff --git a/gazebo/physics/SurfaceParams.hh b/gazebo/physics/SurfaceParams.hh
index 00df455..da11ca1 100644
--- a/gazebo/physics/SurfaceParams.hh
+++ b/gazebo/physics/SurfaceParams.hh
@@ -15,12 +15,13 @@
*
*/
-#ifndef _SURFACEPARAMS_HH_
-#define _SURFACEPARAMS_HH_
+#ifndef _GAZEBO_SURFACEPARAMS_HH_
+#define _GAZEBO_SURFACEPARAMS_HH_
#include <sdf/sdf.hh>
#include "gazebo/msgs/msgs.hh"
+#include "gazebo/physics/PhysicsTypes.hh"
#include "gazebo/util/system.hh"
namespace gazebo
@@ -32,7 +33,7 @@ namespace gazebo
/// \class FrictionPyramid SurfaceParams.hh physics/physics.hh
/// \brief Parameters used for friction pyramid model.
- class GAZEBO_VISIBLE FrictionPyramid
+ class GZ_PHYSICS_VISIBLE FrictionPyramid
{
/// \brief Constructor.
public: FrictionPyramid();
@@ -90,7 +91,7 @@ namespace gazebo
/// \brief SurfaceParams defines various Surface contact parameters.
/// These parameters defines the properties of a
/// physics::Contact constraint.
- class GAZEBO_VISIBLE SurfaceParams
+ class GZ_PHYSICS_VISIBLE SurfaceParams
{
/// \brief Constructor.
public: SurfaceParams();
@@ -110,12 +111,21 @@ namespace gazebo
/// \param[in] _msg Message to read values from.
public: virtual void ProcessMsg(const msgs::Surface &_msg);
+ /// \brief Get access to FrictionPyramid data, if available.
+ /// \return Pointer to FrictionPyramid data or NULL if class does
+ /// not use FrictionPyramid data.
+ public: virtual FrictionPyramidPtr GetFrictionPyramid() const;
+
/// \brief Allow collision checking without generating a contact joint.
public: bool collideWithoutContact;
/// \brief Custom collision filtering used when collideWithoutContact is
/// true.
public: unsigned int collideWithoutContactBitmask;
+
+ /// \brief Custom collision filtering. Will override
+ /// collideWithoutContact.
+ public: unsigned int collideBitmask;
};
/// \}
}
diff --git a/gazebo/physics/UniversalJoint.hh b/gazebo/physics/UniversalJoint.hh
index 0c42723..612263d 100644
--- a/gazebo/physics/UniversalJoint.hh
+++ b/gazebo/physics/UniversalJoint.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// Axis1 and axis2 are body-fixed, with axis1 attached to parent
/// body and axis2 attached to child body.
template<class T>
- class GAZEBO_VISIBLE UniversalJoint : public T
+ class GZ_PHYSICS_VISIBLE UniversalJoint : public T
{
/// \enum AxisIndex
/// \brief Map joint axes to corresponding link.
diff --git a/gazebo/physics/World.cc b/gazebo/physics/World.cc
index fd32e05..aa489b3 100644
--- a/gazebo/physics/World.cc
+++ b/gazebo/physics/World.cc
@@ -15,6 +15,12 @@
*
*/
+#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 <time.h>
#include <tbb/parallel_for.h>
@@ -62,6 +68,7 @@
#include "gazebo/physics/Link.hh"
#include "gazebo/physics/PhysicsEngine.hh"
#include "gazebo/physics/PhysicsFactory.hh"
+#include "gazebo/physics/PresetManager.hh"
#include "gazebo/physics/Model.hh"
#include "gazebo/physics/Actor.hh"
#include "gazebo/physics/WorldPrivate.hh"
@@ -117,6 +124,7 @@ World::World(const std::string &_name)
this->dataPtr->thread = NULL;
this->dataPtr->logThread = NULL;
this->dataPtr->stop = false;
+ this->dataPtr->seekPending = false;
this->dataPtr->currentStateBuffer = 0;
this->dataPtr->stateToggle = 0;
@@ -148,6 +156,7 @@ World::World(const std::string &_name)
//////////////////////////////////////////////////
World::~World()
{
+ this->dataPtr->presetManager.reset();
delete this->dataPtr->receiveMutex;
this->dataPtr->receiveMutex = NULL;
delete this->dataPtr->loadModelMutex;
@@ -218,6 +227,8 @@ void World::Load(sdf::ElementPtr _sdf)
&World::OnFactoryMsg, this);
this->dataPtr->controlSub = this->dataPtr->node->Subscribe("~/world_control",
&World::OnControl, this);
+ this->dataPtr->playbackControlSub = this->dataPtr->node->Subscribe(
+ "~/playback_control", &World::OnPlaybackControl, this);
this->dataPtr->requestSub = this->dataPtr->node->Subscribe("~/request",
&World::OnRequest, this, true);
@@ -239,16 +250,17 @@ void World::Load(sdf::ElementPtr _sdf)
this->dataPtr->lightPub = this->dataPtr->node->Advertise<msgs::Light>(
"~/light");
- std::string type = this->dataPtr->sdf->GetElement(
- "physics")->Get<std::string>("type");
+ // This should come before loading of entities
+ sdf::ElementPtr physicsElem = this->dataPtr->sdf->GetElement("physics");
+
+ std::string type = physicsElem->Get<std::string>("type");
this->dataPtr->physicsEngine = PhysicsFactory::NewPhysicsEngine(type,
shared_from_this());
if (this->dataPtr->physicsEngine == NULL)
gzthrow("Unable to create physics engine\n");
- // This should come before loading of entities
- this->dataPtr->physicsEngine->Load(this->dataPtr->sdf->GetElement("physics"));
+ this->dataPtr->physicsEngine->Load(physicsElem);
// This should also come before loading of entities
{
@@ -257,11 +269,11 @@ void World::Load(sdf::ElementPtr _sdf)
common::SphericalCoordinates::SurfaceType surfaceType =
common::SphericalCoordinates::Convert(
spherical->Get<std::string>("surface_model"));
- math::Angle latitude, longitude, heading;
+ ignition::math::Angle latitude, longitude, heading;
double elevation = spherical->Get<double>("elevation");
- latitude.SetFromDegree(spherical->Get<double>("latitude_deg"));
- longitude.SetFromDegree(spherical->Get<double>("longitude_deg"));
- heading.SetFromDegree(spherical->Get<double>("heading_deg"));
+ latitude.Degree(spherical->Get<double>("latitude_deg"));
+ longitude.Degree(spherical->Get<double>("longitude_deg"));
+ heading.Degree(spherical->Get<double>("heading_deg"));
this->dataPtr->sphericalCoordinates.reset(new common::SphericalCoordinates(
surfaceType, latitude, longitude, elevation, heading));
@@ -347,6 +359,9 @@ void World::Init()
// Initialize the physics engine
this->dataPtr->physicsEngine->Init();
+ this->dataPtr->presetManager = PresetManagerPtr(
+ new PresetManager(this->dataPtr->physicsEngine, this->dataPtr->sdf));
+
this->dataPtr->testRay = boost::dynamic_pointer_cast<RayShape>(
this->GetPhysicsEngine()->CreateShape("ray", CollisionPtr()));
@@ -481,12 +496,62 @@ void World::RunLoop()
//////////////////////////////////////////////////
void World::LogStep()
{
- if (!this->IsPaused() || this->dataPtr->stepInc > 0)
+ bool stay;
+
+ if (this->dataPtr->stepInc < 0)
+ {
+ // Step back: This is implemented by going to the beginning of the log file,
+ // and then, step forward up to the target frame.
+ // ToDo: Use keyframes in the log file to speed up this process.
+ if (!util::LogPlay::Instance()->Rewind())
+ {
+ gzerr << "Error processing a negative multi-step" << std::endl;
+ this->dataPtr->stepInc = 0;
+ return;
+ }
+
+ this->dataPtr->stepInc = this->dataPtr->iterations + this->dataPtr->stepInc;
+
+ // For some reason, the first two chunks contains the same <iterations>
+ // value. If the log file contains <iterations> we will load the same
+ // iterations value twice and this will affect the way we're stepping back.
+ // ToDo: Fix the source of the problem for avoiding this extra step.
+ if (util::LogPlay::Instance()->HasIterations())
+ {
+ this->dataPtr->stepInc +=
+ 2 - util::LogPlay::Instance()->GetInitialIterations();
+ }
+
+ if (this->dataPtr->stepInc < 1)
+ this->dataPtr->stepInc = 1;
+ this->dataPtr->iterations = 0;
+ }
+
{
+ boost::recursive_mutex::scoped_lock lk(*this->dataPtr->worldUpdateMutex);
+ // There are two main reasons to keep iterating in the following loop:
+ // 1. If "stepInc" is positive: This means that a client requested to
+ // advanced the simulation some steps.
+ // 2. If "seekPending" is true: This means that a client requested to
+ // advance the simulation to a given simulation time ("seek"). We will
+ // have to check at the end of each iteration if we reached the target
+ // simulation time.
+ // Note that if the simulation is not paused (play mode) we will enter
+ // the loop. However, the code will only execute one iteration (one step).
+ stay = !this->IsPaused() || this->dataPtr->stepInc > 0 ||
+ this->dataPtr->seekPending;
+ }
+ while (stay)
+ {
+ boost::recursive_mutex::scoped_lock lk(*this->dataPtr->worldUpdateMutex);
+
std::string data;
if (!util::LogPlay::Instance()->Step(data))
{
+ // There are no more chunks, time to exit.
this->SetPaused(true);
+ this->dataPtr->stepInc = 0;
+ break;
}
else
{
@@ -495,6 +560,14 @@ void World::LogStep()
this->dataPtr->logPlayState.Load(this->dataPtr->logPlayStateSDF);
+ // If the log file does not contain iterations we have to manually
+ // increase the iteration counter in logPlayState.
+ if (!util::LogPlay::Instance()->HasIterations())
+ {
+ this->dataPtr->logPlayState.SetIterations(
+ this->dataPtr->iterations + 1);
+ }
+
// Process insertions
if (this->dataPtr->logPlayStateSDF->HasElement("insertions"))
{
@@ -532,11 +605,28 @@ void World::LogStep()
this->SetState(this->dataPtr->logPlayState);
this->Update();
- this->dataPtr->iterations++;
}
if (this->dataPtr->stepInc > 0)
this->dataPtr->stepInc--;
+
+ // We may have entered into the loop because a "seek" command was
+ // requested. At this point we have executed one more step. Now, it's time
+ // to check if we have reached the target simulation time specified in the
+ // "seek" command.
+ if (this->dataPtr->seekPending)
+ {
+ this->dataPtr->seekPending =
+ this->GetSimTime() < this->dataPtr->targetSimTime;
+ }
+
+ stay = !this->IsPaused() || this->dataPtr->stepInc > 0 ||
+ this->dataPtr->seekPending;
+
+ // We only run one step if we are in play mode and we don't have
+ // other pending commands.
+ if (!this->IsPaused() && !this->dataPtr->seekPending)
+ break;
}
this->PublishWorldStats();
@@ -647,7 +737,7 @@ void World::Step(unsigned int _steps)
bool wait = true;
while (wait)
{
- common::Time::MSleep(1);
+ common::Time::NSleep(1);
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->worldUpdateMutex);
if (this->dataPtr->stepInc == 0 || this->dataPtr->stop)
wait = false;
@@ -718,11 +808,9 @@ void World::Update()
boost::recursive_mutex::scoped_lock lock(
*this->dataPtr->physicsEngine->GetPhysicsUpdateMutex());
- for (std::list<Entity*>::iterator iter =
- this->dataPtr->dirtyPoses.begin();
- iter != this->dataPtr->dirtyPoses.end(); ++iter)
+ for (auto &dirtyEntity : this->dataPtr->dirtyPoses)
{
- (*iter)->SetWorldPose((*iter)->GetDirtyPose(), false);
+ dirtyEntity->SetWorldPose(dirtyEntity->GetDirtyPose(), false);
}
this->dataPtr->dirtyPoses.clear();
@@ -793,10 +881,9 @@ void World::ClearModels()
this->dataPtr->publishModelPoses.clear();
// Remove all models
- for (Model_V::iterator iter = this->dataPtr->models.begin();
- iter != this->dataPtr->models.end(); ++iter)
+ for (auto &model : this->dataPtr->models)
{
- this->dataPtr->rootElement->RemoveChild((*iter)->GetId());
+ this->dataPtr->rootElement->RemoveChild(model->GetId());
}
this->dataPtr->models.clear();
@@ -816,6 +903,12 @@ PhysicsEnginePtr World::GetPhysicsEngine() const
}
//////////////////////////////////////////////////
+PresetManagerPtr World::GetPresetManager() const
+{
+ return this->dataPtr->presetManager;
+}
+
+//////////////////////////////////////////////////
common::SphericalCoordinatesPtr World::GetSphericalCoordinates() const
{
return this->dataPtr->sphericalCoordinates;
@@ -1006,18 +1099,16 @@ void World::Reset()
this->SetPaused(true);
{
- boost::recursive_mutex::scoped_lock(*this->dataPtr->worldUpdateMutex);
+ boost::recursive_mutex::scoped_lock lk(*this->dataPtr->worldUpdateMutex);
math::Rand::SetSeed(math::Rand::GetSeed());
this->dataPtr->physicsEngine->SetSeed(math::Rand::GetSeed());
this->ResetTime();
this->ResetEntities(Base::BASE);
- for (std::vector<WorldPluginPtr>::iterator iter =
- this->dataPtr->plugins.begin();
- iter != this->dataPtr->plugins.end(); ++iter)
+ for (auto &plugin : this->dataPtr->plugins)
{
- (*iter)->Reset();
+ plugin->Reset();
}
this->dataPtr->physicsEngine->Reset();
@@ -1099,7 +1190,7 @@ void World::SetPaused(bool _p)
return;
{
- boost::recursive_mutex::scoped_lock(*this->dataPtr->worldUpdateMutex);
+ boost::recursive_mutex::scoped_lock lk(*this->dataPtr->worldUpdateMutex);
this->dataPtr->pause = _p;
}
@@ -1172,6 +1263,49 @@ void World::OnControl(ConstWorldControlPtr &_data)
}
//////////////////////////////////////////////////
+void World::OnPlaybackControl(ConstLogPlaybackControlPtr &_data)
+{
+ boost::recursive_mutex::scoped_lock lock(*this->dataPtr->worldUpdateMutex);
+
+ // Ignore this command if there's another one pending.
+ if (this->dataPtr->seekPending)
+ return;
+
+ if (_data->has_pause())
+ this->SetPaused(_data->pause());
+
+ if (_data->has_multi_step())
+ {
+ // stepWorld is a blocking call so set stepInc directly so that world stats
+ // will still be published
+ this->SetPaused(true);
+ this->dataPtr->stepInc += _data->multi_step();
+ }
+
+ if (_data->has_seek())
+ {
+ this->dataPtr->targetSimTime = msgs::Convert(_data->seek());
+ if (this->GetSimTime() > this->dataPtr->targetSimTime)
+ util::LogPlay::Instance()->Rewind();
+ this->dataPtr->seekPending = true;
+ }
+
+ if (_data->has_rewind() && _data->rewind())
+ {
+ util::LogPlay::Instance()->Rewind();
+ this->dataPtr->stepInc = 1;
+ }
+
+ if (_data->has_forward() && _data->forward())
+ {
+ this->dataPtr->targetSimTime = util::LogPlay::Instance()->GetLogEndTime();
+ if (this->GetSimTime() > this->dataPtr->targetSimTime)
+ util::LogPlay::Instance()->Rewind();
+ this->dataPtr->seekPending = true;
+ }
+}
+
+//////////////////////////////////////////////////
void World::OnRequest(ConstRequestPtr &_msg)
{
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
@@ -1296,13 +1430,12 @@ void World::LoadPlugin(const std::string &_filename,
//////////////////////////////////////////////////
void World::RemovePlugin(const std::string &_name)
{
- std::vector<WorldPluginPtr>::iterator iter;
- for (iter = this->dataPtr->plugins.begin();
- iter != this->dataPtr->plugins.end(); ++iter)
+ for (auto plugin = this->dataPtr->plugins.begin();
+ plugin != this->dataPtr->plugins.end(); ++plugin)
{
- if ((*iter)->GetHandle() == _name)
+ if ((*plugin)->GetHandle() == _name)
{
- this->dataPtr->plugins.erase(iter);
+ this->dataPtr->plugins.erase(plugin);
break;
}
}
@@ -1321,11 +1454,9 @@ void World::ProcessEntityMsgs()
{
boost::mutex::scoped_lock lock(this->dataPtr->entityDeleteMutex);
- std::list<std::string>::iterator iter;
- for (iter = this->dataPtr->deleteEntity.begin();
- iter != this->dataPtr->deleteEntity.end(); ++iter)
+ for (auto &entityName : this->dataPtr->deleteEntity)
{
- this->RemoveModel(*iter);
+ this->RemoveModel(entityName);
}
if (!this->dataPtr->deleteEntity.empty())
@@ -1341,16 +1472,14 @@ void World::ProcessRequestMsgs()
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
msgs::Response response;
- std::list<msgs::Request>::iterator iter;
- for (iter = this->dataPtr->requestMsgs.begin();
- iter != this->dataPtr->requestMsgs.end(); ++iter)
+ for (auto const &requestMsg : this->dataPtr->requestMsgs)
{
bool send = true;
- response.set_id((*iter).id());
- response.set_request((*iter).request());
+ response.set_id(requestMsg.id());
+ response.set_request(requestMsg.request());
response.set_response("success");
- if ((*iter).request() == "entity_list")
+ if (requestMsg.request() == "entity_list")
{
msgs::Model_V modelVMsg;
@@ -1370,14 +1499,14 @@ void World::ProcessRequestMsgs()
std::string *serializedData = response.mutable_serialized_data();
modelVMsg.SerializeToString(serializedData);
}
- else if ((*iter).request() == "entity_delete")
+ else if (requestMsg.request() == "entity_delete")
{
boost::mutex::scoped_lock lock2(this->dataPtr->entityDeleteMutex);
- this->dataPtr->deleteEntity.push_back((*iter).data());
+ this->dataPtr->deleteEntity.push_back(requestMsg.data());
}
- else if ((*iter).request() == "entity_info")
+ else if (requestMsg.request() == "entity_info")
{
- BasePtr entity = this->dataPtr->rootElement->GetByName((*iter).data());
+ BasePtr entity = this->dataPtr->rootElement->GetByName(requestMsg.data());
if (entity)
{
if (entity->HasType(Base::MODEL))
@@ -1425,10 +1554,10 @@ void World::ProcessRequestMsgs()
else
{
response.set_type("error");
- response.set_response("nonexistant");
+ response.set_response("nonexistent");
}
}
- else if ((*iter).request() == "world_sdf")
+ else if (requestMsg.request() == "world_sdf")
{
msgs::GzString msg;
this->UpdateStateSDF();
@@ -1444,7 +1573,7 @@ void World::ProcessRequestMsgs()
msg.SerializeToString(serializedData);
response.set_type(msg.GetTypeName());
}
- else if ((*iter).request() == "scene_info")
+ else if (requestMsg.request() == "scene_info")
{
this->dataPtr->sceneMsg.clear_model();
this->BuildSceneMsg(this->dataPtr->sceneMsg, this->dataPtr->rootElement);
@@ -1453,7 +1582,7 @@ void World::ProcessRequestMsgs()
this->dataPtr->sceneMsg.SerializeToString(serializedData);
response.set_type(this->dataPtr->sceneMsg.GetTypeName());
}
- else if ((*iter).request() == "spherical_coordinates_info")
+ else if (requestMsg.request() == "spherical_coordinates_info")
{
msgs::SphericalCoordinates sphereCoordMsg;
msgs::Set(&sphereCoordMsg, *(this->dataPtr->sphericalCoordinates));
@@ -1478,22 +1607,20 @@ void World::ProcessRequestMsgs()
void World::ProcessModelMsgs()
{
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
- std::list<msgs::Model>::iterator iter;
- for (iter = this->dataPtr->modelMsgs.begin();
- iter != this->dataPtr->modelMsgs.end(); ++iter)
+ for (auto const &modelMsg : this->dataPtr->modelMsgs)
{
ModelPtr model;
- if ((*iter).has_id())
- model = this->GetModelById((*iter).id());
+ if (modelMsg.has_id())
+ model = this->GetModelById(modelMsg.id());
else
- model = this->GetModel((*iter).name());
+ model = this->GetModel(modelMsg.name());
if (!model)
gzerr << "Unable to find model["
- << (*iter).name() << "] Id[" << (*iter).id() << "]\n";
+ << modelMsg.name() << "] Id[" << modelMsg.id() << "]\n";
else
{
- model->ProcessMsg(*iter);
+ model->ProcessMsg(modelMsg);
// May 30, 2013: The following code was removed because it has a
// major performance impact when dragging complex object via the GUI.
@@ -1516,7 +1643,7 @@ void World::ProcessModelMsgs()
// }
// }
- this->dataPtr->modelPub->Publish(*iter);
+ this->dataPtr->modelPub->Publish(modelMsg);
}
}
@@ -1531,28 +1658,27 @@ void World::ProcessModelMsgs()
void World::ProcessFactoryMsgs()
{
std::list<sdf::ElementPtr> modelsToLoad;
- std::list<msgs::Factory>::iterator iter;
{
boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
- for (iter = this->dataPtr->factoryMsgs.begin();
- iter != this->dataPtr->factoryMsgs.end(); ++iter)
+ for (auto const &factoryMsg : this->dataPtr->factoryMsgs)
{
- this->dataPtr->factorySDF->root->ClearElements();
+ this->dataPtr->factorySDF->Root()->ClearElements();
- if ((*iter).has_sdf() && !(*iter).sdf().empty())
+ if (factoryMsg.has_sdf() && !factoryMsg.sdf().empty())
{
// SDF Parsing happens here
- if (!sdf::readString((*iter).sdf(), this->dataPtr->factorySDF))
+ if (!sdf::readString(factoryMsg.sdf(), this->dataPtr->factorySDF))
{
- gzerr << "Unable to read sdf string[" << (*iter).sdf() << "]\n";
+ gzerr << "Unable to read sdf string[" << factoryMsg.sdf() << "]\n";
continue;
}
}
- else if ((*iter).has_sdf_filename() && !(*iter).sdf_filename().empty())
+ else if (factoryMsg.has_sdf_filename() &&
+ !factoryMsg.sdf_filename().empty())
{
std::string filename = common::ModelDatabase::Instance()->GetModelFile(
- (*iter).sdf_filename());
+ factoryMsg.sdf_filename());
if (!sdf::readFile(filename, this->dataPtr->factorySDF))
{
@@ -1560,17 +1686,17 @@ void World::ProcessFactoryMsgs()
continue;
}
}
- else if ((*iter).has_clone_model_name())
+ else if (factoryMsg.has_clone_model_name())
{
- ModelPtr model = this->GetModel((*iter).clone_model_name());
+ ModelPtr model = this->GetModel(factoryMsg.clone_model_name());
if (!model)
{
- gzerr << "Unable to clone model[" << (*iter).clone_model_name()
+ gzerr << "Unable to clone model[" << factoryMsg.clone_model_name()
<< "]. Model not found.\n";
continue;
}
- this->dataPtr->factorySDF->root->InsertElement(
+ this->dataPtr->factorySDF->Root()->InsertElement(
model->GetSDF()->Clone());
std::string newName = model->GetName() + "_clone";
@@ -1582,7 +1708,7 @@ void World::ProcessFactoryMsgs()
i++;
}
- this->dataPtr->factorySDF->root->GetElement("model")->GetAttribute(
+ this->dataPtr->factorySDF->Root()->GetElement("model")->GetAttribute(
"name")->Set(newName);
}
else
@@ -1592,17 +1718,17 @@ void World::ProcessFactoryMsgs()
continue;
}
- if ((*iter).has_edit_name())
+ if (factoryMsg.has_edit_name())
{
BasePtr base =
- this->dataPtr->rootElement->GetByName((*iter).edit_name());
+ this->dataPtr->rootElement->GetByName(factoryMsg.edit_name());
if (base)
{
sdf::ElementPtr elem;
- if (this->dataPtr->factorySDF->root->GetName() == "sdf")
- elem = this->dataPtr->factorySDF->root->GetFirstElement();
+ if (this->dataPtr->factorySDF->Root()->GetName() == "sdf")
+ elem = this->dataPtr->factorySDF->Root()->GetFirstElement();
else
- elem = this->dataPtr->factorySDF->root;
+ elem = this->dataPtr->factorySDF->Root();
base->UpdateParameters(elem);
}
@@ -1613,7 +1739,7 @@ void World::ProcessFactoryMsgs()
bool isModel = false;
bool isLight = false;
- sdf::ElementPtr elem = this->dataPtr->factorySDF->root->Clone();
+ sdf::ElementPtr elem = this->dataPtr->factorySDF->Root()->Clone();
if (elem->HasElement("world"))
elem = elem->GetElement("world");
@@ -1636,21 +1762,23 @@ void World::ProcessFactoryMsgs()
else
{
gzerr << "Unable to find a model, light, or actor in:\n";
- this->dataPtr->factorySDF->root->PrintValues("");
+ this->dataPtr->factorySDF->Root()->PrintValues("");
continue;
}
if (!elem)
{
gzerr << "Invalid SDF:";
- this->dataPtr->factorySDF->root->PrintValues("");
+ this->dataPtr->factorySDF->Root()->PrintValues("");
continue;
}
elem->SetParent(this->dataPtr->sdf);
elem->GetParent()->InsertElement(elem);
- if ((*iter).has_pose())
- elem->GetElement("pose")->Set(msgs::Convert((*iter).pose()));
+ if (factoryMsg.has_pose())
+ {
+ elem->GetElement("pose")->Set(msgs::ConvertIgn(factoryMsg.pose()));
+ }
if (isActor)
{
@@ -1675,14 +1803,13 @@ void World::ProcessFactoryMsgs()
this->dataPtr->factoryMsgs.clear();
}
- for (std::list<sdf::ElementPtr>::iterator iter2 = modelsToLoad.begin();
- iter2 != modelsToLoad.end(); ++iter2)
+ for (auto const &elem : modelsToLoad)
{
try
{
boost::mutex::scoped_lock lock(this->dataPtr->factoryDeleteMutex);
- ModelPtr model = this->LoadModel(*iter2, this->dataPtr->rootElement);
+ ModelPtr model = this->LoadModel(elem, this->dataPtr->rootElement);
model->Init();
model->LoadPlugins();
}
@@ -1728,16 +1855,16 @@ void World::SetState(const WorldState &_state)
{
this->SetSimTime(_state.GetSimTime());
this->dataPtr->logRealTime = _state.GetRealTime();
+ this->dataPtr->iterations = _state.GetIterations();
const ModelState_M modelStates = _state.GetModelStates();
- for (ModelState_M::const_iterator iter = modelStates.begin();
- iter != modelStates.end(); ++iter)
+ for (auto const &modelState : modelStates)
{
- ModelPtr model = this->GetModel(iter->second.GetName());
+ ModelPtr model = this->GetModel(modelState.second.GetName());
if (model)
- model->SetState(iter->second);
+ model->SetState(modelState.second);
else
- gzerr << "Unable to find model[" << iter->second.GetName() << "]\n";
+ gzerr << "Unable to find model[" << modelState.second.GetName() << "]\n";
}
}
@@ -1780,20 +1907,18 @@ std::string World::StripWorldName(const std::string &_name) const
//////////////////////////////////////////////////
void World::EnableAllModels()
{
- for (Model_V::iterator iter = this->dataPtr->models.begin();
- iter != this->dataPtr->models.end(); ++iter)
+ for (auto &model : this->dataPtr->models)
{
- (*iter)->SetEnabled(true);
+ model->SetEnabled(true);
}
}
//////////////////////////////////////////////////
void World::DisableAllModels()
{
- for (Model_V::iterator iter = this->dataPtr->models.begin();
- iter != this->dataPtr->models.end(); ++iter)
+ for (auto &model : this->dataPtr->models)
{
- (*iter)->SetEnabled(false);
+ model->SetEnabled(false);
}
}
@@ -1815,7 +1940,7 @@ bool World::OnLog(std::ostringstream &_stream)
// Save the entire state when its the first call to OnLog.
if (util::LogRecord::Instance()->GetFirstUpdate())
{
- this->UpdateStateSDF();
+ this->dataPtr->sdf->Update();
_stream << "<sdf version ='";
_stream << SDF_VERSION;
_stream << "'>\n";
@@ -1828,11 +1953,11 @@ bool World::OnLog(std::ostringstream &_stream)
boost::mutex::scoped_lock lock(this->dataPtr->logBufferMutex);
this->dataPtr->currentStateBuffer ^= 1;
}
- for (std::deque<WorldState>::iterator iter =
- this->dataPtr->states[bufferIndex].begin();
- iter != this->dataPtr->states[bufferIndex].end(); ++iter)
+ for (auto const &worldState : this->dataPtr->states[bufferIndex])
{
- _stream << "<sdf version='" << SDF_VERSION << "'>" << *iter << "</sdf>";
+ _stream << "<sdf version='" << SDF_VERSION << "'>"
+ << worldState
+ << "</sdf>";
}
this->dataPtr->states[bufferIndex].clear();
@@ -1891,26 +2016,23 @@ void World::ProcessMessages()
if (!this->dataPtr->publishModelPoses.empty())
{
- for (std::set<ModelPtr>::iterator iter =
- this->dataPtr->publishModelPoses.begin();
- iter != this->dataPtr->publishModelPoses.end(); ++iter)
+ for (auto const &model : this->dataPtr->publishModelPoses)
{
msgs::Pose *poseMsg = msg.add_pose();
// Publish the model's relative pose
- poseMsg->set_name((*iter)->GetScopedName());
- poseMsg->set_id((*iter)->GetId());
- msgs::Set(poseMsg, (*iter)->GetRelativePose());
+ poseMsg->set_name(model->GetScopedName());
+ poseMsg->set_id(model->GetId());
+ msgs::Set(poseMsg, model->GetRelativePose().Ign());
// Publish each of the model's children relative poses
- Link_V links = (*iter)->GetLinks();
- for (Link_V::iterator linkIter = links.begin();
- linkIter != links.end(); ++linkIter)
+ Link_V links = model->GetLinks();
+ for (auto const &link : links)
{
poseMsg = msg.add_pose();
- poseMsg->set_name((*linkIter)->GetScopedName());
- poseMsg->set_id((*linkIter)->GetId());
- msgs::Set(poseMsg, (*linkIter)->GetRelativePose());
+ poseMsg->set_name(link->GetScopedName());
+ poseMsg->set_id(link->GetId());
+ msgs::Set(poseMsg, link->GetRelativePose().Ign());
}
}
@@ -1943,6 +2065,8 @@ void World::ProcessMessages()
//////////////////////////////////////////////////
void World::PublishWorldStats()
{
+ this->dataPtr->worldStatsMsg.Clear();
+
msgs::Set(this->dataPtr->worldStatsMsg.mutable_sim_time(),
this->GetSimTime());
msgs::Set(this->dataPtr->worldStatsMsg.mutable_real_time(),
@@ -1953,6 +2077,18 @@ void World::PublishWorldStats()
this->dataPtr->worldStatsMsg.set_iterations(this->dataPtr->iterations);
this->dataPtr->worldStatsMsg.set_paused(this->IsPaused());
+ if (util::LogPlay::Instance()->IsOpen())
+ {
+ msgs::LogPlaybackStatistics logStats;
+ msgs::Set(logStats.mutable_start_time(),
+ util::LogPlay::Instance()->GetLogStartTime());
+ msgs::Set(logStats.mutable_end_time(),
+ util::LogPlay::Instance()->GetLogEndTime());
+
+ this->dataPtr->worldStatsMsg.mutable_log_playback_stats()->CopyFrom(
+ logStats);
+ }
+
if (this->dataPtr->statPub && this->dataPtr->statPub->HasConnections())
this->dataPtr->statPub->Publish(this->dataPtr->worldStatsMsg);
this->dataPtr->prevStatTime = common::Time::GetWallTime();
@@ -2033,16 +2169,16 @@ void World::RemoveModel(const std::string &_name)
// Remove all the dirty poses from the delete entity.
{
- for (std::list<Entity*>::iterator iter2 = this->dataPtr->dirtyPoses.begin();
- iter2 != this->dataPtr->dirtyPoses.end();)
+ for (auto entity = this->dataPtr->dirtyPoses.begin();
+ entity != this->dataPtr->dirtyPoses.end(); ++entity)
{
- if ((*iter2)->GetName() == _name ||
- ((*iter2)->GetParent() && (*iter2)->GetParent()->GetName() == _name))
+ if ((*entity)->GetName() == _name ||
+ ((*entity)->GetParent() && (*entity)->GetParent()->GetName() == _name))
{
- this->dataPtr->dirtyPoses.erase(iter2++);
+ this->dataPtr->dirtyPoses.erase(entity++);
}
else
- ++iter2;
+ ++entity;
}
}
@@ -2083,12 +2219,12 @@ void World::RemoveModel(const std::string &_name)
this->dataPtr->rootElement->RemoveChild(_name);
- for (Model_V::iterator iter = this->dataPtr->models.begin();
- iter != this->dataPtr->models.end(); ++iter)
+ for (auto model = this->dataPtr->models.begin();
+ model != this->dataPtr->models.end(); ++model)
{
- if ((*iter)->GetName() == _name || (*iter)->GetScopedName() == _name)
+ if ((*model)->GetName() == _name || (*model)->GetScopedName() == _name)
{
- this->dataPtr->models.erase(iter);
+ this->dataPtr->models.erase(model);
break;
}
}
@@ -2097,13 +2233,12 @@ void World::RemoveModel(const std::string &_name)
// Cleanup the publishModelPoses list.
{
boost::recursive_mutex::scoped_lock lock2(*this->dataPtr->receiveMutex);
- for (std::set<ModelPtr>::iterator iter =
- this->dataPtr->publishModelPoses.begin();
- iter != this->dataPtr->publishModelPoses.end(); ++iter)
+ for (auto model = this->dataPtr->publishModelPoses.begin();
+ model != this->dataPtr->publishModelPoses.end(); ++model)
{
- if ((*iter)->GetName() == _name || (*iter)->GetScopedName() == _name)
+ if ((*model)->GetName() == _name || (*model)->GetScopedName() == _name)
{
- this->dataPtr->publishModelPoses.erase(iter);
+ this->dataPtr->publishModelPoses.erase(model);
break;
}
}
@@ -2170,3 +2305,9 @@ void World::EnablePhysicsEngine(bool _enable)
this->dataPtr->enablePhysicsEngine = _enable;
}
+/////////////////////////////////////////////////
+void World::_AddDirty(Entity *_entity)
+{
+ GZ_ASSERT(_entity != NULL, "_entity is NULL");
+ this->dataPtr->dirtyPoses.push_back(_entity);
+}
diff --git a/gazebo/physics/World.hh b/gazebo/physics/World.hh
index bdf0126..92333ec 100644
--- a/gazebo/physics/World.hh
+++ b/gazebo/physics/World.hh
@@ -17,6 +17,12 @@
#ifndef _GAZEBO_WORLD_HH_
#define _GAZEBO_WORLD_HH_
+#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 <vector>
#include <list>
#include <set>
@@ -59,7 +65,8 @@ namespace gazebo
/// (links, joints, sensors, plugins, etc), and WorldPlugin instances.
/// Many core function are also handled in the World, including physics
/// update, model updates, and message processing.
- class GAZEBO_VISIBLE World : public boost::enable_shared_from_this<World>
+ class GZ_PHYSICS_VISIBLE World :
+ public boost::enable_shared_from_this<World>
{
/// \brief Constructor.
/// Constructor for the World. Must specify a unique name.
@@ -115,6 +122,10 @@ namespace gazebo
/// \return Pointer to the physics engine.
public: PhysicsEnginePtr GetPhysicsEngine() const;
+ /// \brief Return the preset manager.
+ /// \return Pointer to the preset manager.
+ public: PresetManagerPtr GetPresetManager() const;
+
/// \brief Return the spherical coordinates converter.
/// \return Pointer to the spherical coordinates converter.
public: common::SphericalCoordinatesPtr GetSphericalCoordinates() const;
@@ -321,6 +332,14 @@ namespace gazebo
/// \param[in] _name Name of the model to remove.
public: void RemoveModel(const std::string &_name);
+ /// \internal
+ /// \brief Inform the World that an Entity has moved. The Entity
+ /// is added to a list that will be processed by the World.
+ /// Only a physics engine implementation should call this function.
+ /// If you are unsure whether you should use this function, do not.
+ /// \param[in] _entity Entity that has moved.
+ public: void _AddDirty(Entity *_entity);
+
/// \cond
/// This is an internal function.
/// \brief Get a model by id.
@@ -382,6 +401,10 @@ namespace gazebo
/// \param[in] _data The world control message.
private: void OnControl(ConstWorldControlPtr &_data);
+ /// \brief Called when log playback control message is received.
+ /// \param[in] _data The log playback control message.
+ private: void OnPlaybackControl(ConstLogPlaybackControlPtr &_data);
+
/// \brief Called when a request message is received.
/// \param[in] _msg The request message.
private: void OnRequest(ConstRequestPtr &_msg);
@@ -455,9 +478,6 @@ namespace gazebo
/// \brief Private data pointer.
private: WorldPrivate *dataPtr;
- /// Friend ODELink so that it has access to dataPtr->dirtyPoses
- private: friend class ODELink;
-
/// Friend DARTLink so that it has access to dataPtr->dirtyPoses
private: friend class DARTLink;
diff --git a/gazebo/physics/WorldPrivate.hh b/gazebo/physics/WorldPrivate.hh
index d12b2e8..a25a19d 100644
--- a/gazebo/physics/WorldPrivate.hh
+++ b/gazebo/physics/WorldPrivate.hh
@@ -78,6 +78,13 @@ namespace gazebo
/// \brief Number of steps in increment by.
public: int stepInc;
+ /// \brief Stores the simulation time target during a 'seek' operation.
+ public: common::Time targetSimTime;
+
+ /// \brief When there is a 'seek' command pending during a log file
+ /// playback this member variable should be true.
+ public: bool seekPending;
+
/// \brief All the event connections.
public: event::Connection_V connections;
@@ -108,6 +115,9 @@ namespace gazebo
/// \brief Subscriber to world control messages.
public: transport::SubscriberPtr controlSub;
+ /// \brief Subscriber to log playback control messages.
+ public: transport::SubscriberPtr playbackControlSub;
+
/// \brief Subscriber to factory messages.
public: transport::SubscriberPtr factorySub;
@@ -285,6 +295,9 @@ namespace gazebo
/// this flag is set to trigger Entity::SetWorldPose on the
/// physics::Link in World::Update.
public: std::list<Entity*> dirtyPoses;
+
+ /// \brief Class to manage preset simulation parameter profiles.
+ public: PresetManagerPtr presetManager;
};
}
}
diff --git a/gazebo/physics/WorldState.cc b/gazebo/physics/WorldState.cc
index b4f636e..7478f10 100644
--- a/gazebo/physics/WorldState.cc
+++ b/gazebo/physics/WorldState.cc
@@ -18,6 +18,12 @@
* 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 "gazebo/common/Console.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/physics/World.hh"
@@ -35,7 +41,8 @@ WorldState::WorldState()
/////////////////////////////////////////////////
WorldState::WorldState(const WorldPtr _world)
- : State(_world->GetName(), _world->GetRealTime(), _world->GetSimTime())
+ : State(_world->GetName(), _world->GetRealTime(), _world->GetSimTime(),
+ _world->GetIterations())
{
this->world = _world;
@@ -45,7 +52,7 @@ WorldState::WorldState(const WorldPtr _world)
iter != models.end(); ++iter)
{
this->modelStates.insert(std::make_pair((*iter)->GetName(),
- ModelState(*iter, this->realTime, this->simTime)));
+ ModelState(*iter, this->realTime, this->simTime, this->iterations)));
}
}
@@ -71,6 +78,7 @@ void WorldState::Load(const WorldPtr _world)
this->wallTime = common::Time::GetWallTime();
this->simTime = _world->GetSimTime();
this->realTime = _world->GetRealTime();
+ this->iterations = _world->GetIterations();
// Add a state for all the models
Model_V models = _world->GetModels();
@@ -78,7 +86,7 @@ void WorldState::Load(const WorldPtr _world)
iter != models.end(); ++iter)
{
this->modelStates[(*iter)->GetName()].Load(*iter, this->realTime,
- this->simTime);
+ this->simTime, this->iterations);
}
// Remove models that no longer exist. We determine this by check the time
@@ -101,6 +109,7 @@ void WorldState::Load(const sdf::ElementPtr _elem)
this->simTime = _elem->Get<common::Time>("sim_time");
this->wallTime = _elem->Get<common::Time>("wall_time");
this->realTime = _elem->Get<common::Time>("real_time");
+ this->iterations = _elem->Get<uint64_t>("iterations");
// Add the model states
this->modelStates.clear();
@@ -116,6 +125,7 @@ void WorldState::Load(const sdf::ElementPtr _elem)
this->modelStates[modelName].SetSimTime(this->simTime);
this->modelStates[modelName].SetWallTime(this->wallTime);
this->modelStates[modelName].SetRealTime(this->realTime);
+ this->modelStates[modelName].SetIterations(this->iterations);
childElem = childElem->GetNextElement("model");
}
}
@@ -232,6 +242,7 @@ WorldState WorldState::operator-(const WorldState &_state) const
result.simTime = this->simTime;
result.realTime = this->realTime;
result.wallTime = this->wallTime;
+ result.iterations = this->iterations;
// Subtract the model states.
for (ModelState_M::const_iterator iter =
@@ -276,6 +287,7 @@ WorldState WorldState::operator+(const WorldState &_state) const
result.simTime = this->simTime;
result.realTime = this->realTime;
result.wallTime = this->wallTime;
+ result.iterations = this->iterations;
// Add the states.
for (ModelState_M::const_iterator iter =
@@ -298,6 +310,7 @@ void WorldState::FillSDF(sdf::ElementPtr _sdf)
_sdf->GetElement("sim_time")->Set(this->simTime);
_sdf->GetElement("real_time")->Set(this->realTime);
_sdf->GetElement("wall_time")->Set(this->wallTime);
+ _sdf->GetElement("iterations")->Set(this->iterations);
for (ModelState_M::iterator iter =
this->modelStates.begin(); iter != this->modelStates.end(); ++iter)
@@ -342,3 +355,12 @@ void WorldState::SetSimTime(const common::Time &_time)
iter->second.SetSimTime(_time);
}
}
+
+/////////////////////////////////////////////////
+void WorldState::SetIterations(const uint64_t _iterations)
+{
+ State::SetIterations(_iterations);
+
+ for (auto &modelState : this->modelStates)
+ modelState.second.SetIterations(_iterations);
+}
diff --git a/gazebo/physics/WorldState.hh b/gazebo/physics/WorldState.hh
index 045126c..886e407 100644
--- a/gazebo/physics/WorldState.hh
+++ b/gazebo/physics/WorldState.hh
@@ -43,7 +43,7 @@ namespace gazebo
/// Instances of this class contain the state of a World at a specific
/// time. World state includes the state of all models, and their
/// children.
- class GAZEBO_VISIBLE WorldState : public State
+ class GZ_PHYSICS_VISIBLE WorldState : public State
{
/// \brief Default constructor
public: WorldState();
@@ -130,6 +130,11 @@ namespace gazebo
/// \param[in] _time Simulation time when the data was recorded.
public: virtual void SetSimTime(const common::Time &_time);
+ /// \brief Set the simulation interations when this state was generated
+ /// \param[in] _iterations Simulation iterations when the data was
+ /// recorded.
+ public: virtual void SetIterations(const uint64_t _iterations);
+
/// \brief Assignment operator
/// \param[in] _state State value
/// \return Reference to this
@@ -155,7 +160,8 @@ namespace gazebo
_out << "<state world_name='" << _state.name << "'>"
<< "<sim_time>" << _state.simTime << "</sim_time>"
<< "<wall_time>" << _state.wallTime << "</wall_time>"
- << "<real_time>" << _state.realTime << "</real_time>";
+ << "<real_time>" << _state.realTime << "</real_time>"
+ << "<iterations>" << _state.iterations << "</iterations>";
// List all of the inserted models
if (_state.insertions.size() > 0)
diff --git a/gazebo/physics/bullet/BulletBallJoint.hh b/gazebo/physics/bullet/BulletBallJoint.hh
index 4492208..deaed7b 100644
--- a/gazebo/physics/bullet/BulletBallJoint.hh
+++ b/gazebo/physics/bullet/BulletBallJoint.hh
@@ -38,7 +38,7 @@ namespace gazebo
/// \{
/// \brief BulletBallJoint class models a ball joint in Bullet.
- class GAZEBO_VISIBLE BulletBallJoint : public BallJoint<BulletJoint>
+ class GZ_PHYSICS_VISIBLE BulletBallJoint : public BallJoint<BulletJoint>
{
/// \brief Bullet Ball Joint Constructor
public: BulletBallJoint(btDynamicsWorld *_world, BasePtr _parent);
diff --git a/gazebo/physics/bullet/BulletBoxShape.hh b/gazebo/physics/bullet/BulletBoxShape.hh
index f38902a..eb46c69 100644
--- a/gazebo/physics/bullet/BulletBoxShape.hh
+++ b/gazebo/physics/bullet/BulletBoxShape.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \{
/// \brief Bullet box collision
- class GAZEBO_VISIBLE BulletBoxShape : public BoxShape
+ class GZ_PHYSICS_VISIBLE BulletBoxShape : public BoxShape
{
/// \brief Constructor
public: BulletBoxShape(CollisionPtr _parent) : BoxShape(_parent) {}
diff --git a/gazebo/physics/bullet/BulletCollision.hh b/gazebo/physics/bullet/BulletCollision.hh
index 6a579b9..a7c8f0a 100644
--- a/gazebo/physics/bullet/BulletCollision.hh
+++ b/gazebo/physics/bullet/BulletCollision.hh
@@ -50,7 +50,7 @@ namespace gazebo
/// \{
/// \brief Bullet collisions
- class GAZEBO_VISIBLE BulletCollision : public Collision
+ class GZ_PHYSICS_VISIBLE BulletCollision : public Collision
{
/// \brief Constructor
public: BulletCollision(LinkPtr _parent);
diff --git a/gazebo/physics/bullet/BulletCylinderShape.hh b/gazebo/physics/bullet/BulletCylinderShape.hh
index cdceb56..e013ebc 100644
--- a/gazebo/physics/bullet/BulletCylinderShape.hh
+++ b/gazebo/physics/bullet/BulletCylinderShape.hh
@@ -35,7 +35,7 @@ namespace gazebo
/// \{
/// \brief Cylinder collision
- class GAZEBO_VISIBLE BulletCylinderShape : public CylinderShape
+ class GZ_PHYSICS_VISIBLE BulletCylinderShape : public CylinderShape
{
/// \brief Constructor
public: BulletCylinderShape(CollisionPtr _parent)
diff --git a/gazebo/physics/bullet/BulletFixedJoint.cc b/gazebo/physics/bullet/BulletFixedJoint.cc
new file mode 100644
index 0000000..18bb74e
--- /dev/null
+++ b/gazebo/physics/bullet/BulletFixedJoint.cc
@@ -0,0 +1,235 @@
+/*
+ * 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/common/Assert.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/Exception.hh"
+
+#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/bullet/BulletLink.hh"
+#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletFixedJoint.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+BulletFixedJoint::BulletFixedJoint(btDynamicsWorld *_world, BasePtr _parent)
+ : FixedJoint<BulletJoint>(_parent)
+{
+ GZ_ASSERT(_world, "bullet world pointer is NULL");
+ this->bulletWorld = _world;
+ this->bulletFixed = NULL;
+}
+
+//////////////////////////////////////////////////
+BulletFixedJoint::~BulletFixedJoint()
+{
+}
+
+//////////////////////////////////////////////////
+void BulletFixedJoint::Load(sdf::ElementPtr _sdf)
+{
+ FixedJoint<BulletJoint>::Load(_sdf);
+}
+
+//////////////////////////////////////////////////
+void BulletFixedJoint::Init()
+{
+ FixedJoint<BulletJoint>::Init();
+
+ // Cast to BulletLink
+ BulletLinkPtr bulletChildLink =
+ boost::static_pointer_cast<BulletLink>(this->childLink);
+ BulletLinkPtr bulletParentLink =
+ boost::static_pointer_cast<BulletLink>(this->parentLink);
+
+ // Get axis unit vector (expressed in world frame).
+ math::Vector3 axis = math::Vector3::UnitZ;
+
+ // Local variables used to compute pivots and axes in body-fixed frames
+ // for the parent and child links.
+ math::Vector3 pivotParent, pivotChild, axisParent, axisChild;
+ math::Pose pose;
+
+ // Initialize pivots to anchorPos, which is expressed in the
+ // world coordinate frame.
+ pivotParent = this->anchorPos;
+ pivotChild = this->anchorPos;
+
+ // Check if parentLink exists. If not, the parent will be the world.
+ if (this->parentLink)
+ {
+ // Compute relative pose between joint anchor and CoG of parent link.
+ pose = this->parentLink->GetWorldCoGPose();
+ // Subtract CoG position from anchor position, both in world frame.
+ pivotParent -= pose.pos;
+ // Rotate pivot offset and axis into body-fixed frame of parent.
+ pivotParent = pose.rot.RotateVectorReverse(pivotParent);
+ axisParent = pose.rot.RotateVectorReverse(axis);
+ axisParent = axisParent.Normalize();
+ }
+ // Check if childLink exists. If not, the child will be the world.
+ if (this->childLink)
+ {
+ // Compute relative pose between joint anchor and CoG of child link.
+ pose = this->childLink->GetWorldCoGPose();
+ // Subtract CoG position from anchor position, both in world frame.
+ pivotChild -= pose.pos;
+ // Rotate pivot offset and axis into body-fixed frame of child.
+ pivotChild = pose.rot.RotateVectorReverse(pivotChild);
+ axisChild = pose.rot.RotateVectorReverse(axis);
+ axisChild = axisChild.Normalize();
+ }
+
+ // If both links exist, then create a joint between the two links.
+ if (bulletChildLink && bulletParentLink)
+ {
+ this->bulletFixed = new btHingeConstraint(
+ *(bulletChildLink->GetBulletLink()),
+ *(bulletParentLink->GetBulletLink()),
+ BulletTypes::ConvertVector3(pivotChild),
+ BulletTypes::ConvertVector3(pivotParent),
+ BulletTypes::ConvertVector3(axisChild),
+ BulletTypes::ConvertVector3(axisParent));
+ }
+ // If only the child exists, then create a joint between the child
+ // and the world.
+ else if (bulletChildLink)
+ {
+ this->bulletFixed = new btHingeConstraint(
+ *(bulletChildLink->GetBulletLink()),
+ BulletTypes::ConvertVector3(pivotChild),
+ BulletTypes::ConvertVector3(axisChild));
+ }
+ // If only the parent exists, then create a joint between the parent
+ // and the world.
+ else if (bulletParentLink)
+ {
+ this->bulletFixed = new btHingeConstraint(
+ *(bulletParentLink->GetBulletLink()),
+ BulletTypes::ConvertVector3(pivotParent),
+ BulletTypes::ConvertVector3(axisParent));
+ }
+ // Throw an error if no links are given.
+ else
+ {
+ gzerr << "unable to create bullet hinge without links.\n";
+ return;
+ }
+
+ if (!this->bulletFixed)
+ {
+ gzerr << "unable to create bullet hinge constraint\n";
+ return;
+ }
+
+ // Give parent class BulletJoint a pointer to this constraint.
+ this->constraint = this->bulletFixed;
+
+ this->bulletFixed->setLimit(0.0, 0.0);
+
+ // Add the joint to the world
+ GZ_ASSERT(this->bulletWorld, "bullet world pointer is NULL");
+ this->bulletWorld->addConstraint(this->bulletFixed, true);
+
+ // Allows access to impulse
+ this->bulletFixed->enableFeedback(true);
+
+ // Setup Joint force and torque feedback
+ this->SetupJointFeedback();
+}
+
+//////////////////////////////////////////////////
+void BulletFixedJoint::SetAxis(unsigned int /*_index*/,
+ const math::Vector3 &/*_axis*/)
+{
+ gzwarn << "BulletFixedJoint: called method "
+ << "SetAxis that is not valid for joints of type fixed.\n";
+}
+
+//////////////////////////////////////////////////
+math::Angle BulletFixedJoint::GetAngleImpl(unsigned int /*_index*/) const
+{
+ gzwarn << "BulletFixedJoint: called method "
+ << "GetAngleImpl that is not valid for joints of type fixed.\n";
+ return math::Angle();
+}
+
+//////////////////////////////////////////////////
+void BulletFixedJoint::SetVelocity(unsigned int /*_index*/, double /*_angle*/)
+{
+ gzwarn << "BulletFixedJoint: called method "
+ << "SetVelocity that is not valid for joints of type fixed.\n";
+}
+
+//////////////////////////////////////////////////
+double BulletFixedJoint::GetVelocity(unsigned int /*_index*/) const
+{
+ gzwarn << "BulletFixedJoint: called method "
+ << "GetVelocity that is not valid for joints of type fixed.\n";
+ return 0.0;
+}
+
+//////////////////////////////////////////////////
+void BulletFixedJoint::SetMaxForce(unsigned int /*_index*/, double /*_t*/)
+{
+ gzwarn << "BulletFixedJoint: called method "
+ << "SetMaxForce that is not valid for joints of type fixed.\n";
+}
+
+//////////////////////////////////////////////////
+double BulletFixedJoint::GetMaxForce(unsigned int /*_index*/)
+{
+ gzwarn << "BulletFixedJoint: called method "
+ << "GetMaxForce that is not valid for joints of type fixed.\n";
+ return 0.0;
+}
+
+//////////////////////////////////////////////////
+void BulletFixedJoint::SetForceImpl(unsigned int /*_index*/, double /*_effort*/)
+{
+ gzwarn << "BulletFixedJoint: called method "
+ << "SetForceImpl that is not valid for joints of type fixed.\n";
+ return;
+}
+
+//////////////////////////////////////////////////
+bool BulletFixedJoint::SetHighStop(unsigned int /*_index*/,
+ const math::Angle &/*_angle*/)
+{
+ gzwarn << "BulletFixedJoint: called method "
+ << "SetHighStop that is not valid for joints of type fixed.\n";
+ return false;
+}
+
+//////////////////////////////////////////////////
+bool BulletFixedJoint::SetLowStop(unsigned int /*_index*/,
+ const math::Angle &/*_angle*/)
+{
+ gzwarn << "BulletFixedJoint: called method "
+ << "SetLowStop that is not valid for joints of type fixed.\n";
+ return false;
+}
+
+//////////////////////////////////////////////////
+math::Vector3 BulletFixedJoint::GetGlobalAxis(unsigned int /*_index*/) const
+{
+ gzwarn << "BulletFixedJoint: called method "
+ << "GetGlobalAxis that is not valid for joints of type fixed.\n";
+ return math::Vector3();
+}
diff --git a/gazebo/physics/bullet/BulletFixedJoint.hh b/gazebo/physics/bullet/BulletFixedJoint.hh
new file mode 100644
index 0000000..60d8dc8
--- /dev/null
+++ b/gazebo/physics/bullet/BulletFixedJoint.hh
@@ -0,0 +1,92 @@
+/*
+ * 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_BULLETFIXEDJOINT_HH_
+#define _GAZEBO_BULLETFIXEDJOINT_HH_
+
+#include <string>
+#include "gazebo/math/Angle.hh"
+#include "gazebo/math/Vector3.hh"
+#include "gazebo/physics/FixedJoint.hh"
+#include "gazebo/physics/bullet/BulletJoint.hh"
+#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \ingroup gazebo_physics
+ /// \addtogroup gazebo_physics_bullet Bullet Physics
+ /// \{
+
+ /// \brief A fixed joint.
+ class GZ_PHYSICS_VISIBLE BulletFixedJoint : public FixedJoint<BulletJoint>
+ {
+ /// \brief Constructor
+ /// \param[in] world pointer to the bullet world
+ /// \param[in] _parent pointer to the parent Model
+ public: BulletFixedJoint(btDynamicsWorld *world, BasePtr _parent);
+
+ /// \brief Destructor
+ public: virtual ~BulletFixedJoint();
+
+ // Documentation inherited.
+ protected: virtual void Load(sdf::ElementPtr _sdf);
+
+ // Documentation inherited.
+ public: virtual void Init();
+
+ // Documentation inherited.
+ public: virtual void SetAxis(unsigned int _index,
+ const math::Vector3 &_axis);
+
+ // Documentation inherited.
+ public: virtual void SetVelocity(unsigned int _index, double _vel);
+
+ // Documentation inherited.
+ public: virtual double GetVelocity(unsigned int _index) const;
+
+ // Documentation inherited.
+ public: virtual void SetMaxForce(unsigned int _index, double _t);
+
+ // Documentation inherited.
+ public: virtual double GetMaxForce(unsigned int _index);
+
+ // Documentation inherited.
+ public: virtual bool SetHighStop(unsigned int _index,
+ const math::Angle &_angle);
+
+ // Documentation inherited.
+ public: virtual bool SetLowStop(unsigned int _index,
+ const math::Angle &_angle);
+
+ // Documentation inherited.
+ public: virtual math::Vector3 GetGlobalAxis(unsigned int _index) const;
+
+ // Documentation inherited.
+ public: virtual math::Angle GetAngleImpl(unsigned int _index) const;
+
+ // Documentation inherited.
+ protected: virtual void SetForceImpl(unsigned int _index, double _effort);
+
+ /// \brief Pointer to bullet fixed constraint implementation
+ private: btHingeConstraint *bulletFixed;
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/physics/bullet/BulletHeightmapShape.hh b/gazebo/physics/bullet/BulletHeightmapShape.hh
index 012a228..3db827d 100644
--- a/gazebo/physics/bullet/BulletHeightmapShape.hh
+++ b/gazebo/physics/bullet/BulletHeightmapShape.hh
@@ -39,7 +39,7 @@ namespace gazebo
/// \{
/// \brief Height map collision
- class GAZEBO_VISIBLE BulletHeightmapShape : public HeightmapShape
+ class GZ_PHYSICS_VISIBLE BulletHeightmapShape : public HeightmapShape
{
/// \brief Constructor
public: BulletHeightmapShape(CollisionPtr _parent);
diff --git a/gazebo/physics/bullet/BulletHinge2Joint.cc b/gazebo/physics/bullet/BulletHinge2Joint.cc
index 2a8fcbd..07286fe 100644
--- a/gazebo/physics/bullet/BulletHinge2Joint.cc
+++ b/gazebo/physics/bullet/BulletHinge2Joint.cc
@@ -14,10 +14,6 @@
* limitations under the License.
*
*/
-/* Desc: A hinge joint with 2 degrees of freedom
- * Author: Nate Koenig, Andrew Howard
- * Date: 21 May 2003
- */
#include "gazebo/common/Assert.hh"
#include "gazebo/common/Console.hh"
@@ -38,6 +34,8 @@ BulletHinge2Joint::BulletHinge2Joint(btDynamicsWorld *_world, BasePtr _parent)
GZ_ASSERT(_world, "bullet world pointer is NULL");
this->bulletWorld = _world;
this->bulletHinge2 = NULL;
+ this->angleOffset[0] = 0.0;
+ this->angleOffset[1] = 0.0;
}
//////////////////////////////////////////////////
@@ -211,8 +209,12 @@ math::Angle BulletHinge2Joint::GetHighStop(unsigned int _index)
return math::Angle();
}
- btRotationalLimitMotor *motor =
- this->bulletHinge2->getRotationalLimitMotor(_index);
+#ifndef LIBBULLET_VERSION_GT_282
+ btRotationalLimitMotor *motor;
+#else
+ btRotationalLimitMotor2 *motor;
+#endif
+ motor = this->bulletHinge2->getRotationalLimitMotor(_index);
if (motor)
return motor->m_hiLimit;
@@ -229,8 +231,12 @@ math::Angle BulletHinge2Joint::GetLowStop(unsigned int _index)
return math::Angle(0.0);
}
- btRotationalLimitMotor *motor =
- this->bulletHinge2->getRotationalLimitMotor(_index);
+#ifndef LIBBULLET_VERSION_GT_282
+ btRotationalLimitMotor *motor;
+#else
+ btRotationalLimitMotor2 *motor;
+#endif
+ motor = this->bulletHinge2->getRotationalLimitMotor(_index);
if (motor)
return motor->m_loLimit;
diff --git a/gazebo/physics/bullet/BulletHinge2Joint.hh b/gazebo/physics/bullet/BulletHinge2Joint.hh
index 2549a2d..ad5ad15 100644
--- a/gazebo/physics/bullet/BulletHinge2Joint.hh
+++ b/gazebo/physics/bullet/BulletHinge2Joint.hh
@@ -40,7 +40,7 @@ namespace gazebo
/// \{
/// \brief A two axis hinge joint
- class GAZEBO_VISIBLE BulletHinge2Joint : public Hinge2Joint<BulletJoint>
+ class GZ_PHYSICS_VISIBLE BulletHinge2Joint : public Hinge2Joint<BulletJoint>
{
/// \brief Constructor
public: BulletHinge2Joint(btDynamicsWorld *world, BasePtr _parent);
diff --git a/gazebo/physics/bullet/BulletHingeJoint.cc b/gazebo/physics/bullet/BulletHingeJoint.cc
index ec44959..a2e952b 100644
--- a/gazebo/physics/bullet/BulletHingeJoint.cc
+++ b/gazebo/physics/bullet/BulletHingeJoint.cc
@@ -105,7 +105,11 @@ void BulletHingeJoint::Init()
// If both links exist, then create a joint between the two links.
if (bulletChildLink && bulletParentLink)
{
+#ifdef LIBBULLET_VERSION_GT_282
+ this->bulletHinge = new btHingeAccumulatedAngleConstraint(
+#else
this->bulletHinge = new btHingeConstraint(
+#endif
*(bulletChildLink->GetBulletLink()),
*(bulletParentLink->GetBulletLink()),
BulletTypes::ConvertVector3(pivotChild),
@@ -117,7 +121,11 @@ void BulletHingeJoint::Init()
// and the world.
else if (bulletChildLink)
{
+#ifdef LIBBULLET_VERSION_GT_282
+ this->bulletHinge = new btHingeAccumulatedAngleConstraint(
+#else
this->bulletHinge = new btHingeConstraint(
+#endif
*(bulletChildLink->GetBulletLink()),
BulletTypes::ConvertVector3(pivotChild),
BulletTypes::ConvertVector3(axisChild));
@@ -126,7 +134,11 @@ void BulletHingeJoint::Init()
// and the world.
else if (bulletParentLink)
{
+#ifdef LIBBULLET_VERSION_GT_282
+ this->bulletHinge = new btHingeAccumulatedAngleConstraint(
+#else
this->bulletHinge = new btHingeConstraint(
+#endif
*(bulletParentLink->GetBulletLink()),
BulletTypes::ConvertVector3(pivotParent),
BulletTypes::ConvertVector3(axisParent));
@@ -149,7 +161,7 @@ void BulletHingeJoint::Init()
// Set angleOffset based on hinge angle at joint creation.
// GetAngleImpl will report angles relative to this offset.
- this->angleOffset = this->bulletHinge->getHingeAngle();
+ this->angleOffset = this->GetAngleImpl(0).Radian();
// Apply joint angle limits here.
// TODO: velocity and effort limits.
@@ -219,7 +231,21 @@ math::Angle BulletHingeJoint::GetAngleImpl(unsigned int /*_index*/) const
{
math::Angle result;
if (this->bulletHinge)
- result = this->bulletHinge->getHingeAngle() - this->angleOffset;
+ {
+#ifdef LIBBULLET_VERSION_GT_282
+ btHingeAccumulatedAngleConstraint* hinge =
+ static_cast<btHingeAccumulatedAngleConstraint*>(this->bulletHinge);
+ if (hinge)
+ {
+ result = hinge->getAccumulatedHingeAngle();
+ }
+ else
+#endif
+ {
+ result = this->bulletHinge->getHingeAngle();
+ }
+ result -= this->angleOffset;
+ }
return result;
}
@@ -394,6 +420,10 @@ bool BulletHingeJoint::SetParam(const std::string &_key,
return false;
}
}
+ else
+ {
+ return BulletJoint::SetParam(_key, _index, _value);
+ }
}
catch(const boost::bad_any_cast &e)
{
@@ -402,7 +432,7 @@ bool BulletHingeJoint::SetParam(const std::string &_key,
<< std::endl;
return false;
}
- return BulletJoint::SetParam(_key, _index, _value);
+ return true;
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/bullet/BulletHingeJoint.hh b/gazebo/physics/bullet/BulletHingeJoint.hh
index aff33c8..93f3fb4 100644
--- a/gazebo/physics/bullet/BulletHingeJoint.hh
+++ b/gazebo/physics/bullet/BulletHingeJoint.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \{
/// \brief A single axis hinge joint
- class GAZEBO_VISIBLE BulletHingeJoint : public HingeJoint<BulletJoint>
+ class GZ_PHYSICS_VISIBLE BulletHingeJoint : public HingeJoint<BulletJoint>
{
/// Constructor
public: BulletHingeJoint(btDynamicsWorld *world, BasePtr _parent);
diff --git a/gazebo/physics/bullet/BulletJoint.cc b/gazebo/physics/bullet/BulletJoint.cc
index 6ca150f..178afd2 100644
--- a/gazebo/physics/bullet/BulletJoint.cc
+++ b/gazebo/physics/bullet/BulletJoint.cc
@@ -21,6 +21,7 @@
#include <string>
+#include "gazebo/common/Assert.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Console.hh"
@@ -48,6 +49,9 @@ BulletJoint::BulletJoint(BasePtr _parent)
BulletJoint::~BulletJoint()
{
delete this->constraint;
+ this->constraint = NULL;
+ delete this->feedback;
+ this->feedback = NULL;
this->bulletWorld = NULL;
}
@@ -143,6 +147,15 @@ void BulletJoint::Detach()
if (this->constraint && this->bulletWorld)
this->bulletWorld->removeConstraint(this->constraint);
delete this->constraint;
+ this->constraint = NULL;
+}
+
+//////////////////////////////////////////////////
+void BulletJoint::SetProvideFeedback(bool _enable)
+{
+ Joint::SetProvideFeedback(_enable);
+
+ this->SetupJointFeedback();
}
//////////////////////////////////////////////////
@@ -194,6 +207,7 @@ void BulletJoint::CacheForceTorque()
else
{
/// \TODO: implement for other joint types
+ // note that for fixed joint no further modification is needed
// gzerr << "force torque for joint type [" << this->GetType()
// << "] not implemented, returns false results!!\n";
}
@@ -332,6 +346,7 @@ void BulletJoint::CacheForceTorque()
//////////////////////////////////////////////////
JointWrench BulletJoint::GetForceTorque(unsigned int /*_index*/)
{
+ GZ_ASSERT(this->constraint != NULL, "constraint should be valid");
return this->wrench;
}
@@ -340,19 +355,19 @@ void BulletJoint::SetupJointFeedback()
{
if (this->provideFeedback)
{
- this->feedback = new btJointFeedback;
- this->feedback->m_appliedForceBodyA = btVector3(0, 0, 0);
- this->feedback->m_appliedForceBodyB = btVector3(0, 0, 0);
- this->feedback->m_appliedTorqueBodyA = btVector3(0, 0, 0);
- this->feedback->m_appliedTorqueBodyB = btVector3(0, 0, 0);
+ if (this->feedback == NULL)
+ {
+ this->feedback = new btJointFeedback;
+ this->feedback->m_appliedForceBodyA = btVector3(0, 0, 0);
+ this->feedback->m_appliedForceBodyB = btVector3(0, 0, 0);
+ this->feedback->m_appliedTorqueBodyA = btVector3(0, 0, 0);
+ this->feedback->m_appliedTorqueBodyB = btVector3(0, 0, 0);
+ }
if (this->constraint)
this->constraint->setJointFeedback(this->feedback);
else
- {
gzerr << "Bullet Joint [" << this->GetName() << "] ID is invalid\n";
- getchar();
- }
}
}
@@ -538,19 +553,7 @@ bool BulletJoint::SetParam(const std::string &/*_key*/,
double BulletJoint::GetParam(const std::string &_key,
unsigned int _index)
{
- if (_key == "hi_stop")
- {
- return this->GetHighStop(_index).Radian();
- }
- else if (_key == "lo_stop")
- {
- return this->GetLowStop(_index).Radian();
- }
- gzerr << "GetParam unrecognized parameter ["
- << _key
- << "]"
- << std::endl;
- return 0;
+ return Joint::GetParam(_key, _index);
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/bullet/BulletJoint.hh b/gazebo/physics/bullet/BulletJoint.hh
index ce3c961..4d312ea 100644
--- a/gazebo/physics/bullet/BulletJoint.hh
+++ b/gazebo/physics/bullet/BulletJoint.hh
@@ -38,7 +38,7 @@ namespace gazebo
/// \{
/// \brief Base class for all joints
- class GAZEBO_VISIBLE BulletJoint : public Joint
+ class GZ_PHYSICS_VISIBLE BulletJoint : public Joint
{
/// \brief Constructor
public: BulletJoint(BasePtr _parent);
@@ -107,6 +107,9 @@ namespace gazebo
public: virtual math::Angle GetLowStop(unsigned int _index);
// Documentation inherited.
+ public: virtual void SetProvideFeedback(bool _enable);
+
+ // Documentation inherited.
public: virtual void CacheForceTorque();
// Documentation inherited.
diff --git a/gazebo/physics/bullet/BulletLink.cc b/gazebo/physics/bullet/BulletLink.cc
index 731f6f4..6cbfdb1 100644
--- a/gazebo/physics/bullet/BulletLink.cc
+++ b/gazebo/physics/bullet/BulletLink.cc
@@ -108,8 +108,13 @@ void BulletLink::Init()
collision = boost::static_pointer_cast<BulletCollision>(*iter);
btCollisionShape *shape = collision->GetCollisionShape();
- hackMu1 = collision->GetBulletSurface()->frictionPyramid.GetMuPrimary();
- hackMu2 = collision->GetBulletSurface()->frictionPyramid.GetMuSecondary();
+ SurfaceParamsPtr surface = collision->GetSurface();
+ GZ_ASSERT(surface, "Surface pointer for is invalid");
+ FrictionPyramidPtr friction = surface->GetFrictionPyramid();
+ GZ_ASSERT(friction, "Friction pointer is invalid");
+
+ hackMu1 = friction->GetMuPrimary();
+ hackMu2 = friction->GetMuSecondary();
// gzerr << "link[" << this->GetName()
// << "] mu[" << hackMu1
// << "] mu2[" << hackMu2 << "]\n";
@@ -147,6 +152,7 @@ void BulletLink::Init()
this->rigidLink->setUserPointer(this);
this->rigidLink->setCollisionFlags(this->rigidLink->getCollisionFlags() |
btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+ this->rigidLink->setFlags(BT_ENABLE_GYROPSCOPIC_FORCE);
/// \TODO: get friction from collision object
this->rigidLink->setAnisotropicFriction(btVector3(1, 1, 1),
@@ -576,6 +582,14 @@ void BulletLink::AddForceAtRelativePosition(const math::Vector3 &/*_force*/,
<< std::endl;
}
+//////////////////////////////////////////////////
+void BulletLink::AddLinkForce(const math::Vector3 &/*_force*/,
+ const math::Vector3 &/*_offset*/)
+{
+ gzlog << "BulletLink::AddLinkForce not yet implemented (#1476)."
+ << std::endl;
+}
+
/////////////////////////////////////////////////
void BulletLink::AddTorque(const math::Vector3 &/*_torque*/)
{
diff --git a/gazebo/physics/bullet/BulletLink.hh b/gazebo/physics/bullet/BulletLink.hh
index df68f7f..d9722bc 100644
--- a/gazebo/physics/bullet/BulletLink.hh
+++ b/gazebo/physics/bullet/BulletLink.hh
@@ -41,7 +41,7 @@ namespace gazebo
/// \{
/// \brief Bullet Link class
- class GAZEBO_VISIBLE BulletLink : public Link
+ class GZ_PHYSICS_VISIBLE BulletLink : public Link
{
/// \brief Constructor
public: BulletLink(EntityPtr _parent);
@@ -143,6 +143,10 @@ namespace gazebo
const math::Vector3 &_force,
const math::Vector3 &_relpos);
+ // Documentation inherited
+ public: virtual void AddLinkForce(const math::Vector3 &_force,
+ const math::Vector3 &_offset = math::Vector3::Zero);
+
// Documentation inherited.
public: virtual void AddTorque(const math::Vector3 &_torque);
diff --git a/gazebo/physics/bullet/BulletMesh.hh b/gazebo/physics/bullet/BulletMesh.hh
index 2eb9a92..3c46c05 100644
--- a/gazebo/physics/bullet/BulletMesh.hh
+++ b/gazebo/physics/bullet/BulletMesh.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \{
/// \brief Triangle mesh collision helper class
- class GAZEBO_VISIBLE BulletMesh
+ class GZ_PHYSICS_VISIBLE BulletMesh
{
/// \brief Constructor
public: BulletMesh();
diff --git a/gazebo/physics/bullet/BulletMeshShape.hh b/gazebo/physics/bullet/BulletMeshShape.hh
index df74237..480851d 100644
--- a/gazebo/physics/bullet/BulletMeshShape.hh
+++ b/gazebo/physics/bullet/BulletMeshShape.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \{
/// \brief Triangle mesh collision
- class GAZEBO_VISIBLE BulletMeshShape : public MeshShape
+ class GZ_PHYSICS_VISIBLE BulletMeshShape : public MeshShape
{
/// \brief Constructor
public: BulletMeshShape(CollisionPtr _parent);
diff --git a/gazebo/physics/bullet/BulletMotionState.hh b/gazebo/physics/bullet/BulletMotionState.hh
index 6489cbe..5b1ff5d 100644
--- a/gazebo/physics/bullet/BulletMotionState.hh
+++ b/gazebo/physics/bullet/BulletMotionState.hh
@@ -39,7 +39,7 @@ namespace gazebo
/// \{
/// \brief Bullet btMotionState encapsulation
- class GAZEBO_VISIBLE BulletMotionState : public btMotionState
+ class GZ_PHYSICS_VISIBLE BulletMotionState : public btMotionState
{
/// \brief Constructor
public: BulletMotionState(LinkPtr _link);
diff --git a/gazebo/physics/bullet/BulletMultiRayShape.hh b/gazebo/physics/bullet/BulletMultiRayShape.hh
index 60f465d..ed4251c 100644
--- a/gazebo/physics/bullet/BulletMultiRayShape.hh
+++ b/gazebo/physics/bullet/BulletMultiRayShape.hh
@@ -29,7 +29,7 @@ namespace gazebo
/// \{
/// \brief Bullet specific version of MultiRayShape
- class GAZEBO_VISIBLE BulletMultiRayShape : public MultiRayShape
+ class GZ_PHYSICS_VISIBLE BulletMultiRayShape : public MultiRayShape
{
/// \brief Constructor
public: BulletMultiRayShape(CollisionPtr parent);
diff --git a/gazebo/physics/bullet/BulletPhysics.cc b/gazebo/physics/bullet/BulletPhysics.cc
index 90f45c1..abf7f7f 100644
--- a/gazebo/physics/bullet/BulletPhysics.cc
+++ b/gazebo/physics/bullet/BulletPhysics.cc
@@ -38,6 +38,7 @@
#include "gazebo/physics/bullet/BulletSliderJoint.hh"
#include "gazebo/physics/bullet/BulletHinge2Joint.hh"
#include "gazebo/physics/bullet/BulletScrewJoint.hh"
+#include "gazebo/physics/bullet/BulletFixedJoint.hh"
#include "gazebo/transport/Publisher.hh"
@@ -399,7 +400,10 @@ void BulletPhysics::OnRequest(ConstRequestPtr &_msg)
physicsMsg.set_contact_surface_layer(
boost::any_cast<double>(this->GetParam("contact_surface_layer")));
- physicsMsg.mutable_gravity()->CopyFrom(msgs::Convert(this->GetGravity()));
+ physicsMsg.mutable_gravity()->CopyFrom(
+ msgs::Convert(this->GetGravity().Ign()));
+ physicsMsg.mutable_magnetic_field()->CopyFrom(
+ msgs::Convert(this->MagneticField()));
physicsMsg.set_real_time_update_rate(this->realTimeUpdateRate);
physicsMsg.set_real_time_factor(this->targetRealTimeFactor);
physicsMsg.set_max_step_size(this->maxStepSize);
@@ -413,6 +417,11 @@ void BulletPhysics::OnRequest(ConstRequestPtr &_msg)
/////////////////////////////////////////////////
void BulletPhysics::OnPhysicsMsg(ConstPhysicsPtr &_msg)
{
+ // Parent class handles many generic parameters
+ // This should be done first so that the profile settings
+ // can be over-ridden by other message parameters.
+ PhysicsEngine::OnPhysicsMsg(_msg);
+
if (_msg->has_min_step_size())
this->SetParam("min_step_size", _msg->min_step_size());
@@ -438,7 +447,7 @@ void BulletPhysics::OnPhysicsMsg(ConstPhysicsPtr &_msg)
this->SetParam("contact_surface_layer", _msg->contact_surface_layer());
if (_msg->has_gravity())
- this->SetGravity(msgs::Convert(_msg->gravity()));
+ this->SetGravity(msgs::ConvertIgn(_msg->gravity()));
if (_msg->has_real_time_factor())
this->SetTargetRealTimeFactor(_msg->real_time_factor());
@@ -455,9 +464,6 @@ void BulletPhysics::OnPhysicsMsg(ConstPhysicsPtr &_msg)
/// Make sure all models get at least one update cycle.
this->world->EnableAllModels();
-
- // Parent class handles many generic parameters
- PhysicsEngine::OnPhysicsMsg(_msg);
}
//////////////////////////////////////////////////
@@ -513,196 +519,86 @@ bool BulletPhysics::SetParam(const std::string &_key, const boost::any &_value)
btContactSolverInfo& info = this->dynamicsWorld->getSolverInfo();
- if (_key == "solver_type")
- {
- std::string value;
- try
- {
- value = boost::any_cast<std::string>(_value);
- }
- catch(const boost::bad_any_cast &e)
- {
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
- }
- if (value == "sequential_impulse")
- {
- bulletElem->GetElement("solver")->GetElement("type")->Set(value);
- this->solverType = value;
- }
- else
- {
- gzwarn << "Currently only 'sequential_impulse' solver is supported"
- << std::endl;
- return false;
- }
- }
- else if (_key == "cfm")
+ try
{
- double value;
- try
+ if (_key == "solver_type")
{
- value = boost::any_cast<double>(_value);
- }
- catch(const boost::bad_any_cast &e)
- {
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
- }
- bulletElem->GetElement("constraints")->GetElement("cfm")->Set(value);
- info.m_globalCfm = value;
- }
- else if (_key == "erp")
- {
- double value;
- try
- {
- value = boost::any_cast<double>(_value);
- }
- catch(const boost::bad_any_cast &e)
- {
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
- }
- bulletElem->GetElement("constraints")->GetElement("erp")->Set(value);
- info.m_erp = value;
- }
- else if (_key == "iters")
- {
- int value;
- try
- {
- try
+ std::string value = boost::any_cast<std::string>(_value);
+ if (value == "sequential_impulse")
{
- value = boost::any_cast<int>(_value);
+ bulletElem->GetElement("solver")->GetElement("type")->Set(value);
+ this->solverType = value;
}
- catch(const boost::bad_any_cast &e)
+ else
{
- value = boost::any_cast<unsigned int>(_value);
+ gzwarn << "Currently only 'sequential_impulse' solver is supported"
+ << std::endl;
+ return false;
}
}
- catch(const boost::bad_any_cast &e)
- {
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
- }
- bulletElem->GetElement("solver")->GetElement("iters")->Set(value);
- info.m_numIterations = value;
- }
- else if (_key == "sor")
- {
- double value;
- try
- {
- value = boost::any_cast<double>(_value);
- }
- catch(const boost::bad_any_cast &e)
+ else if (_key == "cfm")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ double value = boost::any_cast<double>(_value);
+ bulletElem->GetElement("constraints")->GetElement("cfm")->Set(value);
+ info.m_globalCfm = value;
}
- bulletElem->GetElement("solver")->GetElement("sor")->Set(value);
- info.m_sor = value;
- }
- else if (_key == "contact_surface_layer")
- {
- double value;
- try
+ else if (_key == "erp")
{
- value = boost::any_cast<double>(_value);
+ double value = boost::any_cast<double>(_value);
+ bulletElem->GetElement("constraints")->GetElement("erp")->Set(value);
+ info.m_erp = value;
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "iters")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ int value = boost::any_cast<int>(_value);
+ bulletElem->GetElement("solver")->GetElement("iters")->Set(value);
+ info.m_numIterations = value;
}
- bulletElem->GetElement("constraints")->GetElement(
- "contact_surface_layer")->Set(value);
- }
- else if (_key == "split_impulse")
- {
- bool value;
- try
+ else if (_key == "sor")
{
- value = boost::any_cast<bool>(_value);
+ double value = boost::any_cast<double>(_value);
+ bulletElem->GetElement("solver")->GetElement("sor")->Set(value);
+ info.m_sor = value;
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "contact_surface_layer")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ double value = boost::any_cast<double>(_value);
+ bulletElem->GetElement("constraints")->GetElement(
+ "contact_surface_layer")->Set(value);
}
- bulletElem->GetElement("constraints")->GetElement(
- "split_impulse")->Set(value);
- }
- else if (_key == "split_impulse_penetration_threshold")
- {
- double value;
- try
+ else if (_key == "split_impulse")
{
- value = boost::any_cast<double>(_value);
+ bool value = boost::any_cast<bool>(_value);
+ bulletElem->GetElement("constraints")->GetElement(
+ "split_impulse")->Set(value);
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "split_impulse_penetration_threshold")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ double value = boost::any_cast<double>(_value);
+ bulletElem->GetElement("constraints")->GetElement(
+ "split_impulse_penetration_threshold")->Set(value);
}
- bulletElem->GetElement("constraints")->GetElement(
- "split_impulse_penetration_threshold")->Set(value);
- }
- else if (_key == "max_contacts")
- {
- /// TODO: Implement max contacts param
- int value;
- try
+ else if (_key == "max_contacts")
{
- try
- {
- value = boost::any_cast<int>(_value);
- }
- catch(const boost::bad_any_cast &e)
- {
- value = boost::any_cast<unsigned int>(_value);
- }
+ /// TODO: Implement max contacts param
+ int value = boost::any_cast<int>(_value);
+ this->sdf->GetElement("max_contacts")->GetValue()->Set(value);
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "min_step_size")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ /// TODO: Implement min step size param
+ double value = boost::any_cast<double>(_value);
+ bulletElem->GetElement("solver")->GetElement("min_step_size")->Set(value);
}
- this->sdf->GetElement("max_contacts")->GetValue()->Set(value);
- }
- else if (_key == "min_step_size")
- {
- /// TODO: Implement min step size param
- double value;
- try
- {
- value = boost::any_cast<double>(_value);
- }
- catch(const boost::bad_any_cast &e)
- {
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
- }
- bulletElem->GetElement("solver")->GetElement("min_step_size")->Set(value);
- }
- else if (_key == "max_step_size")
- {
- double value;
- try
- {
- value = boost::any_cast<double>(_value);
- }
- catch(const boost::bad_any_cast &e)
+ else
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ return PhysicsEngine::SetParam(_key, _value);
}
- this->SetMaxStepSize(value);
}
- else
+ catch(boost::bad_any_cast &e)
{
- gzwarn << _key << " is not supported in bullet" << std::endl;
+ gzerr << "BulletPhysics::SetParam(" << _key << ") boost::any_cast error: "
+ << e.what() << std::endl;
return false;
}
@@ -712,43 +608,49 @@ bool BulletPhysics::SetParam(const std::string &_key, const boost::any &_value)
//////////////////////////////////////////////////
boost::any BulletPhysics::GetParam(const std::string &_key) const
{
+ boost::any value;
+ this->GetParam(_key, value);
+ return value;
+}
+
+//////////////////////////////////////////////////
+bool BulletPhysics::GetParam(const std::string &_key, boost::any &_value) const
+{
sdf::ElementPtr bulletElem = this->sdf->GetElement("bullet");
GZ_ASSERT(bulletElem != NULL, "Bullet SDF element does not exist");
if (_key == "solver_type")
- return bulletElem->GetElement("solver")->Get<std::string>("type");
+ _value = bulletElem->GetElement("solver")->Get<std::string>("type");
else if (_key == "cfm")
- return bulletElem->GetElement("constraints")->Get<double>("cfm");
+ _value = bulletElem->GetElement("constraints")->Get<double>("cfm");
else if (_key == "erp")
- return bulletElem->GetElement("constraints")->Get<double>("erp");
+ _value = bulletElem->GetElement("constraints")->Get<double>("erp");
else if (_key == "iters")
- return bulletElem->GetElement("solver")->Get<int>("iters");
+ _value = bulletElem->GetElement("solver")->Get<int>("iters");
else if (_key == "sor")
- return bulletElem->GetElement("solver")->Get<double>("sor");
+ _value = bulletElem->GetElement("solver")->Get<double>("sor");
else if (_key == "contact_surface_layer")
- return bulletElem->GetElement("constraints")->Get<double>(
+ _value = bulletElem->GetElement("constraints")->Get<double>(
"contact_surface_layer");
else if (_key == "split_impulse")
{
- return bulletElem->GetElement("constraints")->Get<bool>(
+ _value = bulletElem->GetElement("constraints")->Get<bool>(
"split_impulse");
}
else if (_key == "split_impulse_penetration_threshold")
{
- return bulletElem->GetElement("constraints")->Get<double>(
+ _value = bulletElem->GetElement("constraints")->Get<double>(
"split_impulse_penetration_threshold");
}
else if (_key == "max_contacts")
- return this->sdf->GetElement("max_contacts")->Get<int>();
+ _value = this->sdf->GetElement("max_contacts")->Get<int>();
else if (_key == "min_step_size")
- return bulletElem->GetElement("solver")->Get<double>("min_step_size");
- else if (_key == "max_step_size")
- return this->GetMaxStepSize();
+ _value = bulletElem->GetElement("solver")->Get<double>("min_step_size");
else
{
- gzwarn << _key << " is not supported in bullet" << std::endl;
- return 0;
+ return PhysicsEngine::GetParam(_key, _value);
}
+ return true;
}
//////////////////////////////////////////////////
@@ -830,6 +732,8 @@ JointPtr BulletPhysics::CreateJoint(const std::string &_type, ModelPtr _parent)
joint.reset(new BulletHinge2Joint(this->dynamicsWorld, _parent));
else if (_type == "screw")
joint.reset(new BulletScrewJoint(this->dynamicsWorld, _parent));
+ else if (_type == "fixed")
+ joint.reset(new BulletFixedJoint(this->dynamicsWorld, _parent));
else
gzthrow("Unable to create joint of type[" << _type << "]");
diff --git a/gazebo/physics/bullet/BulletPhysics.hh b/gazebo/physics/bullet/BulletPhysics.hh
index 5e02569..750eb16 100644
--- a/gazebo/physics/bullet/BulletPhysics.hh
+++ b/gazebo/physics/bullet/BulletPhysics.hh
@@ -45,7 +45,7 @@ namespace gazebo
/// \{
/// \brief Bullet physics engine
- class GAZEBO_VISIBLE BulletPhysics : public PhysicsEngine
+ class GZ_PHYSICS_VISIBLE BulletPhysics : public PhysicsEngine
{
/// \enum BulletParam
/// \brief Bullet physics parameter types.
@@ -164,6 +164,10 @@ namespace gazebo
/// Documentation inherited
public: virtual boost::any GetParam(const std::string &_key) const;
+ /// Documentation inherited
+ public: virtual bool GetParam(const std::string &_key,
+ boost::any &_value) const;
+
// Documentation inherited
public: virtual void SetSORPGSIters(unsigned int iters);
diff --git a/gazebo/physics/bullet/BulletPhysics_TEST.cc b/gazebo/physics/bullet/BulletPhysics_TEST.cc
index e36724f..e6b9c99 100644
--- a/gazebo/physics/bullet/BulletPhysics_TEST.cc
+++ b/gazebo/physics/bullet/BulletPhysics_TEST.cc
@@ -23,7 +23,7 @@
#include "gazebo/physics/bullet/BulletPhysics.hh"
#include "gazebo/physics/bullet/BulletTypes.hh"
#include "gazebo/msgs/msgs.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
using namespace physics;
diff --git a/gazebo/physics/bullet/BulletPlaneShape.hh b/gazebo/physics/bullet/BulletPlaneShape.hh
index 2dde63a..309c1b4 100644
--- a/gazebo/physics/bullet/BulletPlaneShape.hh
+++ b/gazebo/physics/bullet/BulletPlaneShape.hh
@@ -37,7 +37,7 @@ namespace gazebo
/// \{
/// \brief Bullet collision for an infinite plane.
- class GAZEBO_VISIBLE BulletPlaneShape : public PlaneShape
+ class GZ_PHYSICS_VISIBLE BulletPlaneShape : public PlaneShape
{
/// \brief Constructor
public: BulletPlaneShape(CollisionPtr _parent) : PlaneShape(_parent) {}
diff --git a/gazebo/physics/bullet/BulletPolylineShape.hh b/gazebo/physics/bullet/BulletPolylineShape.hh
index 4c2ad17..95d74d0 100644
--- a/gazebo/physics/bullet/BulletPolylineShape.hh
+++ b/gazebo/physics/bullet/BulletPolylineShape.hh
@@ -27,7 +27,7 @@ namespace gazebo
class BulletMesh;
/// \brief Bullet polyline shape
- class GAZEBO_VISIBLE BulletPolylineShape : public PolylineShape
+ class GZ_PHYSICS_VISIBLE BulletPolylineShape : public PolylineShape
{
/// \brief Constructor
/// \param[in] _parent Collision parent.
diff --git a/gazebo/physics/bullet/BulletRaySensor.hh b/gazebo/physics/bullet/BulletRaySensor.hh
index b84c857..24ddcde 100644
--- a/gazebo/physics/bullet/BulletRaySensor.hh
+++ b/gazebo/physics/bullet/BulletRaySensor.hh
@@ -37,7 +37,7 @@ namespace gazebo
/// \{
/// \brief An Bullet Ray sensor
- class GAZEBO_VISIBLE BulletRaySensor : public PhysicsRaySensor
+ class GZ_PHYSICS_VISIBLE BulletRaySensor : public PhysicsRaySensor
{
/// \brief Constructor
public: BulletRaySensor(Link *body);
diff --git a/gazebo/physics/bullet/BulletRayShape.hh b/gazebo/physics/bullet/BulletRayShape.hh
index d553f17..ada9952 100644
--- a/gazebo/physics/bullet/BulletRayShape.hh
+++ b/gazebo/physics/bullet/BulletRayShape.hh
@@ -35,7 +35,7 @@ namespace gazebo
/// \{
/// \brief Ray shape for bullet
- class GAZEBO_VISIBLE BulletRayShape : public RayShape
+ class GZ_PHYSICS_VISIBLE BulletRayShape : public RayShape
{
public: BulletRayShape(PhysicsEnginePtr _physicsEngine);
diff --git a/gazebo/physics/bullet/BulletScrewJoint.hh b/gazebo/physics/bullet/BulletScrewJoint.hh
index 849cd5d..d0adb0b 100644
--- a/gazebo/physics/bullet/BulletScrewJoint.hh
+++ b/gazebo/physics/bullet/BulletScrewJoint.hh
@@ -39,7 +39,7 @@ namespace gazebo
/// \{
/// \brief A screw joint
- class GAZEBO_VISIBLE BulletScrewJoint : public ScrewJoint<BulletJoint>
+ class GZ_PHYSICS_VISIBLE BulletScrewJoint : public ScrewJoint<BulletJoint>
{
/// \brief Constructor
/// \param[in] _world Pointer to the dynamics world.
diff --git a/gazebo/physics/bullet/BulletSliderJoint.cc b/gazebo/physics/bullet/BulletSliderJoint.cc
index 6348453..096d73b 100644
--- a/gazebo/physics/bullet/BulletSliderJoint.cc
+++ b/gazebo/physics/bullet/BulletSliderJoint.cc
@@ -19,6 +19,7 @@
#include "gazebo/common/Exception.hh"
#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/World.hh"
#include "gazebo/physics/bullet/bullet_inc.h"
#include "gazebo/physics/bullet/BulletLink.hh"
#include "gazebo/physics/bullet/BulletPhysics.hh"
@@ -270,7 +271,7 @@ bool BulletSliderJoint::SetHighStop(unsigned int /*_index*/,
}
else
{
- gzerr << "bulletSlider not yet created.\n";
+ gzlog << "bulletSlider not yet created.\n";
return false;
}
}
@@ -287,7 +288,7 @@ bool BulletSliderJoint::SetLowStop(unsigned int /*_index*/,
}
else
{
- gzerr << "bulletSlider not yet created.\n";
+ gzlog << "bulletSlider not yet created.\n";
return false;
}
}
@@ -299,7 +300,7 @@ math::Angle BulletSliderJoint::GetHighStop(unsigned int /*_index*/)
if (this->bulletSlider)
result = this->bulletSlider->getUpperLinLimit();
else
- gzerr << "Joint must be created before getting high stop\n";
+ gzlog << "Joint must be created before getting high stop\n";
return result;
}
@@ -310,7 +311,7 @@ math::Angle BulletSliderJoint::GetLowStop(unsigned int /*_index*/)
if (this->bulletSlider)
result = this->bulletSlider->getLowerLinLimit();
else
- gzerr << "Joint must be created before getting low stop\n";
+ gzlog << "Joint must be created before getting low stop\n";
return result;
}
@@ -361,7 +362,7 @@ math::Angle BulletSliderJoint::GetAngleImpl(unsigned int _index) const
// if (this->bulletSlider)
// result = this->bulletSlider->getLinearPos();
// else
- // gzwarn << "bulletSlider does not exist, returning default position\n";
+ // gzlog << "bulletSlider does not exist, returning default position\n";
// Compute slider angle from gazebo's cached poses instead
math::Vector3 offset = this->GetWorldPose().pos
@@ -390,8 +391,19 @@ bool BulletSliderJoint::SetParam(const std::string &_key,
{
this->bulletSlider->setPoweredLinMotor(true);
this->bulletSlider->setTargetLinMotorVelocity(0.0);
- this->bulletSlider->setMaxLinMotorForce(
- boost::any_cast<double>(_value));
+ double value = boost::any_cast<double>(_value);
+#ifndef LIBBULLET_VERSION_GT_282
+ // there is an upstream bug in bullet 2.82 and earlier
+ // the maxLinMotorForce parameter is inadvertently divided
+ // by timestep when attempting to compute an impulse in
+ // the bullet solver.
+ // https://github.com/bulletphysics/bullet3/pull/328
+ // As a workaround, multiply the desired friction
+ // parameter by dt^2 when setting
+ double dt = this->world->GetPhysicsEngine()->GetMaxStepSize();
+ value *= dt*dt;
+#endif
+ this->bulletSlider->setMaxLinMotorForce(value);
}
else
{
@@ -399,6 +411,10 @@ bool BulletSliderJoint::SetParam(const std::string &_key,
return false;
}
}
+ else
+ {
+ return BulletJoint::SetParam(_key, _index, _value);
+ }
}
catch(const boost::bad_any_cast &e)
{
@@ -407,7 +423,7 @@ bool BulletSliderJoint::SetParam(const std::string &_key,
<< std::endl;
return false;
}
- return BulletJoint::SetParam(_key, _index, _value);
+ return true;
}
//////////////////////////////////////////////////
@@ -423,7 +439,19 @@ double BulletSliderJoint::GetParam(const std::string &_key, unsigned int _index)
{
if (this->bulletSlider)
{
- return this->bulletSlider->getMaxLinMotorForce();
+ double value = this->bulletSlider->getMaxLinMotorForce();
+#ifndef LIBBULLET_VERSION_GT_282
+ // there is an upstream bug in bullet 2.82 and earlier
+ // the maxLinMotorForce parameter is inadvertently divided
+ // by timestep when attempting to compute an impulse in
+ // the bullet solver.
+ // https://github.com/bulletphysics/bullet3/pull/328
+ // As a workaround, divide the desired friction
+ // parameter by dt^2 when getting
+ double dt = this->world->GetPhysicsEngine()->GetMaxStepSize();
+ value /= dt*dt;
+#endif
+ return value;
}
else
{
diff --git a/gazebo/physics/bullet/BulletSliderJoint.hh b/gazebo/physics/bullet/BulletSliderJoint.hh
index bd07b7c..c96f6de 100644
--- a/gazebo/physics/bullet/BulletSliderJoint.hh
+++ b/gazebo/physics/bullet/BulletSliderJoint.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \{
/// \brief A slider joint
- class GAZEBO_VISIBLE BulletSliderJoint : public SliderJoint<BulletJoint>
+ class GZ_PHYSICS_VISIBLE BulletSliderJoint : public SliderJoint<BulletJoint>
{
/// \brief Constructor
public: BulletSliderJoint(btDynamicsWorld *world, BasePtr _parent);
diff --git a/gazebo/physics/bullet/BulletSphereShape.hh b/gazebo/physics/bullet/BulletSphereShape.hh
index 575aeb6..1742fe5 100644
--- a/gazebo/physics/bullet/BulletSphereShape.hh
+++ b/gazebo/physics/bullet/BulletSphereShape.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \{
/// \brief Bullet sphere collision
- class GAZEBO_VISIBLE BulletSphereShape : public SphereShape
+ class GZ_PHYSICS_VISIBLE BulletSphereShape : public SphereShape
{
/// \brief Constructor
public: BulletSphereShape(CollisionPtr _parent) : SphereShape(_parent) {}
diff --git a/gazebo/physics/bullet/BulletSurfaceParams.cc b/gazebo/physics/bullet/BulletSurfaceParams.cc
index 4f06981..7d617d5 100644
--- a/gazebo/physics/bullet/BulletSurfaceParams.cc
+++ b/gazebo/physics/bullet/BulletSurfaceParams.cc
@@ -15,7 +15,6 @@
*
*/
-#include <float.h>
#include "gazebo/common/Console.hh"
#include "gazebo/physics/bullet/BulletSurfaceParams.hh"
@@ -25,6 +24,7 @@ using namespace physics;
//////////////////////////////////////////////////
BulletSurfaceParams::BulletSurfaceParams()
: SurfaceParams()
+ , frictionPyramid(new FrictionPyramid())
{
}
@@ -56,9 +56,9 @@ void BulletSurfaceParams::Load(sdf::ElementPtr _sdf)
gzerr << "Surface friction ode sdf member is NULL" << std::endl;
else
{
- this->frictionPyramid.SetMuPrimary(
+ this->frictionPyramid->SetMuPrimary(
frictionOdeElem->Get<double>("mu"));
- this->frictionPyramid.SetMuSecondary(
+ this->frictionPyramid->SetMuSecondary(
frictionOdeElem->Get<double>("mu2"));
}
}
@@ -70,8 +70,8 @@ void BulletSurfaceParams::FillMsg(msgs::Surface &_msg)
{
SurfaceParams::FillMsg(_msg);
- _msg.mutable_friction()->set_mu(this->frictionPyramid.GetMuPrimary());
- _msg.mutable_friction()->set_mu2(this->frictionPyramid.GetMuSecondary());
+ _msg.mutable_friction()->set_mu(this->frictionPyramid->GetMuPrimary());
+ _msg.mutable_friction()->set_mu2(this->frictionPyramid->GetMuSecondary());
}
/////////////////////////////////////////////////
@@ -82,8 +82,14 @@ void BulletSurfaceParams::ProcessMsg(const msgs::Surface &_msg)
if (_msg.has_friction())
{
if (_msg.friction().has_mu())
- this->frictionPyramid.SetMuPrimary(_msg.friction().mu());
+ this->frictionPyramid->SetMuPrimary(_msg.friction().mu());
if (_msg.friction().has_mu2())
- this->frictionPyramid.SetMuSecondary(_msg.friction().mu2());
+ this->frictionPyramid->SetMuSecondary(_msg.friction().mu2());
}
}
+
+/////////////////////////////////////////////////
+FrictionPyramidPtr BulletSurfaceParams::GetFrictionPyramid() const
+{
+ return this->frictionPyramid;
+}
diff --git a/gazebo/physics/bullet/BulletSurfaceParams.hh b/gazebo/physics/bullet/BulletSurfaceParams.hh
index cbecd50..bdb0581 100644
--- a/gazebo/physics/bullet/BulletSurfaceParams.hh
+++ b/gazebo/physics/bullet/BulletSurfaceParams.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \{
/// \brief Bullet surface parameters.
- class GAZEBO_VISIBLE BulletSurfaceParams : public SurfaceParams
+ class GZ_PHYSICS_VISIBLE BulletSurfaceParams : public SurfaceParams
{
/// \brief Constructor.
public: BulletSurfaceParams();
@@ -51,11 +51,14 @@ namespace gazebo
// Documentation inherited.
public: virtual void ProcessMsg(const msgs::Surface &_msg);
+ // Documentation inherited.
+ public: virtual FrictionPyramidPtr GetFrictionPyramid() const;
+
/// \brief Friction pyramid parameters (mu1, mu2).
// For further documentation, see:
// github.com/erwincoumans/bullet3/blob/master/src/BulletDynamics
// /ConstraintSolver/btSequentialImpulseConstraintSolver.cpp#L910
- FrictionPyramid frictionPyramid;
+ private: FrictionPyramidPtr frictionPyramid;
};
/// \}
}
diff --git a/gazebo/physics/bullet/BulletTypes.hh b/gazebo/physics/bullet/BulletTypes.hh
index d920dc3..a964665 100644
--- a/gazebo/physics/bullet/BulletTypes.hh
+++ b/gazebo/physics/bullet/BulletTypes.hh
@@ -53,7 +53,7 @@ namespace gazebo
/// \class BulletTypes BulletTypes.hh
/// \brief A set of functions for converting between the math types used
/// by gazebo and bullet.
- class GAZEBO_VISIBLE BulletTypes {
+ class GZ_PHYSICS_VISIBLE BulletTypes {
/// \brief Convert a bullet btVector3 to a gazebo Vector3.
/// \param[in] _bt Bullet Vector3.
/// \return Gazebo Vector3.
diff --git a/gazebo/physics/bullet/BulletUniversalJoint.hh b/gazebo/physics/bullet/BulletUniversalJoint.hh
index ff1c348..0d4a8ab 100644
--- a/gazebo/physics/bullet/BulletUniversalJoint.hh
+++ b/gazebo/physics/bullet/BulletUniversalJoint.hh
@@ -35,7 +35,7 @@ namespace gazebo
/// \{
/// \brief A bullet universal joint class
- class GAZEBO_VISIBLE BulletUniversalJoint
+ class GZ_PHYSICS_VISIBLE BulletUniversalJoint
: public UniversalJoint<BulletJoint>
{
/// \brief Constructor
diff --git a/gazebo/physics/bullet/CMakeLists.txt b/gazebo/physics/bullet/CMakeLists.txt
index 1fb89f7..8a330c2 100644
--- a/gazebo/physics/bullet/CMakeLists.txt
+++ b/gazebo/physics/bullet/CMakeLists.txt
@@ -9,6 +9,7 @@ add_definitions(${BULLET_CFLAGS})
set (sources
BulletBallJoint.cc
BulletCollision.cc
+ BulletFixedJoint.cc
BulletHeightmapShape.cc
BulletHinge2Joint.cc
BulletHingeJoint.cc
@@ -35,6 +36,7 @@ set (headers
BulletBoxShape.hh
BulletCollision.hh
BulletCylinderShape.hh
+ BulletFixedJoint.hh
BulletHeightmapShape.hh
BulletHinge2Joint.hh
BulletHingeJoint.hh
diff --git a/gazebo/physics/dart/CMakeLists.txt b/gazebo/physics/dart/CMakeLists.txt
index 7bec653..abc7a37 100644
--- a/gazebo/physics/dart/CMakeLists.txt
+++ b/gazebo/physics/dart/CMakeLists.txt
@@ -6,21 +6,27 @@ link_directories(${DARTCore_LIBRARY_DIRS})
set (sources
DARTBallJoint.cc
+ DARTBoxShape.cc
DARTCollision.cc
- DARTLink.cc
+ DARTCylinderShape.cc
+ DARTFixedJoint.cc
DARTHeightmapShape.cc
DARTHingeJoint.cc
DARTHinge2Joint.cc
DARTJoint.cc
+ DARTLink.cc
DARTMesh.cc
DARTMeshShape.cc
DARTModel.cc
DARTMultiRayShape.cc
DARTPhysics.cc
+ DARTPlaneShape.cc
DARTPolylineShape.cc
DARTRayShape.cc
DARTScrewJoint.cc
DARTSliderJoint.cc
+ DARTSphereShape.cc
+ DARTSurfaceParams.cc
DARTUniversalJoint.cc
)
@@ -29,6 +35,7 @@ SET (headers
DARTBoxShape.hh
DARTCollision.hh
DARTCylinderShape.hh
+ DARTFixedJoint.hh
DARTHeightmapShape.hh
DARTHingeJoint.hh
DARTHinge2Joint.hh
@@ -45,6 +52,7 @@ SET (headers
DARTScrewJoint.hh
DARTSliderJoint.hh
DARTSphereShape.hh
+ DARTSurfaceParams.hh
DARTTypes.hh
DARTUniversalJoint.hh
dart_inc.h
diff --git a/gazebo/physics/dart/DARTBallJoint.cc b/gazebo/physics/dart/DARTBallJoint.cc
index 7aae10e..30b8476 100644
--- a/gazebo/physics/dart/DARTBallJoint.cc
+++ b/gazebo/physics/dart/DARTBallJoint.cc
@@ -18,6 +18,7 @@
#include "gazebo/gazebo_config.h"
#include "gazebo/common/Console.hh"
#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/dart/DARTJointPrivate.hh"
#include "gazebo/physics/dart/DARTBallJoint.hh"
using namespace gazebo;
@@ -25,16 +26,15 @@ using namespace physics;
//////////////////////////////////////////////////
DARTBallJoint::DARTBallJoint(BasePtr _parent)
- : BallJoint<DARTJoint>(_parent),
- dtBallJoint(new dart::dynamics::BallJoint())
+ : BallJoint<DARTJoint>(_parent)
{
- this->dtJoint = this->dtBallJoint;
+ this->dataPtr->dtJoint = new dart::dynamics::BallJoint();
}
//////////////////////////////////////////////////
DARTBallJoint::~DARTBallJoint()
{
- delete dtBallJoint;
+ delete this->dataPtr->dtJoint;
}
//////////////////////////////////////////////////
@@ -52,8 +52,8 @@ void DARTBallJoint::Init()
//////////////////////////////////////////////////
math::Vector3 DARTBallJoint::GetAnchor(unsigned int /*_index*/) const
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
Eigen::Vector3d worldOrigin = T.translation();
return DARTTypes::ConvVec3(worldOrigin);
diff --git a/gazebo/physics/dart/DARTBallJoint.hh b/gazebo/physics/dart/DARTBallJoint.hh
index 3d9bbfc..53b5046 100644
--- a/gazebo/physics/dart/DARTBallJoint.hh
+++ b/gazebo/physics/dart/DARTBallJoint.hh
@@ -27,7 +27,7 @@ namespace gazebo
namespace physics
{
/// \brief An DARTBallJoint
- class GAZEBO_VISIBLE DARTBallJoint : public BallJoint<DARTJoint>
+ class GZ_PHYSICS_VISIBLE DARTBallJoint : public BallJoint<DARTJoint>
{
/// \brief Constructor
/// \param[in] _parent Parent of the Joint
@@ -83,9 +83,6 @@ namespace gazebo
// Documentation inherited.
public: virtual bool SetLowStop(unsigned int _index,
const math::Angle &_angle);
-
- /// \brief
- protected: dart::dynamics::BallJoint *dtBallJoint;
};
}
}
diff --git a/gazebo/physics/dart/DARTBoxShape.cc b/gazebo/physics/dart/DARTBoxShape.cc
new file mode 100644
index 0000000..ee1222f
--- /dev/null
+++ b/gazebo/physics/dart/DARTBoxShape.cc
@@ -0,0 +1,96 @@
+/*
+ * 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 "gazebo/common/Console.hh"
+#include "gazebo/physics/dart/DARTPhysics.hh"
+#include "gazebo/physics/dart/DARTCollision.hh"
+#include "gazebo/physics/dart/DARTBoxShape.hh"
+#include "gazebo/util/system.hh"
+
+#include "gazebo/physics/dart/DARTBoxShapePrivate.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+DARTBoxShape::DARTBoxShape(DARTCollisionPtr _parent)
+ : BoxShape(_parent),
+ dataPtr(new DARTBoxShapePrivate())
+{
+}
+
+//////////////////////////////////////////////////
+DARTBoxShape::~DARTBoxShape()
+{
+ delete this->dataPtr;
+}
+
+//////////////////////////////////////////////////
+void DARTBoxShape::SetSize(const math::Vector3 &_size)
+{
+ if (_size.x < 0 || _size.y < 0 || _size.z < 0)
+ {
+ gzerr << "Box shape does not support negative size\n";
+ return;
+ }
+ math::Vector3 size = _size;
+ if (math::equal(size.x, 0.0))
+ {
+ // Warn user, but still create shape with very small value
+ // otherwise later resize operations using setLocalScaling
+ // will not be possible
+ gzwarn << "Setting box shape's x to zero is not supported in DART, "
+ << "using 1e-4.\n";
+ size.x = 1e-4;
+ }
+
+ if (math::equal(size.y, 0.0))
+ {
+ gzwarn << "Setting box shape's y to zero is not supported in DART, "
+ << "using 1e-4.\n";
+ size.y = 1e-4;
+ }
+
+ if (math::equal(size.z, 0.0))
+ {
+ gzwarn << "Setting box shape's z to zero is not supported in DART "
+ << "using 1e-4.\n";
+ size.z = 1e-4;
+ }
+
+ BoxShape::SetSize(size);
+
+ DARTCollisionPtr dartCollisionParent =
+ boost::dynamic_pointer_cast<DARTCollision>(this->collisionParent);
+
+ if (dartCollisionParent->GetDARTCollisionShape() == NULL)
+ {
+ dart::dynamics::BodyNode *dtBodyNode =
+ dartCollisionParent->GetDARTBodyNode();
+ dart::dynamics::BoxShape *dtBoxShape =
+ new dart::dynamics::BoxShape(DARTTypes::ConvVec3(size));
+ dtBodyNode->addCollisionShape(dtBoxShape);
+ dartCollisionParent->SetDARTCollisionShape(dtBoxShape);
+ }
+ else
+ {
+ dart::dynamics::BoxShape *dtBoxShape =
+ dynamic_cast<dart::dynamics::BoxShape*>(
+ dartCollisionParent->GetDARTCollisionShape());
+ dtBoxShape->setSize(DARTTypes::ConvVec3(size));
+ }
+}
diff --git a/gazebo/physics/dart/DARTBoxShape.hh b/gazebo/physics/dart/DARTBoxShape.hh
index 8e604c0..afe4dd8 100644
--- a/gazebo/physics/dart/DARTBoxShape.hh
+++ b/gazebo/physics/dart/DARTBoxShape.hh
@@ -17,88 +17,34 @@
#ifndef _GAZEBO_DARTBOXSHAPE_HH_
#define _GAZEBO_DARTBOXSHAPE_HH_
-#include "gazebo/common/Console.hh"
-
#include "gazebo/math/Vector3.hh"
-
-#include "gazebo/physics/dart/DARTPhysics.hh"
-#include "gazebo/physics/dart/DARTTypes.hh"
-#include "gazebo/physics/dart/DARTCollision.hh"
-
-#include "gazebo/physics/PhysicsTypes.hh"
#include "gazebo/physics/BoxShape.hh"
+#include "gazebo/physics/dart/DARTTypes.hh"
#include "gazebo/util/system.hh"
namespace gazebo
{
namespace physics
{
+ /// Forward declare private data class
+ class DARTBoxShapePrivate;
+
/// \brief DART Box shape
- class GAZEBO_VISIBLE DARTBoxShape : public BoxShape
+ class GZ_PHYSICS_VISIBLE DARTBoxShape : public BoxShape
{
/// \brief Constructor.
/// \param[in] _parent Parent Collision.
- public: explicit DARTBoxShape(DARTCollisionPtr _parent)
- : BoxShape(_parent) {}
+ public: explicit DARTBoxShape(DARTCollisionPtr _parent);
/// \brief Destructor.
- public: virtual ~DARTBoxShape() {}
+ public: virtual ~DARTBoxShape();
// Documentation inherited.
- public: virtual void SetSize(const math::Vector3 &_size)
- {
- if (_size.x < 0 || _size.y < 0 || _size.z < 0)
- {
- gzerr << "Box shape does not support negative size\n";
- return;
- }
- math::Vector3 size = _size;
- if (math::equal(size.x, 0.0))
- {
- // Warn user, but still create shape with very small value
- // otherwise later resize operations using setLocalScaling
- // will not be possible
- gzwarn << "Setting box shape's x to zero is not supported in DART, "
- << "using 1e-4.\n";
- size.x = 1e-4;
- }
-
- if (math::equal(size.y, 0.0))
- {
- gzwarn << "Setting box shape's y to zero is not supported in DART, "
- << "using 1e-4.\n";
- size.y = 1e-4;
- }
-
- if (math::equal(size.z, 0.0))
- {
- gzwarn << "Setting box shape's z to zero is not supported in DART "
- << "using 1e-4.\n";
- size.z = 1e-4;
- }
-
- BoxShape::SetSize(size);
-
- DARTCollisionPtr dartCollisionParent =
- boost::dynamic_pointer_cast<DARTCollision>(this->collisionParent);
+ public: virtual void SetSize(const math::Vector3 &_size);
- if (dartCollisionParent->GetDARTCollisionShape() == NULL)
- {
- dart::dynamics::BodyNode *dtBodyNode =
- dartCollisionParent->GetDARTBodyNode();
- dart::dynamics::BoxShape *dtBoxShape =
- new dart::dynamics::BoxShape(DARTTypes::ConvVec3(size));
- dtBodyNode->addCollisionShape(dtBoxShape);
- dartCollisionParent->SetDARTCollisionShape(dtBoxShape);
- }
- else
- {
- dart::dynamics::BoxShape *dtBoxShape =
- dynamic_cast<dart::dynamics::BoxShape*>(
- dartCollisionParent->GetDARTCollisionShape());
- dtBoxShape->setSize(DARTTypes::ConvVec3(size));
- }
- }
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTBoxShapePrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/dart/DARTBoxShapePrivate.hh b/gazebo/physics/dart/DARTBoxShapePrivate.hh
new file mode 100644
index 0000000..052687a
--- /dev/null
+++ b/gazebo/physics/dart/DARTBoxShapePrivate.hh
@@ -0,0 +1,40 @@
+/*
+ * 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_DARTBOXSHAPE_PRIVATE_HH_
+#define _GAZEBO_DARTBOXSHAPE_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTBoxShape
+ class DARTBoxShapePrivate
+ {
+ /// \brief Constructor
+ public: DARTBoxShapePrivate() = default;
+
+ /// \brief Default destructor
+ public: ~DARTBoxShapePrivate() = default;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTCollision.cc b/gazebo/physics/dart/DARTCollision.cc
index 3bdf2f7..203ef8e 100644
--- a/gazebo/physics/dart/DARTCollision.cc
+++ b/gazebo/physics/dart/DARTCollision.cc
@@ -26,6 +26,9 @@
#include "gazebo/physics/dart/DARTLink.hh"
#include "gazebo/physics/dart/DARTCollision.hh"
#include "gazebo/physics/dart/DARTPlaneShape.hh"
+#include "gazebo/physics/dart/DARTSurfaceParams.hh"
+
+#include "gazebo/physics/dart/DARTCollisionPrivate.hh"
using namespace gazebo;
using namespace physics;
@@ -33,17 +36,18 @@ using namespace physics;
//////////////////////////////////////////////////
DARTCollision::DARTCollision(LinkPtr _link)
: Collision(_link),
- dtCollisionShape(NULL)
+ dataPtr(new DARTCollisionPrivate(
+ boost::static_pointer_cast<DARTLink>(this->link)->GetDARTBodyNode()))
+
{
this->SetName("DART_Collision");
- this->surface.reset(new SurfaceParams());
- this->dtBodyNode
- = boost::static_pointer_cast<DARTLink>(this->link)->GetDARTBodyNode();
+ this->surface.reset(new DARTSurfaceParams());
}
//////////////////////////////////////////////////
DARTCollision::~DARTCollision()
{
+ delete this->dataPtr;
}
//////////////////////////////////////////////////
@@ -64,7 +68,7 @@ void DARTCollision::Init()
Collision::Init();
// Offset
- if (this->dtCollisionShape)
+ if (this->dataPtr->dtCollisionShape)
{
// TODO: Don't change offset of shape until dart supports plane shape.
boost::shared_ptr<DARTPlaneShape> planeShape =
@@ -73,13 +77,15 @@ void DARTCollision::Init()
if (!planeShape)
{
math::Pose relativePose = this->GetRelativePose();
- this->dtCollisionShape->setOffset(DARTTypes::ConvVec3(relativePose.pos));
+ this->dataPtr->dtCollisionShape->setOffset(
+ DARTTypes::ConvVec3(relativePose.pos));
}
else
{
// change ground plane to be near semi-infinite.
dart::dynamics::BoxShape *dtBoxShape =
- dynamic_cast<dart::dynamics::BoxShape *>(this->dtCollisionShape);
+ dynamic_cast<dart::dynamics::BoxShape *>(
+ this->dataPtr->dtCollisionShape);
dtBoxShape->setSize(Eigen::Vector3d(2100, 2100, 2100.0));
dtBoxShape->setOffset(Eigen::Vector3d(0.0, 0.0, -2100.0/2.0));
// gzerr << "plane box modified\n";
@@ -102,25 +108,25 @@ void DARTCollision::OnPoseChange()
//////////////////////////////////////////////////
void DARTCollision::SetCategoryBits(unsigned int _bits)
{
- this->categoryBits = _bits;
+ this->dataPtr->categoryBits = _bits;
}
//////////////////////////////////////////////////
void DARTCollision::SetCollideBits(unsigned int _bits)
{
- this->collideBits = _bits;
+ this->dataPtr->collideBits = _bits;
}
//////////////////////////////////////////////////
unsigned int DARTCollision::GetCategoryBits() const
{
- return this->categoryBits;
+ return this->dataPtr->categoryBits;
}
//////////////////////////////////////////////////
unsigned int DARTCollision::GetCollideBits() const
{
- return this->collideBits;
+ return this->dataPtr->collideBits;
}
//////////////////////////////////////////////////
@@ -136,7 +142,7 @@ gazebo::math::Box DARTCollision::GetBoundingBox() const
//////////////////////////////////////////////////
dart::dynamics::BodyNode *DARTCollision::GetDARTBodyNode() const
{
- return dtBodyNode;
+ return this->dataPtr->dtBodyNode;
}
//////////////////////////////////////////////////
@@ -144,11 +150,17 @@ void DARTCollision::SetDARTCollisionShape(dart::dynamics::Shape *_shape,
bool _placeable)
{
Collision::SetCollision(_placeable);
- this->dtCollisionShape = _shape;
+ this->dataPtr->dtCollisionShape = _shape;
}
//////////////////////////////////////////////////
dart::dynamics::Shape *DARTCollision::GetDARTCollisionShape() const
{
- return dtCollisionShape;
+ return this->dataPtr->dtCollisionShape;
+}
+
+/////////////////////////////////////////////////
+DARTSurfaceParamsPtr DARTCollision::GetDARTSurface() const
+{
+ return boost::dynamic_pointer_cast<DARTSurfaceParams>(this->surface);
}
diff --git a/gazebo/physics/dart/DARTCollision.hh b/gazebo/physics/dart/DARTCollision.hh
index 486fd5e..876527d 100644
--- a/gazebo/physics/dart/DARTCollision.hh
+++ b/gazebo/physics/dart/DARTCollision.hh
@@ -19,19 +19,21 @@
#define _GAZEBO_DARTCOLLISION_HH_
#include "gazebo/common/CommonTypes.hh"
-
#include "gazebo/physics/PhysicsTypes.hh"
#include "gazebo/physics/Collision.hh"
-
#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
#include "gazebo/util/system.hh"
namespace gazebo
{
namespace physics
{
+ /// Forward declare private data class
+ class DARTCollisionPrivate;
+
/// \brief Base class for all DART collisions.
- class GAZEBO_VISIBLE DARTCollision : public Collision
+ class GZ_PHYSICS_VISIBLE DARTCollision : public Collision
{
/// \brief Constructor.
/// \param[in] _link Parent Link
@@ -80,19 +82,16 @@ namespace gazebo
bool _placeable = true);
/// \brief Get DART collision shape.
- public: dart::dynamics::Shape* GetDARTCollisionShape() const;
-
- /// \brief DART body node associated with this collision.
- private: dart::dynamics::BodyNode *dtBodyNode;
-
- /// \brief DART collision shape associated with this collision.
- private: dart::dynamics::Shape *dtCollisionShape;
+ public: dart::dynamics::Shape *GetDARTCollisionShape() const;
- /// \brief Category bits for collision detection
- private: unsigned int categoryBits;
+ /// \brief Similar to Collision::GetSurface, but provides dynamically
+ /// casted pointer to DARTSurfaceParams.
+ /// \return Dynamically casted pointer to DARTSurfaceParams.
+ public: DARTSurfaceParamsPtr GetDARTSurface() const;
- /// \brief Collide bits for collision detection
- private: unsigned int collideBits;
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTCollisionPrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/dart/DARTCollisionPrivate.hh b/gazebo/physics/dart/DARTCollisionPrivate.hh
new file mode 100644
index 0000000..7f3f609
--- /dev/null
+++ b/gazebo/physics/dart/DARTCollisionPrivate.hh
@@ -0,0 +1,57 @@
+/*
+ * 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_DARTCOLLISION_PRIVATE_HH_
+#define _GAZEBO_DARTCOLLISION_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTCollision
+ class DARTCollisionPrivate
+ {
+ /// \brief Constructor
+ public: DARTCollisionPrivate(dart::dynamics::BodyNode *_dtBodyNode)
+ : dtBodyNode(_dtBodyNode),
+ dtCollisionShape(NULL),
+ categoryBits(0),
+ collideBits(0)
+ {
+ }
+
+ /// \brief Default destructor
+ public: ~DARTCollisionPrivate() = default;
+
+ /// \brief DART body node associated with this collision.
+ public: dart::dynamics::BodyNode *dtBodyNode;
+
+ /// \brief DART collision shape associated with this collision.
+ public: dart::dynamics::Shape *dtCollisionShape;
+
+ /// \brief Category bits for collision detection
+ public: unsigned int categoryBits;
+
+ /// \brief Collide bits for collision detection
+ public: unsigned int collideBits;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTCylinderShape.cc b/gazebo/physics/dart/DARTCylinderShape.cc
new file mode 100644
index 0000000..02e37f9
--- /dev/null
+++ b/gazebo/physics/dart/DARTCylinderShape.cc
@@ -0,0 +1,96 @@
+/*
+ * 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 "gazebo/common/Console.hh"
+#include "gazebo/physics/dart/DARTPhysics.hh"
+#include "gazebo/physics/dart/DARTCollision.hh"
+#include "gazebo/physics/dart/DARTCylinderShape.hh"
+#include "gazebo/util/system.hh"
+
+#include "gazebo/physics/dart/DARTCylinderShapePrivate.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+DARTCylinderShape::DARTCylinderShape(CollisionPtr _parent)
+ : CylinderShape(_parent),
+ dataPtr(new DARTCylinderShapePrivate())
+{
+}
+
+//////////////////////////////////////////////////
+DARTCylinderShape::~DARTCylinderShape()
+{
+ delete this->dataPtr;
+}
+
+//////////////////////////////////////////////////
+void DARTCylinderShape::SetSize(double _radius, double _length)
+{
+ if (_radius < 0)
+ {
+ gzerr << "Cylinder shape does not support negative radius\n";
+ return;
+ }
+
+ if (_length < 0)
+ {
+ gzerr << "Cylinder shape does not support negative length\n";
+ return;
+ }
+
+ if (math::equal(_radius, 0.0))
+ {
+ // Warn user, but still create shape with very small value
+ // otherwise later resize operations using setLocalScaling
+ // will not be possible
+ gzwarn << "Setting cylinder shape's radius to zero not supported "
+ << "in DART, using 1e-4.\n";
+ _radius = 1e-4;
+ }
+
+ if (math::equal(_length, 0.0))
+ {
+ gzwarn << "Setting cylinder shape's length to zero not supported "
+ << "in DART, using 1e-4.\n";
+ _length = 1e-4;
+ }
+
+ CylinderShape::SetSize(_radius, _length);
+
+ DARTCollisionPtr dartCollisionParent =
+ boost::dynamic_pointer_cast<DARTCollision>(this->collisionParent);
+
+ if (dartCollisionParent->GetDARTCollisionShape() == NULL)
+ {
+ dart::dynamics::BodyNode *dtBodyNode =
+ dartCollisionParent->GetDARTBodyNode();
+ dart::dynamics::CylinderShape *dtCylinderShape =
+ new dart::dynamics::CylinderShape(_radius, _length);
+ dtBodyNode->addCollisionShape(dtCylinderShape);
+ dartCollisionParent->SetDARTCollisionShape(dtCylinderShape);
+ }
+ else
+ {
+ dart::dynamics::CylinderShape *dtCylinderShape =
+ dynamic_cast<dart::dynamics::CylinderShape*>(
+ dartCollisionParent->GetDARTCollisionShape());
+ dtCylinderShape->setRadius(_radius);
+ dtCylinderShape->setHeight(_length);
+ }
+}
diff --git a/gazebo/physics/dart/DARTCylinderShape.hh b/gazebo/physics/dart/DARTCylinderShape.hh
index df7c2f8..65254bc 100644
--- a/gazebo/physics/dart/DARTCylinderShape.hh
+++ b/gazebo/physics/dart/DARTCylinderShape.hh
@@ -18,8 +18,6 @@
#ifndef _GAZEBO_DARTCYLINDERSHAPE_HH_
#define _GAZEBO_DARTCYLINDERSHAPE_HH_
-#include "gazebo/common/Console.hh"
-
#include "gazebo/physics/CylinderShape.hh"
#include "gazebo/physics/dart/DARTPhysics.hh"
#include "gazebo/util/system.hh"
@@ -28,72 +26,25 @@ namespace gazebo
{
namespace physics
{
+ /// Forward declare private data class
+ class DARTCylinderShapePrivate;
+
/// \brief DART cylinder shape
- class GAZEBO_VISIBLE DARTCylinderShape : public CylinderShape
+ class GZ_PHYSICS_VISIBLE DARTCylinderShape : public CylinderShape
{
/// \brief Constructor
/// \param[in] _parent Collision parent.
- public: explicit DARTCylinderShape(CollisionPtr _parent)
- : CylinderShape(_parent) {}
+ public: explicit DARTCylinderShape(CollisionPtr _parent);
/// \brief Destructor.
- public: virtual ~DARTCylinderShape() {}
+ public: virtual ~DARTCylinderShape();
// Documentation inerited.
- public: void SetSize(double _radius, double _length)
- {
- if (_radius < 0)
- {
- gzerr << "Cylinder shape does not support negative radius\n";
- return;
- }
-
- if (_length < 0)
- {
- gzerr << "Cylinder shape does not support negative length\n";
- return;
- }
-
- if (math::equal(_radius, 0.0))
- {
- // Warn user, but still create shape with very small value
- // otherwise later resize operations using setLocalScaling
- // will not be possible
- gzwarn << "Setting cylinder shape's radius to zero not supported "
- << "in DART, using 1e-4.\n";
- _radius = 1e-4;
- }
-
- if (math::equal(_length, 0.0))
- {
- gzwarn << "Setting cylinder shape's length to zero not supported "
- << "in DART, using 1e-4.\n";
- _length = 1e-4;
- }
-
- CylinderShape::SetSize(_radius, _length);
-
- DARTCollisionPtr dartCollisionParent =
- boost::dynamic_pointer_cast<DARTCollision>(this->collisionParent);
+ public: void SetSize(double _radius, double _length);
- if (dartCollisionParent->GetDARTCollisionShape() == NULL)
- {
- dart::dynamics::BodyNode *dtBodyNode =
- dartCollisionParent->GetDARTBodyNode();
- dart::dynamics::CylinderShape *dtCylinderShape =
- new dart::dynamics::CylinderShape(_radius, _length);
- dtBodyNode->addCollisionShape(dtCylinderShape);
- dartCollisionParent->SetDARTCollisionShape(dtCylinderShape);
- }
- else
- {
- dart::dynamics::CylinderShape *dtCylinderShape =
- dynamic_cast<dart::dynamics::CylinderShape*>(
- dartCollisionParent->GetDARTCollisionShape());
- dtCylinderShape->setRadius(_radius);
- dtCylinderShape->setHeight(_length);
- }
- }
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTCylinderShapePrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/dart/DARTCylinderShapePrivate.hh b/gazebo/physics/dart/DARTCylinderShapePrivate.hh
new file mode 100644
index 0000000..a579c64
--- /dev/null
+++ b/gazebo/physics/dart/DARTCylinderShapePrivate.hh
@@ -0,0 +1,40 @@
+/*
+ * 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_DARTCYLINDERSHAPE_PRIVATE_HH_
+#define _GAZEBO_DARTCYLINDERSHAPE_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTCylinderShape
+ class DARTCylinderShapePrivate
+ {
+ /// \brief Constructor
+ public: DARTCylinderShapePrivate() = default;
+
+ /// \brief Default destructor
+ public: ~DARTCylinderShapePrivate() = default;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTFixedJoint.cc b/gazebo/physics/dart/DARTFixedJoint.cc
new file mode 100644
index 0000000..4556dd8
--- /dev/null
+++ b/gazebo/physics/dart/DARTFixedJoint.cc
@@ -0,0 +1,126 @@
+/*
+ * 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 <boost/bind.hpp>
+
+#include "gazebo/gazebo_config.h"
+#include "gazebo/common/Console.hh"
+#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/dart/DARTJointPrivate.hh"
+#include "gazebo/physics/dart/DARTFixedJoint.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+DARTFixedJoint::DARTFixedJoint(BasePtr _parent)
+ : FixedJoint<DARTJoint>(_parent)
+{
+ this->dataPtr->dtJoint = new dart::dynamics::WeldJoint();
+}
+
+//////////////////////////////////////////////////
+DARTFixedJoint::~DARTFixedJoint()
+{
+ delete this->dataPtr->dtJoint;
+}
+
+//////////////////////////////////////////////////
+void DARTFixedJoint::Load(sdf::ElementPtr _sdf)
+{
+ FixedJoint<DARTJoint>::Load(_sdf);
+}
+
+//////////////////////////////////////////////////
+void DARTFixedJoint::Init()
+{
+ FixedJoint<DARTJoint>::Init();
+}
+
+//////////////////////////////////////////////////
+math::Vector3 DARTFixedJoint::GetAnchor(unsigned int /*index*/) const
+{
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Vector3d worldOrigin = T.translation();
+
+ return DARTTypes::ConvVec3(worldOrigin);
+}
+
+//////////////////////////////////////////////////
+math::Vector3 DARTFixedJoint::GetGlobalAxis(unsigned int /*_index*/) const
+{
+ gzwarn << "DARTFixedJoint: called method "
+ << "GetGlobalAxis that is not valid for joints of type fixed.\n";
+
+ return math::Vector3();
+}
+
+//////////////////////////////////////////////////
+void DARTFixedJoint::SetAxis(unsigned int /*_index*/,
+ const math::Vector3& /*_axis*/)
+{
+ gzwarn << "DARTFixedJoint: called method "
+ << "SetAxis that is not valid for joints of type fixed.\n";
+ return;
+}
+
+//////////////////////////////////////////////////
+math::Angle DARTFixedJoint::GetAngleImpl(unsigned int /*_index*/) const
+{
+ gzwarn << "DARTFixedJoint: called method "
+ << "GetAngleImpl that is not valid for joints of type fixed.\n";
+ return math::Angle();
+}
+
+//////////////////////////////////////////////////
+void DARTFixedJoint::SetVelocity(unsigned int /*_index*/, double /*_vel*/)
+{
+ gzwarn << "DARTFixedJoint: called method "
+ << "SetVelocity that is not valid for joints of type fixed.\n";
+ return;
+}
+
+//////////////////////////////////////////////////
+double DARTFixedJoint::GetVelocity(unsigned int /*_index*/) const
+{
+ gzwarn << "DARTFixedJoint: called method "
+ << "GetVelocity that is not valid for joints of type fixed.\n";
+ return 0.0;
+}
+
+//////////////////////////////////////////////////
+void DARTFixedJoint::SetMaxForce(unsigned int /*_index*/, double /*_force*/)
+{
+ gzwarn << "DARTFixedJoint: called method "
+ << "SetMaxForce that is not valid for joints of type fixed.\n";
+}
+
+//////////////////////////////////////////////////
+double DARTFixedJoint::GetMaxForce(unsigned int /*_index*/)
+{
+ gzwarn << "DARTFixedJoint: called method "
+ << "GetMaxForce that is not valid for joints of type fixed.\n";
+ return 0.0;
+}
+
+//////////////////////////////////////////////////
+void DARTFixedJoint::SetForceImpl(unsigned int /*_index*/, double /*_effort*/)
+{
+ gzwarn << "DARTFixedJoint: called method "
+ << "SetForceImpl that is not valid for joints of type fixed.\n";
+}
diff --git a/gazebo/physics/dart/DARTFixedJoint.hh b/gazebo/physics/dart/DARTFixedJoint.hh
new file mode 100644
index 0000000..b728a04
--- /dev/null
+++ b/gazebo/physics/dart/DARTFixedJoint.hh
@@ -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.
+ *
+*/
+
+#ifndef _GAZEBO_DARTFIXEDJOINT_HH_
+#define _GAZEBO_DARTFIXEDJOINT_HH_
+
+#include "gazebo/math/Angle.hh"
+#include "gazebo/math/Vector3.hh"
+
+#include "gazebo/physics/FixedJoint.hh"
+#include "gazebo/physics/dart/DARTJoint.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \brief A single axis hinge joint.
+ class GZ_PHYSICS_VISIBLE DARTFixedJoint : public FixedJoint<DARTJoint>
+ {
+ /// \brief Constructor
+ /// \param[in] _parent Parent of the Joint
+ public: DARTFixedJoint(BasePtr _parent);
+
+ /// \brief Destructor
+ public: virtual ~DARTFixedJoint();
+
+ // Documentation inherited.
+ public: virtual void Load(sdf::ElementPtr _sdf);
+
+ // Documentation inherited.
+ public: virtual void Init();
+
+ // Documentation inherited
+ public: virtual math::Vector3 GetAnchor(unsigned int _index) const;
+
+ // Documentation inherited
+ public: virtual math::Vector3 GetGlobalAxis(unsigned int _index) const;
+
+ // Documentation inherited
+ public: virtual void SetAxis(unsigned int _index,
+ const math::Vector3 &_axis);
+
+ // Documentation inherited
+ public: virtual math::Angle GetAngleImpl(unsigned int _index) const;
+
+ // Documentation inherited
+ public: virtual void SetVelocity(unsigned int _index, double _vel);
+
+ // Documentation inherited
+ public: virtual double GetVelocity(unsigned int _index) const;
+
+ // Documentation inherited
+ public: virtual void SetMaxForce(unsigned int _index, double _force);
+
+ // Documentation inherited
+ public: virtual double GetMaxForce(unsigned int _index);
+
+ // Documentation inherited
+ protected: virtual void SetForceImpl(unsigned int _index, double _effort);
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTHeightmapShape.cc b/gazebo/physics/dart/DARTHeightmapShape.cc
index 254a9af..6f4b4d4 100644
--- a/gazebo/physics/dart/DARTHeightmapShape.cc
+++ b/gazebo/physics/dart/DARTHeightmapShape.cc
@@ -20,18 +20,22 @@
#include "gazebo/physics/dart/DARTCollision.hh"
#include "gazebo/physics/dart/DARTHeightmapShape.hh"
+#include "gazebo/physics/dart/DARTHeightmapShapePrivate.hh"
+
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
DARTHeightmapShape::DARTHeightmapShape(CollisionPtr _parent)
- : HeightmapShape(_parent)
+ : HeightmapShape(_parent),
+ dataPtr(new DARTHeightmapShapePrivate())
{
}
//////////////////////////////////////////////////
DARTHeightmapShape::~DARTHeightmapShape()
{
+ delete dataPtr;
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/dart/DARTHeightmapShape.hh b/gazebo/physics/dart/DARTHeightmapShape.hh
index 09e01a2..7739875 100644
--- a/gazebo/physics/dart/DARTHeightmapShape.hh
+++ b/gazebo/physics/dart/DARTHeightmapShape.hh
@@ -29,8 +29,11 @@ namespace gazebo
{
namespace physics
{
+ /// Forward declare private data class
+ class DARTHeightmapShapePrivate;
+
/// \brief DART Height map collision.
- class GAZEBO_VISIBLE DARTHeightmapShape : public HeightmapShape
+ class GZ_PHYSICS_VISIBLE DARTHeightmapShape : public HeightmapShape
{
/// \brief Constructor.
/// \param[in] _parent Collision parent.
@@ -42,14 +45,9 @@ namespace gazebo
// Documentation inerited.
public: virtual void Init();
- /// \brief Called by ODE to get the height at a vertex.
- /// \param[in] _data Pointer to the heightmap data.
- /// \param[in] _x X location.
- /// \param[in] _y Y location.
- // private: static dReal GetHeightCallback(void *_data, int _x, int _y);
-
- /// \brief The heightmap data.
- // private: dHeightfieldDataID odeData;
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTHeightmapShapePrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/dart/DARTHeightmapShapePrivate.hh b/gazebo/physics/dart/DARTHeightmapShapePrivate.hh
new file mode 100644
index 0000000..c74fd91
--- /dev/null
+++ b/gazebo/physics/dart/DARTHeightmapShapePrivate.hh
@@ -0,0 +1,40 @@
+/*
+ * 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_DARTHEIGHTMAPSHAPE_PRIVATE_HH_
+#define _GAZEBO_DARTHEIGHTMAPSHAPE_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTHeightmapShape
+ class DARTHeightmapShapePrivate
+ {
+ /// \brief Constructor
+ public: DARTHeightmapShapePrivate() = default;
+
+ /// \brief Default destructor
+ public: ~DARTHeightmapShapePrivate() = default;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTHinge2Joint.cc b/gazebo/physics/dart/DARTHinge2Joint.cc
index 6bd98ed..4b4f660 100644
--- a/gazebo/physics/dart/DARTHinge2Joint.cc
+++ b/gazebo/physics/dart/DARTHinge2Joint.cc
@@ -19,6 +19,7 @@
#include "gazebo/common/Console.hh"
#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/dart/DARTJointPrivate.hh"
#include "gazebo/physics/dart/DARTHinge2Joint.hh"
using namespace gazebo;
@@ -26,16 +27,15 @@ using namespace physics;
//////////////////////////////////////////////////
DARTHinge2Joint::DARTHinge2Joint(BasePtr _parent)
- : Hinge2Joint<DARTJoint>(_parent),
- dtUniveralJoint(new dart::dynamics::UniversalJoint())
+ : Hinge2Joint<DARTJoint>(_parent)
{
- this->dtJoint = dtUniveralJoint;
+ this->dataPtr->dtJoint = new dart::dynamics::UniversalJoint();
}
//////////////////////////////////////////////////
DARTHinge2Joint::~DARTHinge2Joint()
{
- delete dtUniveralJoint;
+ delete this->dataPtr->dtJoint;
}
//////////////////////////////////////////////////
@@ -53,8 +53,8 @@ void DARTHinge2Joint::Init()
//////////////////////////////////////////////////
math::Vector3 DARTHinge2Joint::GetAnchor(unsigned int /*_index*/) const
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
Eigen::Vector3d worldOrigin = T.translation();
return DARTTypes::ConvVec3(worldOrigin);
@@ -67,23 +67,31 @@ void DARTHinge2Joint::SetAxis(unsigned int _index, const math::Vector3 &_axis)
if (_index == 0)
{
+ dart::dynamics::UniversalJoint *dtUniveralJoint =
+ reinterpret_cast<dart::dynamics::UniversalJoint *>(
+ this->dataPtr->dtJoint);
+
// TODO: Issue #494
// See: https://bitbucket.org/osrf/gazebo/issue/494/joint-axis-reference
Eigen::Isometry3d dartTransfJointLeftToParentLink
- = this->dtJoint->getTransformFromParentBodyNode().inverse();
+ = this->dataPtr->dtJoint->getTransformFromParentBodyNode().inverse();
dartAxis = dartTransfJointLeftToParentLink.linear() * dartAxis;
- this->dtUniveralJoint->setAxis1(dartAxis);
+ dtUniveralJoint->setAxis1(dartAxis);
}
else if (_index == 1)
{
+ dart::dynamics::UniversalJoint *dtUniveralJoint =
+ reinterpret_cast<dart::dynamics::UniversalJoint *>(
+ this->dataPtr->dtJoint);
+
// TODO: Issue #494
// See: https://bitbucket.org/osrf/gazebo/issue/494/joint-axis-reference
Eigen::Isometry3d dartTransfJointLeftToParentLink
- = this->dtJoint->getTransformFromParentBodyNode().inverse();
+ = this->dataPtr->dtJoint->getTransformFromParentBodyNode().inverse();
dartAxis = dartTransfJointLeftToParentLink.linear() * dartAxis;
- this->dtUniveralJoint->setAxis2(dartAxis);
+ dtUniveralJoint->setAxis2(dartAxis);
}
else
{
@@ -98,18 +106,26 @@ math::Vector3 DARTHinge2Joint::GetGlobalAxis(unsigned int _index) const
if (_index == 0)
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getLocalTransform().inverse() *
- this->dtJoint->getTransformFromParentBodyNode();
- Eigen::Vector3d axis = this->dtUniveralJoint->getAxis1();
+ dart::dynamics::UniversalJoint *dtUniveralJoint =
+ reinterpret_cast<dart::dynamics::UniversalJoint *>(
+ this->dataPtr->dtJoint);
+
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getLocalTransform().inverse() *
+ this->dataPtr->dtJoint->getTransformFromParentBodyNode();
+ Eigen::Vector3d axis = dtUniveralJoint->getAxis1();
globalAxis = T.linear() * axis;
}
else if (_index == 1)
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getTransformFromChildBodyNode();
- Eigen::Vector3d axis = this->dtUniveralJoint->getAxis2();
+ dart::dynamics::UniversalJoint *dtUniveralJoint =
+ reinterpret_cast<dart::dynamics::UniversalJoint *>(
+ this->dataPtr->dtJoint);
+
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Vector3d axis = dtUniveralJoint->getAxis2();
globalAxis = T.linear() * axis;
}
@@ -131,12 +147,12 @@ math::Angle DARTHinge2Joint::GetAngleImpl(unsigned int _index) const
if (_index == 0)
{
- double radianAngle = this->dtJoint->getPosition(0);
+ double radianAngle = this->dataPtr->dtJoint->getPosition(0);
result.SetFromRadian(radianAngle);
}
else if (_index == 1)
{
- double radianAngle = this->dtJoint->getPosition(1);
+ double radianAngle = this->dataPtr->dtJoint->getPosition(1);
result.SetFromRadian(radianAngle);
}
else
@@ -153,9 +169,9 @@ double DARTHinge2Joint::GetVelocity(unsigned int _index) const
double result = 0.0;
if (_index == 0)
- result = this->dtJoint->getVelocity(0);
+ result = this->dataPtr->dtJoint->getVelocity(0);
else if (_index == 1)
- result = this->dtJoint->getVelocity(1);
+ result = this->dataPtr->dtJoint->getVelocity(1);
else
gzerr << "Invalid index[" << _index << "]\n";
@@ -166,9 +182,9 @@ double DARTHinge2Joint::GetVelocity(unsigned int _index) const
void DARTHinge2Joint::SetVelocity(unsigned int _index, double _vel)
{
if (_index == 0)
- this->dtJoint->setVelocity(0, _vel);
+ this->dataPtr->dtJoint->setVelocity(0, _vel);
else if (_index == 1)
- this->dtJoint->setVelocity(1, _vel);
+ this->dataPtr->dtJoint->setVelocity(1, _vel);
else
gzerr << "Invalid index[" << _index << "]\n";
}
@@ -181,14 +197,14 @@ double DARTHinge2Joint::GetMaxForce(unsigned int _index)
if (_index == 0)
{
// Assume that the lower limit and upper limit has equal magnitute
- // result = this->dtJoint->getForceLowerLimit(0);
- result = this->dtJoint->getForceUpperLimit(0);
+ // result = this->dataPtr->dtJoint->getForceLowerLimit(0);
+ result = this->dataPtr->dtJoint->getForceUpperLimit(0);
}
else if (_index == 1)
{
// Assume that the lower limit and upper limit has equal magnitute
- // result = this->dtJoint->getForceLowerLimit(1);
- result = this->dtJoint->getForceUpperLimit(1);
+ // result = this->dataPtr->dtJoint->getForceLowerLimit(1);
+ result = this->dataPtr->dtJoint->getForceUpperLimit(1);
}
else
{
@@ -203,13 +219,13 @@ void DARTHinge2Joint::SetMaxForce(unsigned int _index, double _force)
{
if (_index == 0)
{
- this->dtJoint->setForceLowerLimit(0, -_force);
- this->dtJoint->setForceUpperLimit(0, _force);
+ this->dataPtr->dtJoint->setForceLowerLimit(0, -_force);
+ this->dataPtr->dtJoint->setForceUpperLimit(0, _force);
}
else if (_index == 1)
{
- this->dtJoint->setForceLowerLimit(1, -_force);
- this->dtJoint->setForceUpperLimit(1, _force);
+ this->dataPtr->dtJoint->setForceLowerLimit(1, -_force);
+ this->dataPtr->dtJoint->setForceUpperLimit(1, _force);
}
else
{
@@ -221,9 +237,9 @@ void DARTHinge2Joint::SetMaxForce(unsigned int _index, double _force)
void DARTHinge2Joint::SetForceImpl(unsigned int _index, double _effort)
{
if (_index == 0)
- this->dtJoint->setForce(0, _effort);
+ this->dataPtr->dtJoint->setForce(0, _effort);
else if (_index == 1)
- this->dtJoint->setForce(1, _effort);
+ this->dataPtr->dtJoint->setForce(1, _effort);
else
gzerr << "Invalid index[" << _index << "]\n";
}
diff --git a/gazebo/physics/dart/DARTHinge2Joint.hh b/gazebo/physics/dart/DARTHinge2Joint.hh
index fceb771..98a92d6 100644
--- a/gazebo/physics/dart/DARTHinge2Joint.hh
+++ b/gazebo/physics/dart/DARTHinge2Joint.hh
@@ -30,7 +30,7 @@ namespace gazebo
namespace physics
{
/// \brief A two axis hinge joint
- class GAZEBO_VISIBLE DARTHinge2Joint : public Hinge2Joint<DARTJoint>
+ class GZ_PHYSICS_VISIBLE DARTHinge2Joint : public Hinge2Joint<DARTJoint>
{
/// \brief Constructor
/// \param[in] _parent Parent of the Joint
@@ -72,9 +72,6 @@ namespace gazebo
// Documentation inherited
protected: virtual void SetForceImpl(unsigned int _index, double _effort);
-
- /// \brief Universal joint of DART
- protected: dart::dynamics::UniversalJoint *dtUniveralJoint;
};
}
}
diff --git a/gazebo/physics/dart/DARTHingeJoint.cc b/gazebo/physics/dart/DARTHingeJoint.cc
index 0b38f27..ab7003e 100644
--- a/gazebo/physics/dart/DARTHingeJoint.cc
+++ b/gazebo/physics/dart/DARTHingeJoint.cc
@@ -20,6 +20,7 @@
#include "gazebo/gazebo_config.h"
#include "gazebo/common/Console.hh"
#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/dart/DARTJointPrivate.hh"
#include "gazebo/physics/dart/DARTHingeJoint.hh"
using namespace gazebo;
@@ -27,16 +28,15 @@ using namespace physics;
//////////////////////////////////////////////////
DARTHingeJoint::DARTHingeJoint(BasePtr _parent)
- : HingeJoint<DARTJoint>(_parent),
- dtRevoluteJoint(new dart::dynamics::RevoluteJoint())
+ : HingeJoint<DARTJoint>(_parent)
{
- this->dtJoint = this->dtRevoluteJoint;
+ this->dataPtr->dtJoint = new dart::dynamics::RevoluteJoint();
}
//////////////////////////////////////////////////
DARTHingeJoint::~DARTHingeJoint()
{
- delete dtRevoluteJoint;
+ delete this->dataPtr->dtJoint;
}
//////////////////////////////////////////////////
@@ -54,8 +54,8 @@ void DARTHingeJoint::Init()
//////////////////////////////////////////////////
math::Vector3 DARTHingeJoint::GetAnchor(unsigned int /*index*/) const
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
Eigen::Vector3d worldOrigin = T.translation();
return DARTTypes::ConvVec3(worldOrigin);
@@ -68,9 +68,13 @@ math::Vector3 DARTHingeJoint::GetGlobalAxis(unsigned int _index) const
if (_index == 0)
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getTransformFromChildBodyNode();
- Eigen::Vector3d axis = this->dtRevoluteJoint->getAxis();
+ dart::dynamics::RevoluteJoint *dtRevoluteJoint =
+ reinterpret_cast<dart::dynamics::RevoluteJoint *>(
+ this->dataPtr->dtJoint);
+
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Vector3d axis = dtRevoluteJoint->getAxis();
globalAxis = T.linear() * axis;
}
else
@@ -86,14 +90,18 @@ void DARTHingeJoint::SetAxis(unsigned int _index, const math::Vector3& _axis)
{
if (_index == 0)
{
+ dart::dynamics::RevoluteJoint *dtRevoluteJoint =
+ reinterpret_cast<dart::dynamics::RevoluteJoint *>(
+ this->dataPtr->dtJoint);
+
Eigen::Vector3d dartAxis = DARTTypes::ConvVec3(
this->GetAxisFrameOffset(0).RotateVector(_axis));
Eigen::Isometry3d dartTransfJointLeftToParentLink
- = this->dtJoint->getTransformFromParentBodyNode().inverse();
+ = this->dataPtr->dtJoint->getTransformFromParentBodyNode().inverse();
dartAxis = dartTransfJointLeftToParentLink.linear() * dartAxis;
//--------------------------------------------------------------------------
- this->dtRevoluteJoint->setAxis(dartAxis);
+ dtRevoluteJoint->setAxis(dartAxis);
}
else
{
@@ -108,7 +116,7 @@ math::Angle DARTHingeJoint::GetAngleImpl(unsigned int _index) const
if (_index == 0)
{
- double radianAngle = this->dtJoint->getPosition(0);
+ double radianAngle = this->dataPtr->dtJoint->getPosition(0);
result.SetFromRadian(radianAngle);
}
else
@@ -124,8 +132,9 @@ void DARTHingeJoint::SetVelocity(unsigned int _index, double _vel)
{
if (_index == 0)
{
- this->dtJoint->setVelocity(0, _vel);
- this->dtJoint->getSkeleton()->computeForwardKinematics(false, true, false);
+ this->dataPtr->dtJoint->setVelocity(0, _vel);
+ this->dataPtr->dtJoint->getSkeleton()->computeForwardKinematics(
+ false, true, false);
}
else
gzerr << "Invalid index[" << _index << "]\n";
@@ -137,7 +146,7 @@ double DARTHingeJoint::GetVelocity(unsigned int _index) const
double result = 0.0;
if (_index == 0)
- result = this->dtJoint->getVelocity(0);
+ result = this->dataPtr->dtJoint->getVelocity(0);
else
gzerr << "Invalid index[" << _index << "]\n";
@@ -149,8 +158,8 @@ void DARTHingeJoint::SetMaxForce(unsigned int _index, double _force)
{
if (_index == 0)
{
- this->dtJoint->setForceLowerLimit(0, -_force);
- this->dtJoint->setForceUpperLimit(0, _force);
+ this->dataPtr->dtJoint->setForceLowerLimit(0, -_force);
+ this->dataPtr->dtJoint->setForceUpperLimit(0, _force);
}
else
{
@@ -166,8 +175,8 @@ double DARTHingeJoint::GetMaxForce(unsigned int _index)
if (_index == 0)
{
// Assume that the lower limit and upper limit has equal magnitute
- // result = this->dtJoint->getForceLowerLimit(0);
- result = this->dtJoint->getForceUpperLimit(0);
+ // result = this->dataPtr->dtJoint->getForceLowerLimit(0);
+ result = this->dataPtr->dtJoint->getForceUpperLimit(0);
}
else
{
@@ -181,7 +190,7 @@ double DARTHingeJoint::GetMaxForce(unsigned int _index)
void DARTHingeJoint::SetForceImpl(unsigned int _index, double _effort)
{
if (_index == 0)
- this->dtJoint->setForce(0, _effort);
+ this->dataPtr->dtJoint->setForce(0, _effort);
else
gzerr << "Invalid index[" << _index << "]\n";
}
diff --git a/gazebo/physics/dart/DARTHingeJoint.hh b/gazebo/physics/dart/DARTHingeJoint.hh
index e0eb3be..c122534 100644
--- a/gazebo/physics/dart/DARTHingeJoint.hh
+++ b/gazebo/physics/dart/DARTHingeJoint.hh
@@ -30,7 +30,7 @@ namespace gazebo
namespace physics
{
/// \brief A single axis hinge joint.
- class GAZEBO_VISIBLE DARTHingeJoint : public HingeJoint<DARTJoint>
+ class GZ_PHYSICS_VISIBLE DARTHingeJoint : public HingeJoint<DARTJoint>
{
/// \brief Constructor
/// \param[in] _parent Parent of the Joint
@@ -72,9 +72,6 @@ namespace gazebo
// Documentation inherited
protected: virtual void SetForceImpl(unsigned int _index, double _effort);
-
- /// \brief Revolute joint of DART
- protected: dart::dynamics::RevoluteJoint *dtRevoluteJoint;
};
}
}
diff --git a/gazebo/physics/dart/DARTJoint.cc b/gazebo/physics/dart/DARTJoint.cc
index 197edd1..dc08d50 100644
--- a/gazebo/physics/dart/DARTJoint.cc
+++ b/gazebo/physics/dart/DARTJoint.cc
@@ -27,19 +27,17 @@
#include "gazebo/physics/dart/DARTModel.hh"
#include "gazebo/physics/dart/DARTJoint.hh"
+#include "gazebo/physics/dart/DARTJointPrivate.hh"
+
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
DARTJoint::DARTJoint(BasePtr _parent)
: Joint(_parent),
- dtJoint(NULL),
- dtChildBodyNode(NULL)
+ dataPtr(new DARTJointPrivate(boost::dynamic_pointer_cast<DARTPhysics>(
+ this->GetWorld()->GetPhysicsEngine())))
{
- this->dartPhysicsEngine = boost::dynamic_pointer_cast<DARTPhysics>(
- this->GetWorld()->GetPhysicsEngine());
- this->forceApplied[0] = 0.0;
- this->forceApplied[1] = 0.0;
}
//////////////////////////////////////////////////
@@ -47,8 +45,7 @@ DARTJoint::~DARTJoint()
{
this->Detach();
- if (dtJoint)
- delete dtJoint;
+ delete this->dataPtr;
}
//////////////////////////////////////////////////
@@ -66,7 +63,7 @@ void DARTJoint::Init()
// Name
std::string jointName = this->GetName();
- this->dtJoint->setName(jointName.c_str());
+ this->dataPtr->dtJoint->setName(jointName.c_str());
// Parent and child link information
DARTLinkPtr dartParentLink =
@@ -85,8 +82,8 @@ void DARTJoint::Init()
{
dtTransformChildBodyNode =
DARTTypes::ConvPose(dartChildLink->GetWorldPose());
- this->dtChildBodyNode = dartChildLink->GetDARTBodyNode();
- this->dtChildBodyNode->setParentJoint(this->dtJoint);
+ this->dataPtr->dtChildBodyNode = dartChildLink->GetDARTBodyNode();
+ this->dataPtr->dtChildBodyNode->setParentJoint(this->dataPtr->dtJoint);
}
dtTransformChildLinkToJoint = DARTTypes::ConvPose(this->anchorPose);
@@ -94,9 +91,9 @@ void DARTJoint::Init()
{
dtTransformParentBodyNode =
DARTTypes::ConvPose(dartParentLink->GetWorldPose());
- dart::dynamics::BodyNode* dtParentBodyNode =
+ dart::dynamics::BodyNode *dtParentBodyNode =
dartParentLink->GetDARTBodyNode();
- dtParentBodyNode->addChildBodyNode(this->dtChildBodyNode);
+ dtParentBodyNode->addChildBodyNode(this->dataPtr->dtChildBodyNode);
}
dtTransformParentLinkToJoint = dtTransformParentBodyNode.inverse() *
@@ -104,42 +101,10 @@ void DARTJoint::Init()
dtTransformChildLinkToJoint;
// We assume that the joint angles are all zero.
- this->dtJoint->setTransformFromParentBodyNode(dtTransformParentLinkToJoint);
- this->dtJoint->setTransformFromChildBodyNode(dtTransformChildLinkToJoint);
-
- //----------------------------------------------------------------------------
- // TODO: Currently, dampingCoefficient seems not to be initialized when
- // this joint is loaded. Therefore, we need below code...
- //----------------------------------------------------------------------------
- if (this->sdf->HasElement("axis"))
- {
- sdf::ElementPtr axisElem = this->sdf->GetElement("axis");
- if (axisElem->HasElement("dynamics"))
- {
- sdf::ElementPtr dynamicsElem = axisElem->GetElement("dynamics");
-
- if (dynamicsElem->HasElement("friction"))
- {
- sdf::ElementPtr frictionElem = dynamicsElem->GetElement("friction");
- gzlog << "joint friction not implemented in DART.\n";
- }
- }
- }
-
- if (this->sdf->HasElement("axis2"))
- {
- sdf::ElementPtr axisElem = this->sdf->GetElement("axis");
- if (axisElem->HasElement("dynamics"))
- {
- sdf::ElementPtr dynamicsElem = axisElem->GetElement("dynamics");
-
- if (dynamicsElem->HasElement("friction"))
- {
- sdf::ElementPtr frictionElem = dynamicsElem->GetElement("friction");
- gzlog << "joint friction not implemented in DART.\n";
- }
- }
- }
+ this->dataPtr->dtJoint->setTransformFromParentBodyNode(
+ dtTransformParentLinkToJoint);
+ this->dataPtr->dtJoint->setTransformFromChildBodyNode(
+ dtTransformChildLinkToJoint);
}
//////////////////////////////////////////////////
@@ -275,11 +240,11 @@ void DARTJoint::SetStiffnessDamping(unsigned int _index,
{
if (!parentStatic && !childStatic)
{
- this->dtJoint->setSpringStiffness(
+ this->dataPtr->dtJoint->setSpringStiffness(
static_cast<int>(_index), _stiffness);
- this->dtJoint->setRestPosition(
+ this->dataPtr->dtJoint->setRestPosition(
static_cast<int>(_index), _reference);
- this->dtJoint->setDampingCoefficient(
+ this->dataPtr->dtJoint->setDampingCoefficient(
static_cast<int>(_index), _damping);
this->applyDamping = physics::Joint::ConnectJointUpdate(
boost::bind(&DARTJoint::ApplyDamping, this));
@@ -306,7 +271,7 @@ bool DARTJoint::SetHighStop(unsigned int _index, const math::Angle &_angle)
case 0:
case 1:
case 2:
- this->dtJoint->setPositionUpperLimit(_index, _angle.Radian());
+ this->dataPtr->dtJoint->setPositionUpperLimit(_index, _angle.Radian());
return true;
default:
gzerr << "Invalid index[" << _index << "]\n";
@@ -322,7 +287,7 @@ bool DARTJoint::SetLowStop(unsigned int _index, const math::Angle &_angle)
case 0:
case 1:
case 2:
- this->dtJoint->setPositionLowerLimit(_index, _angle.Radian());
+ this->dataPtr->dtJoint->setPositionLowerLimit(_index, _angle.Radian());
return true;
default:
gzerr << "Invalid index[" << _index << "]\n";
@@ -338,7 +303,7 @@ math::Angle DARTJoint::GetHighStop(unsigned int _index)
case 0:
case 1:
case 2:
- return this->dtJoint->getPositionUpperLimit(_index);
+ return this->dataPtr->dtJoint->getPositionUpperLimit(_index);
default:
gzerr << "Invalid index[" << _index << "]\n";
};
@@ -354,7 +319,7 @@ math::Angle DARTJoint::GetLowStop(unsigned int _index)
case 0:
case 1:
case 2:
- return this->dtJoint->getPositionLowerLimit(_index);
+ return this->dataPtr->dtJoint->getPositionLowerLimit(_index);
default:
gzerr << "Invalid index[" << _index << "]\n";
};
@@ -367,7 +332,7 @@ math::Vector3 DARTJoint::GetLinkForce(unsigned int _index) const
{
math::Vector3 result;
- if (!this->dtJoint)
+ if (!this->dataPtr->dtJoint)
{
gzerr << "DART joint is invalid\n";
return result;
@@ -381,7 +346,7 @@ math::Vector3 DARTJoint::GetLinkForce(unsigned int _index) const
Eigen::Vector6d F1 = Eigen::Vector6d::Zero();
Eigen::Vector6d F2 = Eigen::Vector6d::Zero();
- Eigen::Isometry3d T12 = dtJoint->getLocalTransform();
+ Eigen::Isometry3d T12 = this->dataPtr->dtJoint->getLocalTransform();
// JointWrench.body1Force contains the
// force applied by the parent Link on the Joint specified in
@@ -390,8 +355,9 @@ math::Vector3 DARTJoint::GetLinkForce(unsigned int _index) const
{
dart::dynamics::BodyNode *dartChildBody = theChildLink->GetDARTBodyNode();
GZ_ASSERT(dartChildBody, "dartChildBody pointer is NULL");
- F2 = -dart::math::dAdT(dtJoint->getTransformFromChildBodyNode(),
- dartChildBody->getBodyForce());
+ F2 = -dart::math::dAdT(
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode(),
+ dartChildBody->getBodyForce());
}
// JointWrench.body2Force contains
@@ -412,7 +378,7 @@ math::Vector3 DARTJoint::GetLinkTorque(unsigned int _index) const
{
math::Vector3 result;
- if (!this->dtJoint)
+ if (!this->dataPtr->dtJoint)
{
gzerr << "DART joint is invalid\n";
return result;
@@ -424,7 +390,7 @@ math::Vector3 DARTJoint::GetLinkTorque(unsigned int _index) const
Eigen::Vector6d F1 = Eigen::Vector6d::Zero();
Eigen::Vector6d F2 = Eigen::Vector6d::Zero();
- Eigen::Isometry3d T12 = dtJoint->getLocalTransform();
+ Eigen::Isometry3d T12 = this->dataPtr->dtJoint->getLocalTransform();
// JointWrench.body1Force contains the
// force applied by the parent Link on the Joint specified in
@@ -434,7 +400,8 @@ math::Vector3 DARTJoint::GetLinkTorque(unsigned int _index) const
dart::dynamics::BodyNode *dartChildBody = theChildLink->GetDARTBodyNode();
GZ_ASSERT(dartChildBody, "dartChildBody pointer is NULL");
F2 = -dart::math::dAdT(
- dtJoint->getTransformFromChildBodyNode(), dartChildBody->getBodyForce());
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode(),
+ dartChildBody->getBodyForce());
}
// JointWrench.body2Force contains
@@ -452,73 +419,65 @@ math::Vector3 DARTJoint::GetLinkTorque(unsigned int _index) const
//////////////////////////////////////////////////
bool DARTJoint::SetParam(const std::string &_key, unsigned int _index,
- const boost::any &_value)
+ const boost::any &_value)
{
- if (_key == "hi_stop")
+ // try because boost::any_cast can throw
+ try
{
- try
+ if (_key == "hi_stop")
{
this->SetHighStop(_index, boost::any_cast<double>(_value));
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "lo_stop")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ this->SetLowStop(_index, boost::any_cast<double>(_value));
}
- }
- else if (_key == "lo_stop")
- {
- try
+ else if (_key == "friction")
{
- this->SetLowStop(_index, boost::any_cast<double>(_value));
+ this->dataPtr->dtJoint->setCoulombFriction(_index,
+ boost::any_cast<double>(_value));
}
- catch(const boost::bad_any_cast &e)
+ else
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
+ gzerr << "Unable to handle joint attribute[" << _key << "]\n";
return false;
}
}
- else
+ catch(const boost::bad_any_cast &e)
{
- gzerr << "Unable to handle joint attribute[" << _key << "]\n";
+ gzerr << "SetParam(" << _key << ")"
+ << " boost any_cast error:" << e.what()
+ << std::endl;
return false;
}
+
return true;
}
//////////////////////////////////////////////////
-double DARTJoint::GetParam(const std::string& _key,
- unsigned int _index)
+double DARTJoint::GetParam(const std::string &_key, unsigned int _index)
{
- if (_key == "hi_stop")
+ try
{
- try
- {
- return this->GetHighStop(_index).Radian();
- }
- catch(common::Exception &e)
- {
- gzerr << "GetParam error:" << e.GetErrorStr() << "\n";
- return 0;
- }
- }
- else if (_key == "lo_stop")
- {
- try
- {
- return this->GetLowStop(_index).Radian();
- }
- catch(common::Exception &e)
+ if (_key == "friction")
{
- gzerr << "GetParam error:" << e.GetErrorStr() << "\n";
- return 0;
+ return this->dataPtr->dtJoint->getCoulombFriction(_index);
}
}
- else
+ catch(const common::Exception &e)
{
- gzerr << "Unable to get joint attribute[" << _key << "]\n";
+ gzerr << "GetParam(" << _key << ") error:"
+ << e.GetErrorStr()
+ << std::endl;
return 0;
}
+ return Joint::GetParam(_key, _index);
+}
+
+//////////////////////////////////////////////////
+void DARTJoint::CacheForceTorque()
+{
+ // Does nothing for now, will add when recovering pull request #1721
}
//////////////////////////////////////////////////
@@ -534,22 +493,25 @@ JointWrench DARTJoint::GetForceTorque(unsigned int /*_index*/)
Eigen::Vector6d F1 = Eigen::Vector6d::Zero();
Eigen::Vector6d F2 = Eigen::Vector6d::Zero();
- Eigen::Isometry3d T12 = dtJoint->getLocalTransform();
+ Eigen::Isometry3d T12 = this->dataPtr->dtJoint->getLocalTransform();
- // JointWrench.body1Force contains the
- // force applied by the parent Link on the Joint specified in
- // the parent Link frame.
+ // JointWrench.body2Force (F2) contains
+ // the force applied by the child Link on the parent link specified
+ // in the child Link orientation frame and with respect to the joint origin
if (theChildLink != NULL)
{
dart::dynamics::BodyNode *dartChildBody = theChildLink->GetDARTBodyNode();
GZ_ASSERT(dartChildBody, "dartChildBody pointer is NULL");
- F2 = -dart::math::dAdT(dtJoint->getTransformFromChildBodyNode(),
+ Eigen::Isometry3d TJ2 = Eigen::Isometry3d::Identity();
+ TJ2.translation() =
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode().translation();
+ F2 = -dart::math::dAdT(TJ2,
dartChildBody->getBodyForce());
}
- // JointWrench.body2Force contains
- // the force applied by the child Link on the Joint specified
- // in the child Link frame.
+ // JointWrench.body1Force (F1) contains the
+ // force applied by the parent Link on the child Link specified in
+ // the parent Link orientation frame and with respect to the joint origin
F1 = -dart::math::dAdInvR(T12, F2);
// kind of backwards here, body1 (parent) corresponds go f2, t2
@@ -581,7 +543,7 @@ double DARTJoint::GetForce(unsigned int _index)
{
if (_index < this->GetAngleCount())
{
- return this->forceApplied[_index];
+ return this->dataPtr->forceApplied[_index];
}
else
{
@@ -596,7 +558,7 @@ unsigned int DARTJoint::GetAngleCount() const
{
unsigned int angleCount = 0;
- angleCount = this->dtJoint->getNumDofs();
+ angleCount = this->dataPtr->dtJoint->getNumDofs();
return angleCount;
}
@@ -623,7 +585,7 @@ DARTModelPtr DARTJoint::GetDARTModel() const
/////////////////////////////////////////////////
dart::dynamics::Joint *DARTJoint::GetDARTJoint()
{
- return this->dtJoint;
+ return this->dataPtr->dtJoint;
}
/////////////////////////////////////////////////
@@ -633,14 +595,14 @@ void DARTJoint::SaveForce(unsigned int _index, double _force)
// it simply records the forces commanded inside forceApplied.
if (_index < this->GetAngleCount())
{
- if (this->forceAppliedTime < this->GetWorld()->GetSimTime())
+ if (this->dataPtr->forceAppliedTime < this->GetWorld()->GetSimTime())
{
// reset forces if time step is new
- this->forceAppliedTime = this->GetWorld()->GetSimTime();
- this->forceApplied[0] = this->forceApplied[1] = 0.0;
+ this->dataPtr->forceAppliedTime = this->GetWorld()->GetSimTime();
+ this->dataPtr->forceApplied[0] = this->dataPtr->forceApplied[1] = 0.0;
}
- this->forceApplied[_index] += _force;
+ this->dataPtr->forceApplied[_index] += _force;
}
else
gzerr << "Something's wrong, joint [" << this->GetName()
diff --git a/gazebo/physics/dart/DARTJoint.hh b/gazebo/physics/dart/DARTJoint.hh
index 0030d5a..1064359 100644
--- a/gazebo/physics/dart/DARTJoint.hh
+++ b/gazebo/physics/dart/DARTJoint.hh
@@ -31,8 +31,11 @@ namespace gazebo
{
namespace physics
{
+ /// Forward declare private data class
+ class DARTJointPrivate;
+
/// \brief DART joint interface
- class GAZEBO_VISIBLE DARTJoint : public Joint
+ class GZ_PHYSICS_VISIBLE DARTJoint : public Joint
{
/// \brief Constructor.
/// \param[in] _parent Parent of the Joint.
@@ -60,6 +63,9 @@ namespace gazebo
public: virtual void Attach(LinkPtr _parent, LinkPtr _child);
// Documentation inherited.
+ public: virtual void CacheForceTorque();
+
+ // Documentation inherited.
public: virtual void Detach();
/// \brief Set the anchor point
@@ -145,25 +151,9 @@ namespace gazebo
/// \return A pointer to the DART joint.
public: dart::dynamics::Joint *GetDARTJoint();
- /// \brief Save force applied by user
- /// This plus the joint feedback (joint contstraint forces) is the
- /// equivalent of simulated force torque sensor reading
- /// Allocate a 2 vector in case hinge2 joint is used.
- /// This is used by DART to store external force applied by the user.
- private: double forceApplied[MAX_JOINT_AXIS];
-
- /// \brief Save time at which force is applied by user
- /// This will let us know if it's time to clean up forceApplied.
- private: common::Time forceAppliedTime;
-
- /// \brief DARTPhysics engine pointer
- protected: DARTPhysicsPtr dartPhysicsEngine;
-
- /// \brief DART joint pointer
- protected: dart::dynamics::Joint *dtJoint;
-
- /// \brief DART child body node pointer
- protected: dart::dynamics::BodyNode *dtChildBodyNode;
+ /// \internal
+ /// \brief Pointer to private data
+ protected: DARTJointPrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/dart/DARTJointPrivate.hh b/gazebo/physics/dart/DARTJointPrivate.hh
new file mode 100644
index 0000000..b64536f
--- /dev/null
+++ b/gazebo/physics/dart/DARTJointPrivate.hh
@@ -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.
+ *
+*/
+
+#ifndef _GAZEBO_DARTJOINT_PRIVATE_HH_
+#define _GAZEBO_DARTJOINT_PRIVATE_HH_
+
+#include "gazebo/common/Time.hh"
+#include "gazebo/physics/Joint.hh"
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTJoint
+ class DARTJointPrivate
+ {
+ /// \brief Constructor
+ public: DARTJointPrivate(const DARTPhysicsPtr &_dartPhysicsEngine)
+ : forceApplied {0.0, 0.0},
+ forceAppliedTime(),
+ dartPhysicsEngine(_dartPhysicsEngine),
+ dtJoint(NULL),
+ dtChildBodyNode(NULL)
+ {
+ }
+
+ /// \brief Default destructor
+ public: ~DARTJointPrivate() = default;
+
+ /// \brief Save force applied by user
+ /// This plus the joint feedback (joint contstraint forces) is the
+ /// equivalent of simulated force torque sensor reading
+ /// Allocate a 2 vector in case hinge2 joint is used.
+ /// This is used by DART to store external force applied by the user.
+ public: double forceApplied[MAX_JOINT_AXIS];
+
+ /// \brief Save time at which force is applied by user
+ /// This will let us know if it's time to clean up forceApplied.
+ public: common::Time forceAppliedTime;
+
+ /// \brief DARTPhysics engine pointer
+ public: DARTPhysicsPtr dartPhysicsEngine;
+
+ /// \brief DART joint pointer
+ public: dart::dynamics::Joint *dtJoint;
+
+ /// \brief DART child body node pointer
+ public: dart::dynamics::BodyNode *dtChildBodyNode;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTLink.cc b/gazebo/physics/dart/DARTLink.cc
index 3d78c60..e3cecfb 100644
--- a/gazebo/physics/dart/DARTLink.cc
+++ b/gazebo/physics/dart/DARTLink.cc
@@ -23,37 +23,37 @@
#include "gazebo/physics/WorldPrivate.hh"
#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTCollision.hh"
#include "gazebo/physics/dart/DARTPhysics.hh"
#include "gazebo/physics/dart/DARTModel.hh"
#include "gazebo/physics/dart/DARTLink.hh"
#include "gazebo/physics/dart/DARTJoint.hh"
+#include "gazebo/physics/dart/DARTSurfaceParams.hh"
+
+#include "gazebo/physics/dart/DARTLinkPrivate.hh"
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
DARTLink::DARTLink(EntityPtr _parent)
- : Link(_parent),
- dtBodyNode(NULL),
- staticLink(false),
- weldJointConst(NULL)
+ : Link(_parent), dataPtr(new DARTLinkPrivate())
{
}
//////////////////////////////////////////////////
DARTLink::~DARTLink()
{
- // We don't need to delete dartBodyNode because skeletone will delete
- // dartBodyNode if this is registered to the skeletone.
+ delete this->dataPtr;
}
//////////////////////////////////////////////////
void DARTLink::Load(sdf::ElementPtr _sdf)
{
- this->dartPhysics = boost::dynamic_pointer_cast<DARTPhysics>(
+ this->dataPtr->dartPhysics = boost::dynamic_pointer_cast<DARTPhysics>(
this->GetWorld()->GetPhysicsEngine());
- if (this->dartPhysics == NULL)
+ if (this->dataPtr->dartPhysics == NULL)
gzthrow("Not using the dart physics engine");
// Check if soft_contact element is contained in this link. If so,
@@ -103,7 +103,7 @@ void DARTLink::Load(sdf::ElementPtr _sdf)
if (dartElem != NULL)
{
// Create DART SoftBodyNode
- dart::dynamics::SoftBodyNode* dtSoftBodyNode
+ dart::dynamics::SoftBodyNode *dtSoftBodyNode
= new dart::dynamics::SoftBodyNode();
// Mass
@@ -166,12 +166,12 @@ void DARTLink::Load(sdf::ElementPtr _sdf)
gzerr << "Unknown soft shape" << std::endl;
}
- dtBodyNode = dtSoftBodyNode;
+ this->dataPtr->dtBodyNode = dtSoftBodyNode;
}
else
{
// Create DART BodyNode
- dtBodyNode = new dart::dynamics::BodyNode();
+ this->dataPtr->dtBodyNode = new dart::dynamics::BodyNode();
}
Link::Load(_sdf);
@@ -184,11 +184,11 @@ void DARTLink::Init()
// Name
std::string bodyName = this->GetName();
- this->dtBodyNode->setName(bodyName);
+ this->dataPtr->dtBodyNode->setName(bodyName);
// Mass
double mass = this->inertial->GetMass();
- this->dtBodyNode->setMass(mass);
+ this->dataPtr->dtBodyNode->setMass(mass);
// Inertia
double Ixx = this->inertial->GetIXX();
@@ -197,18 +197,58 @@ void DARTLink::Init()
double Ixy = this->inertial->GetIXY();
double Ixz = this->inertial->GetIXZ();
double Iyz = this->inertial->GetIYZ();
- this->dtBodyNode->setMomentOfInertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz);
+ this->dataPtr->dtBodyNode->setMomentOfInertia(Ixx, Iyy, Izz, Ixy, Ixz, Iyz);
// Visual
this->visuals;
// COG offset
math::Vector3 cog = this->inertial->GetCoG();
- this->dtBodyNode->setLocalCOM(DARTTypes::ConvVec3(cog));
+ this->dataPtr->dtBodyNode->setLocalCOM(DARTTypes::ConvVec3(cog));
// Gravity mode
this->SetGravityMode(this->sdf->Get<bool>("gravity"));
+ // Friction coefficient
+
+ /// \todo FIXME: Friction Parameters
+ /// Gazebo allows different friction parameters per collision objects,
+ /// while DART stores the friction parameter per link (BodyNode in DART). For
+ /// now, the average friction parameter of all the child collision objects is
+ /// stored in this->dataPtr->dtBodyNode.
+ /// Final friction coefficient is applied in DART's constraint solver by
+ /// taking the lower of the 2 colliding rigidLink's.
+ /// See also:
+ /// - https://github.com/dartsim/dart/issues/141
+ /// - https://github.com/dartsim/dart/issues/266
+
+ double hackAvgMu1 = 0;
+ double hackAvgMu2 = 0;
+ int numCollisions = 0;
+
+ for (auto const &child : this->children)
+ {
+ if (child->HasType(Base::COLLISION))
+ {
+ CollisionPtr collision =
+ boost::static_pointer_cast<Collision>(child);
+
+ SurfaceParamsPtr surface = collision->GetSurface();
+ GZ_ASSERT(surface, "Surface pointer for is invalid");
+ FrictionPyramidPtr friction = surface->GetFrictionPyramid();
+ GZ_ASSERT(friction, "Friction pointer for is invalid");
+
+ numCollisions++;
+ hackAvgMu1 += friction->GetMuPrimary();
+ hackAvgMu2 += friction->GetMuSecondary();
+ }
+ }
+
+ hackAvgMu1 /= static_cast<double>(numCollisions);
+ hackAvgMu2 /= static_cast<double>(numCollisions);
+
+ this->dataPtr->dtBodyNode->setFrictionCoeff(0.5 * (hackAvgMu1 + hackAvgMu2));
+
// We don't add dart body node to the skeleton here because dart body node
// should be set its parent joint before being added. This body node will be
// added to the skeleton in DARTModel::Init().
@@ -226,14 +266,14 @@ void DARTLink::OnPoseChange()
Link::OnPoseChange();
// DART body node always have its parent joint.
- dart::dynamics::Joint* joint = this->dtBodyNode->getParentJoint();
+ dart::dynamics::Joint *joint = this->dataPtr->dtBodyNode->getParentJoint();
// This is for the case this function called before DARTModel::Init() is
// called.
if (joint == NULL)
return;
- dart::dynamics::FreeJoint* freeJoint =
+ dart::dynamics::FreeJoint *freeJoint =
dynamic_cast<dart::dynamics::FreeJoint*>(joint);
if (freeJoint)
{
@@ -243,13 +283,32 @@ void DARTLink::OnPoseChange()
const Eigen::Isometry3d &InvT2 = joint->getTransformFromChildBodyNode();
Eigen::Isometry3d P = Eigen::Isometry3d::Identity();
- if (this->dtBodyNode->getParentBodyNode())
- P = this->dtBodyNode->getParentBodyNode()->getTransform();
+ if (this->dataPtr->dtBodyNode->getParentBodyNode())
+ P = this->dataPtr->dtBodyNode->getParentBodyNode()->getTransform();
Eigen::Isometry3d Q = T1.inverse() * P.inverse() * W * InvT2;
- // Set generalized coordinate and update the transformations only
- freeJoint->setPositions(dart::math::logMap(Q));
+
+ // Convert homogeneous transformation matrix to 6-dimensional generalized
+ // coordinates. There are several ways of conversions. Here is the way of
+ // DART. The orientation part is converted by using logarithm map, which
+ // maps SO(3) to so(3), and it takes the first three components of the
+ // generalized coordinates. On the other hand, the position part just takes
+ // the last three components of the generalized coordinates without any
+ // conversion.
+ Eigen::Vector6d q;
+ q.head<3>() = dart::math::logMap(Q.linear());
+ q.tail<3>() = Q.translation();
+ freeJoint->setPositions(q);
+ // TODO: The above 4 lines will be reduced to single line as:
+ // freeJoint->setPositions(FreeJoint::convertToPositions(Q));
+ // after the following PR is merged:
+ // https://github.com/dartsim/dart/pull/322
+
+ // Update all the transformations of the links in the parent model.
freeJoint->getSkeleton()->computeForwardKinematics(true, false, false);
+ // TODO: This kinematic updating will be done automatically after pull
+ // request (https://github.com/dartsim/dart/pull/319) is merged so that
+ // we don't need this line anymore.
}
else
{
@@ -275,7 +334,7 @@ bool DARTLink::GetEnabled() const
void DARTLink::SetLinearVel(const math::Vector3 &_vel)
{
// DART body node always have its parent joint.
- dart::dynamics::Joint* joint = this->dtBodyNode->getParentJoint();
+ dart::dynamics::Joint *joint = this->dataPtr->dtBodyNode->getParentJoint();
// This is for the case this function called before DARTModel::Init() is
// called.
@@ -296,18 +355,20 @@ void DARTLink::SetLinearVel(const math::Vector3 &_vel)
// Generalized velocities
Eigen::Vector3d genVel = DARTTypes::ConvVec3(_vel);
+ dart::dynamics::BodyNode *dtBodyNode = this->dataPtr->dtBodyNode;
+
// If this link has parent link then subtract the effect of parent link's
// linear and angular velocities
- if (this->dtBodyNode->getParentBodyNode())
+ if (dtBodyNode->getParentBodyNode())
{
// Local transformation from the parent link frame to this link frame
Eigen::Isometry3d T = freeJoint->getLocalTransform();
// Parent link's linear and angular velocities
Eigen::Vector3d parentLinVel =
- this->dtBodyNode->getParentBodyNode()->getBodyLinearVelocity();
+ dtBodyNode->getParentBodyNode()->getBodyLinearVelocity();
Eigen::Vector3d parentAngVel =
- this->dtBodyNode->getParentBodyNode()->getBodyAngularVelocity();
+ dtBodyNode->getParentBodyNode()->getBodyAngularVelocity();
// The effect of the parent link's velocities
Eigen::Vector3d propagatedLinVel =
@@ -319,7 +380,7 @@ void DARTLink::SetLinearVel(const math::Vector3 &_vel)
}
// Rotation matrix from world frame to this link frame
- Eigen::Matrix3d R = this->dtBodyNode->getTransform().linear();
+ Eigen::Matrix3d R = dtBodyNode->getTransform().linear();
// Change the reference frame to world
genVel = R * genVel;
@@ -343,7 +404,7 @@ void DARTLink::SetLinearVel(const math::Vector3 &_vel)
void DARTLink::SetAngularVel(const math::Vector3 &_vel)
{
// DART body node always have its parent joint.
- dart::dynamics::Joint *joint = this->dtBodyNode->getParentJoint();
+ dart::dynamics::Joint *joint = this->dataPtr->dtBodyNode->getParentJoint();
// This is for the case this function called before DARTModel::Init() is
// called.
@@ -364,16 +425,18 @@ void DARTLink::SetAngularVel(const math::Vector3 &_vel)
// Generalized velocities
Eigen::Vector3d genVel = DARTTypes::ConvVec3(_vel);
+ dart::dynamics::BodyNode *dtBodyNode = this->dataPtr->dtBodyNode;
+
// If this link has parent link then subtract the effect of parent link's
// linear and angular velocities
- if (this->dtBodyNode->getParentBodyNode())
+ if (dtBodyNode->getParentBodyNode())
{
// Local transformation from the parent link frame to this link frame
Eigen::Isometry3d T = freeJoint->getLocalTransform();
// Parent link's linear and angular velocities
Eigen::Vector3d parentAngVel =
- this->dtBodyNode->getParentBodyNode()->getBodyAngularVelocity();
+ dtBodyNode->getParentBodyNode()->getBodyAngularVelocity();
// The effect of the parent link's velocities
Eigen::Vector3d propagatedAngVel = T.linear().transpose() * parentAngVel;
@@ -383,7 +446,7 @@ void DARTLink::SetAngularVel(const math::Vector3 &_vel)
}
// Rotation matrix from world frame to this link frame
- Eigen::Matrix3d R = this->dtBodyNode->getTransform().linear();
+ Eigen::Matrix3d R = dtBodyNode->getTransform().linear();
// Change the reference frame to world
genVel = R * genVel;
@@ -407,26 +470,26 @@ void DARTLink::SetAngularVel(const math::Vector3 &_vel)
void DARTLink::SetForce(const math::Vector3 &_force)
{
// DART assume that _force is external force.
- this->dtBodyNode->setExtForce(DARTTypes::ConvVec3(_force));
+ this->dataPtr->dtBodyNode->setExtForce(DARTTypes::ConvVec3(_force));
}
//////////////////////////////////////////////////
void DARTLink::SetTorque(const math::Vector3 &_torque)
{
// DART assume that _torque is external torque.
- this->dtBodyNode->setExtTorque(DARTTypes::ConvVec3(_torque));
+ this->dataPtr->dtBodyNode->setExtTorque(DARTTypes::ConvVec3(_torque));
}
//////////////////////////////////////////////////
void DARTLink::AddForce(const math::Vector3 &_force)
{
- this->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_force));
+ this->dataPtr->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_force));
}
/////////////////////////////////////////////////
void DARTLink::AddRelativeForce(const math::Vector3 &_force)
{
- this->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_force),
+ this->dataPtr->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_force),
Eigen::Vector3d::Zero(),
true, true);
}
@@ -435,7 +498,7 @@ void DARTLink::AddRelativeForce(const math::Vector3 &_force)
void DARTLink::AddForceAtWorldPosition(const math::Vector3 &_force,
const math::Vector3 &_pos)
{
- this->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_pos),
+ this->dataPtr->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_pos),
DARTTypes::ConvVec3(_force),
false, false);
}
@@ -444,21 +507,29 @@ void DARTLink::AddForceAtWorldPosition(const math::Vector3 &_force,
void DARTLink::AddForceAtRelativePosition(const math::Vector3 &_force,
const math::Vector3 &_relpos)
{
- this->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_force),
+ this->dataPtr->dtBodyNode->addExtForce(DARTTypes::ConvVec3(_force),
DARTTypes::ConvVec3(_relpos),
true, true);
}
+//////////////////////////////////////////////////
+void DARTLink::AddLinkForce(const math::Vector3 &/*_force*/,
+ const math::Vector3 &/*_offset*/)
+{
+ gzlog << "DARTLink::AddLinkForce not yet implemented (issue #1477)."
+ << std::endl;
+}
+
/////////////////////////////////////////////////
void DARTLink::AddTorque(const math::Vector3 &_torque)
{
- this->dtBodyNode->addExtTorque(DARTTypes::ConvVec3(_torque));
+ this->dataPtr->dtBodyNode->addExtTorque(DARTTypes::ConvVec3(_torque));
}
/////////////////////////////////////////////////
void DARTLink::AddRelativeTorque(const math::Vector3 &_torque)
{
- this->dtBodyNode->addExtTorque(DARTTypes::ConvVec3(_torque), true);
+ this->dataPtr->dtBodyNode->addExtTorque(DARTTypes::ConvVec3(_torque), true);
}
//////////////////////////////////////////////////
@@ -466,7 +537,8 @@ gazebo::math::Vector3 DARTLink::GetWorldLinearVel(
const math::Vector3 &_offset) const
{
Eigen::Vector3d linVel =
- this->dtBodyNode->getWorldLinearVelocity(DARTTypes::ConvVec3(_offset));
+ this->dataPtr->dtBodyNode->getWorldLinearVelocity(
+ DARTTypes::ConvVec3(_offset));
return DARTTypes::ConvVec3(linVel);
}
@@ -479,9 +551,10 @@ math::Vector3 DARTLink::GetWorldLinearVel(
Eigen::Matrix3d R1 = Eigen::Matrix3d(DARTTypes::ConvQuat(_q));
Eigen::Vector3d worldOffset = R1 * DARTTypes::ConvVec3(_offset);
Eigen::Vector3d bodyOffset =
- this->dtBodyNode->getTransform().linear().transpose() * worldOffset;
+ this->dataPtr->dtBodyNode->getTransform().linear().transpose() *
+ worldOffset;
Eigen::Vector3d linVel =
- this->dtBodyNode->getWorldLinearVelocity(bodyOffset);
+ this->dataPtr->dtBodyNode->getWorldLinearVelocity(bodyOffset);
return DARTTypes::ConvVec3(linVel);
}
@@ -489,7 +562,7 @@ math::Vector3 DARTLink::GetWorldLinearVel(
//////////////////////////////////////////////////
math::Vector3 DARTLink::GetWorldCoGLinearVel() const
{
- Eigen::Vector3d linVel = this->dtBodyNode->getWorldCOMVelocity();
+ Eigen::Vector3d linVel = this->dataPtr->dtBodyNode->getWorldCOMVelocity();
return DARTTypes::ConvVec3(linVel);
}
@@ -497,7 +570,7 @@ math::Vector3 DARTLink::GetWorldCoGLinearVel() const
//////////////////////////////////////////////////
math::Vector3 DARTLink::GetWorldAngularVel() const
{
- Eigen::Vector3d angVel = this->dtBodyNode->getWorldAngularVelocity();
+ Eigen::Vector3d angVel = this->dataPtr->dtBodyNode->getWorldAngularVelocity();
return DARTTypes::ConvVec3(angVel);
}
@@ -505,7 +578,7 @@ math::Vector3 DARTLink::GetWorldAngularVel() const
/////////////////////////////////////////////////
math::Vector3 DARTLink::GetWorldForce() const
{
- Eigen::Vector6d F = this->dtBodyNode->getExternalForceGlobal();
+ Eigen::Vector6d F = this->dataPtr->dtBodyNode->getExternalForceGlobal();
return DARTTypes::ConvVec3(F.tail<3>());
}
@@ -515,10 +588,10 @@ math::Vector3 DARTLink::GetWorldTorque() const
// TODO: Need verification
math::Vector3 torque;
- Eigen::Isometry3d W = this->dtBodyNode->getTransform();
- Eigen::Matrix6d G = this->dtBodyNode->getSpatialInertia();
- Eigen::VectorXd V = this->dtBodyNode->getBodyVelocity();
- Eigen::VectorXd dV = this->dtBodyNode->getBodyAcceleration();
+ Eigen::Isometry3d W = this->dataPtr->dtBodyNode->getTransform();
+ Eigen::Matrix6d G = this->dataPtr->dtBodyNode->getSpatialInertia();
+ Eigen::VectorXd V = this->dataPtr->dtBodyNode->getBodyVelocity();
+ Eigen::VectorXd dV = this->dataPtr->dtBodyNode->getBodyAcceleration();
Eigen::Vector6d F = G * dV - dart::math::dad(V, G * V);
torque = DARTTypes::ConvVec3(W.linear() * F.head<3>());
@@ -530,13 +603,13 @@ math::Vector3 DARTLink::GetWorldTorque() const
void DARTLink::SetGravityMode(bool _mode)
{
this->sdf->GetElement("gravity")->Set(_mode);
- this->dtBodyNode->setGravityMode(_mode);
+ this->dataPtr->dtBodyNode->setGravityMode(_mode);
}
//////////////////////////////////////////////////
bool DARTLink::GetGravityMode() const
{
- return this->dtBodyNode->getGravityMode();
+ return this->dataPtr->dtBodyNode->getGravityMode();
}
//////////////////////////////////////////////////
@@ -544,21 +617,23 @@ void DARTLink::SetSelfCollide(bool _collide)
{
this->sdf->GetElement("self_collide")->Set(_collide);
+ dart::dynamics::BodyNode *dtBodyNode = this->dataPtr->dtBodyNode;
+
// If this function is called before the body node is not added to a skeleton,
// the body node does not have parent skeleton. So we just return here. Self
// collision setting will be done later in DARTModel::Init().
if (dtBodyNode->getSkeleton() == NULL)
return;
- dart::simulation::World *dtWorld = this->dartPhysics->GetDARTWorld();
- dart::dynamics::Skeleton *dtSkeleton = this->dtBodyNode->getSkeleton();
+ dart::simulation::World *dtWorld = this->dataPtr->dartPhysics->GetDARTWorld();
+ dart::dynamics::Skeleton *dtSkeleton = dtBodyNode->getSkeleton();
dart::collision::CollisionDetector *dtCollDet =
dtWorld->getConstraintSolver()->getCollisionDetector();
Link_V links = this->GetModel()->GetLinks();
bool isSkeletonSelfCollidable =
- this->dtBodyNode->getSkeleton()->isEnabledSelfCollisionCheck();
+ dtBodyNode->getSkeleton()->isEnabledSelfCollisionCheck();
if (_collide)
{
@@ -574,14 +649,14 @@ void DARTLink::SetSelfCollide(bool _collide)
dart::dynamics::BodyNode *itdtBodyNode =
boost::dynamic_pointer_cast<DARTLink>(links[i])->GetDARTBodyNode();
- // If this->dtBodyNode and itdtBodyNode are connected then don't
- // enable the pair.
+ // If this->dataPtr->dtBodyNode and itdtBodyNode are connected then
+ // don't enable the pair.
// Please see: https://bitbucket.org/osrf/gazebo/issue/899
- if ((this->dtBodyNode->getParentBodyNode() == itdtBodyNode) ||
- itdtBodyNode->getParentBodyNode() == this->dtBodyNode)
+ if ((dtBodyNode->getParentBodyNode() == itdtBodyNode) ||
+ itdtBodyNode->getParentBodyNode() == this->dataPtr->dtBodyNode)
continue;
- dtCollDet->enablePair(this->dtBodyNode, itdtBodyNode);
+ dtCollDet->enablePair(dtBodyNode, itdtBodyNode);
}
}
}
@@ -602,8 +677,8 @@ void DARTLink::SetSelfCollide(bool _collide)
dart::dynamics::BodyNode *itdtBodyNode2 =
boost::dynamic_pointer_cast<DARTLink>(links[j])->GetDARTBodyNode();
- // If this->dtBodyNode and itdtBodyNode are connected then don't
- // enable the pair.
+ // If this->dataPtr->dtBodyNode and itdtBodyNode are connected then
+ // don't enable the pair.
// Please see: https://bitbucket.org/osrf/gazebo/issue/899
if ((itdtBodyNode1->getParentBodyNode() == itdtBodyNode2) ||
itdtBodyNode2->getParentBodyNode() == itdtBodyNode1)
@@ -627,7 +702,7 @@ void DARTLink::SetSelfCollide(bool _collide)
{
dart::dynamics::BodyNode *itdtBodyNode =
boost::dynamic_pointer_cast<DARTLink>(links[i])->GetDARTBodyNode();
- dtCollDet->disablePair(this->dtBodyNode, itdtBodyNode);
+ dtCollDet->disablePair(dtBodyNode, itdtBodyNode);
}
}
}
@@ -686,23 +761,27 @@ void DARTLink::SetAutoDisable(bool /*_disable*/)
//////////////////////////////////////////////////
void DARTLink::SetLinkStatic(bool _static)
{
- if (_static == staticLink)
+ if (_static == this->dataPtr->staticLink)
return;
if (_static == true)
{
// Add weld joint constraint to DART
- this->weldJointConst =
- new dart::constraint::WeldJointConstraint(this->dtBodyNode);
- GetDARTWorld()->getConstraintSolver()->addConstraint(weldJointConst);
+ this->dataPtr->dtWeldJointConst =
+ new dart::constraint::WeldJointConstraint(this->dataPtr->dtBodyNode);
+ GetDARTWorld()->getConstraintSolver()->addConstraint(
+ this->dataPtr->dtWeldJointConst);
}
else
{
// Remove ball and revolute joint constraints from DART
- GetDARTWorld()->getConstraintSolver()->removeConstraint(weldJointConst);
+ GetDARTWorld()->getConstraintSolver()->removeConstraint(
+ this->dataPtr->dtWeldJointConst);
+ delete this->dataPtr->dtWeldJointConst;
+ this->dataPtr->dtWeldJointConst = NULL;
}
- staticLink = _static;
+ this->dataPtr->staticLink = _static;
}
//////////////////////////////////////////////////
@@ -711,7 +790,7 @@ void DARTLink::updateDirtyPoseFromDARTTransformation()
// Step 1: get dart body's transformation
// Step 2: set gazebo link's pose using the transformation
math::Pose newPose = DARTTypes::ConvPose(
- this->dtBodyNode->getTransform());
+ this->dataPtr->dtBodyNode->getTransform());
// Set the new pose to this link
this->dirtyPose = newPose;
@@ -743,17 +822,17 @@ DARTModelPtr DARTLink::GetDARTModel() const
//////////////////////////////////////////////////
dart::dynamics::BodyNode *DARTLink::GetDARTBodyNode() const
{
- return dtBodyNode;
+ return this->dataPtr->dtBodyNode;
}
//////////////////////////////////////////////////
void DARTLink::SetDARTParentJoint(DARTJointPtr _dartParentJoint)
{
- dartParentJoint = _dartParentJoint;
+ this->dataPtr->dartParentJoint = _dartParentJoint;
}
//////////////////////////////////////////////////
void DARTLink::AddDARTChildJoint(DARTJointPtr _dartChildJoint)
{
- dartChildJoints.push_back(_dartChildJoint);
+ this->dataPtr->dartChildJoints.push_back(_dartChildJoint);
}
diff --git a/gazebo/physics/dart/DARTLink.hh b/gazebo/physics/dart/DARTLink.hh
index 3e757b5..c92023c 100644
--- a/gazebo/physics/dart/DARTLink.hh
+++ b/gazebo/physics/dart/DARTLink.hh
@@ -30,13 +30,16 @@ namespace gazebo
{
namespace physics
{
+ /// Forward declare private data class
+ class DARTLinkPrivate;
+
/// \ingroup gazebo_physics
/// \addtogroup gazebo_physics_dart DART Physics
/// \brief dart physics engine wrapper
/// \{
/// \brief DART Link class
- class GAZEBO_VISIBLE DARTLink : public Link
+ class GZ_PHYSICS_VISIBLE DARTLink : public Link
{
/// \brief Constructor
public: explicit DARTLink(EntityPtr _parent);
@@ -90,6 +93,10 @@ namespace gazebo
const math::Vector3 &_relpos);
// Documentation inherited
+ public: virtual void AddLinkForce(const math::Vector3 &_force,
+ const math::Vector3 &_offset = math::Vector3::Zero);
+
+ // Documentation inherited
public: virtual void AddTorque(const math::Vector3 &_torque);
// Documentation inherited
@@ -172,23 +179,9 @@ namespace gazebo
/// \param[in] _dartChildJoint Pointer to the child joint.
public: void AddDARTChildJoint(DARTJointPtr _dartChildJoint);
- /// \brief Pointer to the DART physics engine.
- private: DARTPhysicsPtr dartPhysics;
-
- /// \brief Pointer to the DART BodyNode.
- private: dart::dynamics::BodyNode *dtBodyNode;
-
- /// \brief Pointer to the parent joint.
- private: DARTJointPtr dartParentJoint;
-
- /// \brief List of pointers to the child joints.
- private: std::vector<DARTJointPtr> dartChildJoints;
-
- /// \brief If true, freeze link to world (inertial) frame.
- private: bool staticLink;
-
- /// \brief Weld joint constraint for SetLinkStatic()
- private: dart::constraint::WeldJointConstraint *weldJointConst;
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTLinkPrivate *dataPtr;
};
/// \}
}
diff --git a/gazebo/physics/dart/DARTLinkPrivate.hh b/gazebo/physics/dart/DARTLinkPrivate.hh
new file mode 100644
index 0000000..47e05d1
--- /dev/null
+++ b/gazebo/physics/dart/DARTLinkPrivate.hh
@@ -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.
+ *
+*/
+
+#ifndef _GAZEBO_DARTLINK_PRIVATE_HH_
+#define _GAZEBO_DARTLINK_PRIVATE_HH_
+
+#include <vector>
+
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTLink
+ class DARTLinkPrivate
+ {
+ /// \brief Constructor
+ public: DARTLinkPrivate()
+ : dartPhysics(NULL),
+ dtBodyNode(NULL),
+ dartParentJoint(NULL),
+ dartChildJoints {},
+ staticLink(false),
+ dtWeldJointConst(NULL)
+ {
+ }
+
+ /// \brief Default destructor
+ public: ~DARTLinkPrivate()
+ {
+ // We don't need to delete dtBodyNode because skeletone will delete
+ // dtBodyNode if it is registered to the skeletone.
+
+ delete dtWeldJointConst;
+ }
+
+ /// \brief Pointer to the DART physics engine.
+ public: DARTPhysicsPtr dartPhysics;
+
+ /// \brief Pointer to the DART BodyNode.
+ public: dart::dynamics::BodyNode *dtBodyNode;
+
+ /// \brief Pointer to the parent joint.
+ public: DARTJointPtr dartParentJoint;
+
+ /// \brief List of pointers to the child joints.
+ public: std::vector<DARTJointPtr> dartChildJoints;
+
+ /// \brief If true, freeze link to world (inertial) frame.
+ public: bool staticLink;
+
+ /// \brief Weld joint constraint for SetLinkStatic()
+ public: dart::constraint::WeldJointConstraint *dtWeldJointConst;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTMesh.cc b/gazebo/physics/dart/DARTMesh.cc
index c1d6d51..3108988 100644
--- a/gazebo/physics/dart/DARTMesh.cc
+++ b/gazebo/physics/dart/DARTMesh.cc
@@ -22,11 +22,17 @@
#include "gazebo/physics/dart/DARTPhysics.hh"
#include "gazebo/physics/dart/DARTMesh.hh"
+#include "gazebo/physics/dart/DARTMeshPrivate.hh"
+
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
-// Constructor of aiScene is missing so we define it here.
+// Constructor of aiScene is missing so we define it here. This is temporary
+// workaround. For further discussion, please see:
+// https://github.com/dartsim/dart/issues/451
+// https://github.com/dartsim/dart/issues/452
+// https://github.com/dartsim/dart/issues/453
aiScene::aiScene()
{
mFlags = 0;
@@ -46,13 +52,14 @@ aiScene::aiScene()
}
//////////////////////////////////////////////////
-DARTMesh::DARTMesh()
+DARTMesh::DARTMesh() : dataPtr(new DARTMeshPrivate())
{
}
//////////////////////////////////////////////////
DARTMesh::~DARTMesh()
{
+ delete this->dataPtr;
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/dart/DARTMesh.hh b/gazebo/physics/dart/DARTMesh.hh
index 9f6ffe1..cc1f35c 100644
--- a/gazebo/physics/dart/DARTMesh.hh
+++ b/gazebo/physics/dart/DARTMesh.hh
@@ -29,8 +29,11 @@ namespace gazebo
/// \addtogroup gazebo_physics_dart Dart Physics
/// \{
+ /// Forward declare private data class
+ class DARTMeshPrivate;
+
/// \brief Triangle mesh collision helper class
- class GAZEBO_VISIBLE DARTMesh
+ class GZ_PHYSICS_VISIBLE DARTMesh
{
/// \brief Constructor
public: DARTMesh();
@@ -64,6 +67,10 @@ namespace gazebo
unsigned int _numVertices, unsigned int _numIndices,
DARTCollisionPtr _collision,
const math::Vector3 &_scale);
+
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTMeshPrivate *dataPtr;
};
/// \}
}
diff --git a/gazebo/physics/dart/DARTMeshPrivate.hh b/gazebo/physics/dart/DARTMeshPrivate.hh
new file mode 100644
index 0000000..c9495ab
--- /dev/null
+++ b/gazebo/physics/dart/DARTMeshPrivate.hh
@@ -0,0 +1,40 @@
+/*
+ * 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_DARTMESH_PRIVATE_HH_
+#define _GAZEBO_DARTMESH_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTMesh
+ class DARTMeshPrivate
+ {
+ /// \brief Constructor
+ public: DARTMeshPrivate() = default;
+
+ /// \brief Default destructor
+ public: ~DARTMeshPrivate() = default;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTMeshShape.cc b/gazebo/physics/dart/DARTMeshShape.cc
index 9b9c04c..ab86295 100644
--- a/gazebo/physics/dart/DARTMeshShape.cc
+++ b/gazebo/physics/dart/DARTMeshShape.cc
@@ -25,19 +25,22 @@
#include "gazebo/physics/dart/DARTMeshShape.hh"
#include "gazebo/physics/dart/DARTPhysics.hh"
+#include "gazebo/physics/dart/DARTMeshShapePrivate.hh"
+
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
-DARTMeshShape::DARTMeshShape(CollisionPtr _parent) : MeshShape(_parent)
+DARTMeshShape::DARTMeshShape(CollisionPtr _parent)
+ : MeshShape(_parent),
+ dataPtr(new DARTMeshShapePrivate())
{
- this->dartMesh = new DARTMesh();
}
//////////////////////////////////////////////////
DARTMeshShape::~DARTMeshShape()
{
- delete this->dartMesh;
+ delete this->dataPtr;
}
//////////////////////////////////////////////////
@@ -59,13 +62,13 @@ void DARTMeshShape::Init()
if (this->submesh)
{
- this->dartMesh->Init(this->submesh,
+ this->dataPtr->dartMesh->Init(this->submesh,
boost::dynamic_pointer_cast<DARTCollision>(this->collisionParent),
this->sdf->Get<math::Vector3>("scale"));
}
else
{
- this->dartMesh->Init(this->mesh,
+ this->dataPtr->dartMesh->Init(this->mesh,
boost::dynamic_pointer_cast<DARTCollision>(this->collisionParent),
this->sdf->Get<math::Vector3>("scale"));
}
diff --git a/gazebo/physics/dart/DARTMeshShape.hh b/gazebo/physics/dart/DARTMeshShape.hh
index b555408..ecb4e65 100644
--- a/gazebo/physics/dart/DARTMeshShape.hh
+++ b/gazebo/physics/dart/DARTMeshShape.hh
@@ -25,10 +25,11 @@ namespace gazebo
{
namespace physics
{
- class DARTMesh;
+ /// Forward declare private data class
+ class DARTMeshShapePrivate;
/// \brief Triangle mesh collision.
- class GAZEBO_VISIBLE DARTMeshShape : public MeshShape
+ class GZ_PHYSICS_VISIBLE DARTMeshShape : public MeshShape
{
/// \brief Constructor.
/// \param[in] _parent Parent collision object.
@@ -46,8 +47,9 @@ namespace gazebo
// Documentation inherited
public: virtual void Update();
- /// \brief DART collision mesh helper class
- private: DARTMesh *dartMesh;
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTMeshShapePrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/dart/DARTMeshShapePrivate.hh b/gazebo/physics/dart/DARTMeshShapePrivate.hh
new file mode 100644
index 0000000..832c29f
--- /dev/null
+++ b/gazebo/physics/dart/DARTMeshShapePrivate.hh
@@ -0,0 +1,53 @@
+/*
+ * 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_DARTMESHSHAPE_PRIVATE_HH_
+#define _GAZEBO_DARTMESHSHAPE_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTMesh.hh"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTMeshShape
+ class DARTMeshShapePrivate
+ {
+ /// \brief Constructor
+ public: DARTMeshShapePrivate()
+ : dartMesh(new DARTMesh())
+ {
+ }
+
+ /// \brief Default destructor
+ public: ~DARTMeshShapePrivate()
+ {
+ delete this->dartMesh;
+ }
+
+ /// \brief Disabled copy constructor
+ public: DARTMeshShapePrivate(const DARTMeshShapePrivate&) = delete;
+
+ /// \brief DART collision mesh helper class
+ public: DARTMesh *dartMesh;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTModel.cc b/gazebo/physics/dart/DARTModel.cc
index 38e8218..2d91d2b 100644
--- a/gazebo/physics/dart/DARTModel.cc
+++ b/gazebo/physics/dart/DARTModel.cc
@@ -23,27 +23,28 @@
#include "gazebo/physics/dart/DARTLink.hh"
#include "gazebo/physics/dart/DARTModel.hh"
+#include "gazebo/physics/dart/DARTModelPrivate.hh"
+
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
DARTModel::DARTModel(BasePtr _parent)
- : Model(_parent), dtSkeleton(NULL)
+ : Model(_parent), dataPtr(new DARTModelPrivate())
{
}
//////////////////////////////////////////////////
DARTModel::~DARTModel()
{
- if (dtSkeleton)
- delete dtSkeleton;
+ delete this->dataPtr;
}
//////////////////////////////////////////////////
void DARTModel::Load(sdf::ElementPtr _sdf)
{
// create skeleton of DART
- this->dtSkeleton = new dart::dynamics::Skeleton();
+ this->dataPtr->dtSkeleton = new dart::dynamics::Skeleton();
Model::Load(_sdf);
}
@@ -56,11 +57,11 @@ void DARTModel::Init()
//----------------------------------------------
// Name
std::string modelName = this->GetName();
- this->dtSkeleton->setName(modelName.c_str());
+ this->dataPtr->dtSkeleton->setName(modelName.c_str());
//----------------------------------------------
// Static
- this->dtSkeleton->setMobile(!this->IsStatic());
+ this->dataPtr->dtSkeleton->setMobile(!this->IsStatic());
//----------------------------------------------
// Check if this link is free floating body
@@ -84,11 +85,11 @@ void DARTModel::Init()
dtBodyNode->setParentJoint(newFreeJoint);
}
- dtSkeleton->addBodyNode(dtBodyNode);
+ this->dataPtr->dtSkeleton->addBodyNode(dtBodyNode);
}
// Add the skeleton to the world
- this->GetDARTWorld()->addSkeleton(dtSkeleton);
+ this->GetDARTWorld()->addSkeleton(this->dataPtr->dtSkeleton);
// Self collision
// Note: This process should be done after this skeleton is added to the
@@ -117,7 +118,7 @@ void DARTModel::Init()
// collidable.
if (hasPairOfSelfCollidableLinks)
{
- this->dtSkeleton->enableSelfCollision();
+ this->dataPtr->dtSkeleton->enableSelfCollision();
dart::simulation::World *dtWorld = this->GetDARTPhysics()->GetDARTWorld();
dart::collision::CollisionDetector *dtCollDet =
@@ -170,29 +171,29 @@ void DARTModel::Fini()
//////////////////////////////////////////////////
void DARTModel::BackupState()
{
- dtConfig = this->dtSkeleton->getPositions();
- dtVelocity = this->dtSkeleton->getVelocities();
+ this->dataPtr->dtConfig = this->dataPtr->dtSkeleton->getPositions();
+ this->dataPtr->dtVelocity = this->dataPtr->dtSkeleton->getVelocities();
}
//////////////////////////////////////////////////
void DARTModel::RestoreState()
{
- GZ_ASSERT(static_cast<size_t>(dtConfig.size()) ==
- this->dtSkeleton->getNumDofs(),
+ GZ_ASSERT(static_cast<size_t>(this->dataPtr->dtConfig.size()) ==
+ this->dataPtr->dtSkeleton->getNumDofs(),
"Cannot RestoreState, invalid size");
- GZ_ASSERT(static_cast<size_t>(dtVelocity.size()) ==
- this->dtSkeleton->getNumDofs(),
+ GZ_ASSERT(static_cast<size_t>(this->dataPtr->dtVelocity.size()) ==
+ this->dataPtr->dtSkeleton->getNumDofs(),
"Cannot RestoreState, invalid size");
- this->dtSkeleton->setPositions(dtConfig);
- this->dtSkeleton->setVelocities(dtVelocity);
- this->dtSkeleton->computeForwardKinematics(true, true, false);
+ this->dataPtr->dtSkeleton->setPositions(this->dataPtr->dtConfig);
+ this->dataPtr->dtSkeleton->setVelocities(this->dataPtr->dtVelocity);
+ this->dataPtr->dtSkeleton->computeForwardKinematics(true, true, false);
}
//////////////////////////////////////////////////
dart::dynamics::Skeleton *DARTModel::GetDARTSkeleton()
{
- return dtSkeleton;
+ return this->dataPtr->dtSkeleton;
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/dart/DARTModel.hh b/gazebo/physics/dart/DARTModel.hh
index 9694420..4da53bc 100644
--- a/gazebo/physics/dart/DARTModel.hh
+++ b/gazebo/physics/dart/DARTModel.hh
@@ -27,6 +27,9 @@ namespace gazebo
{
namespace physics
{
+ /// Forward declare private data class
+ class DARTModelPrivate;
+
/// \ingroup gazebo_physics
/// \addtogroup gazebo_physics_dart DART Physics
/// \brief dart physics engine wrapper
@@ -34,7 +37,7 @@ namespace gazebo
/// \class DARTModel
/// \brief DART model class
- class GAZEBO_VISIBLE DARTModel : public Model
+ class GZ_PHYSICS_VISIBLE DARTModel : public Model
{
/// \brief Constructor.
/// \param[in] _parent Parent object.
@@ -70,14 +73,9 @@ namespace gazebo
/// \brief
public: dart::simulation::World *GetDARTWorld(void) const;
- /// \brief
- protected: dart::dynamics::Skeleton *dtSkeleton;
-
- /// \brief
- protected: Eigen::VectorXd dtConfig;
-
- /// \brief
- protected: Eigen::VectorXd dtVelocity;
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTModelPrivate *dataPtr;
};
/// \}
}
diff --git a/gazebo/physics/dart/DARTModelPrivate.hh b/gazebo/physics/dart/DARTModelPrivate.hh
new file mode 100644
index 0000000..876301a
--- /dev/null
+++ b/gazebo/physics/dart/DARTModelPrivate.hh
@@ -0,0 +1,57 @@
+/*
+ * 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_DARTMODEL_PRIVATE_HH_
+#define _GAZEBO_DARTMODEL_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTModel
+ class DARTModelPrivate
+ {
+ /// \brief Constructor
+ public: DARTModelPrivate()
+ : dtSkeleton(NULL),
+ dtConfig(),
+ dtVelocity()
+ {
+ }
+
+ /// \brief Default destructor
+ public: ~DARTModelPrivate()
+ {
+ // We don't need to delete dtSkeleton because world will delete
+ // dtSkeleton if it is registered to the world.
+ }
+
+ /// \brief Pointer to DART Skeleton
+ public: dart::dynamics::Skeleton *dtSkeleton;
+
+ /// \brief Generalized positions
+ public: Eigen::VectorXd dtConfig;
+
+ /// \brief Generalized velocities
+ public: Eigen::VectorXd dtVelocity;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTMultiRayShape.cc b/gazebo/physics/dart/DARTMultiRayShape.cc
index 2271041..e70f00d 100644
--- a/gazebo/physics/dart/DARTMultiRayShape.cc
+++ b/gazebo/physics/dart/DARTMultiRayShape.cc
@@ -25,22 +25,23 @@
#include "gazebo/physics/dart/DARTRayShape.hh"
#include "gazebo/physics/dart/DARTMultiRayShape.hh"
+#include "gazebo/physics/dart/DARTMultiRayShapePrivate.hh"
+
using namespace gazebo;
using namespace physics;
-
//////////////////////////////////////////////////
DARTMultiRayShape::DARTMultiRayShape(CollisionPtr _parent)
- : MultiRayShape(_parent)
+ : MultiRayShape(_parent),
+ dataPtr(new DARTMultiRayShapePrivate())
{
this->SetName("DART_multiray_shape");
- this->physicsEngine = boost::static_pointer_cast<DARTPhysics>(
- this->collisionParent->GetWorld()->GetPhysicsEngine());
}
//////////////////////////////////////////////////
DARTMultiRayShape::~DARTMultiRayShape()
{
+ delete this->dataPtr;
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/dart/DARTMultiRayShape.hh b/gazebo/physics/dart/DARTMultiRayShape.hh
index 4aa03b0..61b8f63 100644
--- a/gazebo/physics/dart/DARTMultiRayShape.hh
+++ b/gazebo/physics/dart/DARTMultiRayShape.hh
@@ -30,8 +30,11 @@ namespace gazebo
/// \addtogroup gazebo_physics_bullet Bullet Physics
/// \{
+ /// Forward declare private data class
+ class DARTMultiRayShapePrivate;
+
/// \brief DART specific version of MultiRayShape
- class GAZEBO_VISIBLE DARTMultiRayShape : public MultiRayShape
+ class GZ_PHYSICS_VISIBLE DARTMultiRayShape : public MultiRayShape
{
/// \brief Constructor.
/// \param[in] _parent Parent Collision.
@@ -49,8 +52,9 @@ namespace gazebo
protected: void AddRay(const math::Vector3 &_start,
const math::Vector3 &_end);
- /// \brief Pointer to the DART physics engine.
- private: DARTPhysicsPtr physicsEngine;
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTMultiRayShapePrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/dart/DARTMultiRayShapePrivate.hh b/gazebo/physics/dart/DARTMultiRayShapePrivate.hh
new file mode 100644
index 0000000..0a5945b
--- /dev/null
+++ b/gazebo/physics/dart/DARTMultiRayShapePrivate.hh
@@ -0,0 +1,40 @@
+/*
+ * 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_DARTMULTIRAYSHAPE_PRIVATE_HH_
+#define _GAZEBO_DARTMULTIRAYSHAPE_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTMultiRayShape
+ class DARTMultiRayShapePrivate
+ {
+ /// \brief Constructor
+ public: DARTMultiRayShapePrivate() = default;
+
+ /// \brief Default destructor
+ public: ~DARTMultiRayShapePrivate() = default;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTPhysics.cc b/gazebo/physics/dart/DARTPhysics.cc
index c81d967..3be7a70 100644
--- a/gazebo/physics/dart/DARTPhysics.cc
+++ b/gazebo/physics/dart/DARTPhysics.cc
@@ -38,6 +38,7 @@
#include "gazebo/physics/dart/DARTSliderJoint.hh"
#include "gazebo/physics/dart/DARTBallJoint.hh"
#include "gazebo/physics/dart/DARTUniversalJoint.hh"
+#include "gazebo/physics/dart/DARTFixedJoint.hh"
#include "gazebo/physics/dart/DARTRayShape.hh"
#include "gazebo/physics/dart/DARTBoxShape.hh"
@@ -54,6 +55,8 @@
#include "gazebo/physics/dart/DARTPhysics.hh"
+#include "gazebo/physics/dart/DARTPhysicsPrivate.hh"
+
using namespace gazebo;
using namespace physics;
@@ -61,24 +64,14 @@ GZ_REGISTER_PHYSICS_ENGINE("dart", DARTPhysics)
//////////////////////////////////////////////////
DARTPhysics::DARTPhysics(WorldPtr _world)
- : PhysicsEngine(_world)
+ : PhysicsEngine(_world), dataPtr(new DARTPhysicsPrivate())
{
- this->dtWorld = new dart::simulation::World;
-// this->dtWorld->getConstraintSolver()->setCollisionDetector(
-// new dart::collision::DARTCollisionDetector());
-// this->dtWorld->getConstraintHandler()->setAllowablePenetration(1e-6);
-// this->dtWorld->getConstraintHandler()->setMaxReducingPenetrationVelocity(
-// 0.01);
-// this->dtWorld->getConstraintHandler()->setAllowableJointViolation(
-// DART_TO_RADIAN*1e-1);
-// this->dtWorld->getConstraintHandler()->setMaxReducingJointViolationVelocity(
-// DART_TO_RADIAN*1e-0);
}
//////////////////////////////////////////////////
DARTPhysics::~DARTPhysics()
{
- delete this->dtWorld;
+ delete this->dataPtr;
}
//////////////////////////////////////////////////
@@ -91,7 +84,7 @@ void DARTPhysics::Load(sdf::ElementPtr _sdf)
// ODEPhysics checks this, so we will too.
if (g == math::Vector3(0, 0, 0))
gzwarn << "Gravity vector is (0, 0, 0). Objects will float.\n";
- this->dtWorld->setGravity(Eigen::Vector3d(g.x, g.y, g.z));
+ this->dataPtr->dtWorld->setGravity(Eigen::Vector3d(g.x, g.y, g.z));
// Time step
// double timeStep = this->sdf->GetValueDouble("time_step");
@@ -144,7 +137,7 @@ void DARTPhysics::UpdateCollision()
this->contactManager->ResetCount();
dart::constraint::ConstraintSolver *dtConstraintSolver =
- this->dtWorld->getConstraintSolver();
+ this->dataPtr->dtWorld->getConstraintSolver();
dart::collision::CollisionDetector *dtCollisionDetector =
dtConstraintSolver->getCollisionDetector();
int numContacts = dtCollisionDetector->getNumContacts();
@@ -231,8 +224,8 @@ void DARTPhysics::UpdatePhysics()
// common::Time currTime = this->world->GetRealTime();
- this->dtWorld->setTimeStep(this->maxStepSize);
- this->dtWorld->step();
+ this->dataPtr->dtWorld->setTimeStep(this->maxStepSize);
+ this->dataPtr->dtWorld->step();
// Update all the transformation of DART's links to gazebo's links
// TODO: How to visit all the links in the world?
@@ -358,6 +351,8 @@ JointPtr DARTPhysics::CreateJoint(const std::string &_type, ModelPtr _parent)
joint.reset(new DARTBallJoint(_parent));
else if (_type == "universal")
joint.reset(new DARTUniversalJoint(_parent));
+ else if (_type == "fixed")
+ joint.reset(new DARTFixedJoint(_parent));
else
gzerr << "Unable to create joint of type[" << _type << "]";
@@ -368,7 +363,7 @@ JointPtr DARTPhysics::CreateJoint(const std::string &_type, ModelPtr _parent)
void DARTPhysics::SetGravity(const gazebo::math::Vector3 &_gravity)
{
this->sdf->GetElement("gravity")->Set(_gravity);
- this->dtWorld->setGravity(
+ this->dataPtr->dtWorld->setGravity(
Eigen::Vector3d(_gravity.x, _gravity.y, _gravity.z));
}
@@ -381,92 +376,69 @@ void DARTPhysics::DebugPrint() const
//////////////////////////////////////////////////
boost::any DARTPhysics::GetParam(const std::string &_key) const
{
- if (_key == "max_step_size")
+ boost::any value;
+ this->GetParam(_key, value);
+ return value;
+}
+
+//////////////////////////////////////////////////
+bool DARTPhysics::GetParam(const std::string &_key, boost::any &_value) const
+{
+ if (!this->sdf->HasElement("dart"))
{
- return this->GetMaxStepSize();
+ return PhysicsEngine::GetParam(_key, _value);
}
-
sdf::ElementPtr dartElem = this->sdf->GetElement("dart");
// physics dart element not yet added to sdformat
// GZ_ASSERT(dartElem != NULL, "DART SDF element does not exist");
- if (dartElem == NULL)
- {
- gzerr << "DART SDF element not found"
- << ", unable to get param ["
- << _key << "]"
- << std::endl;
- return 0;
- }
if (_key == "max_contacts")
{
- return dartElem->GetElement("max_contacts")->Get<int>();
+ _value = dartElem->GetElement("max_contacts")->Get<int>();
}
else if (_key == "min_step_size")
{
- return dartElem->GetElement("solver")->Get<double>("min_step_size");
+ _value = dartElem->GetElement("solver")->Get<double>("min_step_size");
}
else
{
- gzwarn << _key << " is not supported in dart" << std::endl;
- return 0;
+ return PhysicsEngine::GetParam(_key, _value);
}
- gzerr << "We should not be here, something is wrong." << std::endl;
- return 0;
+ return true;
}
//////////////////////////////////////////////////
bool DARTPhysics::SetParam(const std::string &_key, const boost::any &_value)
{
/// \TODO fill this out, see issue #1115
- if (_key == "max_contacts")
+ try
{
- int value;
- try
- {
- value = boost::any_cast<int>(_value);
- }
- catch(const boost::bad_any_cast &e)
+ if (_key == "max_contacts")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ int value = boost::any_cast<int>(_value);
+ gzerr << "Setting [" << _key << "] in DART to [" << value
+ << "] not yet supported.\n";
}
- gzerr << "Setting [" << _key << "] in DART to [" << value
- << "] not yet supported.\n";
- }
- else if (_key == "min_step_size")
- {
- double value;
- try
+ else if (_key == "min_step_size")
{
- value = boost::any_cast<double>(_value);
+ double value = boost::any_cast<double>(_value);
+ gzerr << "Setting [" << _key << "] in DART to [" << value
+ << "] not yet supported.\n";
}
- catch(const boost::bad_any_cast &e)
- {
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
- }
- gzerr << "Setting [" << _key << "] in DART to [" << value
- << "] not yet supported.\n";
- }
- else if (_key == "max_step_size")
- {
- double value;
- try
- {
- value = boost::any_cast<double>(_value);
- }
- catch(const boost::bad_any_cast &e)
+ else
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ if (_key == "max_step_size")
+ {
+ this->dataPtr->dtWorld->setTimeStep(boost::any_cast<double>(_value));
+ }
+ return PhysicsEngine::SetParam(_key, _value);
}
- this->dtWorld->setTimeStep(value);
}
- else
+ catch(boost::bad_any_cast &e)
{
- gzwarn << _key << " is not supported in DART" << std::endl;
+ gzerr << "DARTPhysics::SetParam(" << _key << ") boost::any_cast error: "
+ << e.what() << std::endl;
return false;
}
return true;
@@ -475,7 +447,7 @@ bool DARTPhysics::SetParam(const std::string &_key, const boost::any &_value)
//////////////////////////////////////////////////
dart::simulation::World *DARTPhysics::GetDARTWorld()
{
- return this->dtWorld;
+ return this->dataPtr->dtWorld;
}
//////////////////////////////////////////////////
@@ -491,7 +463,10 @@ void DARTPhysics::OnRequest(ConstRequestPtr &_msg)
{
msgs::Physics physicsMsg;
physicsMsg.set_type(msgs::Physics::DART);
- physicsMsg.mutable_gravity()->CopyFrom(msgs::Convert(this->GetGravity()));
+ physicsMsg.mutable_gravity()->CopyFrom(
+ msgs::Convert(this->GetGravity().Ign()));
+ physicsMsg.mutable_magnetic_field()->CopyFrom(
+ msgs::Convert(this->MagneticField()));
physicsMsg.set_enable_physics(this->world->GetEnablePhysicsEngine());
physicsMsg.set_real_time_update_rate(this->realTimeUpdateRate);
physicsMsg.set_real_time_factor(this->targetRealTimeFactor);
@@ -506,11 +481,16 @@ void DARTPhysics::OnRequest(ConstRequestPtr &_msg)
//////////////////////////////////////////////////
void DARTPhysics::OnPhysicsMsg(ConstPhysicsPtr& _msg)
{
+ // Parent class handles many generic parameters
+ // This should be done first so that the profile settings
+ // can be over-ridden by other message parameters.
+ PhysicsEngine::OnPhysicsMsg(_msg);
+
if (_msg->has_enable_physics())
this->world->EnablePhysicsEngine(_msg->enable_physics());
if (_msg->has_gravity())
- this->SetGravity(msgs::Convert(_msg->gravity()));
+ this->SetGravity(msgs::ConvertIgn(_msg->gravity()));
if (_msg->has_real_time_factor())
this->SetTargetRealTimeFactor(_msg->real_time_factor());
@@ -527,9 +507,6 @@ void DARTPhysics::OnPhysicsMsg(ConstPhysicsPtr& _msg)
/// Make sure all models get at least on update cycle.
this->world->EnableAllModels();
-
- // Parent class handles many generic parameters
- PhysicsEngine::OnPhysicsMsg(_msg);
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/dart/DARTPhysics.hh b/gazebo/physics/dart/DARTPhysics.hh
index 2f91a75..ddda1ba 100644
--- a/gazebo/physics/dart/DARTPhysics.hh
+++ b/gazebo/physics/dart/DARTPhysics.hh
@@ -35,12 +35,15 @@ namespace gazebo
{
namespace physics
{
+ /// Forward declare private data class.
+ class DARTPhysicsPrivate;
+
/// \ingroup gazebo_physics
/// \addtogroup gazebo_physics_dart DART Physics
/// \{
/// \brief DART physics engine
- class GAZEBO_VISIBLE DARTPhysics : public PhysicsEngine
+ class GZ_PHYSICS_VISIBLE DARTPhysics : public PhysicsEngine
{
/// \enum DARTParam
/// \brief DART physics parameter types.
@@ -132,6 +135,10 @@ namespace gazebo
public: virtual boost::any GetParam(const std::string &_key) const;
// Documentation inherited
+ public: virtual bool GetParam(const std::string &_key,
+ boost::any &_value) const;
+
+ // Documentation inherited
public: virtual bool SetParam(const std::string &_key,
const boost::any &_value);
@@ -151,8 +158,9 @@ namespace gazebo
private: DARTLinkPtr FindDARTLink(
const dart::dynamics::BodyNode *_dtBodyNode);
- /// \brief Pointer to DART World associated with this DART Physics.
- private: dart::simulation::World *dtWorld;
+ /// \internal
+ /// \brief Pointer to private data.
+ private: DARTPhysicsPrivate *dataPtr;
};
/// \}
diff --git a/gazebo/physics/dart/DARTPhysicsPrivate.hh b/gazebo/physics/dart/DARTPhysicsPrivate.hh
new file mode 100644
index 0000000..40ba2ff
--- /dev/null
+++ b/gazebo/physics/dart/DARTPhysicsPrivate.hh
@@ -0,0 +1,51 @@
+/*
+ * 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_DARTPHYSICS_PRIVATE_HH_
+#define _GAZEBO_DARTPHYSICS_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTPhysics
+ class DARTPhysicsPrivate
+ {
+ /// \brief Constructor
+ public: DARTPhysicsPrivate()
+ : dtWorld(new dart::simulation::World())
+ {
+ }
+
+ /// \brief Default destructor
+ public: ~DARTPhysicsPrivate()
+ {
+ delete dtWorld;
+ }
+
+ /// \brief Disabled copy constructor
+ public: DARTPhysicsPrivate(const DARTPhysicsPrivate&) = delete;
+
+ /// \brief Pointer to DART World associated with this DART Physics.
+ public: dart::simulation::World *dtWorld;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTPlaneShape.cc b/gazebo/physics/dart/DARTPlaneShape.cc
new file mode 100644
index 0000000..8e483fd
--- /dev/null
+++ b/gazebo/physics/dart/DARTPlaneShape.cc
@@ -0,0 +1,66 @@
+/*
+ * 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 "gazebo/common/Console.hh"
+#include "gazebo/physics/dart/DARTPhysics.hh"
+#include "gazebo/physics/dart/DARTCollision.hh"
+#include "gazebo/physics/dart/DARTPlaneShape.hh"
+#include "gazebo/util/system.hh"
+
+#include "gazebo/physics/dart/DARTPlaneShapePrivate.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+DARTPlaneShape::DARTPlaneShape(CollisionPtr _parent)
+ : PlaneShape(_parent),
+ dataPtr(new DARTPlaneShapePrivate())
+{
+}
+
+//////////////////////////////////////////////////
+DARTPlaneShape::~DARTPlaneShape()
+{
+ delete this->dataPtr;
+}
+
+//////////////////////////////////////////////////
+void DARTPlaneShape::CreatePlane()
+{
+ PlaneShape::CreatePlane();
+
+ DARTCollisionPtr dartCollisionParent =
+ boost::dynamic_pointer_cast<DARTCollision>(this->collisionParent);
+
+ // math::Vector3 n = this->GetNormal();
+
+ dart::dynamics::BodyNode *dtBodyNode =
+ dartCollisionParent->GetDARTBodyNode();
+ dart::dynamics::BoxShape *dtBoxShape =
+ new dart::dynamics::BoxShape(Eigen::Vector3d(2100, 2100, 0.01));
+ dtBodyNode->addCollisionShape(dtBoxShape);
+ dtBoxShape->setOffset(Eigen::Vector3d(0.0, 0.0, -0.005));
+ dartCollisionParent->SetDARTCollisionShape(dtBoxShape, false);
+}
+
+//////////////////////////////////////////////////
+void DARTPlaneShape::SetAltitude(const math::Vector3 &_pos)
+{
+ PlaneShape::SetAltitude(_pos);
+}
+
diff --git a/gazebo/physics/dart/DARTPlaneShape.hh b/gazebo/physics/dart/DARTPlaneShape.hh
index 0b0acb8..bdf5e35 100644
--- a/gazebo/physics/dart/DARTPlaneShape.hh
+++ b/gazebo/physics/dart/DARTPlaneShape.hh
@@ -25,41 +25,28 @@ namespace gazebo
{
namespace physics
{
+ /// Forward declare private data class
+ class DARTPlaneShapePrivate;
+
/// \brief An DART Plane shape.
- class GAZEBO_VISIBLE DARTPlaneShape : public PlaneShape
+ class GZ_PHYSICS_VISIBLE DARTPlaneShape : public PlaneShape
{
/// \brief Constructor.
/// \param[in] _parent Parent Collision.
- public: explicit DARTPlaneShape(CollisionPtr _parent)
- : PlaneShape(_parent) {}
+ public: explicit DARTPlaneShape(CollisionPtr _parent);
/// \brief Destructor.
- public: virtual ~DARTPlaneShape() {}
+ public: virtual ~DARTPlaneShape();
// Documentation inherited
- public: virtual void CreatePlane()
- {
- PlaneShape::CreatePlane();
-
- DARTCollisionPtr dartCollisionParent =
- boost::dynamic_pointer_cast<DARTCollision>(this->collisionParent);
-
- // math::Vector3 n = this->GetNormal();
-
- dart::dynamics::BodyNode *dtBodyNode =
- dartCollisionParent->GetDARTBodyNode();
- dart::dynamics::BoxShape *dtBoxShape =
- new dart::dynamics::BoxShape(Eigen::Vector3d(2100, 2100, 0.01));
- dtBodyNode->addCollisionShape(dtBoxShape);
- dtBoxShape->setOffset(Eigen::Vector3d(0.0, 0.0, -0.005));
- dartCollisionParent->SetDARTCollisionShape(dtBoxShape, false);
- }
+ public: virtual void CreatePlane();
// Documentation inherited
- public: virtual void SetAltitude(const math::Vector3 &_pos)
- {
- PlaneShape::SetAltitude(_pos);
- }
+ public: virtual void SetAltitude(const math::Vector3 &_pos);
+
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTPlaneShapePrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/dart/DARTPlaneShapePrivate.hh b/gazebo/physics/dart/DARTPlaneShapePrivate.hh
new file mode 100644
index 0000000..7c80e21
--- /dev/null
+++ b/gazebo/physics/dart/DARTPlaneShapePrivate.hh
@@ -0,0 +1,40 @@
+/*
+ * 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_DARTPLANESHAPE_PRIVATE_HH_
+#define _GAZEBO_DARTPLANESHAPE_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTPlaneShape
+ class DARTPlaneShapePrivate
+ {
+ /// \brief Constructor
+ public: DARTPlaneShapePrivate() = default;
+
+ /// \brief Default destructor
+ public: ~DARTPlaneShapePrivate() = default;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTPolylineShape.cc b/gazebo/physics/dart/DARTPolylineShape.cc
index b518042..23ecf45 100644
--- a/gazebo/physics/dart/DARTPolylineShape.cc
+++ b/gazebo/physics/dart/DARTPolylineShape.cc
@@ -22,20 +22,22 @@
#include "gazebo/physics/dart/DARTPhysics.hh"
#include "gazebo/physics/dart/DARTPolylineShape.hh"
+#include "gazebo/physics/dart/DARTPolylineShapePrivate.hh"
+
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
DARTPolylineShape::DARTPolylineShape(CollisionPtr _parent)
-: PolylineShape(_parent)
+ : PolylineShape(_parent),
+ dataPtr(new DARTPolylineShapePrivate())
{
- this->dartMesh = new DARTMesh();
}
//////////////////////////////////////////////////
DARTPolylineShape::~DARTPolylineShape()
{
- delete this->dartMesh;
+ delete this->dataPtr;
}
//////////////////////////////////////////////////
@@ -54,7 +56,7 @@ void DARTPolylineShape::Init()
return;
}
- this->dartMesh->Init(this->mesh,
+ this->dataPtr->dartMesh->Init(this->mesh,
boost::static_pointer_cast<DARTCollision>(this->collisionParent),
math::Vector3(1, 1, 1));
}
diff --git a/gazebo/physics/dart/DARTPolylineShape.hh b/gazebo/physics/dart/DARTPolylineShape.hh
index 4f41cba..f1e6033 100644
--- a/gazebo/physics/dart/DARTPolylineShape.hh
+++ b/gazebo/physics/dart/DARTPolylineShape.hh
@@ -24,10 +24,11 @@ namespace gazebo
{
namespace physics
{
- class DARTMesh;
+ /// Forward declare private data class
+ class DARTPolylineShapePrivate;
/// \brief DART polyline shape
- class GAZEBO_VISIBLE DARTPolylineShape : public PolylineShape
+ class GZ_PHYSICS_VISIBLE DARTPolylineShape : public PolylineShape
{
/// \brief Constructor
/// \param[in] _parent Collision parent.
@@ -42,8 +43,9 @@ namespace gazebo
// Documentation inherited
public: virtual void Init();
- /// \brief DART collsion mesh helper class.
- private: DARTMesh *dartMesh;
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTPolylineShapePrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/dart/DARTPolylineShapePrivate.hh b/gazebo/physics/dart/DARTPolylineShapePrivate.hh
new file mode 100644
index 0000000..b7f7b39
--- /dev/null
+++ b/gazebo/physics/dart/DARTPolylineShapePrivate.hh
@@ -0,0 +1,54 @@
+/*
+ * 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_DARTPOLYLINESHAPE_PRIVATE_HH_
+#define _GAZEBO_DARTPOLYLINESHAPE_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTMesh.hh"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTPolylineShape
+ class DARTPolylineShapePrivate
+ {
+ /// \brief Constructor
+ public: DARTPolylineShapePrivate()
+ : dartMesh(new DARTMesh())
+ {
+ }
+
+ /// \brief Default destructor
+ public: ~DARTPolylineShapePrivate()
+ {
+ delete this->dartMesh;
+ }
+
+ /// \brief Disabled copy constructor
+ public: DARTPolylineShapePrivate(const DARTPolylineShapePrivate&)
+ = delete;
+
+ /// \brief DART collision mesh helper class
+ public: DARTMesh *dartMesh;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTRayShape.cc b/gazebo/physics/dart/DARTRayShape.cc
index cad5131..e9b0357 100644
--- a/gazebo/physics/dart/DARTRayShape.cc
+++ b/gazebo/physics/dart/DARTRayShape.cc
@@ -25,25 +25,25 @@
#include "gazebo/physics/dart/DARTCollision.hh"
#include "gazebo/physics/dart/DARTRayShape.hh"
+#include "gazebo/physics/dart/DARTRayShapePrivate.hh"
+
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
DARTRayShape::DARTRayShape(PhysicsEnginePtr _physicsEngine)
- : RayShape(_physicsEngine)
+ : RayShape(_physicsEngine),
+ dataPtr(new DARTRayShapePrivate())
{
this->SetName("DART_ray_shape");
- this->physicsEngine =
- boost::static_pointer_cast<DARTPhysics>(_physicsEngine);
}
//////////////////////////////////////////////////
DARTRayShape::DARTRayShape(CollisionPtr _parent)
- : RayShape(_parent)
+ : RayShape(_parent),
+ dataPtr(new DARTRayShapePrivate())
{
this->SetName("DART_ray_shape");
- this->physicsEngine = boost::static_pointer_cast<DARTPhysics>(
- this->collisionParent->GetWorld()->GetPhysicsEngine());
}
//////////////////////////////////////////////////
@@ -63,10 +63,6 @@ void DARTRayShape::GetIntersection(double &_dist, std::string &_entity)
_dist = 0;
_entity = "";
- if (this->physicsEngine)
- {
- }
-
// Not implemented yet, please see issue #911
}
diff --git a/gazebo/physics/dart/DARTRayShape.hh b/gazebo/physics/dart/DARTRayShape.hh
index ccfc549..a9a05ad 100644
--- a/gazebo/physics/dart/DARTRayShape.hh
+++ b/gazebo/physics/dart/DARTRayShape.hh
@@ -33,8 +33,11 @@ namespace gazebo
/// \addtogroup gazebo_physics_dart DART Physics
/// \{
+ /// Forward declare private data class
+ class DARTRayShapePrivate;
+
/// \brief Ray collision
- class GAZEBO_VISIBLE DARTRayShape : public RayShape
+ class GZ_PHYSICS_VISIBLE DARTRayShape : public RayShape
{
/// \brief Constructor for a global ray.
/// \param[in] _physicsEngine Pointer to the physics engine.
@@ -62,8 +65,9 @@ namespace gazebo
public: virtual void SetPoints(const math::Vector3 &_posStart,
const math::Vector3 &_posEnd);
- /// \brief Pointer to the DART physics engine
- private: DARTPhysicsPtr physicsEngine;
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTRayShapePrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/dart/DARTRayShapePrivate.hh b/gazebo/physics/dart/DARTRayShapePrivate.hh
new file mode 100644
index 0000000..743be9e
--- /dev/null
+++ b/gazebo/physics/dart/DARTRayShapePrivate.hh
@@ -0,0 +1,40 @@
+/*
+ * 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_DARTRAYSHAPE_PRIVATE_HH_
+#define _GAZEBO_DARTRAYSHAPE_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTRayShape
+ class DARTRayShapePrivate
+ {
+ /// \brief Constructor
+ public: DARTRayShapePrivate() = default;
+
+ /// \brief Default destructor
+ public: ~DARTRayShapePrivate() = default;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTScrewJoint.cc b/gazebo/physics/dart/DARTScrewJoint.cc
index c9dd843..d7f0dc5 100644
--- a/gazebo/physics/dart/DARTScrewJoint.cc
+++ b/gazebo/physics/dart/DARTScrewJoint.cc
@@ -21,6 +21,7 @@
#include "gazebo/common/Console.hh"
#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/dart/DARTJointPrivate.hh"
#include "gazebo/physics/dart/DARTScrewJoint.hh"
using namespace gazebo;
@@ -28,16 +29,15 @@ using namespace physics;
//////////////////////////////////////////////////
DARTScrewJoint::DARTScrewJoint(BasePtr _parent)
- : ScrewJoint<DARTJoint>(_parent),
- dartScrewJoint(new dart::dynamics::ScrewJoint())
+ : ScrewJoint<DARTJoint>(_parent)
{
- this->dtJoint = dartScrewJoint;
+ this->dataPtr->dtJoint = new dart::dynamics::ScrewJoint();
}
//////////////////////////////////////////////////
DARTScrewJoint::~DARTScrewJoint()
{
- delete dartScrewJoint;
+ delete this->dataPtr->dtJoint;
}
//////////////////////////////////////////////////
@@ -63,8 +63,8 @@ void DARTScrewJoint::Init()
//////////////////////////////////////////////////
math::Vector3 DARTScrewJoint::GetAnchor(unsigned int /*index*/) const
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
Eigen::Vector3d worldOrigin = T.translation();
return DARTTypes::ConvVec3(worldOrigin);
@@ -77,9 +77,12 @@ math::Vector3 DARTScrewJoint::GetGlobalAxis(unsigned int _index) const
if (_index < this->GetAngleCount())
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getTransformFromChildBodyNode();
- Eigen::Vector3d axis = this->dartScrewJoint->getAxis();
+ dart::dynamics::ScrewJoint *dtScrewJoint =
+ reinterpret_cast<dart::dynamics::ScrewJoint *>(this->dataPtr->dtJoint);
+
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Vector3d axis = dtScrewJoint->getAxis();
globalAxis = T.linear() * axis;
}
else
@@ -98,15 +101,18 @@ void DARTScrewJoint::SetAxis(unsigned int _index, const math::Vector3 &_axis)
{
if (_index == 0)
{
+ dart::dynamics::ScrewJoint *dtScrewJoint =
+ reinterpret_cast<dart::dynamics::ScrewJoint *>(this->dataPtr->dtJoint);
+
// TODO: Issue #494
// See: https://bitbucket.org/osrf/gazebo/issue/494
// joint-axis-reference-frame-doesnt-match
Eigen::Vector3d dartVec3 = DARTTypes::ConvVec3(_axis);
Eigen::Isometry3d dartTransfJointLeftToParentLink
- = this->dtJoint->getTransformFromParentBodyNode().inverse();
+ = this->dataPtr->dtJoint->getTransformFromParentBodyNode().inverse();
dartVec3 = dartTransfJointLeftToParentLink.linear() * dartVec3;
- this->dartScrewJoint->setAxis(dartVec3);
+ dtScrewJoint->setAxis(dartVec3);
}
else
{
@@ -120,7 +126,7 @@ double DARTScrewJoint::GetVelocity(unsigned int _index) const
double result = 0.0;
if (_index == 0)
- result = this->dtJoint->getVelocity(0);
+ result = this->dataPtr->dtJoint->getVelocity(0);
else
gzerr << "Invalid index[" << _index << "]\n";
@@ -131,7 +137,7 @@ double DARTScrewJoint::GetVelocity(unsigned int _index) const
void DARTScrewJoint::SetVelocity(unsigned int _index, double _vel)
{
if (_index == 0)
- this->dtJoint->setVelocity(0, _vel);
+ this->dataPtr->dtJoint->setVelocity(0, _vel);
else
gzerr << "Invalid index[" << _index << "]\n";
}
@@ -147,8 +153,11 @@ void DARTScrewJoint::SetThreadPitch(unsigned int _index, double _threadPitch)
//////////////////////////////////////////////////
void DARTScrewJoint::SetThreadPitch(double _threadPitch)
{
+ dart::dynamics::ScrewJoint *dtScrewJoint =
+ reinterpret_cast<dart::dynamics::ScrewJoint *>(this->dataPtr->dtJoint);
+
this->threadPitch = _threadPitch;
- this->dartScrewJoint->setPitch(_threadPitch * 2.0 * M_PI);
+ dtScrewJoint->setPitch(_threadPitch * 2.0 * M_PI);
}
//////////////////////////////////////////////////
@@ -162,9 +171,12 @@ double DARTScrewJoint::GetThreadPitch(unsigned int _index)
//////////////////////////////////////////////////
double DARTScrewJoint::GetThreadPitch()
{
+ dart::dynamics::ScrewJoint *dtScrewJoint =
+ reinterpret_cast<dart::dynamics::ScrewJoint *>(this->dataPtr->dtJoint);
+
double result = this->threadPitch;
- if (this->dartScrewJoint)
- result = this->dartScrewJoint->getPitch() * 0.5 / M_PI;
+ if (dtScrewJoint)
+ result = dtScrewJoint->getPitch() * 0.5 / M_PI;
else
gzwarn << "dartScrewJoint not created yet, returning cached threadPitch.\n";
return result;
@@ -184,27 +196,25 @@ math::Angle DARTScrewJoint::GetAngleImpl(unsigned int _index) const
{
math::Angle result;
- if (this->dartScrewJoint)
+ if (_index == 0)
{
- if (_index == 0)
- {
- // angular position
- result.SetFromRadian(this->dartScrewJoint->getPosition(0));
- }
- else if (_index == 1)
- {
- // linear position
- double angPos = this->dartScrewJoint->getPosition(0);
- result = dartScrewJoint->getPitch() * angPos * 0.5 / M_PI;
- }
- else
- {
- gzerr << "Invalid index[" << _index << "]\n";
- }
+ // angular position
+ const double radianAngle = this->dataPtr->dtJoint->getPosition(0);
+ result.SetFromRadian(radianAngle);
+ }
+ 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;
}
else
{
- gzerr << "dartScrewJoint not created yet\n";
+ gzerr << "Invalid index[" << _index << "]\n";
}
return result;
@@ -215,8 +225,8 @@ void DARTScrewJoint::SetMaxForce(unsigned int _index, double _force)
{
if (_index == 0)
{
- this->dtJoint->setForceLowerLimit(0, -_force);
- this->dtJoint->setForceUpperLimit(0, _force);
+ this->dataPtr->dtJoint->setForceLowerLimit(0, -_force);
+ this->dataPtr->dtJoint->setForceUpperLimit(0, _force);
}
else
{
@@ -232,8 +242,8 @@ double DARTScrewJoint::GetMaxForce(unsigned int _index)
if (_index == 0)
{
// Assume that the lower limit and upper limit has equal magnitute
- // result = this->dtJoint->getForceLowerLimit(0);
- result = this->dtJoint->getForceUpperLimit(0);
+ // result = this->dataPtr->dtJoint->getForceLowerLimit(0);
+ result = this->dataPtr->dtJoint->getForceUpperLimit(0);
}
else
{
@@ -247,7 +257,7 @@ double DARTScrewJoint::GetMaxForce(unsigned int _index)
void DARTScrewJoint::SetForceImpl(unsigned int _index, double _effort)
{
if (_index == 0)
- this->dtJoint->setForce(0, _effort);
+ this->dataPtr->dtJoint->setForce(0, _effort);
else
gzerr << "Invalid index[" << _index << "]\n";
}
@@ -258,7 +268,7 @@ math::Angle DARTScrewJoint::GetHighStop(unsigned int _index)
switch (_index)
{
case 0:
- return this->dtJoint->getPositionUpperLimit(0);
+ return this->dataPtr->dtJoint->getPositionUpperLimit(0);
default:
gzerr << "Invalid index[" << _index << "]\n";
};
@@ -272,7 +282,7 @@ math::Angle DARTScrewJoint::GetLowStop(unsigned int _index)
switch (_index)
{
case 0:
- return this->dtJoint->getPositionLowerLimit(0);
+ return this->dataPtr->dtJoint->getPositionLowerLimit(0);
default:
gzerr << "Invalid index[" << _index << "]\n";
};
diff --git a/gazebo/physics/dart/DARTScrewJoint.hh b/gazebo/physics/dart/DARTScrewJoint.hh
index f277caa..a525b41 100644
--- a/gazebo/physics/dart/DARTScrewJoint.hh
+++ b/gazebo/physics/dart/DARTScrewJoint.hh
@@ -29,7 +29,7 @@ namespace gazebo
namespace physics
{
/// \brief A screw joint.
- class GAZEBO_VISIBLE DARTScrewJoint : public ScrewJoint<DARTJoint>
+ class GZ_PHYSICS_VISIBLE DARTScrewJoint : public ScrewJoint<DARTJoint>
{
/// \brief Constructor.
/// \param[in] _parent Pointer to the Link that is the joint' parent
@@ -98,9 +98,6 @@ namespace gazebo
// Documentation inherited.
protected: virtual void SetForceImpl(unsigned int _index, double _effort);
-
- /// \brief Universal joint of DART
- protected: dart::dynamics::ScrewJoint *dartScrewJoint;
};
}
}
diff --git a/gazebo/physics/dart/DARTSliderJoint.cc b/gazebo/physics/dart/DARTSliderJoint.cc
index a5b2826..09e6bac 100644
--- a/gazebo/physics/dart/DARTSliderJoint.cc
+++ b/gazebo/physics/dart/DARTSliderJoint.cc
@@ -20,6 +20,7 @@
#include "gazebo/gazebo_config.h"
#include "gazebo/common/Console.hh"
#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/dart/DARTJointPrivate.hh"
#include "gazebo/physics/dart/DARTSliderJoint.hh"
using namespace gazebo;
@@ -27,16 +28,15 @@ using namespace physics;
//////////////////////////////////////////////////
DARTSliderJoint::DARTSliderJoint(BasePtr _parent)
- : SliderJoint<DARTJoint>(_parent),
- dtPrismaticJoint(new dart::dynamics::PrismaticJoint())
+ : SliderJoint<DARTJoint>(_parent)
{
- this->dtJoint = this->dtPrismaticJoint;
+ this->dataPtr->dtJoint = new dart::dynamics::PrismaticJoint();
}
//////////////////////////////////////////////////
DARTSliderJoint::~DARTSliderJoint()
{
- delete dtPrismaticJoint;
+ delete this->dataPtr->dtJoint;
}
//////////////////////////////////////////////////
@@ -54,8 +54,8 @@ void DARTSliderJoint::Init()
//////////////////////////////////////////////////
math::Vector3 DARTSliderJoint::GetAnchor(unsigned int /*_index*/) const
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
Eigen::Vector3d worldOrigin = T.translation();
return DARTTypes::ConvVec3(worldOrigin);
@@ -68,9 +68,13 @@ math::Vector3 DARTSliderJoint::GetGlobalAxis(unsigned int _index) const
if (_index == 0)
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getTransformFromChildBodyNode();
- Eigen::Vector3d axis = this->dtPrismaticJoint->getAxis();
+ dart::dynamics::PrismaticJoint *dtPrismaticJoint =
+ reinterpret_cast<dart::dynamics::PrismaticJoint *>(
+ this->dataPtr->dtJoint);
+
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Vector3d axis = dtPrismaticJoint->getAxis();
globalAxis = T.linear() * axis;
}
else
@@ -86,13 +90,17 @@ void DARTSliderJoint::SetAxis(unsigned int _index, const math::Vector3 &_axis)
{
if (_index == 0)
{
+ dart::dynamics::PrismaticJoint *dtPrismaticJoint =
+ reinterpret_cast<dart::dynamics::PrismaticJoint *>(
+ this->dataPtr->dtJoint);
+
Eigen::Vector3d dartVec3 = DARTTypes::ConvVec3(
this->GetAxisFrameOffset(0).RotateVector(_axis));
Eigen::Isometry3d dartTransfJointLeftToParentLink
- = this->dtJoint->getTransformFromParentBodyNode().inverse();
+ = this->dataPtr->dtJoint->getTransformFromParentBodyNode().inverse();
dartVec3 = dartTransfJointLeftToParentLink.linear() * dartVec3;
- this->dtPrismaticJoint->setAxis(dartVec3);
+ dtPrismaticJoint->setAxis(dartVec3);
}
else
{
@@ -107,7 +115,7 @@ math::Angle DARTSliderJoint::GetAngleImpl(unsigned int _index) const
if (_index == 0)
{
- double radianAngle = this->dtJoint->getPosition(0);
+ double radianAngle = this->dataPtr->dtJoint->getPosition(0);
result.SetFromRadian(radianAngle);
}
else
@@ -123,8 +131,9 @@ void DARTSliderJoint::SetVelocity(unsigned int _index, double _vel)
{
if (_index == 0)
{
- this->dtJoint->setVelocity(0, _vel);
- this->dtJoint->getSkeleton()->computeForwardKinematics(false, true, false);
+ this->dataPtr->dtJoint->setVelocity(0, _vel);
+ this->dataPtr->dtJoint->getSkeleton()->computeForwardKinematics(
+ false, true, false);
}
else
gzerr << "Invalid index[" << _index << "]\n";
@@ -136,7 +145,7 @@ double DARTSliderJoint::GetVelocity(unsigned int _index) const
double result = 0.0;
if (_index == 0)
- result = this->dtJoint->getVelocity(0);
+ result = this->dataPtr->dtJoint->getVelocity(0);
else
gzerr << "Invalid index[" << _index << "]\n";
@@ -148,8 +157,8 @@ void DARTSliderJoint::SetMaxForce(unsigned int _index, double _force)
{
if (_index == 0)
{
- this->dtJoint->setForceLowerLimit(0, -_force);
- this->dtJoint->setForceUpperLimit(0, _force);
+ this->dataPtr->dtJoint->setForceLowerLimit(0, -_force);
+ this->dataPtr->dtJoint->setForceUpperLimit(0, _force);
}
else
{
@@ -165,8 +174,8 @@ double DARTSliderJoint::GetMaxForce(unsigned int _index)
if (_index == 0)
{
// Assume that the lower limit and upper limit has equal magnitute
- // result = this->dtJoint->getForceLowerLimit(0);
- result = this->dtJoint->getForceUpperLimit(0);
+ // result = this->dataPtr->dtJoint->getForceLowerLimit(0);
+ result = this->dataPtr->dtJoint->getForceUpperLimit(0);
}
else
{
@@ -180,7 +189,7 @@ double DARTSliderJoint::GetMaxForce(unsigned int _index)
void DARTSliderJoint::SetForceImpl(unsigned int _index, double _effort)
{
if (_index == 0)
- this->dtJoint->setForce(0, _effort);
+ this->dataPtr->dtJoint->setForce(0, _effort);
else
gzerr << "Invalid index[" << _index << "]\n";
}
diff --git a/gazebo/physics/dart/DARTSliderJoint.hh b/gazebo/physics/dart/DARTSliderJoint.hh
index cc6033e..a3b4bb0 100644
--- a/gazebo/physics/dart/DARTSliderJoint.hh
+++ b/gazebo/physics/dart/DARTSliderJoint.hh
@@ -27,7 +27,7 @@ namespace gazebo
namespace physics
{
/// \brief A slider joint
- class GAZEBO_VISIBLE DARTSliderJoint : public SliderJoint<DARTJoint>
+ class GZ_PHYSICS_VISIBLE DARTSliderJoint : public SliderJoint<DARTJoint>
{
/// \brief Constructor
/// \param[in] _parent Pointer to the Link that is the joint' parent
@@ -69,9 +69,6 @@ namespace gazebo
// Documentation inherited.
protected: virtual void SetForceImpl(unsigned int _index, double _effort);
-
- /// \brief Prismatic joint of DART
- protected: dart::dynamics::PrismaticJoint *dtPrismaticJoint;
};
}
}
diff --git a/gazebo/physics/dart/DARTSphereShape.cc b/gazebo/physics/dart/DARTSphereShape.cc
new file mode 100644
index 0000000..221cc61
--- /dev/null
+++ b/gazebo/physics/dart/DARTSphereShape.cc
@@ -0,0 +1,86 @@
+/*
+ * 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 "gazebo/common/Console.hh"
+#include "gazebo/physics/dart/DARTPhysics.hh"
+#include "gazebo/physics/dart/DARTCollision.hh"
+#include "gazebo/physics/dart/DARTSphereShape.hh"
+#include "gazebo/util/system.hh"
+
+#include "gazebo/physics/dart/DARTSphereShapePrivate.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+DARTSphereShape::DARTSphereShape(DARTCollisionPtr _parent)
+ : SphereShape(_parent),
+ dataPtr(new DARTSphereShapePrivate())
+{
+}
+
+//////////////////////////////////////////////////
+DARTSphereShape::~DARTSphereShape()
+{
+ delete this->dataPtr;
+}
+
+//////////////////////////////////////////////////
+void DARTSphereShape::SetRadius(double _radius)
+{
+ if (_radius < 0)
+ {
+ gzerr << "Sphere shape does not support negative radius.\n";
+ return;
+ }
+ if (math::equal(_radius, 0.0))
+ {
+ // Warn user, but still create shape with very small value
+ // otherwise later resize operations using setLocalScaling
+ // will not be possible
+ gzwarn << "Setting sphere shape's radius to zero is not supported "
+ << "in DART, using 1e-4.\n";
+ _radius = 1e-4;
+ }
+
+ SphereShape::SetRadius(_radius);
+
+ DARTCollisionPtr dartCollisionParent =
+ boost::dynamic_pointer_cast<DARTCollision>(this->collisionParent);
+
+ if (dartCollisionParent->GetDARTCollisionShape() == NULL)
+ {
+ dart::dynamics::BodyNode *dtBodyNode =
+ dartCollisionParent->GetDARTBodyNode();
+ dart::dynamics::EllipsoidShape *dtEllisoidShape =
+ new dart::dynamics::EllipsoidShape(Eigen::Vector3d(_radius*2.0,
+ _radius*2.0,
+ _radius*2.0));
+ dtBodyNode->addCollisionShape(dtEllisoidShape);
+ dartCollisionParent->SetDARTCollisionShape(dtEllisoidShape);
+ }
+ else
+ {
+ dart::dynamics::EllipsoidShape *dtEllipsoidShape =
+ dynamic_cast<dart::dynamics::EllipsoidShape*>(
+ dartCollisionParent->GetDARTCollisionShape());
+ dtEllipsoidShape->setSize(Eigen::Vector3d(_radius*2.0,
+ _radius*2.0,
+ _radius*2.0));
+ }
+}
+
diff --git a/gazebo/physics/dart/DARTSphereShape.hh b/gazebo/physics/dart/DARTSphereShape.hh
index e12759b..404a80c 100644
--- a/gazebo/physics/dart/DARTSphereShape.hh
+++ b/gazebo/physics/dart/DARTSphereShape.hh
@@ -27,61 +27,25 @@ namespace gazebo
{
namespace physics
{
+ /// Forward declare private data class
+ class DARTSphereShapePrivate;
+
/// \brief A DART sphere shape
- class GAZEBO_VISIBLE DARTSphereShape : public SphereShape
+ class GZ_PHYSICS_VISIBLE DARTSphereShape : public SphereShape
{
/// \brief Constructor.
/// \param[in] _parent Parent Collision.
- public: explicit DARTSphereShape(DARTCollisionPtr _parent)
- : SphereShape(_parent) {}
+ public: explicit DARTSphereShape(DARTCollisionPtr _parent);
/// \brief Destructor.
- public: virtual ~DARTSphereShape() {}
+ public: virtual ~DARTSphereShape();
// Documentation inherited.
- public: virtual void SetRadius(double _radius)
- {
- if (_radius < 0)
- {
- gzerr << "Sphere shape does not support negative radius.\n";
- return;
- }
- if (math::equal(_radius, 0.0))
- {
- // Warn user, but still create shape with very small value
- // otherwise later resize operations using setLocalScaling
- // will not be possible
- gzwarn << "Setting sphere shape's radius to zero is not supported "
- << "in DART, using 1e-4.\n";
- _radius = 1e-4;
- }
-
- SphereShape::SetRadius(_radius);
-
- DARTCollisionPtr dartCollisionParent =
- boost::dynamic_pointer_cast<DARTCollision>(this->collisionParent);
+ public: virtual void SetRadius(double _radius);
- if (dartCollisionParent->GetDARTCollisionShape() == NULL)
- {
- dart::dynamics::BodyNode *dtBodyNode =
- dartCollisionParent->GetDARTBodyNode();
- dart::dynamics::EllipsoidShape *dtEllisoidShape =
- new dart::dynamics::EllipsoidShape(Eigen::Vector3d(_radius*2.0,
- _radius*2.0,
- _radius*2.0));
- dtBodyNode->addCollisionShape(dtEllisoidShape);
- dartCollisionParent->SetDARTCollisionShape(dtEllisoidShape);
- }
- else
- {
- dart::dynamics::EllipsoidShape *dtEllipsoidShape =
- dynamic_cast<dart::dynamics::EllipsoidShape*>(
- dartCollisionParent->GetDARTCollisionShape());
- dtEllipsoidShape->setSize(Eigen::Vector3d(_radius*2.0,
- _radius*2.0,
- _radius*2.0));
- }
- }
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTSphereShapePrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/dart/DARTSphereShapePrivate.hh b/gazebo/physics/dart/DARTSphereShapePrivate.hh
new file mode 100644
index 0000000..425561f
--- /dev/null
+++ b/gazebo/physics/dart/DARTSphereShapePrivate.hh
@@ -0,0 +1,40 @@
+/*
+ * 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_DARTSPHERESHAPE_PRIVATE_HH_
+#define _GAZEBO_DARTSPHERESHAPE_PRIVATE_HH_
+
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTSphereShape
+ class DARTSphereShapePrivate
+ {
+ /// \brief Constructor
+ public: DARTSphereShapePrivate() = default;
+
+ /// \brief Default destructor
+ public: ~DARTSphereShapePrivate() = default;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTSurfaceParams.cc b/gazebo/physics/dart/DARTSurfaceParams.cc
new file mode 100644
index 0000000..576dbd2
--- /dev/null
+++ b/gazebo/physics/dart/DARTSurfaceParams.cc
@@ -0,0 +1,100 @@
+/*
+ * 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/common/Console.hh"
+#include "gazebo/physics/dart/DARTSurfaceParams.hh"
+
+#include "gazebo/physics/dart/DARTSurfaceParamsPrivate.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+DARTSurfaceParams::DARTSurfaceParams()
+ : SurfaceParams(),
+ dataPtr(new DARTSurfaceParamsPrivate())
+{
+}
+
+//////////////////////////////////////////////////
+DARTSurfaceParams::~DARTSurfaceParams()
+{
+ delete this->dataPtr;
+}
+
+//////////////////////////////////////////////////
+void DARTSurfaceParams::Load(sdf::ElementPtr _sdf)
+{
+ // Load parent class
+ SurfaceParams::Load(_sdf);
+
+ if (!_sdf)
+ gzerr << "Surface _sdf is NULL" << std::endl;
+ else
+ {
+ sdf::ElementPtr frictionElem = _sdf->GetElement("friction");
+ if (!frictionElem)
+ gzerr << "Surface friction sdf member is NULL" << std::endl;
+ {
+ // Note this should not be looking in the "ode" block
+ // Update this when sdformat has bullet friction parameters
+ // See sdformat issue #31
+ // https://bitbucket.org/osrf/sdformat/issue/31
+ sdf::ElementPtr frictionOdeElem = frictionElem->GetElement("ode");
+ if (!frictionOdeElem)
+ gzerr << "Surface friction ode sdf member is NULL" << std::endl;
+ else
+ {
+ this->dataPtr->frictionPyramid->SetMuPrimary(
+ frictionOdeElem->Get<double>("mu"));
+ this->dataPtr->frictionPyramid->SetMuSecondary(
+ frictionOdeElem->Get<double>("mu2"));
+ }
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void DARTSurfaceParams::FillMsg(msgs::Surface &_msg)
+{
+ SurfaceParams::FillMsg(_msg);
+
+ _msg.mutable_friction()->set_mu(
+ this->dataPtr->frictionPyramid->GetMuPrimary());
+ _msg.mutable_friction()->set_mu2(
+ this->dataPtr->frictionPyramid->GetMuSecondary());
+}
+
+/////////////////////////////////////////////////
+void DARTSurfaceParams::ProcessMsg(const msgs::Surface &_msg)
+{
+ SurfaceParams::ProcessMsg(_msg);
+
+ if (_msg.has_friction())
+ {
+ if (_msg.friction().has_mu())
+ this->dataPtr->frictionPyramid->SetMuPrimary(_msg.friction().mu());
+ if (_msg.friction().has_mu2())
+ this->dataPtr->frictionPyramid->SetMuSecondary(_msg.friction().mu2());
+ }
+}
+
+/////////////////////////////////////////////////
+FrictionPyramidPtr DARTSurfaceParams::GetFrictionPyramid() const
+{
+ return this->dataPtr->frictionPyramid;
+}
diff --git a/gazebo/physics/dart/DARTSurfaceParams.hh b/gazebo/physics/dart/DARTSurfaceParams.hh
new file mode 100644
index 0000000..46dca95
--- /dev/null
+++ b/gazebo/physics/dart/DARTSurfaceParams.hh
@@ -0,0 +1,67 @@
+/*
+ * 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_DARTSURFACEPARAMS_HH_
+#define _GAZEBO_DARTSURFACEPARAMS_HH_
+
+#include <sdf/sdf.hh>
+
+#include "gazebo/math/Vector3.hh"
+#include "gazebo/msgs/msgs.hh"
+#include "gazebo/physics/SurfaceParams.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \addtogroup gazebo_physics
+ /// \{
+
+ /// Forward declare private data class
+ class DARTSurfaceParamsPrivate;
+
+ /// \brief DART surface parameters.
+ class GZ_PHYSICS_VISIBLE DARTSurfaceParams : public SurfaceParams
+ {
+ /// \brief Constructor.
+ public: DARTSurfaceParams();
+
+ /// \brief Destructor.
+ public: virtual ~DARTSurfaceParams();
+
+ /// \brief Load the contact params.
+ /// \param[in] _sdf SDF values to load from.
+ public: virtual void Load(sdf::ElementPtr _sdf);
+
+ // Documentation inherited.
+ public: virtual void FillMsg(msgs::Surface &_msg);
+
+ // Documentation inherited.
+ public: virtual void ProcessMsg(const msgs::Surface &_msg);
+
+ // Documentation inherited.
+ public: virtual FrictionPyramidPtr GetFrictionPyramid() const;
+
+ /// \internal
+ /// \brief Pointer to private data
+ private: DARTSurfaceParamsPrivate *dataPtr;
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTSurfaceParamsPrivate.hh b/gazebo/physics/dart/DARTSurfaceParamsPrivate.hh
new file mode 100644
index 0000000..312b5cd
--- /dev/null
+++ b/gazebo/physics/dart/DARTSurfaceParamsPrivate.hh
@@ -0,0 +1,49 @@
+/*
+ * 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_DARTSURFACEPARAMS_PRIVATE_HH_
+#define _GAZEBO_DARTSURFACEPARAMS_PRIVATE_HH_
+
+#include "gazebo/physics/SurfaceParams.hh"
+#include "gazebo/physics/dart/dart_inc.h"
+#include "gazebo/physics/dart/DARTTypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \internal
+ /// \brief Private data class for DARTSurfaceParams
+ class DARTSurfaceParamsPrivate
+ {
+ /// \brief Constructor
+ public: DARTSurfaceParamsPrivate()
+ : frictionPyramid(new FrictionPyramid())
+ {
+ }
+
+ /// \brief Default destructor
+ public: ~DARTSurfaceParamsPrivate()
+ {
+ }
+
+ /// \brief Friction pyramid parameters (mu1, mu2).
+ public: FrictionPyramidPtr frictionPyramid;
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/dart/DARTTypes.hh b/gazebo/physics/dart/DARTTypes.hh
index aafad51..9e96a6c 100644
--- a/gazebo/physics/dart/DARTTypes.hh
+++ b/gazebo/physics/dart/DARTTypes.hh
@@ -37,6 +37,7 @@ namespace gazebo
class DARTJoint;
class DARTCollision;
class DARTRayShape;
+ class DARTSurfaceParams;
typedef boost::shared_ptr<DARTPhysics> DARTPhysicsPtr;
typedef boost::shared_ptr<DARTModel> DARTModelPtr;
@@ -44,6 +45,7 @@ namespace gazebo
typedef boost::shared_ptr<DARTJoint> DARTJointPtr;
typedef boost::shared_ptr<DARTCollision> DARTCollisionPtr;
typedef boost::shared_ptr<DARTRayShape> DARTRayShapePtr;
+ typedef boost::shared_ptr<DARTSurfaceParams> DARTSurfaceParamsPtr;
/// \addtogroup gazebo_physics_dart
/// \{
@@ -51,7 +53,7 @@ namespace gazebo
/// \class DARTTypes DARTTypes.hh
/// \brief A set of functions for converting between the math types used
/// by gazebo and dart.
- class GAZEBO_VISIBLE DARTTypes
+ class GZ_PHYSICS_VISIBLE DARTTypes
{
/// \brief
public: static Eigen::Vector3d ConvVec3(const math::Vector3 &_vec3)
diff --git a/gazebo/physics/dart/DARTUniversalJoint.cc b/gazebo/physics/dart/DARTUniversalJoint.cc
index 027968a..54a3256 100644
--- a/gazebo/physics/dart/DARTUniversalJoint.cc
+++ b/gazebo/physics/dart/DARTUniversalJoint.cc
@@ -18,6 +18,7 @@
#include "gazebo/gazebo_config.h"
#include "gazebo/common/Console.hh"
#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/dart/DARTJointPrivate.hh"
#include "gazebo/physics/dart/DARTUniversalJoint.hh"
using namespace gazebo;
@@ -25,16 +26,15 @@ using namespace physics;
//////////////////////////////////////////////////
DARTUniversalJoint::DARTUniversalJoint(BasePtr _parent)
- : UniversalJoint<DARTJoint>(_parent),
- dtUniveralJoint(new dart::dynamics::UniversalJoint())
+ : UniversalJoint<DARTJoint>(_parent)
{
- this->dtJoint = dtUniveralJoint;
+ this->dataPtr->dtJoint = new dart::dynamics::UniversalJoint();
}
//////////////////////////////////////////////////
DARTUniversalJoint::~DARTUniversalJoint()
{
- delete dtUniveralJoint;
+ delete this->dataPtr->dtJoint;
}
//////////////////////////////////////////////////
@@ -52,8 +52,8 @@ void DARTUniversalJoint::Init()
//////////////////////////////////////////////////
math::Vector3 DARTUniversalJoint::GetAnchor(unsigned int /*index*/) const
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
Eigen::Vector3d worldOrigin = T.translation();
return DARTTypes::ConvVec3(worldOrigin);
@@ -66,18 +66,26 @@ math::Vector3 DARTUniversalJoint::GetGlobalAxis(unsigned int _index) const
if (_index == 0)
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getLocalTransform().inverse() *
- this->dtJoint->getTransformFromParentBodyNode();
- Eigen::Vector3d axis = this->dtUniveralJoint->getAxis1();
+ dart::dynamics::UniversalJoint *dtUniveralJoint =
+ reinterpret_cast<dart::dynamics::UniversalJoint *>(
+ this->dataPtr->dtJoint);
+
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getLocalTransform().inverse() *
+ this->dataPtr->dtJoint->getTransformFromParentBodyNode();
+ Eigen::Vector3d axis = dtUniveralJoint->getAxis1();
globalAxis = T.linear() * axis;
}
else if (_index == 1)
{
- Eigen::Isometry3d T = this->dtChildBodyNode->getTransform() *
- this->dtJoint->getTransformFromChildBodyNode();
- Eigen::Vector3d axis = this->dtUniveralJoint->getAxis2();
+ dart::dynamics::UniversalJoint *dtUniveralJoint =
+ reinterpret_cast<dart::dynamics::UniversalJoint *>(
+ this->dataPtr->dtJoint);
+
+ Eigen::Isometry3d T = this->dataPtr->dtChildBodyNode->getTransform() *
+ this->dataPtr->dtJoint->getTransformFromChildBodyNode();
+ Eigen::Vector3d axis = dtUniveralJoint->getAxis2();
globalAxis = T.linear() * axis;
}
@@ -96,16 +104,22 @@ void DARTUniversalJoint::SetAxis(unsigned int _index,
Eigen::Vector3d dtAxis = DARTTypes::ConvVec3(
this->GetAxisFrameOffset(_index).RotateVector(_axis));
Eigen::Isometry3d dtTransfJointLeftToParentLink
- = this->dtJoint->getTransformFromParentBodyNode().inverse();
+ = this->dataPtr->dtJoint->getTransformFromParentBodyNode().inverse();
dtAxis = dtTransfJointLeftToParentLink.linear() * dtAxis;
if (_index == 0)
{
- this->dtUniveralJoint->setAxis1(dtAxis);
+ dart::dynamics::UniversalJoint *dtUniveralJoint =
+ reinterpret_cast<dart::dynamics::UniversalJoint *>(
+ this->dataPtr->dtJoint);
+ dtUniveralJoint->setAxis1(dtAxis);
}
else if (_index == 1)
{
- this->dtUniveralJoint->setAxis2(dtAxis);
+ dart::dynamics::UniversalJoint *dtUniveralJoint =
+ reinterpret_cast<dart::dynamics::UniversalJoint *>(
+ this->dataPtr->dtJoint);
+ dtUniveralJoint->setAxis2(dtAxis);
}
else
{
@@ -120,12 +134,12 @@ math::Angle DARTUniversalJoint::GetAngleImpl(unsigned int _index) const
if (_index == 0)
{
- double radianAngle = this->dtJoint->getPosition(0);
+ double radianAngle = this->dataPtr->dtJoint->getPosition(0);
result.SetFromRadian(radianAngle);
}
else if (_index == 1)
{
- double radianAngle = this->dtJoint->getPosition(1);
+ double radianAngle = this->dataPtr->dtJoint->getPosition(1);
result.SetFromRadian(radianAngle);
}
else
@@ -142,9 +156,9 @@ double DARTUniversalJoint::GetVelocity(unsigned int _index) const
double result = 0.0;
if (_index == 0)
- result = this->dtJoint->getVelocity(0);
+ result = this->dataPtr->dtJoint->getVelocity(0);
else if (_index == 1)
- result = this->dtJoint->getVelocity(1);
+ result = this->dataPtr->dtJoint->getVelocity(1);
else
gzerr << "Invalid index[" << _index << "]\n";
@@ -156,8 +170,9 @@ void DARTUniversalJoint::SetVelocity(unsigned int _index, double _vel)
{
if (_index < this->GetAngleCount())
{
- this->dtJoint->setVelocity(_index, _vel);
- this->dtJoint->getSkeleton()->computeForwardKinematics(false, true, false);
+ this->dataPtr->dtJoint->setVelocity(_index, _vel);
+ this->dataPtr->dtJoint->getSkeleton()->computeForwardKinematics(
+ false, true, false);
}
else
gzerr << "Invalid index[" << _index << "]\n";
@@ -168,13 +183,13 @@ void DARTUniversalJoint::SetMaxForce(unsigned int _index, double _force)
{
if (_index == 0)
{
- this->dtJoint->setForceLowerLimit(0, -_force);
- this->dtJoint->setForceUpperLimit(0, _force);
+ this->dataPtr->dtJoint->setForceLowerLimit(0, -_force);
+ this->dataPtr->dtJoint->setForceUpperLimit(0, _force);
}
else if (_index == 1)
{
- this->dtJoint->setForceLowerLimit(1, -_force);
- this->dtJoint->setForceUpperLimit(1, _force);
+ this->dataPtr->dtJoint->setForceLowerLimit(1, -_force);
+ this->dataPtr->dtJoint->setForceUpperLimit(1, _force);
}
else
{
@@ -190,14 +205,14 @@ double DARTUniversalJoint::GetMaxForce(unsigned int _index)
if (_index == 0)
{
// Assume that the lower limit and upper limit has equal magnitute
- // result = this->dtJoint->getForceLowerLimit(0);
- result = this->dtJoint->getForceUpperLimit(0);
+ // result = this->dataPtr->dtJoint->getForceLowerLimit(0);
+ result = this->dataPtr->dtJoint->getForceUpperLimit(0);
}
else if (_index == 1)
{
// Assume that the lower limit and upper limit has equal magnitute
- // result = this->dtJoint->getForceLowerLimit(1);
- result = this->dtJoint->getForceUpperLimit(1);
+ // result = this->dataPtr->dtJoint->getForceLowerLimit(1);
+ result = this->dataPtr->dtJoint->getForceUpperLimit(1);
}
else
{
@@ -211,9 +226,9 @@ double DARTUniversalJoint::GetMaxForce(unsigned int _index)
void DARTUniversalJoint::SetForceImpl(unsigned int _index, double _effort)
{
if (_index == 0)
- this->dtJoint->setForce(0, _effort);
+ this->dataPtr->dtJoint->setForce(0, _effort);
else if (_index == 1)
- this->dtJoint->setForce(1, _effort);
+ this->dataPtr->dtJoint->setForce(1, _effort);
else
gzerr << "Invalid index[" << _index << "]\n";
}
diff --git a/gazebo/physics/dart/DARTUniversalJoint.hh b/gazebo/physics/dart/DARTUniversalJoint.hh
index 111d29d..32856de 100644
--- a/gazebo/physics/dart/DARTUniversalJoint.hh
+++ b/gazebo/physics/dart/DARTUniversalJoint.hh
@@ -27,7 +27,8 @@ namespace gazebo
namespace physics
{
/// \brief A universal joint.
- class GAZEBO_VISIBLE DARTUniversalJoint : public UniversalJoint<DARTJoint>
+ class GZ_PHYSICS_VISIBLE DARTUniversalJoint :
+ public UniversalJoint<DARTJoint>
{
/// \brief Constructor.
/// \param[in] _parent Pointer to the Link that is the joint' parent
@@ -69,9 +70,6 @@ namespace gazebo
// Documentation inherited
protected: virtual void SetForceImpl(unsigned int _index, double _effort);
-
- /// \brief Universal joint of DART
- protected: dart::dynamics::UniversalJoint *dtUniveralJoint;
};
}
}
diff --git a/gazebo/physics/dart/dart_inc.h b/gazebo/physics/dart/dart_inc.h
index 0c6b0a2..03da4c0 100644
--- a/gazebo/physics/dart/dart_inc.h
+++ b/gazebo/physics/dart/dart_inc.h
@@ -21,43 +21,12 @@
// This disables warning messages for ODE
#pragma GCC system_header
-#include <dart/math/Helpers.h>
-#include <dart/math/Geometry.h>
-
-#include <dart/collision/CollisionDetector.h>
-#include <dart/collision/CollisionNode.h>
-#include <dart/collision/dart/DARTCollisionDetector.h>
-#include <dart/collision/fcl_mesh/FCLMeshCollisionDetector.h>
-
-#include <dart/integration/Integrator.h>
-#include <dart/integration/EulerIntegrator.h>
-#include <dart/integration/RK4Integrator.h>
-
-#include <dart/dynamics/BallJoint.h>
-#include <dart/dynamics/BodyNode.h>
-#include <dart/dynamics/BoxShape.h>
-#include <dart/dynamics/CylinderShape.h>
-#include <dart/dynamics/EllipsoidShape.h>
-#include <dart/dynamics/FreeJoint.h>
-#include <dart/dynamics/Joint.h>
-#include <dart/dynamics/MeshShape.h>
-#include <dart/dynamics/PointMass.h>
-#include <dart/dynamics/PrismaticJoint.h>
-#include <dart/dynamics/RevoluteJoint.h>
-#include <dart/dynamics/Shape.h>
-#include <dart/dynamics/Skeleton.h>
-#include <dart/dynamics/ScrewJoint.h>
-#include <dart/dynamics/UniversalJoint.h>
-#include <dart/dynamics/WeldJoint.h>
-#include <dart/dynamics/SoftBodyNode.h>
-#include <dart/dynamics/SoftMeshShape.h>
-
-#include <dart/constraint/Constraint.h>
-#include <dart/constraint/ConstraintSolver.h>
-#include <dart/constraint/ContactConstraint.h>
-#include <dart/constraint/JointLimitConstraint.h>
-#include <dart/constraint/WeldJointConstraint.h>
-
-#include <dart/simulation/World.h>
+#include <dart/common/common.h>
+#include <dart/math/math.h>
+#include <dart/collision/collision.h>
+#include <dart/integration/integration.h>
+#include <dart/dynamics/dynamics.h>
+#include <dart/constraint/constraint.h>
+#include <dart/simulation/simulation.h>
#endif
diff --git a/gazebo/physics/ode/CMakeLists.txt b/gazebo/physics/ode/CMakeLists.txt
index 5077954..ff68f43 100644
--- a/gazebo/physics/ode/CMakeLists.txt
+++ b/gazebo/physics/ode/CMakeLists.txt
@@ -4,11 +4,17 @@ include_directories(SYSTEM ${CMAKE_SOURCE_DIR}/deps/opende/include)
link_directories(
${CCD_LIBRARY_DIRS}
+ ${Boost_LIBRARY_DIRS}
)
+if (WIN32)
+ add_definitions(-DBUILDING_DLL_GZ_PHYSICS_ODE)
+endif()
+
set (sources
ODEBallJoint.cc
ODECollision.cc
+ ODEFixedJoint.cc
ODEGearboxJoint.cc
ODEHeightmapShape.cc
ODEHinge2Joint.cc
@@ -33,6 +39,7 @@ SET (headers
ODEBoxShape.hh
ODECollision.hh
ODECylinderShape.hh
+ ODEFixedJoint.hh
ODEGearboxJoint.hh
ODEHeightmapShape.hh
ODEHinge2Joint.hh
@@ -64,8 +71,8 @@ gz_add_library(gazebo_physics_ode ${headers} ${sources})
target_link_libraries(gazebo_physics_ode
gazebo_ode
gazebo_opcode
- gazebo_math
gazebo_common
+ gazebo_math
${TBB_LIBRARIES}
)
diff --git a/gazebo/physics/ode/ODEBallJoint.hh b/gazebo/physics/ode/ODEBallJoint.hh
index 4b828cf..3caf9bf 100644
--- a/gazebo/physics/ode/ODEBallJoint.hh
+++ b/gazebo/physics/ode/ODEBallJoint.hh
@@ -31,7 +31,7 @@ namespace gazebo
namespace physics
{
/// \brief An ODEBallJoint
- class GAZEBO_VISIBLE ODEBallJoint : public BallJoint<ODEJoint>
+ class GZ_PHYSICS_ODE_VISIBLE ODEBallJoint : public BallJoint<ODEJoint>
{
/// \brief Constructor
/// \param[in] _worldID ODE id of the world.
diff --git a/gazebo/physics/ode/ODEBoxShape.hh b/gazebo/physics/ode/ODEBoxShape.hh
index 090e647..531cac1 100644
--- a/gazebo/physics/ode/ODEBoxShape.hh
+++ b/gazebo/physics/ode/ODEBoxShape.hh
@@ -32,7 +32,7 @@ namespace gazebo
namespace physics
{
/// \brief ODE Box shape
- class GAZEBO_VISIBLE ODEBoxShape : public BoxShape
+ class GZ_PHYSICS_ODE_VISIBLE ODEBoxShape : public BoxShape
{
/// \brief Constructor.
/// \param[in] _parent Parent Collision.
diff --git a/gazebo/physics/ode/ODECollision.cc b/gazebo/physics/ode/ODECollision.cc
index fcd0daa..63d8fcd 100644
--- a/gazebo/physics/ode/ODECollision.cc
+++ b/gazebo/physics/ode/ODECollision.cc
@@ -19,6 +19,12 @@
* Date: 13 Feb 2006
*/
+#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 "gazebo/common/Assert.hh"
diff --git a/gazebo/physics/ode/ODECollision.hh b/gazebo/physics/ode/ODECollision.hh
index 5d4650c..913c26a 100644
--- a/gazebo/physics/ode/ODECollision.hh
+++ b/gazebo/physics/ode/ODECollision.hh
@@ -36,7 +36,7 @@ namespace gazebo
namespace physics
{
/// \brief Base class for all ODE collisions.
- class GAZEBO_VISIBLE ODECollision : public Collision
+ class GZ_PHYSICS_ODE_VISIBLE ODECollision : public Collision
{
/// \brief Constructor.
/// \param[in] _link Parent Link
diff --git a/gazebo/physics/ode/ODECylinderShape.hh b/gazebo/physics/ode/ODECylinderShape.hh
index ac8c3b5..7c9313e 100644
--- a/gazebo/physics/ode/ODECylinderShape.hh
+++ b/gazebo/physics/ode/ODECylinderShape.hh
@@ -26,7 +26,7 @@ namespace gazebo
namespace physics
{
/// \brief ODE cylinder shape
- class GAZEBO_VISIBLE ODECylinderShape : public CylinderShape
+ class GZ_PHYSICS_ODE_VISIBLE ODECylinderShape : public CylinderShape
{
/// \brief Constructor
/// \param[in] _parent Collision parent.
diff --git a/gazebo/physics/ode/ODEFixedJoint.cc b/gazebo/physics/ode/ODEFixedJoint.cc
new file mode 100644
index 0000000..2804a4b
--- /dev/null
+++ b/gazebo/physics/ode/ODEFixedJoint.cc
@@ -0,0 +1,138 @@
+/*
+ * 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 <boost/bind.hpp>
+
+#include "gazebo/gazebo_config.h"
+#include "gazebo/common/Console.hh"
+
+#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/Link.hh"
+#include "gazebo/physics/ode/ODEFixedJoint.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+ODEFixedJoint::ODEFixedJoint(dWorldID _worldId, BasePtr _parent)
+ : FixedJoint<ODEJoint>(_parent)
+{
+ this->jointId = dJointCreateFixed(_worldId, NULL);
+}
+
+//////////////////////////////////////////////////
+ODEFixedJoint::~ODEFixedJoint()
+{
+}
+
+//////////////////////////////////////////////////
+void ODEFixedJoint::Load(sdf::ElementPtr _sdf)
+{
+ FixedJoint<ODEJoint>::Load(_sdf);
+}
+
+//////////////////////////////////////////////////
+math::Vector3 ODEFixedJoint::GetAnchor(unsigned int /*index*/) const
+{
+ gzwarn << "SimbodyFixedJoint: called method "
+ << "GetAnchor that is not valid for joints of type fixed.\n";
+ return math::Vector3();
+}
+
+//////////////////////////////////////////////////
+void ODEFixedJoint::SetAnchor(unsigned int /*index*/,
+ const math::Vector3 &/*_anchor*/)
+{
+ if (this->childLink)
+ this->childLink->SetEnabled(true);
+ if (this->parentLink)
+ this->parentLink->SetEnabled(true);
+}
+
+//////////////////////////////////////////////////
+math::Vector3 ODEFixedJoint::GetGlobalAxis(unsigned int /*_index*/) const
+{
+ gzwarn << "SimbodyFixedJoint: called method "
+ << "GetGlobalAxis that is not valid for joints of type fixed.\n";
+ return math::Vector3();
+}
+
+//////////////////////////////////////////////////
+void ODEFixedJoint::SetAxis(unsigned int /*_index*/,
+ const math::Vector3 &/*_axis*/)
+{
+ gzwarn << "ODEFixedJoint: called method "
+ << "SetAxis that is not valid for joints of type fixed.\n";
+}
+
+//////////////////////////////////////////////////
+math::Angle ODEFixedJoint::GetAngleImpl(unsigned int /*index*/) const
+{
+ gzwarn << "ODEFixedJoint: called method "
+ << "GetAngleImpl that is not valid for joints of type fixed.\n";
+ return math::Angle();
+}
+
+//////////////////////////////////////////////////
+double ODEFixedJoint::GetVelocity(unsigned int /*index*/) const
+{
+ gzwarn << "ODEFixedJoint: called method "
+ << "GetVelocity that is not valid for joints of type fixed.\n";
+ return 0.0;
+}
+
+//////////////////////////////////////////////////
+void ODEFixedJoint::SetVelocity(unsigned int /*_index*/, double /*_angle*/)
+{
+ gzwarn << "ODEFixedJoint: called method "
+ << "SetVelocity that is not valid for joints of type fixed.\n";
+}
+
+//////////////////////////////////////////////////
+void ODEFixedJoint::SetMaxForce(unsigned int /*index*/, double /*_t*/)
+{
+ gzwarn << "ODEFixedJoint: called method "
+ << "SetMaxForce that is not valid for joints of type fixed.\n";
+}
+
+//////////////////////////////////////////////////
+double ODEFixedJoint::GetMaxForce(unsigned int /*index*/)
+{
+ gzwarn << "ODEFixedJoint: called method "
+ << "GetMaxForce that is not valid for joints of type fixed.\n";
+ return 0.0;
+}
+
+//////////////////////////////////////////////////
+void ODEFixedJoint::SetForceImpl(unsigned int /*_index*/, double /*_effort*/)
+{
+ gzwarn << "ODEFixedJoint: called method "
+ << "SetForceImpl that is not valid for joints of type fixed.\n";
+}
+
+//////////////////////////////////////////////////
+void ODEFixedJoint::Attach(LinkPtr _parent, LinkPtr _child)
+{
+ gazebo::physics::ODEJoint::Attach(_parent, _child);
+
+ // Once we attach the links to the fixed joint,
+ // we also call the dJointSetFixed method to
+ // the current desired relative offset and desired
+ // relative rotation between the bodies.
+
+ dJointSetFixed(this->jointId);
+}
diff --git a/gazebo/physics/ode/ODEFixedJoint.hh b/gazebo/physics/ode/ODEFixedJoint.hh
new file mode 100644
index 0000000..ea2721e
--- /dev/null
+++ b/gazebo/physics/ode/ODEFixedJoint.hh
@@ -0,0 +1,83 @@
+/*
+ * 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_ODEFIXEDJOINT_HH_
+#define _GAZEBO_ODEFIXEDJOINT_HH_
+
+#include "gazebo/math/Angle.hh"
+#include "gazebo/math/Vector3.hh"
+
+#include "gazebo/physics/FixedJoint.hh"
+#include "gazebo/physics/ode/ODEJoint.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \brief A fixed joint.
+ class GZ_PHYSICS_ODE_VISIBLE ODEFixedJoint : public FixedJoint<ODEJoint>
+ {
+ /// \brief Constructor
+ /// \param[in] _worldID ODE id of the world.
+ /// \param[in] _parent Parent of the Joint
+ public: ODEFixedJoint(dWorldID _worldId, BasePtr _parent);
+
+ /// \brief Destructor.
+ public: virtual ~ODEFixedJoint();
+
+ // Documentation inherited
+ public: virtual void Load(sdf::ElementPtr _sdf);
+
+ // Documentation inherited
+ public: virtual math::Vector3 GetAnchor(unsigned int _index) const;
+
+ // Documentation inherited
+ public: virtual void SetAnchor(unsigned int _index,
+ const math::Vector3 &_anchor);
+
+ // Documentation inherited
+ public: virtual math::Vector3 GetGlobalAxis(unsigned int _index) const;
+
+ // Documentation inherited
+ public: virtual void SetAxis(unsigned int _index,
+ const math::Vector3 &_axis);
+
+ // Documentation inherited
+ public: virtual math::Angle GetAngleImpl(unsigned int _index) const;
+
+ // Documentation inherited
+ public: virtual void SetVelocity(unsigned int _index, double _angle);
+
+ // Documentation inherited
+ public: virtual double GetVelocity(unsigned int _index) const;
+
+ // Documentation inherited
+ public: virtual void SetMaxForce(unsigned int _index, double _t);
+
+ // Documentation inherited
+ public: virtual double GetMaxForce(unsigned int _index);
+
+ // Documentation inherited
+ public: virtual void Attach(LinkPtr _parent, LinkPtr _child);
+
+ // Documentation inherited
+ protected: virtual void SetForceImpl(unsigned int _index, double _effort);
+ };
+ }
+}
+#endif
diff --git a/gazebo/physics/ode/ODEGearboxJoint.hh b/gazebo/physics/ode/ODEGearboxJoint.hh
index f25df79..290a955 100644
--- a/gazebo/physics/ode/ODEGearboxJoint.hh
+++ b/gazebo/physics/ode/ODEGearboxJoint.hh
@@ -31,7 +31,7 @@ namespace gazebo
{
/// \class ODEGearboxJoint ODEGearboxJoint.hh physics/physics.hh
/// \brief A double axis gearbox joint.
- class GAZEBO_VISIBLE ODEGearboxJoint : public GearboxJoint<ODEJoint>
+ class GZ_PHYSICS_ODE_VISIBLE ODEGearboxJoint : public GearboxJoint<ODEJoint>
{
/// \brief Constructor
/// \param[in] _worldID ODE id of the world.
diff --git a/gazebo/physics/ode/ODEHeightmapShape.hh b/gazebo/physics/ode/ODEHeightmapShape.hh
index 6c68f8b..5249d37 100644
--- a/gazebo/physics/ode/ODEHeightmapShape.hh
+++ b/gazebo/physics/ode/ODEHeightmapShape.hh
@@ -34,7 +34,7 @@ namespace gazebo
namespace physics
{
/// \brief ODE Height map collision.
- class GAZEBO_VISIBLE ODEHeightmapShape : public HeightmapShape
+ class GZ_PHYSICS_ODE_VISIBLE ODEHeightmapShape : public HeightmapShape
{
/// \brief Constructor.
/// \param[in] _parent Collision parent.
diff --git a/gazebo/physics/ode/ODEHinge2Joint.hh b/gazebo/physics/ode/ODEHinge2Joint.hh
index 0253f04..d4bed9b 100644
--- a/gazebo/physics/ode/ODEHinge2Joint.hh
+++ b/gazebo/physics/ode/ODEHinge2Joint.hh
@@ -34,7 +34,7 @@ namespace gazebo
namespace physics
{
/// \brief A two axis hinge joint
- class GAZEBO_VISIBLE ODEHinge2Joint : public Hinge2Joint<ODEJoint>
+ class GZ_PHYSICS_ODE_VISIBLE ODEHinge2Joint : public Hinge2Joint<ODEJoint>
{
/// \brief Constructor
/// \param[in] _worldID ODE id of the world.
diff --git a/gazebo/physics/ode/ODEHingeJoint.hh b/gazebo/physics/ode/ODEHingeJoint.hh
index 0f4e40f..b86462a 100644
--- a/gazebo/physics/ode/ODEHingeJoint.hh
+++ b/gazebo/physics/ode/ODEHingeJoint.hh
@@ -34,7 +34,7 @@ namespace gazebo
namespace physics
{
/// \brief A single axis hinge joint.
- class GAZEBO_VISIBLE ODEHingeJoint : public HingeJoint<ODEJoint>
+ class GZ_PHYSICS_ODE_VISIBLE ODEHingeJoint : public HingeJoint<ODEJoint>
{
/// \brief Constructor
/// \param[in] _worldID ODE id of the world.
diff --git a/gazebo/physics/ode/ODEJoint.cc b/gazebo/physics/ode/ODEJoint.cc
index a3b9521..c45f05e 100644
--- a/gazebo/physics/ode/ODEJoint.cc
+++ b/gazebo/physics/ode/ODEJoint.cc
@@ -345,6 +345,11 @@ math::Vector3 ODEJoint::GetLinkTorque(unsigned int _index) const
}
dJointFeedback *jointFeedback = dJointGetFeedback(this->jointId);
+ if (!jointFeedback)
+ {
+ gzerr << "Joint feedback uninitialized" << std::endl;
+ return result;
+ }
if (_index == 0)
result.Set(jointFeedback->t1[0], jointFeedback->t1[1],
@@ -636,6 +641,12 @@ void ODEJoint::Reset()
}
//////////////////////////////////////////////////
+void ODEJoint::CacheForceTorque()
+{
+ // Does nothing for now, will add when recovering pull request #1721
+}
+
+//////////////////////////////////////////////////
JointWrench ODEJoint::GetForceTorque(unsigned int /*_index*/)
{
// Note that:
@@ -673,6 +684,10 @@ JointWrench ODEJoint::GetForceTorque(unsigned int /*_index*/)
this->GetForce(0u) * this->GetLocalAxis(0u);
wrenchAppliedWorld.body1Force = -wrenchAppliedWorld.body2Force;
}
+ else if (this->HasType(physics::Base::FIXED_JOINT))
+ {
+ // no correction are necessary for fixed joint
+ }
else
{
/// \TODO: fix for multi-axis joints
@@ -687,7 +702,10 @@ JointWrench ODEJoint::GetForceTorque(unsigned int /*_index*/)
// convert torque from about child CG to joint anchor location
// cg position specified in child link frame
- math::Pose cgPose = this->childLink->GetInertial()->GetPose();
+ math::Pose cgPose;
+ auto inertial = this->childLink->GetInertial();
+ if (inertial)
+ cgPose = inertial->GetPose();
// anchorPose location of joint in child frame
// childMomentArm: from child CG to joint location in child link frame
@@ -730,7 +748,10 @@ JointWrench ODEJoint::GetForceTorque(unsigned int /*_index*/)
// CG to joint anchor location
// parent cg specified in parent link frame
- math::Pose cgPose = this->parentLink->GetInertial()->GetPose();
+ math::Pose cgPose;
+ auto inertial = this->parentLink->GetInertial();
+ if (inertial)
+ cgPose = inertial->GetPose();
// get parent CG pose in child link frame
math::Pose parentCGInChildLink =
@@ -1073,19 +1094,22 @@ void ODEJoint::SetProvideFeedback(bool _enable)
if (this->provideFeedback)
{
- this->feedback = new dJointFeedback;
- this->feedback->f1[0] = 0;
- this->feedback->f1[1] = 0;
- this->feedback->f1[2] = 0;
- this->feedback->t1[0] = 0;
- this->feedback->t1[1] = 0;
- this->feedback->t1[2] = 0;
- this->feedback->f2[0] = 0;
- this->feedback->f2[1] = 0;
- this->feedback->f2[2] = 0;
- this->feedback->t2[0] = 0;
- this->feedback->t2[1] = 0;
- this->feedback->t2[2] = 0;
+ if (this->feedback == NULL)
+ {
+ this->feedback = new dJointFeedback;
+ this->feedback->f1[0] = 0;
+ this->feedback->f1[1] = 0;
+ this->feedback->f1[2] = 0;
+ this->feedback->t1[0] = 0;
+ this->feedback->t1[1] = 0;
+ this->feedback->t1[2] = 0;
+ this->feedback->f2[0] = 0;
+ this->feedback->f2[1] = 0;
+ this->feedback->f2[2] = 0;
+ this->feedback->t2[0] = 0;
+ this->feedback->t2[1] = 0;
+ this->feedback->t2[2] = 0;
+ }
if (this->jointId)
dJointSetFeedback(this->jointId, this->feedback);
diff --git a/gazebo/physics/ode/ODEJoint.hh b/gazebo/physics/ode/ODEJoint.hh
index eb09a10..65ac01a 100644
--- a/gazebo/physics/ode/ODEJoint.hh
+++ b/gazebo/physics/ode/ODEJoint.hh
@@ -29,7 +29,7 @@ namespace gazebo
namespace physics
{
/// \brief ODE joint interface
- class GAZEBO_VISIBLE ODEJoint : public Joint
+ class GZ_PHYSICS_ODE_VISIBLE ODEJoint : public Joint
{
/// \brief internal variables used for implicit damping
public: enum CFMMode
@@ -61,6 +61,9 @@ namespace gazebo
// Documentation inherited.
public: virtual bool AreConnected(LinkPtr _one, LinkPtr _two) const;
+ // Documentation inherited.
+ public: virtual void CacheForceTorque();
+
/// \brief Get an ODE joint parameter.
///
/// The default function does nothing. This should be
diff --git a/gazebo/physics/ode/ODEJoint_TEST.cc b/gazebo/physics/ode/ODEJoint_TEST.cc
index e53953b..c5c92fd 100644
--- a/gazebo/physics/ode/ODEJoint_TEST.cc
+++ b/gazebo/physics/ode/ODEJoint_TEST.cc
@@ -19,7 +19,7 @@
#include "gazebo/physics/ode/ODETypes.hh"
#include "gazebo/physics/ode/ODEJoint.hh"
#include "gazebo/physics/physics.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#define TOL 1e-6
using namespace gazebo;
diff --git a/gazebo/physics/ode/ODELink.cc b/gazebo/physics/ode/ODELink.cc
index bf007e2..f784ffd 100644
--- a/gazebo/physics/ode/ODELink.cc
+++ b/gazebo/physics/ode/ODELink.cc
@@ -122,10 +122,18 @@ void ODELink::Init()
dGeomSetOffsetQuaternion(g->GetCollisionId(), q);
// Set max_vel and min_depth
- if (g->GetODESurface()->maxVel < 0)
+ boost::any value;
+ if (g->GetODESurface()->maxVel < 0 && this->GetWorld()->
+ GetPhysicsEngine()->GetParam("contact_max_correcting_vel", value))
{
- g->GetODESurface()->maxVel =
- this->GetWorld()->GetPhysicsEngine()->GetContactMaxCorrectingVel();
+ 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);
@@ -182,8 +190,8 @@ void ODELink::MoveCallback(dBodyID _id)
self->dirtyPose.pos -= cog;
- // TODO: this is an ugly line of code. It's like this for speed.
- self->world->dataPtr->dirtyPoses.push_back(self);
+ // Tell the world that our pose has changed.
+ self->world->_AddDirty(self);
// self->poseMutex->unlock();
@@ -589,6 +597,31 @@ void ODELink::AddForceAtWorldPosition(const math::Vector3 &_force,
}
}
+//////////////////////////////////////////////////
+void ODELink::AddLinkForce(const math::Vector3 &_force,
+ const math::Vector3 &_offset)
+{
+ if (this->linkId)
+ {
+ // Force vector represents a direction only, so it should be rotated but
+ // not translated
+ math::Vector3 forceWorld = this->GetWorldPose().rot.RotateVector(_force);
+ // Does this need to be rotated?
+ math::Vector3 offsetCoG = _offset - this->inertial->GetCoG();
+
+ this->SetEnabled(true);
+ dBodyAddForceAtRelPos(this->linkId,
+ forceWorld.x, forceWorld.y, forceWorld.z,
+ offsetCoG.x, offsetCoG.y, offsetCoG.z);
+ }
+ else if (!this->IsStatic())
+ {
+ gzlog << "ODE body for link [" << this->GetScopedName() << "]"
+ << " does not exist, unable to AddLinkForce"
+ << std::endl;
+ }
+}
+
/////////////////////////////////////////////////
void ODELink::AddTorque(const math::Vector3 &_torque)
{
diff --git a/gazebo/physics/ode/ODELink.hh b/gazebo/physics/ode/ODELink.hh
index 82610ca..ea0aa46 100644
--- a/gazebo/physics/ode/ODELink.hh
+++ b/gazebo/physics/ode/ODELink.hh
@@ -31,7 +31,7 @@ namespace gazebo
namespace physics
{
/// \brief ODE Link class.
- class GAZEBO_VISIBLE ODELink : public Link
+ class GZ_PHYSICS_ODE_VISIBLE ODELink : public Link
{
/// \brief Constructor.
/// \param[in] _parent Parent model.
@@ -92,6 +92,10 @@ namespace gazebo
const math::Vector3 &_relpos);
// Documentation inherited
+ public: virtual void AddLinkForce(const math::Vector3 &_force,
+ const math::Vector3 &_offset = math::Vector3::Zero);
+
+ // Documentation inherited
public: virtual void AddTorque(const math::Vector3 &_torque);
// Documentation inherited
diff --git a/gazebo/physics/ode/ODEMesh.hh b/gazebo/physics/ode/ODEMesh.hh
index 8b6fcdd..bdf4b4f 100644
--- a/gazebo/physics/ode/ODEMesh.hh
+++ b/gazebo/physics/ode/ODEMesh.hh
@@ -28,7 +28,7 @@ namespace gazebo
namespace physics
{
/// \brief Triangle mesh helper class.
- class GAZEBO_VISIBLE ODEMesh
+ class GZ_PHYSICS_ODE_VISIBLE ODEMesh
{
/// \brief Constructor.
public: explicit ODEMesh();
diff --git a/gazebo/physics/ode/ODEMeshShape.hh b/gazebo/physics/ode/ODEMeshShape.hh
index 22c172d..c95984c 100644
--- a/gazebo/physics/ode/ODEMeshShape.hh
+++ b/gazebo/physics/ode/ODEMeshShape.hh
@@ -28,7 +28,7 @@ namespace gazebo
class ODEMesh;
/// \brief Triangle mesh collision.
- class GAZEBO_VISIBLE ODEMeshShape : public MeshShape
+ class GZ_PHYSICS_ODE_VISIBLE ODEMeshShape : public MeshShape
{
/// \brief Constructor.
/// \param[in] _parent Parent collision object.
diff --git a/gazebo/physics/ode/ODEMultiRayShape.hh b/gazebo/physics/ode/ODEMultiRayShape.hh
index 983d436..dc7a40a 100644
--- a/gazebo/physics/ode/ODEMultiRayShape.hh
+++ b/gazebo/physics/ode/ODEMultiRayShape.hh
@@ -25,7 +25,7 @@ namespace gazebo
namespace physics
{
/// \brief ODE specific version of MultiRayShape
- class GAZEBO_VISIBLE ODEMultiRayShape : public MultiRayShape
+ class GZ_PHYSICS_ODE_VISIBLE ODEMultiRayShape : public MultiRayShape
{
/// \brief Constructor.
/// \param[in] _parent Parent Collision.
diff --git a/gazebo/physics/ode/ODEPhysics.cc b/gazebo/physics/ode/ODEPhysics.cc
index dfd0a30..de1be62 100644
--- a/gazebo/physics/ode/ODEPhysics.cc
+++ b/gazebo/physics/ode/ODEPhysics.cc
@@ -14,6 +14,13 @@
* 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 <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
@@ -55,6 +62,7 @@
#include "gazebo/physics/ode/ODESliderJoint.hh"
#include "gazebo/physics/ode/ODEBallJoint.hh"
#include "gazebo/physics/ode/ODEUniversalJoint.hh"
+#include "gazebo/physics/ode/ODEFixedJoint.hh"
#include "gazebo/physics/ode/ODERayShape.hh"
#include "gazebo/physics/ode/ODEBoxShape.hh"
@@ -69,6 +77,8 @@
#include "gazebo/physics/ode/ODEPhysics.hh"
#include "gazebo/physics/ode/ODESurfaceParams.hh"
+#include "gazebo/physics/ode/ODEPhysicsPrivate.hh"
+
using namespace gazebo;
using namespace physics;
@@ -121,21 +131,24 @@ class Colliders_TBB
//////////////////////////////////////////////////
ODEPhysics::ODEPhysics(WorldPtr _world)
- : PhysicsEngine(_world), physicsStepFunc(NULL), maxContacts(0)
+ : PhysicsEngine(_world), dataPtr(new ODEPhysicsPrivate)
{
+ this->dataPtr->physicsStepFunc = NULL;
+ this->dataPtr->maxContacts = 0;
+
// Collision detection init
dInitODE2(0);
dAllocateODEDataForThread(dAllocateMaskAll);
- this->worldId = dWorldCreate();
+ this->dataPtr->worldId = dWorldCreate();
- this->spaceId = dHashSpaceCreate(0);
- dHashSpaceSetLevels(this->spaceId, -2, 8);
+ this->dataPtr->spaceId = dHashSpaceCreate(0);
+ dHashSpaceSetLevels(this->dataPtr->spaceId, -2, 8);
- this->contactGroup = dJointGroupCreate(0);
+ this->dataPtr->contactGroup = dJointGroupCreate(0);
- this->colliders.resize(100);
+ this->dataPtr->colliders.resize(100);
// Set random seed for physics engine based on gazebo's random seed.
// Note: this was moved from physics::PhysicsEngine constructor.
@@ -147,27 +160,30 @@ ODEPhysics::~ODEPhysics()
{
dCloseODE();
- dJointGroupDestroy(this->contactGroup);
+ dJointGroupDestroy(this->dataPtr->contactGroup);
// Delete all the joint feedbacks.
for (std::vector<ODEJointFeedback*>::iterator iter =
- this->jointFeedbacks.begin(); iter != this->jointFeedbacks.end(); ++iter)
+ this->dataPtr->jointFeedbacks.begin(); iter !=
+ this->dataPtr->jointFeedbacks.end(); ++iter)
{
delete *iter;
}
- this->jointFeedbacks.clear();
+ this->dataPtr->jointFeedbacks.clear();
- if (this->spaceId)
+ if (this->dataPtr->spaceId)
{
- dSpaceSetCleanup(this->spaceId, 0);
- dSpaceDestroy(this->spaceId);
+ dSpaceSetCleanup(this->dataPtr->spaceId, 0);
+ dSpaceDestroy(this->dataPtr->spaceId);
}
- if (this->worldId)
- dWorldDestroy(this->worldId);
+ if (this->dataPtr->worldId)
+ dWorldDestroy(this->dataPtr->worldId);
- this->spaceId = NULL;
- this->worldId = NULL;
+ this->dataPtr->spaceId = NULL;
+ this->dataPtr->worldId = NULL;
+ delete this->dataPtr;
+ this->dataPtr = NULL;
}
//////////////////////////////////////////////////
@@ -175,68 +191,70 @@ void ODEPhysics::Load(sdf::ElementPtr _sdf)
{
PhysicsEngine::Load(_sdf);
- this->maxContacts = _sdf->Get<unsigned int>("max_contacts");
- this->SetMaxContacts(this->maxContacts);
+ this->dataPtr->maxContacts = _sdf->Get<unsigned int>("max_contacts");
+ this->SetMaxContacts(this->dataPtr->maxContacts);
sdf::ElementPtr odeElem = this->sdf->GetElement("ode");
sdf::ElementPtr solverElem = odeElem->GetElement("solver");
- this->stepType = solverElem->Get<std::string>("type");
+ this->dataPtr->stepType = solverElem->Get<std::string>("type");
if (solverElem->HasElement("use_dynamic_moi_rescaling"))
{
- dWorldSetQuickStepInertiaRatioReduction(this->worldId,
+ dWorldSetQuickStepInertiaRatioReduction(this->dataPtr->worldId,
solverElem->Get<bool>("use_dynamic_moi_rescaling"));
}
else
{
- dWorldSetQuickStepInertiaRatioReduction(this->worldId, true);
+ dWorldSetQuickStepInertiaRatioReduction(this->dataPtr->worldId, true);
}
- dWorldSetDamping(this->worldId, 0.0001, 0.0001);
+ /// \TODO: defaultvelocity decay!? This is BAD if it's true.
+ dWorldSetDamping(this->dataPtr->worldId, 0.0001, 0.0001);
// Help prevent "popping of deeply embedded object
- dWorldSetContactMaxCorrectingVel(this->worldId,
+ dWorldSetContactMaxCorrectingVel(this->dataPtr->worldId,
odeElem->GetElement("constraints")->Get<double>(
"contact_max_correcting_vel"));
// This helps prevent jittering problems.
- dWorldSetContactSurfaceLayer(this->worldId,
+ dWorldSetContactSurfaceLayer(this->dataPtr->worldId,
odeElem->GetElement("constraints")->Get<double>(
"contact_surface_layer"));
// Enable auto-disable by default. Models with joints are excluded from
// auto-disable
- dWorldSetAutoDisableFlag(this->worldId, 1);
+ dWorldSetAutoDisableFlag(this->dataPtr->worldId, 1);
- dWorldSetAutoDisableTime(this->worldId, 1);
- dWorldSetAutoDisableLinearThreshold(this->worldId, 0.1);
- dWorldSetAutoDisableAngularThreshold(this->worldId, 0.1);
- dWorldSetAutoDisableSteps(this->worldId, 5);
+ dWorldSetAutoDisableTime(this->dataPtr->worldId, 1);
+ dWorldSetAutoDisableLinearThreshold(this->dataPtr->worldId, 0.1);
+ dWorldSetAutoDisableAngularThreshold(this->dataPtr->worldId, 0.1);
+ dWorldSetAutoDisableSteps(this->dataPtr->worldId, 5);
math::Vector3 g = this->sdf->Get<math::Vector3>("gravity");
if (g == math::Vector3(0, 0, 0))
gzwarn << "Gravity vector is (0, 0, 0). Objects will float.\n";
- dWorldSetGravity(this->worldId, g.x, g.y, g.z);
+ dWorldSetGravity(this->dataPtr->worldId, g.x, g.y, g.z);
if (odeElem->HasElement("constraints"))
{
- dWorldSetCFM(this->worldId,
+ dWorldSetCFM(this->dataPtr->worldId,
odeElem->GetElement("constraints")->Get<double>("cfm"));
- dWorldSetERP(this->worldId,
+ dWorldSetERP(this->dataPtr->worldId,
odeElem->GetElement("constraints")->Get<double>("erp"));
}
else
- dWorldSetERP(this->worldId, 0.2);
+ dWorldSetERP(this->dataPtr->worldId, 0.2);
- dWorldSetQuickStepNumIterations(this->worldId, this->GetSORPGSIters());
- dWorldSetQuickStepW(this->worldId, this->GetSORPGSW());
+ dWorldSetQuickStepNumIterations(this->dataPtr->worldId,
+ this->GetSORPGSIters());
+ dWorldSetQuickStepW(this->dataPtr->worldId, this->GetSORPGSW());
// Set the physics update function
- this->SetStepType(this->stepType);
- if (this->physicsStepFunc == NULL)
- gzthrow(std::string("Invalid step type[") + this->stepType);
+ this->SetStepType(this->dataPtr->stepType);
+ if (this->dataPtr->physicsStepFunc == NULL)
+ gzthrow(std::string("Invalid step type[") + this->dataPtr->stepType);
}
/////////////////////////////////////////////////
@@ -252,10 +270,18 @@ void ODEPhysics::OnRequest(ConstRequestPtr &_msg)
{
msgs::Physics physicsMsg;
physicsMsg.set_type(msgs::Physics::ODE);
- physicsMsg.set_solver_type(this->stepType);
+ physicsMsg.set_solver_type(this->dataPtr->stepType);
// min_step_size is defined but not yet used
- physicsMsg.set_min_step_size(
- boost::any_cast<double>(this->GetParam("min_step_size")));
+ boost::any min_step_size;
+ try
+ {
+ if (this->GetParam("min_step_size", min_step_size))
+ physicsMsg.set_min_step_size(boost::any_cast<double>(min_step_size));
+ }
+ catch(boost::bad_any_cast &_e)
+ {
+ gzerr << "Failed boost::any_cast in ODEPhysics.cc: " << _e.what();
+ }
physicsMsg.set_precon_iters(this->GetSORPGSPreconIters());
physicsMsg.set_iters(this->GetSORPGSIters());
physicsMsg.set_enable_physics(this->world->GetEnablePhysicsEngine());
@@ -266,7 +292,10 @@ void ODEPhysics::OnRequest(ConstRequestPtr &_msg)
this->GetContactMaxCorrectingVel());
physicsMsg.set_contact_surface_layer(
this->GetContactSurfaceLayer());
- physicsMsg.mutable_gravity()->CopyFrom(msgs::Convert(this->GetGravity()));
+ physicsMsg.mutable_gravity()->CopyFrom(
+ msgs::Convert(this->GetGravity().Ign()));
+ physicsMsg.mutable_magnetic_field()->CopyFrom(
+ msgs::Convert(this->MagneticField()));
physicsMsg.set_real_time_update_rate(this->realTimeUpdateRate);
physicsMsg.set_real_time_factor(this->targetRealTimeFactor);
physicsMsg.set_max_step_size(this->maxStepSize);
@@ -280,6 +309,11 @@ void ODEPhysics::OnRequest(ConstRequestPtr &_msg)
/////////////////////////////////////////////////
void ODEPhysics::OnPhysicsMsg(ConstPhysicsPtr &_msg)
{
+ // Parent class handles many generic parameters
+ // This should be done first so that the profile settings
+ // can be over-ridden by other message parameters.
+ PhysicsEngine::OnPhysicsMsg(_msg);
+
if (_msg->has_solver_type())
this->SetStepType(_msg->solver_type());
@@ -311,7 +345,7 @@ void ODEPhysics::OnPhysicsMsg(ConstPhysicsPtr &_msg)
this->SetContactSurfaceLayer(_msg->contact_surface_layer());
if (_msg->has_gravity())
- this->SetGravity(msgs::Convert(_msg->gravity()));
+ this->SetGravity(msgs::ConvertIgn(_msg->gravity()));
if (_msg->has_real_time_factor())
this->SetTargetRealTimeFactor(_msg->real_time_factor());
@@ -328,9 +362,6 @@ void ODEPhysics::OnPhysicsMsg(ConstPhysicsPtr &_msg)
/// Make sure all models get at least on update cycle.
this->world->EnableAllModels();
-
- // Parent class handles many generic parameters
- PhysicsEngine::OnPhysicsMsg(_msg);
}
@@ -352,35 +383,35 @@ void ODEPhysics::UpdateCollision()
DIAG_TIMER_START("ODEPhysics::UpdateCollision");
boost::recursive_mutex::scoped_lock lock(*this->physicsUpdateMutex);
- dJointGroupEmpty(this->contactGroup);
+ dJointGroupEmpty(this->dataPtr->contactGroup);
unsigned int i = 0;
- this->collidersCount = 0;
- this->trimeshCollidersCount = 0;
- this->jointFeedbackIndex = 0;
+ this->dataPtr->collidersCount = 0;
+ this->dataPtr->trimeshCollidersCount = 0;
+ this->dataPtr->jointFeedbackIndex = 0;
// Reset the contact count
this->contactManager->ResetCount();
// Do collision detection; this will add contacts to the contact group
- dSpaceCollide(this->spaceId, this, CollisionCallback);
+ dSpaceCollide(this->dataPtr->spaceId, this, CollisionCallback);
DIAG_TIMER_LAP("ODEPhysics::UpdateCollision", "dSpaceCollide");
// Generate non-trimesh collisions.
- for (i = 0; i < this->collidersCount; ++i)
+ for (i = 0; i < this->dataPtr->collidersCount; ++i)
{
- this->Collide(this->colliders[i].first,
- this->colliders[i].second, this->contactCollisions);
+ this->Collide(this->dataPtr->colliders[i].first,
+ this->dataPtr->colliders[i].second, this->dataPtr->contactCollisions);
}
DIAG_TIMER_LAP("ODEPhysics::UpdateCollision", "collideShapes");
// Generate trimesh collision.
// This must happen in this thread sequentially
- for (i = 0; i < this->trimeshCollidersCount; ++i)
+ for (i = 0; i < this->dataPtr->trimeshCollidersCount; ++i)
{
- ODECollision *collision1 = this->trimeshColliders[i].first;
- ODECollision *collision2 = this->trimeshColliders[i].second;
- this->Collide(collision1, collision2, this->contactCollisions);
+ ODECollision *collision1 = this->dataPtr->trimeshColliders[i].first;
+ ODECollision *collision2 = this->dataPtr->trimeshColliders[i].second;
+ this->Collide(collision1, collision2, this->dataPtr->contactCollisions);
}
DIAG_TIMER_LAP("ODEPhysics::UpdateCollision", "collideTrimeshes");
@@ -397,36 +428,37 @@ void ODEPhysics::UpdatePhysics()
boost::recursive_mutex::scoped_lock lock(*this->physicsUpdateMutex);
// Update the dynamical model
- (*physicsStepFunc)(this->worldId, this->maxStepSize);
+ (*(this->dataPtr->physicsStepFunc))
+ (this->dataPtr->worldId, this->maxStepSize);
math::Vector3 f1, f2, t1, t2;
// Set the joint contact feedback for each contact.
- for (unsigned int i = 0; i < this->jointFeedbackIndex; ++i)
+ for (unsigned int i = 0; i < this->dataPtr->jointFeedbackIndex; ++i)
{
- Contact *contactFeedback = this->jointFeedbacks[i]->contact;
+ Contact *contactFeedback = this->dataPtr->jointFeedbacks[i]->contact;
Collision *col1 = contactFeedback->collision1;
Collision *col2 = contactFeedback->collision2;
GZ_ASSERT(col1 != NULL, "Collision 1 is NULL");
GZ_ASSERT(col2 != NULL, "Collision 2 is NULL");
- for (int j = 0; j < this->jointFeedbacks[i]->count; ++j)
+ for (int j = 0; j < this->dataPtr->jointFeedbacks[i]->count; ++j)
{
- dJointFeedback fb = this->jointFeedbacks[i]->feedbacks[j];
+ dJointFeedback fb = this->dataPtr->jointFeedbacks[i]->feedbacks[j];
f1.Set(fb.f1[0], fb.f1[1], fb.f1[2]);
f2.Set(fb.f2[0], fb.f2[1], fb.f2[2]);
t1.Set(fb.t1[0], fb.t1[1], fb.t1[2]);
t2.Set(fb.t2[0], fb.t2[1], fb.t2[2]);
// set force torque in link frame
- this->jointFeedbacks[i]->contact->wrench[j].body1Force =
+ this->dataPtr->jointFeedbacks[i]->contact->wrench[j].body1Force =
col1->GetLink()->GetWorldPose().rot.RotateVectorReverse(f1);
- this->jointFeedbacks[i]->contact->wrench[j].body2Force =
+ this->dataPtr->jointFeedbacks[i]->contact->wrench[j].body2Force =
col2->GetLink()->GetWorldPose().rot.RotateVectorReverse(f2);
- this->jointFeedbacks[i]->contact->wrench[j].body1Torque =
+ this->dataPtr->jointFeedbacks[i]->contact->wrench[j].body1Torque =
col1->GetLink()->GetWorldPose().rot.RotateVectorReverse(t1);
- this->jointFeedbacks[i]->contact->wrench[j].body2Torque =
+ this->dataPtr->jointFeedbacks[i]->contact->wrench[j].body2Torque =
col2->GetLink()->GetWorldPose().rot.RotateVectorReverse(t2);
}
}
@@ -446,7 +478,7 @@ void ODEPhysics::Reset()
{
boost::recursive_mutex::scoped_lock lock(*this->physicsUpdateMutex);
// Very important to clear out the contact group
- dJointGroupEmpty(this->contactGroup);
+ dJointGroupEmpty(this->dataPtr->contactGroup);
}
//////////////////////////////////////////////////
@@ -456,14 +488,15 @@ LinkPtr ODEPhysics::CreateLink(ModelPtr _parent)
gzthrow("Link must have a parent\n");
std::map<std::string, dSpaceID>::iterator iter;
- iter = this->spaces.find(_parent->GetName());
+ iter = this->dataPtr->spaces.find(_parent->GetName());
- if (iter == this->spaces.end())
- this->spaces[_parent->GetName()] = dSimpleSpaceCreate(this->spaceId);
+ if (iter == this->dataPtr->spaces.end())
+ this->dataPtr->spaces[_parent->GetName()] =
+ dSimpleSpaceCreate(this->dataPtr->spaceId);
ODELinkPtr link(new ODELink(_parent));
- link->SetSpaceId(this->spaces[_parent->GetName()]);
+ link->SetSpaceId(this->dataPtr->spaces[_parent->GetName()]);
link->SetWorld(_parent->GetWorld());
return link;
@@ -520,7 +553,7 @@ ShapePtr ODEPhysics::CreateShape(const std::string &_type,
//////////////////////////////////////////////////
dWorldID ODEPhysics::GetWorldId()
{
- return this->worldId;
+ return this->dataPtr->worldId;
}
//////////////////////////////////////////////////
@@ -536,12 +569,114 @@ void ODEPhysics::ConvertMass(InertialPtr _inertial, void *_engineMass)
}
//////////////////////////////////////////////////
+Friction_Model ODEPhysics::ConvertFrictionModel(const std::string &_fricModel)
+{
+ Friction_Model result = pyramid_friction;
+ if (_fricModel.compare("pyramid_model") == 0)
+ result = pyramid_friction;
+ else if (_fricModel.compare("cone_model") == 0)
+ result = cone_friction;
+ else if (_fricModel.compare("box_model") == 0)
+ result = box_friction;
+ else
+ gzerr << "Unrecognized friction model ["
+ << _fricModel
+ << "], returning pyramid friction"
+ << std::endl;
+ return result;
+}
+
+//////////////////////////////////////////////////
+std::string ODEPhysics::ConvertFrictionModel(const Friction_Model _fricModel)
+{
+ std::string result;
+ switch (_fricModel)
+ {
+ case pyramid_friction:
+ {
+ result = "pyramid_model";
+ break;
+ }
+ case cone_friction:
+ {
+ result = "cone_model";
+ break;
+ }
+ case box_friction:
+ {
+ result = "box_model";
+ break;
+ }
+ default:
+ {
+ result = "unknown";
+ gzerr << "Unrecognized friction model [" << _fricModel << "]"
+ << std::endl;
+ }
+ }
+ return result;
+}
+
+//////////////////////////////////////////////////
+World_Solver_Type
+ODEPhysics::ConvertWorldStepSolverType(const std::string &_solverType)
+{
+ World_Solver_Type result = ODE_DEFAULT;
+ if (_solverType.compare("ODE_DANTZIG") == 0)
+ result = ODE_DEFAULT;
+ else if (_solverType.compare("DART_PGS") == 0)
+ result = DART_PGS;
+ else if (_solverType.compare("BULLET_PGS") == 0)
+ result = BULLET_PGS;
+ else
+ {
+ gzerr << "Unrecognized world step solver ["
+ << _solverType
+ << "], returning ODE_DANTZIG"
+ << std::endl;
+ }
+ return result;
+}
+
+//////////////////////////////////////////////////
+std::string
+ODEPhysics::ConvertWorldStepSolverType(const World_Solver_Type _solverType)
+{
+ std::string result;
+ switch (_solverType)
+ {
+ case ODE_DEFAULT:
+ {
+ result = "ODE_DANTZIG";
+ break;
+ }
+ case DART_PGS:
+ {
+ result = "DART_PGS";
+ break;
+ }
+ case BULLET_PGS:
+ {
+ result = "BULLET_PGS";
+ break;
+ }
+ default:
+ {
+ result = "unknown";
+ gzerr << "Unrecognized world step solver [" << _solverType << "]"
+ << std::endl;
+ }
+ }
+ return result;
+}
+
+//////////////////////////////////////////////////
void ODEPhysics::SetSORPGSPreconIters(unsigned int _iters)
{
this->sdf->GetElement("ode")->GetElement("solver")->
GetElement("precon_iters")->Set(_iters);
- dWorldSetQuickStepPreconIterations(this->worldId, _iters);
+ dWorldSetQuickStepPreconIterations(this->dataPtr->worldId, _iters);
}
//////////////////////////////////////////////////
@@ -549,7 +684,7 @@ void ODEPhysics::SetSORPGSIters(unsigned int _iters)
{
this->sdf->GetElement("ode")->GetElement(
"solver")->GetElement("iters")->Set(_iters);
- dWorldSetQuickStepNumIterations(this->worldId, _iters);
+ dWorldSetQuickStepNumIterations(this->dataPtr->worldId, _iters);
}
//////////////////////////////////////////////////
@@ -557,7 +692,18 @@ void ODEPhysics::SetSORPGSW(double _w)
{
this->sdf->GetElement("ode")->GetElement(
"solver")->GetElement("sor")->Set(_w);
- dWorldSetQuickStepW(this->worldId, _w);
+ dWorldSetQuickStepW(this->dataPtr->worldId, _w);
+}
+
+//////////////////////////////////////////////////
+void ODEPhysics::SetFrictionModel(const std::string &_fricModel)
+{
+ /// Uncomment this until sdformat changes (sdformat repo issue #96)
+ ///
+ /// this->sdf->GetElement("ode")->GetElement(
+ /// "solver")->GetElement("friction_model")->Set(_fricModel);
+ dWorldSetQuickStepFrictionModel(this->dataPtr->worldId,
+ ConvertFrictionModel(_fricModel));
}
//////////////////////////////////////////////////
@@ -567,7 +713,7 @@ void ODEPhysics::SetWorldCFM(double _cfm)
elem = elem->GetElement("constraints");
elem->GetElement("cfm")->Set(_cfm);
- dWorldSetCFM(this->worldId, _cfm);
+ dWorldSetCFM(this->dataPtr->worldId, _cfm);
}
//////////////////////////////////////////////////
@@ -576,7 +722,7 @@ void ODEPhysics::SetWorldERP(double _erp)
sdf::ElementPtr elem = this->sdf->GetElement("ode");
elem = elem->GetElement("constraints");
elem->GetElement("erp")->Set(_erp);
- dWorldSetERP(this->worldId, _erp);
+ dWorldSetERP(this->dataPtr->worldId, _erp);
}
//////////////////////////////////////////////////
@@ -585,7 +731,7 @@ void ODEPhysics::SetContactMaxCorrectingVel(double _vel)
this->sdf->GetElement("ode")->GetElement(
"constraints")->GetElement(
"contact_max_correcting_vel")->Set(_vel);
- dWorldSetContactMaxCorrectingVel(this->worldId, _vel);
+ dWorldSetContactMaxCorrectingVel(this->dataPtr->worldId, _vel);
}
//////////////////////////////////////////////////
@@ -593,17 +739,24 @@ void ODEPhysics::SetContactSurfaceLayer(double _depth)
{
this->sdf->GetElement("ode")->GetElement(
"constraints")->GetElement("contact_surface_layer")->Set(_depth);
- dWorldSetContactSurfaceLayer(this->worldId, _depth);
+ dWorldSetContactSurfaceLayer(this->dataPtr->worldId, _depth);
}
//////////////////////////////////////////////////
void ODEPhysics::SetMaxContacts(unsigned int _maxContacts)
{
- this->maxContacts = _maxContacts;
+ this->dataPtr->maxContacts = _maxContacts;
this->sdf->GetElement("max_contacts")->GetValue()->Set(_maxContacts);
}
//////////////////////////////////////////////////
+void ODEPhysics::SetWorldStepSolverType(const std::string &_worldSolverType)
+{
+ dWorldSetWorldStepSolverType(this->dataPtr->worldId,
+ ConvertWorldStepSolverType(_worldSolverType));
+}
+
+//////////////////////////////////////////////////
int ODEPhysics::GetSORPGSPreconIters()
{
return this->sdf->GetElement("ode")->GetElement(
@@ -624,6 +777,20 @@ double ODEPhysics::GetSORPGSW()
}
//////////////////////////////////////////////////
+std::string ODEPhysics::GetFrictionModel() const
+{
+ return ConvertFrictionModel(
+ dWorldGetQuickStepFrictionModel(this->dataPtr->worldId));
+}
+
+//////////////////////////////////////////////////
+std::string ODEPhysics::GetWorldStepSolverType() const
+{
+ return ConvertWorldStepSolverType(
+ dWorldGetWorldStepSolverType(this->dataPtr->worldId));
+}
+
+//////////////////////////////////////////////////
double ODEPhysics::GetWorldCFM()
{
sdf::ElementPtr elem = this->sdf->GetElement("ode");
@@ -656,7 +823,7 @@ double ODEPhysics::GetContactSurfaceLayer()
//////////////////////////////////////////////////
unsigned int ODEPhysics::GetMaxContacts()
{
- return this->maxContacts;
+ return this->dataPtr->maxContacts;
}
//////////////////////////////////////////////////
@@ -689,19 +856,21 @@ JointPtr ODEPhysics::CreateJoint(const std::string &_type, ModelPtr _parent)
JointPtr joint;
if (_type == "prismatic")
- joint.reset(new ODESliderJoint(this->worldId, _parent));
+ joint.reset(new ODESliderJoint(this->dataPtr->worldId, _parent));
else if (_type == "screw")
- joint.reset(new ODEScrewJoint(this->worldId, _parent));
+ joint.reset(new ODEScrewJoint(this->dataPtr->worldId, _parent));
else if (_type == "revolute")
- joint.reset(new ODEHingeJoint(this->worldId, _parent));
+ joint.reset(new ODEHingeJoint(this->dataPtr->worldId, _parent));
else if (_type == "gearbox")
- joint.reset(new ODEGearboxJoint(this->worldId, _parent));
+ joint.reset(new ODEGearboxJoint(this->dataPtr->worldId, _parent));
else if (_type == "revolute2")
- joint.reset(new ODEHinge2Joint(this->worldId, _parent));
+ joint.reset(new ODEHinge2Joint(this->dataPtr->worldId, _parent));
else if (_type == "ball")
- joint.reset(new ODEBallJoint(this->worldId, _parent));
+ joint.reset(new ODEBallJoint(this->dataPtr->worldId, _parent));
else if (_type == "universal")
- joint.reset(new ODEUniversalJoint(this->worldId, _parent));
+ joint.reset(new ODEUniversalJoint(this->dataPtr->worldId, _parent));
+ else if (_type == "fixed")
+ joint.reset(new ODEFixedJoint(this->dataPtr->worldId, _parent));
else
gzthrow("Unable to create joint of type[" << _type << "]");
@@ -711,7 +880,7 @@ JointPtr ODEPhysics::CreateJoint(const std::string &_type, ModelPtr _parent)
//////////////////////////////////////////////////
dSpaceID ODEPhysics::GetSpaceId() const
{
- return this->spaceId;
+ return this->dataPtr->spaceId;
}
//////////////////////////////////////////////////
@@ -726,22 +895,23 @@ void ODEPhysics::SetStepType(const std::string &_type)
{
sdf::ElementPtr elem = this->sdf->GetElement("ode")->GetElement("solver");
elem->GetElement("type")->Set(_type);
- this->stepType = _type;
+ this->dataPtr->stepType = _type;
// Set the physics update function
- if (this->stepType == "quick")
- this->physicsStepFunc = &dWorldQuickStep;
- else if (this->stepType == "world")
- this->physicsStepFunc = &dWorldStep;
+ if (this->dataPtr->stepType == "quick")
+ this->dataPtr->physicsStepFunc = &dWorldQuickStep;
+ else if (this->dataPtr->stepType == "world")
+ this->dataPtr->physicsStepFunc = &dWorldStep;
else
- gzerr << "Invalid step type[" << this->stepType << "]" << std::endl;
+ gzerr << "Invalid step type[" << this->dataPtr->stepType
+ << "]" << std::endl;
}
//////////////////////////////////////////////////
void ODEPhysics::SetGravity(const gazebo::math::Vector3 &_gravity)
{
this->sdf->GetElement("gravity")->Set(_gravity);
- dWorldSetGravity(this->worldId, _gravity.x, _gravity.y, _gravity.z);
+ dWorldSetGravity(this->dataPtr->worldId, _gravity.x, _gravity.y, _gravity.z);
}
//////////////////////////////////////////////////
@@ -810,6 +980,11 @@ void ODEPhysics::CollisionCallback(void *_data, dGeomID _o1, dGeomID _o2)
void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
dContactGeom *_contactCollisions)
{
+ // Filter collisions based on collide bitmask.
+ if ((_collision1->GetSurface()->collideBitmask &
+ _collision2->GetSurface()->collideBitmask) == 0)
+ return;
+
// Filter collisions based on contact bitmask if collide_without_contact is
// on.The bitmask is set mainly for speed improvements otherwise a collision
// with collide_without_contact may potentially generate a large number of
@@ -827,16 +1002,20 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
/*
if (_collision1->GetCollisionId() && _collision2->GetCollisionId())
{
- const dVector3 *pos1 = (const dVector3*)dGeomGetPosition(_collision1->GetCollisionId());
- const dVector3 *pos2 = (const dVector3*)dGeomGetPosition(_collision2->GetCollisionId());
- std::cout << "1[" << (*pos1)[0]<< " " << (*pos1)[1] << " " << (*pos1)[2] << "] "
+ const dVector3 *pos1 =
+ (const dVector3*)dGeomGetPosition(_collision1->GetCollisionId());
+ const dVector3 *pos2 =
+ (const dVector3*)dGeomGetPosition(_collision2->GetCollisionId());
+ std::cout << "1[" << (*pos1)[0]<< " " << (*pos1)[1] << " "
+ << (*pos1)[2] << "] "
<< "2[" << (*pos2)[0]<< " " << (*pos2)[1] << " " << (*pos2)[2] << "]\n";
}*/
unsigned int numc = 0;
dContact contact;
- // maxCollide must less than the size of this->indices. Check the header
+ // maxCollide must less than the size of this->dataPtr->indices
+ // Check the header
unsigned int maxCollide = MAX_CONTACT_JOINTS;
// max_contacts specified globally
@@ -860,7 +1039,7 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
// Store the indices of the contacts.
for (int i = 0; i < MAX_CONTACT_JOINTS; i++)
- this->indices[i] = i;
+ this->dataPtr->indices[i] = i;
// Choose only the best contacts if too many were generated.
if (numc > maxCollide)
@@ -871,7 +1050,7 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
if (_contactCollisions[i].depth > max)
{
max = _contactCollisions[i].depth;
- this->indices[maxCollide-1] = i;
+ this->dataPtr->indices[maxCollide-1] = i;
}
}
@@ -907,7 +1086,7 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
// _collision2->surface->softCFM);
// assign fdir1 if not set as 0
- math::Vector3 fd = surf1->frictionPyramid.direction1;
+ math::Vector3 fd = surf1->GetFrictionPyramid()->direction1;
if (fd != math::Vector3::Zero)
{
// fdir1 is in body local frame, rotate it into world frame
@@ -921,10 +1100,10 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
/// As a hack, we'll simply compare mu1 from
/// both surfaces for now, and use fdir1 specified by
/// surface with smaller mu1.
- math::Vector3 fd2 = surf2->frictionPyramid.direction1;
+ math::Vector3 fd2 = surf2->GetFrictionPyramid()->direction1;
if (fd2 != math::Vector3::Zero && (fd == math::Vector3::Zero ||
- surf1->frictionPyramid.GetMuPrimary() >
- surf2->frictionPyramid.GetMuPrimary()))
+ surf1->GetFrictionPyramid()->GetMuPrimary() >
+ surf2->GetFrictionPyramid()->GetMuPrimary()))
{
// fdir1 is in body local frame, rotate it into world frame
fd2 = _collision2->GetWorldPose().rot.RotateVector(fd2);
@@ -947,10 +1126,10 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
}
// Set the friction coefficients.
- contact.surface.mu = std::min(surf1->frictionPyramid.GetMuPrimary(),
- surf2->frictionPyramid.GetMuPrimary());
- contact.surface.mu2 = std::min(surf1->frictionPyramid.GetMuSecondary(),
- surf2->frictionPyramid.GetMuSecondary());
+ contact.surface.mu = std::min(surf1->GetFrictionPyramid()->GetMuPrimary(),
+ surf2->GetFrictionPyramid()->GetMuPrimary());
+ contact.surface.mu2 = std::min(surf1->GetFrictionPyramid()->GetMuSecondary(),
+ surf2->GetFrictionPyramid()->GetMuSecondary());
// Set the slip values
@@ -980,15 +1159,17 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
// Create a joint feedback mechanism
if (contactFeedback)
{
- if (this->jointFeedbackIndex < this->jointFeedbacks.size())
- jointFeedback = this->jointFeedbacks[this->jointFeedbackIndex];
+ if (this->dataPtr->jointFeedbackIndex <
+ this->dataPtr->jointFeedbacks.size())
+ jointFeedback =
+ this->dataPtr->jointFeedbacks[this->dataPtr->jointFeedbackIndex];
else
{
jointFeedback = new ODEJointFeedback();
- this->jointFeedbacks.push_back(jointFeedback);
+ this->dataPtr->jointFeedbacks.push_back(jointFeedback);
}
- this->jointFeedbackIndex++;
+ this->dataPtr->jointFeedbackIndex++;
jointFeedback->count = 0;
jointFeedback->contact = contactFeedback;
}
@@ -996,30 +1177,31 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
// Create a joint for each contact
for (unsigned int j = 0; j < numc; ++j)
{
- contact.geom = _contactCollisions[this->indices[j]];
+ contact.geom = _contactCollisions[this->dataPtr->indices[j]];
// Create the contact joint. This introduces the contact constraint to
// ODE
- dJointID contactJoint =
- dJointCreateContact(this->worldId, this->contactGroup, &contact);
+ dJointID contactJoint = dJointCreateContact(this->dataPtr->worldId,
+ this->dataPtr->contactGroup, &contact);
// Store contact information.
if (contactFeedback && jointFeedback)
{
// Store the contact depth
- contactFeedback->depths[j] = _contactCollisions[this->indices[j]].depth;
+ contactFeedback->depths[j] =
+ _contactCollisions[this->dataPtr->indices[j]].depth;
// Store the contact position
contactFeedback->positions[j].Set(
- _contactCollisions[this->indices[j]].pos[0],
- _contactCollisions[this->indices[j]].pos[1],
- _contactCollisions[this->indices[j]].pos[2]);
+ _contactCollisions[this->dataPtr->indices[j]].pos[0],
+ _contactCollisions[this->dataPtr->indices[j]].pos[1],
+ _contactCollisions[this->dataPtr->indices[j]].pos[2]);
// Store the contact normal
contactFeedback->normals[j].Set(
- _contactCollisions[this->indices[j]].normal[0],
- _contactCollisions[this->indices[j]].normal[1],
- _contactCollisions[this->indices[j]].normal[2]);
+ _contactCollisions[this->dataPtr->indices[j]].normal[0],
+ _contactCollisions[this->dataPtr->indices[j]].normal[1],
+ _contactCollisions[this->dataPtr->indices[j]].normal[2]);
// Set the joint feedback.
dJointSetFeedback(contactJoint, &(jointFeedback->feedbacks[j]));
@@ -1040,34 +1222,39 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
void ODEPhysics::AddTrimeshCollider(ODECollision *_collision1,
ODECollision *_collision2)
{
- if (this->trimeshCollidersCount >= this->trimeshColliders.size())
- this->trimeshColliders.resize(this->trimeshColliders.size() + 100);
-
- this->trimeshColliders[this->trimeshCollidersCount].first = _collision1;
- this->trimeshColliders[this->trimeshCollidersCount].second = _collision2;
- this->trimeshCollidersCount++;
+ if (this->dataPtr->trimeshCollidersCount >=
+ this->dataPtr->trimeshColliders.size())
+ this->dataPtr->trimeshColliders.resize(
+ this->dataPtr->trimeshColliders.size() + 100);
+
+ this->dataPtr->trimeshColliders[this->dataPtr->trimeshCollidersCount].first =
+ _collision1;
+ this->dataPtr->trimeshColliders[this->dataPtr->trimeshCollidersCount].second =
+ _collision2;
+ this->dataPtr->trimeshCollidersCount++;
}
/////////////////////////////////////////////////
void ODEPhysics::AddCollider(ODECollision *_collision1,
ODECollision *_collision2)
{
- if (this->collidersCount >= this->colliders.size())
- this->colliders.resize(this->colliders.size() + 100);
+ if (this->dataPtr->collidersCount >= this->dataPtr->colliders.size())
+ this->dataPtr->colliders.resize(this->dataPtr->colliders.size() + 100);
- this->colliders[this->collidersCount].first = _collision1;
- this->colliders[this->collidersCount].second = _collision2;
- this->collidersCount++;
+ this->dataPtr->colliders[this->dataPtr->collidersCount].first = _collision1;
+ this->dataPtr->colliders[this->dataPtr->collidersCount].second = _collision2;
+ this->dataPtr->collidersCount++;
}
/////////////////////////////////////////////////
void ODEPhysics::DebugPrint() const
{
dBodyID b;
- std::cout << "Debug Print[" << dWorldGetBodyCount(this->worldId) << "]\n";
- for (int i = 0; i < dWorldGetBodyCount(this->worldId); ++i)
+ std::cout << "Debug Print[" <<
+ dWorldGetBodyCount(this->dataPtr->worldId) << "]\n";
+ for (int i = 0; i < dWorldGetBodyCount(this->dataPtr->worldId); ++i)
{
- b = dWorldGetBody(this->worldId, i);
+ b = dWorldGetBody(this->dataPtr->worldId, i);
ODELink *link = static_cast<ODELink*>(dBodyGetData(b));
math::Pose pose = link->GetWorldPose();
const dReal *pos = dBodyGetPosition(b);
@@ -1123,217 +1310,128 @@ bool ODEPhysics::SetParam(const std::string &_key, const boost::any &_value)
sdf::ElementPtr odeElem = this->sdf->GetElement("ode");
GZ_ASSERT(odeElem != NULL, "ODE SDF element does not exist");
- if (_key == "solver_type")
+ try
{
- std::string value;
- try
+ if (_key == "solver_type")
{
- value = boost::any_cast<std::string>(_value);
+ this->SetStepType(boost::any_cast<std::string>(_value));
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "cfm")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ double value = boost::any_cast<double>(_value);
+ odeElem->GetElement("constraints")->GetElement("cfm")->Set(value);
+ dWorldSetCFM(this->dataPtr->worldId, value);
}
- this->SetStepType(value);
- }
- else if (_key == "cfm")
- {
- double value;
- try
+ else if (_key == "erp")
{
- value = boost::any_cast<double>(_value);
+ double value = boost::any_cast<double>(_value);
+ odeElem->GetElement("constraints")->GetElement("erp")->Set(value);
+ dWorldSetERP(this->dataPtr->worldId, value);
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "precon_iters")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ int value = boost::any_cast<int>(_value);
+ odeElem->GetElement("solver")->GetElement("precon_iters")->Set(value);
+ dWorldSetQuickStepPreconIterations(this->dataPtr->worldId, value);
}
- odeElem->GetElement("constraints")->GetElement("cfm")->Set(value);
- dWorldSetCFM(this->worldId, value);
- }
- else if (_key == "erp")
- {
- double value;
- try
+ else if (_key == "iters")
{
- value = boost::any_cast<double>(_value);
+ int value = boost::any_cast<int>(_value);
+ odeElem->GetElement("solver")->GetElement("iters")->Set(value);
+ dWorldSetQuickStepNumIterations(this->dataPtr->worldId, value);
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "sor")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ double value = boost::any_cast<double>(_value);
+ odeElem->GetElement("solver")->GetElement("sor")->Set(value);
+ dWorldSetQuickStepW(this->dataPtr->worldId, value);
}
- odeElem->GetElement("constraints")->GetElement("erp")->Set(value);
- dWorldSetERP(this->worldId, value);
- }
- else if (_key == "precon_iters")
- {
- int value;
- try
+ else if (_key == "friction_model")
+ this->SetFrictionModel(boost::any_cast<std::string>(_value));
+ else if (_key == "world_step_solver")
+ this->SetWorldStepSolverType(boost::any_cast<std::string>(_value));
+ else if (_key == "contact_max_correcting_vel")
{
- try
- {
- value = boost::any_cast<int>(_value);
- }
- catch(const boost::bad_any_cast &e)
- {
- value = boost::any_cast<unsigned int>(_value);
- }
+ double value = boost::any_cast<double>(_value);
+ odeElem->GetElement("constraints")->GetElement(
+ "contact_max_correcting_vel")->Set(value);
+ dWorldSetContactMaxCorrectingVel(this->dataPtr->worldId, value);
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "contact_surface_layer")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ double value = boost::any_cast<double>(_value);
+ odeElem->GetElement("constraints")->GetElement(
+ "contact_surface_layer")->Set(value);
+ dWorldSetContactSurfaceLayer(this->dataPtr->worldId, value);
}
- odeElem->GetElement("solver")->GetElement("precon_iters")->Set(value);
- dWorldSetQuickStepPreconIterations(this->worldId, value);
- }
- else if (_key == "iters")
- {
- int value;
- try
+ else if (_key == "max_contacts")
{
- try
- {
- value = boost::any_cast<int>(_value);
- }
- catch(const boost::bad_any_cast &e)
- {
- value = boost::any_cast<unsigned int>(_value);
- }
+ int value = boost::any_cast<int>(_value);
+ this->sdf->GetElement("max_contacts")->GetValue()->Set(value);
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "min_step_size")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ /// TODO: Implement min step size param
+ double value = boost::any_cast<double>(_value);
+ odeElem->GetElement("solver")->GetElement("min_step_size")->Set(value);
}
- odeElem->GetElement("solver")->GetElement("iters")->Set(value);
- dWorldSetQuickStepNumIterations(this->worldId, value);
- }
- else if (_key == "sor")
- {
- double value = boost::any_cast<double>(_value);
- odeElem->GetElement("solver")->GetElement("sor")->Set(value);
- dWorldSetQuickStepW(this->worldId, value);
- }
- else if (_key == "contact_max_correcting_vel")
- {
- double value;
- try
+ else if (_key == "sor_lcp_tolerance")
{
- value = boost::any_cast<double>(_value);
+ dWorldSetQuickStepTolerance(this->dataPtr->worldId,
+ boost::any_cast<double>(_value));
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "rms_error_tolerance")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ gzwarn << "please use sor_lcp_tolerance in the future.\n";
+ dWorldSetQuickStepTolerance(this->dataPtr->worldId,
+ boost::any_cast<double>(_value));
}
- odeElem->GetElement("constraints")->GetElement(
- "contact_max_correcting_vel")->Set(value);
- dWorldSetContactMaxCorrectingVel(this->worldId, value);
- }
- else if (_key == "contact_surface_layer")
- {
- double value;
- try
+ else if (_key == "inertia_ratio_reduction" ||
+ _key == "use_dynamic_moi_rescaling")
{
- value = boost::any_cast<double>(_value);
+ bool value = boost::any_cast<bool>(_value);
+ dWorldSetQuickStepInertiaRatioReduction(this->dataPtr->worldId, value);
+ if (odeElem->GetElement("solver")->HasElement(
+ "use_dynamic_moi_rescaling"))
+ {
+ odeElem->GetElement("solver")->GetElement(
+ "use_dynamic_moi_rescaling")->Set(value);
+ }
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "contact_residual_smoothing")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ dWorldSetQuickStepContactResidualSmoothing(this->dataPtr->worldId,
+ boost::any_cast<double>(_value));
}
- odeElem->GetElement("constraints")->GetElement(
- "contact_surface_layer")->Set(value);
- dWorldSetContactSurfaceLayer(this->worldId, value);
- }
- else if (_key == "max_contacts")
- {
- int value;
- try
+ else if (_key == "thread_position_correction")
{
- try
- {
- value = boost::any_cast<int>(_value);
- }
- catch(const boost::bad_any_cast &e)
- {
- value = boost::any_cast<unsigned int>(_value);
- }
+ dWorldSetQuickStepThreadPositionCorrection(this->dataPtr->worldId,
+ boost::any_cast<bool>(_value));
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "experimental_row_reordering")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ dWorldSetQuickStepExperimentalRowReordering(this->dataPtr->worldId,
+ boost::any_cast<bool>(_value));
}
- this->sdf->GetElement("max_contacts")->GetValue()->Set(value);
- }
- else if (_key == "min_step_size")
- {
- /// TODO: Implement min step size param
- double value;
- try
+ else if (_key == "warm_start_factor")
{
- value = boost::any_cast<double>(_value);
+ dWorldSetQuickStepWarmStartFactor(this->dataPtr->worldId,
+ boost::any_cast<double>(_value));
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "extra_friction_iterations")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ dWorldSetQuickStepExtraFrictionIterations(this->dataPtr->worldId,
+ boost::any_cast<int>(_value));
}
- odeElem->GetElement("solver")->GetElement("min_step_size")->Set(value);
- }
- else if (_key == "max_step_size")
- {
- this->SetMaxStepSize(boost::any_cast<double>(_value));
- }
- else if (_key == "sor_lcp_tolerance")
- {
- dWorldSetQuickStepTolerance(this->worldId,
- boost::any_cast<double>(_value));
- }
- else if (_key == "rms_error_tolerance")
- {
- gzwarn << "please use sor_lcp_tolerance in the future.\n";
- dWorldSetQuickStepTolerance(this->worldId,
- boost::any_cast<double>(_value));
- }
- else if (_key == "inertia_ratio_reduction")
- {
- bool value = boost::any_cast<bool>(_value);
- dWorldSetQuickStepInertiaRatioReduction(this->worldId, value);
- if (odeElem->GetElement("solver")->HasElement(
- "use_dynamic_moi_rescaling"))
+ else
{
- odeElem->GetElement("solver")->GetElement(
- "use_dynamic_moi_rescaling")->Set(value);
+ return PhysicsEngine::SetParam(_key, _value);
}
}
- else if (_key == "contact_residual_smoothing")
- {
- dWorldSetQuickStepContactResidualSmoothing(this->worldId,
- boost::any_cast<double>(_value));
- }
- else if (_key == "experimental_row_reordering")
- {
- dWorldSetQuickStepExperimentalRowReordering(this->worldId,
- boost::any_cast<bool>(_value));
- }
- else if (_key == "warm_start_factor")
- {
- dWorldSetQuickStepWarmStartFactor(this->worldId,
- boost::any_cast<double>(_value));
- }
- else if (_key == "extra_friction_iterations")
+ catch(boost::bad_any_cast &e)
{
- dWorldSetQuickStepExtraFrictionIterations(this->worldId,
- boost::any_cast<int>(_value));
- }
- else
- {
- gzwarn << _key << " is not supported in ode" << std::endl;
+ gzerr << "ODEPhysics::SetParam(" << _key << ") boost::any_cast error: "
+ << e.what() << std::endl;
return false;
}
return true;
@@ -1342,63 +1440,79 @@ bool ODEPhysics::SetParam(const std::string &_key, const boost::any &_value)
//////////////////////////////////////////////////
boost::any ODEPhysics::GetParam(const std::string &_key) const
{
+ boost::any value;
+ this->GetParam(_key, value);
+ return value;
+}
+
+//////////////////////////////////////////////////
+bool ODEPhysics::GetParam(const std::string &_key, boost::any &_value) const
+{
sdf::ElementPtr odeElem = this->sdf->GetElement("ode");
GZ_ASSERT(odeElem != NULL, "ODE SDF element does not exist");
if (_key == "solver_type")
{
- return odeElem->GetElement("solver")->Get<std::string>("type");
+ _value = odeElem->GetElement("solver")->Get<std::string>("type");
}
else if (_key == "cfm")
{
- return odeElem->GetElement("constraints")->Get<double>("cfm");
+ _value = odeElem->GetElement("constraints")->Get<double>("cfm");
}
else if (_key == "erp")
- return odeElem->GetElement("constraints")->Get<double>("erp");
+ _value = odeElem->GetElement("constraints")->Get<double>("erp");
else if (_key == "precon_iters")
- return odeElem->GetElement("solver")->Get<int>("precon_iters");
+ _value = odeElem->GetElement("solver")->Get<int>("precon_iters");
else if (_key == "iters")
- return odeElem->GetElement("solver")->Get<int>("iters");
+ _value = odeElem->GetElement("solver")->Get<int>("iters");
else if (_key == "sor")
- return odeElem->GetElement("solver")->Get<double>("sor");
+ _value = odeElem->GetElement("solver")->Get<double>("sor");
else if (_key == "contact_max_correcting_vel")
- return odeElem->GetElement("constraints")->Get<double>(
+ _value = odeElem->GetElement("constraints")->Get<double>(
"contact_max_correcting_vel");
else if (_key == "contact_surface_layer")
- return odeElem->GetElement("constraints")->Get<double>(
+ _value = odeElem->GetElement("constraints")->Get<double>(
"contact_surface_layer");
else if (_key == "max_contacts")
- return this->sdf->Get<int>("max_contacts");
+ _value = this->sdf->Get<int>("max_contacts");
else if (_key == "min_step_size")
- return odeElem->GetElement("solver")->Get<double>("min_step_size");
- else if (_key == "max_step_size")
- return this->GetMaxStepSize();
+ _value = odeElem->GetElement("solver")->Get<double>("min_step_size");
else if (_key == "sor_lcp_tolerance")
- return dWorldGetQuickStepTolerance(this->worldId);
+ _value = dWorldGetQuickStepTolerance(this->dataPtr->worldId);
else if (_key == "rms_error_tolerance")
{
gzwarn << "please use sor_lcp_tolerance in the future.\n";
- return dWorldGetQuickStepTolerance(this->worldId);
+ _value = dWorldGetQuickStepTolerance(this->dataPtr->worldId);
}
else if (_key == "rms_error")
- return dWorldGetQuickStepRMSDeltaLambda(this->worldId);
+ _value = dWorldGetQuickStepRMSDeltaLambda(this->dataPtr->worldId);
else if (_key == "constraint_residual")
- return dWorldGetQuickStepRMSConstraintResidual(this->worldId);
+ _value = dWorldGetQuickStepRMSConstraintResidual(this->dataPtr->worldId);
else if (_key == "num_contacts")
- return dWorldGetQuickStepNumContacts(this->worldId);
- else if (_key == "inertia_ratio_reduction")
- return dWorldGetQuickStepInertiaRatioReduction(this->worldId);
+ _value = dWorldGetQuickStepNumContacts(this->dataPtr->worldId);
+ else if (_key == "inertia_ratio_reduction" ||
+ _key == "use_dynamic_moi_rescaling")
+ _value = dWorldGetQuickStepInertiaRatioReduction(this->dataPtr->worldId);
else if (_key == "contact_residual_smoothing")
- return dWorldGetQuickStepContactResidualSmoothing (this->worldId);
+ _value = dWorldGetQuickStepContactResidualSmoothing(this->dataPtr->worldId);
+ else if (_key == "thread_position_correction")
+ _value = dWorldGetQuickStepThreadPositionCorrection(this->dataPtr->worldId);
else if (_key == "experimental_row_reordering")
- return dWorldGetQuickStepExperimentalRowReordering (this->worldId);
+ {
+ _value = dWorldGetQuickStepExperimentalRowReordering
+ (this->dataPtr->worldId);
+ }
else if (_key == "warm_start_factor")
- return dWorldGetQuickStepWarmStartFactor (this->worldId);
+ _value = dWorldGetQuickStepWarmStartFactor(this->dataPtr->worldId);
else if (_key == "extra_friction_iterations")
- return dWorldGetQuickStepExtraFrictionIterations (this->worldId);
+ _value = dWorldGetQuickStepExtraFrictionIterations(this->dataPtr->worldId);
+ else if (_key == "friction_model")
+ _value = this->GetFrictionModel();
+ else if (_key == "world_step_solver")
+ _value = this->GetWorldStepSolverType();
else
{
- gzwarn << _key << " is not supported in ode" << std::endl;
- return 0;
+ return PhysicsEngine::GetParam(_key, _value);
}
+ return true;
}
diff --git a/gazebo/physics/ode/ODEPhysics.hh b/gazebo/physics/ode/ODEPhysics.hh
index a11ee5a..79c6a08 100644
--- a/gazebo/physics/ode/ODEPhysics.hh
+++ b/gazebo/physics/ode/ODEPhysics.hh
@@ -19,9 +19,7 @@
#include <tbb/spin_mutex.h>
#include <tbb/concurrent_vector.h>
-#include <map>
#include <string>
-#include <vector>
#include <utility>
#include <boost/thread/thread.hpp>
@@ -38,23 +36,11 @@ namespace gazebo
{
namespace physics
{
- /// \brief Data structure for contact feedbacks
- class GAZEBO_VISIBLE ODEJointFeedback
- {
- public: ODEJointFeedback() : contact(NULL), count(0) {}
-
- /// \brief Contact information.
- public: Contact *contact;
-
- /// \brief Number of elements in feedbacks array.
- public: int count;
-
- /// \brief Contact joint feedback information.
- public: dJointFeedback feedbacks[MAX_CONTACT_JOINTS];
- };
+ class ODEJointFeedback;
+ class ODEPhysicsPrivate;
/// \brief ODE physics engine.
- class GAZEBO_VISIBLE ODEPhysics : public PhysicsEngine
+ class GZ_PHYSICS_ODE_VISIBLE ODEPhysics : public PhysicsEngine
{
/// \enum ODEParam
/// \brief ODE Physics parameter types.
@@ -92,7 +78,13 @@ namespace gazebo
/// \brief Limit ratios of inertias of adjacent links (note that the
/// corresponding SDF tag is "use_dynamic_moi_rescaling")
- INERTIA_RATIO_REDUCTION
+ INERTIA_RATIO_REDUCTION,
+
+ /// \brief friction model
+ FRICTION_MODEL,
+
+ /// \brief LCP Solver
+ WORLD_SOLVER_TYPE
};
/// \brief Constructor.
@@ -166,6 +158,15 @@ namespace gazebo
// Documentation inherited
public: virtual void SetContactSurfaceLayer(double layer_depth);
+ /// \brief Set friction model type.
+ /// \param[in] _fricModel Type of friction model.
+ public: virtual void SetFrictionModel(const std::string &_fricModel);
+
+ /// \brief Set world step solver type.
+ /// \param[in] _worldSolverType Type of solver used by world step.
+ public: virtual void
+ SetWorldStepSolverType(const std::string &_worldSolverType);
+
// Documentation inherited
public: virtual void SetMaxContacts(unsigned int max_contacts);
@@ -187,6 +188,14 @@ namespace gazebo
// Documentation inherited
public: virtual double GetContactMaxCorrectingVel();
+ /// \brief Get friction model.
+ /// \return Friction model type.
+ public: virtual std::string GetFrictionModel() const;
+
+ /// \brief Get solver type for world step.
+ /// \return Type of solver used by world step.
+ public: virtual std::string GetWorldStepSolverType() const;
+
// Documentation inherited
public: virtual double GetContactSurfaceLayer();
@@ -206,6 +215,10 @@ namespace gazebo
/// Documentation inherited
public: virtual boost::any GetParam(const std::string &_key) const;
+ /// Documentation inherited
+ public: virtual bool GetParam(const std::string &_key,
+ boost::any &_value) const;
+
/// \brief Return the world space id.
/// \return The space id for the world.
public: dSpaceID GetSpaceId() const;
@@ -225,6 +238,34 @@ namespace gazebo
/// converted.
public: static void ConvertMass(void *_odeMass, InertialPtr _inertial);
+ /// \brief Convert a string to a Friction_Model enum.
+ /// \param[in] _fricModel Friction model string.
+ /// \return A Friction_Model enum. Defaults to pyramid_friction
+ /// if _fricModel is unrecognized.
+ public: static Friction_Model
+ ConvertFrictionModel(const std::string &_fricModel);
+
+ /// \brief Convert a Friction_Model enum to a string.
+ /// \param[in] _fricModel Friction_Model enum.
+ /// \return Friction model string. Returns "unknown" if
+ /// _fricModel is unrecognized.
+ public: static std::string
+ ConvertFrictionModel(const Friction_Model _fricModel);
+
+ /// \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
+ /// _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.
+ /// \return A World_Solver_Type enum. Defaults to ODE_DEFAULT
+ /// if _solverType is unrecognized.
+ public: static World_Solver_Type
+ ConvertWorldStepSolverType(const std::string &_solverType);
+
/// \brief Get the step type (quick, world).
/// \return The step type.
public: virtual std::string GetStepType() const;
@@ -269,51 +310,9 @@ namespace gazebo
private: void AddCollider(ODECollision *_collision1,
ODECollision *_collision2);
- /// \brief Top-level world for all bodies
- private: dWorldID worldId;
-
- /// \brief Top-level space for all sub-spaces/collisions
- private: dSpaceID spaceId;
-
- /// \brief Collision attributes
- private: dJointGroupID contactGroup;
-
- /// \brief The type of the solver.
- private: std::string stepType;
-
- /// \brief Buffer of contact feedback information.
- private: std::vector<ODEJointFeedback*> jointFeedbacks;
-
- /// \brief Current index into the contactFeedbacks buffer
- private: unsigned int jointFeedbackIndex;
-
- /// \brief All the collsiion spaces.
- private: std::map<std::string, dSpaceID> spaces;
-
- /// \brief All the normal colliders.
- private: std::vector< std::pair<ODECollision*, ODECollision*> > colliders;
-
- /// \brief All the triangle mesh colliders.
- private: std::vector< std::pair<ODECollision*, ODECollision*> >
- trimeshColliders;
-
- /// \brief Number of normal colliders.
- private: unsigned int collidersCount;
-
- /// \brief Number of triangle mesh colliders.
- private: unsigned int trimeshCollidersCount;
-
- /// \brief Array of contact collisions.
- private: dContactGeom contactCollisions[MAX_COLLIDE_RETURNS];
-
- /// \brief Physics step function.
- private: int (*physicsStepFunc)(dxWorld*, dReal);
-
- /// \brief Indices used during creation of contact joints.
- private: int indices[MAX_CONTACT_JOINTS];
-
- /// \brief Maximum number of contact points per collision pair.
- private: unsigned int maxContacts;
+ /// \internal
+ /// \brief Private data pointer.
+ private: ODEPhysicsPrivate *dataPtr;
};
}
}
diff --git a/gazebo/physics/ode/ODEPhysicsPrivate.hh b/gazebo/physics/ode/ODEPhysicsPrivate.hh
new file mode 100644
index 0000000..e6b563c
--- /dev/null
+++ b/gazebo/physics/ode/ODEPhysicsPrivate.hh
@@ -0,0 +1,99 @@
+/*
+ * Copyright (C) 2012-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 _ODEPHYSICS_PRIVATE_HH_
+#define _ODEPHYSICS_PRIVATE_HH_
+
+#include <map>
+#include <string>
+#include <vector>
+#include <utility>
+
+#include "gazebo/physics/Contact.hh"
+#include "gazebo/physics/ode/ODETypes.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \brief Data structure for contact feedbacks
+ class ODEJointFeedback
+ {
+ public: ODEJointFeedback() : contact(NULL), count(0) {}
+
+ /// \brief Contact information.
+ public: Contact *contact;
+
+ /// \brief Number of elements in feedbacks array.
+ public: int count;
+
+ /// \brief Contact joint feedback information.
+ public: dJointFeedback feedbacks[MAX_CONTACT_JOINTS];
+ };
+
+ class ODEPhysicsPrivate
+ {
+ /// \brief Top-level world for all bodies
+ public: dWorldID worldId;
+
+ /// \brief Top-level space for all sub-spaces/collisions
+ public: dSpaceID spaceId;
+
+ /// \brief Collision attributes
+ public: dJointGroupID contactGroup;
+
+ /// \brief The type of the solver.
+ public: std::string stepType;
+
+ /// \brief Buffer of contact feedback information.
+ public: std::vector<ODEJointFeedback*> jointFeedbacks;
+
+ /// \brief Physics step function.
+ public: int (*physicsStepFunc)(dxWorld*, dReal);
+
+ /// \brief All the collsiion spaces.
+ public: std::map<std::string, dSpaceID> spaces;
+
+ /// \brief All the normal colliders.
+ public: std::vector< std::pair<ODECollision*, ODECollision*> > colliders;
+
+ /// \brief All the triangle mesh colliders.
+ public: std::vector< std::pair<ODECollision*, ODECollision*> >
+ trimeshColliders;
+
+ /// \brief Array of contact collisions.
+ public: dContactGeom contactCollisions[MAX_COLLIDE_RETURNS];
+
+ /// \brief Indices used during creation of contact joints.
+ public: int indices[MAX_CONTACT_JOINTS];
+
+ /// \brief Current index into the contactFeedbacks buffer
+ public: unsigned int jointFeedbackIndex;
+
+ /// \brief Number of normal colliders.
+ public: unsigned int collidersCount;
+
+ /// \brief Number of triangle mesh colliders.
+ public: unsigned int trimeshCollidersCount;
+
+ /// \brief Maximum number of contact points per collision pair.
+ public: unsigned int maxContacts;
+ };
+ }
+}
+
+#endif
diff --git a/gazebo/physics/ode/ODEPhysics_TEST.cc b/gazebo/physics/ode/ODEPhysics_TEST.cc
index 3715f52..79d5d90 100644
--- a/gazebo/physics/ode/ODEPhysics_TEST.cc
+++ b/gazebo/physics/ode/ODEPhysics_TEST.cc
@@ -21,7 +21,7 @@
#include "gazebo/physics/PhysicsEngine.hh"
#include "gazebo/physics/ode/ODEPhysics.hh"
#include "gazebo/physics/ode/ODETypes.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
using namespace physics;
@@ -64,14 +64,16 @@ TEST_F(ODEPhysics_TEST, PhysicsParam)
double contactSurfaceLayer = 0.02;
// test setting/getting physics engine params
- odePhysics->SetParam("solver_type", type);
- odePhysics->SetParam("precon_iters", preconIters);
- odePhysics->SetParam("iters", iters);
- odePhysics->SetParam("sor", sor);
- odePhysics->SetParam("cfm", cfm);
- odePhysics->SetParam("erp", erp);
- odePhysics->SetParam("contact_max_correcting_vel", contactMaxCorrectingVel);
- odePhysics->SetParam("contact_surface_layer", contactSurfaceLayer);
+ EXPECT_TRUE(odePhysics->SetParam("solver_type", type));
+ EXPECT_TRUE(odePhysics->SetParam("precon_iters", preconIters));
+ EXPECT_TRUE(odePhysics->SetParam("iters", iters));
+ EXPECT_TRUE(odePhysics->SetParam("sor", sor));
+ EXPECT_TRUE(odePhysics->SetParam("cfm", cfm));
+ EXPECT_TRUE(odePhysics->SetParam("erp", erp));
+ EXPECT_TRUE(odePhysics->SetParam("contact_max_correcting_vel",
+ contactMaxCorrectingVel));
+ EXPECT_TRUE(odePhysics->SetParam("contact_surface_layer",
+ contactSurfaceLayer));
boost::any value;
value = odePhysics->GetParam("solver_type");
@@ -121,16 +123,16 @@ TEST_F(ODEPhysics_TEST, PhysicsParam)
contactMaxCorrectingVel = 40;
contactSurfaceLayer = 0.03;
- odePhysics->SetParam("solver_type", type);
- odePhysics->SetParam("precon_iters", preconIters);
- odePhysics->SetParam("iters", iters);
- odePhysics->SetParam("sor", sor);
- odePhysics->SetParam("cfm", cfm);
- odePhysics->SetParam("erp", erp);
- odePhysics->SetParam("contact_max_correcting_vel",
- contactMaxCorrectingVel);
- odePhysics->SetParam("contact_surface_layer",
- contactSurfaceLayer);
+ EXPECT_TRUE(odePhysics->SetParam("solver_type", type));
+ EXPECT_TRUE(odePhysics->SetParam("precon_iters", preconIters));
+ EXPECT_TRUE(odePhysics->SetParam("iters", iters));
+ EXPECT_TRUE(odePhysics->SetParam("sor", sor));
+ EXPECT_TRUE(odePhysics->SetParam("cfm", cfm));
+ EXPECT_TRUE(odePhysics->SetParam("erp", erp));
+ EXPECT_TRUE(odePhysics->SetParam("contact_max_correcting_vel",
+ contactMaxCorrectingVel));
+ EXPECT_TRUE(odePhysics->SetParam("contact_surface_layer",
+ contactSurfaceLayer));
value = odePhysics->GetParam("solver_type");
typeRet = boost::any_cast<std::string>(value);
@@ -166,6 +168,99 @@ TEST_F(ODEPhysics_TEST, PhysicsParam)
EXPECT_DOUBLE_EQ(contactMaxCorrectingVel,
odePhysics->GetContactMaxCorrectingVel());
EXPECT_DOUBLE_EQ(contactSurfaceLayer, odePhysics->GetContactSurfaceLayer());
+
+ // Test dynamic MOI modification flag
+ {
+ std::vector<std::string> keys;
+ const std::string key1 = "inertia_ratio_reduction";
+ const std::string key2 = "use_dynamic_moi_rescaling";
+ keys.push_back(key1);
+ keys.push_back(key2);
+
+ std::vector<bool> bools;
+ bools.push_back(true);
+ bools.push_back(false);
+
+ // Set each keys with each flag value
+ for (auto const &key : keys)
+ {
+ for (const bool &flag : bools)
+ {
+ gzdbg << "SetParam(" << key << ", " << flag << ")" << std::endl;
+ EXPECT_TRUE(odePhysics->SetParam(key, flag));
+
+ // Check both keys
+ EXPECT_EQ(flag, boost::any_cast<bool>(odePhysics->GetParam(key1)));
+ EXPECT_EQ(flag, boost::any_cast<bool>(odePhysics->GetParam(key2)));
+ }
+ }
+ }
+
+ // Test friction model
+ {
+ // Default value "pyramid_model"
+ const std::string frictionModel = "pyramid_model";
+ EXPECT_EQ(odePhysics->GetFrictionModel(), frictionModel);
+ std::string param;
+ EXPECT_NO_THROW(param = boost::any_cast<std::string>(
+ odePhysics->GetParam("friction_model")));
+ EXPECT_EQ(param, frictionModel);
+ }
+
+ {
+ // Switch to "cone_model" using SetFrictionModel
+ const std::string frictionModel = "cone_model";
+ odePhysics->SetFrictionModel(frictionModel);
+ EXPECT_EQ(odePhysics->GetFrictionModel(), frictionModel);
+ std::string param;
+ EXPECT_NO_THROW(param = boost::any_cast<std::string>(
+ odePhysics->GetParam("friction_model")));
+ EXPECT_EQ(param, frictionModel);
+ }
+
+ {
+ // Switch to "box_model" using SetParam
+ const std::string frictionModel = "box_model";
+ odePhysics->SetParam("friction_model", frictionModel);
+ EXPECT_EQ(odePhysics->GetFrictionModel(), frictionModel);
+ std::string param;
+ EXPECT_NO_THROW(param = boost::any_cast<std::string>(
+ odePhysics->GetParam("friction_model")));
+ EXPECT_EQ(param, frictionModel);
+ }
+
+ // Test world step solvers
+ {
+ // Default value "ODE_DANTZIG"
+ const std::string worldSolverType = "ODE_DANTZIG";
+ EXPECT_EQ(odePhysics->GetWorldStepSolverType(), worldSolverType);
+ std::string param;
+ EXPECT_NO_THROW(param = boost::any_cast<std::string>(
+ odePhysics->GetParam("world_step_solver")));
+ EXPECT_EQ(param, worldSolverType);
+ }
+
+ {
+ // Switch to "DART_PGS" using SetWorldStepSolverType
+ const std::string worldSolverType = "DART_PGS";
+ odePhysics->SetWorldStepSolverType(worldSolverType);
+ EXPECT_EQ(odePhysics->GetWorldStepSolverType(), worldSolverType);
+ std::string param;
+ EXPECT_NO_THROW(param = boost::any_cast<std::string>(
+ odePhysics->GetParam("world_step_solver")));
+ EXPECT_EQ(param, worldSolverType);
+ }
+
+ {
+ // Switch to "BULLET_PGS" using SetParam
+ const std::string worldSolverType = "BULLET_PGS";
+ odePhysics->SetParam("world_step_solver", worldSolverType);
+ EXPECT_EQ(odePhysics->GetWorldStepSolverType(), worldSolverType);
+ std::string param;
+ EXPECT_NO_THROW(param = boost::any_cast<std::string>(
+ odePhysics->GetParam("world_step_solver")));
+ EXPECT_EQ(param, worldSolverType);
+ }
}
/////////////////////////////////////////////////
diff --git a/gazebo/physics/ode/ODEPlaneShape.hh b/gazebo/physics/ode/ODEPlaneShape.hh
index 570bcf8..c21d2ff 100644
--- a/gazebo/physics/ode/ODEPlaneShape.hh
+++ b/gazebo/physics/ode/ODEPlaneShape.hh
@@ -26,7 +26,7 @@ namespace gazebo
namespace physics
{
/// \brief An ODE Plane shape.
- class GAZEBO_VISIBLE ODEPlaneShape : public PlaneShape
+ class GZ_PHYSICS_ODE_VISIBLE ODEPlaneShape : public PlaneShape
{
/// \brief Constructor.
/// \param[in] _parent Parent Collision.
diff --git a/gazebo/physics/ode/ODEPolylineShape.hh b/gazebo/physics/ode/ODEPolylineShape.hh
index 80dd032..f382212 100644
--- a/gazebo/physics/ode/ODEPolylineShape.hh
+++ b/gazebo/physics/ode/ODEPolylineShape.hh
@@ -27,7 +27,7 @@ namespace gazebo
class ODEMesh;
/// \brief ODE polyline shape
- class GAZEBO_VISIBLE ODEPolylineShape : public PolylineShape
+ class GZ_PHYSICS_ODE_VISIBLE ODEPolylineShape : public PolylineShape
{
/// \brief Constructor
/// \param[in] _parent Collision parent.
diff --git a/gazebo/physics/ode/ODERayShape.hh b/gazebo/physics/ode/ODERayShape.hh
index 46f0ecf..869480a 100644
--- a/gazebo/physics/ode/ODERayShape.hh
+++ b/gazebo/physics/ode/ODERayShape.hh
@@ -33,7 +33,7 @@ namespace gazebo
namespace physics
{
/// \brief Ray collision
- class GAZEBO_VISIBLE ODERayShape : public RayShape
+ class GZ_PHYSICS_ODE_VISIBLE ODERayShape : public RayShape
{
/// \brief Constructor for a global ray.
/// \param[in] _physicsEngine Pointer to the physics engine.
diff --git a/gazebo/physics/ode/ODEScrewJoint.hh b/gazebo/physics/ode/ODEScrewJoint.hh
index 3d1422f..a299171 100644
--- a/gazebo/physics/ode/ODEScrewJoint.hh
+++ b/gazebo/physics/ode/ODEScrewJoint.hh
@@ -27,7 +27,7 @@ namespace gazebo
namespace physics
{
/// \brief A screw joint.
- class GAZEBO_VISIBLE ODEScrewJoint : public ScrewJoint<ODEJoint>
+ class GZ_PHYSICS_ODE_VISIBLE ODEScrewJoint : public ScrewJoint<ODEJoint>
{
/// \brief Constructor.
/// \param[in] _worldId ODE world id.
diff --git a/gazebo/physics/ode/ODESliderJoint.hh b/gazebo/physics/ode/ODESliderJoint.hh
index 52fb0a6..39aa3c3 100644
--- a/gazebo/physics/ode/ODESliderJoint.hh
+++ b/gazebo/physics/ode/ODESliderJoint.hh
@@ -31,7 +31,7 @@ namespace gazebo
namespace physics
{
/// \brief A slider joint
- class GAZEBO_VISIBLE ODESliderJoint : public SliderJoint<ODEJoint>
+ class GZ_PHYSICS_ODE_VISIBLE ODESliderJoint : public SliderJoint<ODEJoint>
{
/// \brief Constructor
/// \param[in] _worldId ODE world id.
diff --git a/gazebo/physics/ode/ODESphereShape.hh b/gazebo/physics/ode/ODESphereShape.hh
index 1270f15..ec20988 100644
--- a/gazebo/physics/ode/ODESphereShape.hh
+++ b/gazebo/physics/ode/ODESphereShape.hh
@@ -28,7 +28,7 @@ namespace gazebo
namespace physics
{
/// \brief A ODE sphere shape
- class GAZEBO_VISIBLE ODESphereShape : public SphereShape
+ class GZ_PHYSICS_ODE_VISIBLE ODESphereShape : public SphereShape
{
/// \brief Constructor.
/// \param[in] _parent Parent Collision.
diff --git a/gazebo/physics/ode/ODESurfaceParams.cc b/gazebo/physics/ode/ODESurfaceParams.cc
index fe47271..dc51054 100644
--- a/gazebo/physics/ode/ODESurfaceParams.cc
+++ b/gazebo/physics/ode/ODESurfaceParams.cc
@@ -15,7 +15,7 @@
*
*/
-#include <float.h>
+#include "gazebo/common/Assert.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/physics/ode/ODESurfaceParams.hh"
@@ -28,7 +28,8 @@ ODESurfaceParams::ODESurfaceParams()
bounce(0), bounceThreshold(100000),
kp(1000000000000), kd(1), cfm(0), erp(0.2),
maxVel(0.01), minDepth(0),
- slip1(0), slip2(0)
+ slip1(0), slip2(0),
+ frictionPyramid(new FrictionPyramid())
{
}
@@ -83,11 +84,11 @@ void ODESurfaceParams::Load(sdf::ElementPtr _sdf)
gzerr << "Surface friction ode sdf member is NULL" << std::endl;
else
{
- this->frictionPyramid.SetMuPrimary(
+ this->frictionPyramid->SetMuPrimary(
frictionOdeElem->Get<double>("mu"));
- this->frictionPyramid.SetMuSecondary(
+ this->frictionPyramid->SetMuSecondary(
frictionOdeElem->Get<double>("mu2"));
- this->frictionPyramid.direction1 =
+ this->frictionPyramid->direction1 =
frictionOdeElem->Get<math::Vector3>("fdir1");
this->slip1 = frictionOdeElem->Get<double>("slip1");
@@ -123,12 +124,12 @@ void ODESurfaceParams::FillMsg(msgs::Surface &_msg)
{
SurfaceParams::FillMsg(_msg);
- _msg.mutable_friction()->set_mu(this->frictionPyramid.GetMuPrimary());
- _msg.mutable_friction()->set_mu2(this->frictionPyramid.GetMuSecondary());
+ _msg.mutable_friction()->set_mu(this->frictionPyramid->GetMuPrimary());
+ _msg.mutable_friction()->set_mu2(this->frictionPyramid->GetMuSecondary());
_msg.mutable_friction()->set_slip1(this->slip1);
_msg.mutable_friction()->set_slip2(this->slip2);
msgs::Set(_msg.mutable_friction()->mutable_fdir1(),
- this->frictionPyramid.direction1);
+ this->frictionPyramid->direction1.Ign());
_msg.set_restitution_coefficient(this->bounce);
_msg.set_bounce_threshold(this->bounceThreshold);
@@ -149,16 +150,16 @@ void ODESurfaceParams::ProcessMsg(const msgs::Surface &_msg)
if (_msg.has_friction())
{
if (_msg.friction().has_mu())
- this->frictionPyramid.SetMuPrimary(_msg.friction().mu());
+ this->frictionPyramid->SetMuPrimary(_msg.friction().mu());
if (_msg.friction().has_mu2())
- this->frictionPyramid.SetMuSecondary(_msg.friction().mu2());
+ this->frictionPyramid->SetMuSecondary(_msg.friction().mu2());
if (_msg.friction().has_slip1())
this->slip1 = _msg.friction().slip1();
if (_msg.friction().has_slip2())
this->slip2 = _msg.friction().slip2();
if (_msg.friction().has_fdir1())
- this->frictionPyramid.direction1 =
- msgs::Convert(_msg.friction().fdir1());
+ this->frictionPyramid->direction1 =
+ msgs::ConvertIgn(_msg.friction().fdir1());
}
if (_msg.has_restitution_coefficient())
@@ -178,3 +179,9 @@ void ODESurfaceParams::ProcessMsg(const msgs::Surface &_msg)
if (_msg.has_min_depth())
this->minDepth = _msg.min_depth();
}
+
+/////////////////////////////////////////////////
+FrictionPyramidPtr ODESurfaceParams::GetFrictionPyramid() const
+{
+ return this->frictionPyramid;
+}
diff --git a/gazebo/physics/ode/ODESurfaceParams.hh b/gazebo/physics/ode/ODESurfaceParams.hh
index 8d88f81..0ee6729 100644
--- a/gazebo/physics/ode/ODESurfaceParams.hh
+++ b/gazebo/physics/ode/ODESurfaceParams.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \{
/// \brief ODE surface parameters.
- class GAZEBO_VISIBLE ODESurfaceParams : public SurfaceParams
+ class GZ_PHYSICS_ODE_VISIBLE ODESurfaceParams : public SurfaceParams
{
/// \brief Constructor.
public: ODESurfaceParams();
@@ -51,6 +51,9 @@ namespace gazebo
// Documentation inherited.
public: virtual void ProcessMsg(const msgs::Surface &_msg);
+ // Documentation inherited.
+ public: virtual FrictionPyramidPtr GetFrictionPyramid() const;
+
/// \brief bounce restitution coefficient [0,1], with 0 being inelastic,
/// and 1 being perfectly elastic.
/// \sa http://www.ode.org/ode-latest-userguide.html#sec_7_3_7
@@ -99,13 +102,6 @@ namespace gazebo
/// (http://www.ode.org/ode-latest-userguide.html#sec_5_2_0)
public: double minDepth;
- /// \brief Friction pyramid parameters (mu1, mu2).
- /// Note that the primary friction pyramid direction can be specified
- /// by fdir1, otherwise a vector constrained to be perpendicular to the
- /// contact normal in the global y-z plane is used.
- /// \sa http://www.ode.org/ode-latest-userguide.html#sec_7_3_7
- public: FrictionPyramid frictionPyramid;
-
/// \brief Artificial contact slip in the primary friction direction.
/// \sa See dContactSlip1 in
/// http://www.ode.org/ode-latest-userguide.html#sec_7_3_7
@@ -115,6 +111,13 @@ namespace gazebo
/// \sa See dContactSlip2 in
/// http://www.ode.org/ode-latest-userguide.html#sec_7_3_7
public: double slip2;
+
+ /// \brief Friction pyramid parameters (mu1, mu2).
+ /// Note that the primary friction pyramid direction can be specified
+ /// by fdir1, otherwise a vector constrained to be perpendicular to the
+ /// contact normal in the global y-z plane is used.
+ /// \sa http://www.ode.org/ode-latest-userguide.html#sec_7_3_7
+ private: FrictionPyramidPtr frictionPyramid;
};
/// \}
}
diff --git a/gazebo/physics/ode/ODEUniversalJoint.hh b/gazebo/physics/ode/ODEUniversalJoint.hh
index b688af9..6a3bb6d 100644
--- a/gazebo/physics/ode/ODEUniversalJoint.hh
+++ b/gazebo/physics/ode/ODEUniversalJoint.hh
@@ -30,7 +30,8 @@ namespace gazebo
namespace physics
{
/// \brief A universal joint.
- class GAZEBO_VISIBLE ODEUniversalJoint : public UniversalJoint<ODEJoint>
+ class GZ_PHYSICS_ODE_VISIBLE ODEUniversalJoint :
+ public UniversalJoint<ODEJoint>
{
/// \brief Constructor.
/// \param[in] _worldId ODE world id.
diff --git a/gazebo/physics/simbody/CMakeLists.txt b/gazebo/physics/simbody/CMakeLists.txt
index 7870e80..57141ff 100644
--- a/gazebo/physics/simbody/CMakeLists.txt
+++ b/gazebo/physics/simbody/CMakeLists.txt
@@ -14,6 +14,7 @@ add_definitions(${Simbody_CFLAGS})
set (sources
SimbodyBallJoint.cc
SimbodyCollision.cc
+SimbodyFixedJoint.cc
SimbodyHeightmapShape.cc
SimbodyHinge2Joint.cc
SimbodyHingeJoint.cc
@@ -36,6 +37,7 @@ set (headers
SimbodyBallJoint.hh
SimbodyBoxShape.hh
SimbodyCollision.hh
+SimbodyFixedJoint.hh
SimbodyCylinderShape.hh
SimbodyHeightmapShape.hh
SimbodyHinge2Joint.hh
@@ -59,7 +61,8 @@ SimbodyUniversalJoint.hh
)
gz_add_library(gazebo_physics_simbody ${sources})
-target_link_libraries(gazebo_physics_simbody ${Simbody_LIBRARIES})
+target_link_libraries(gazebo_physics_simbody
+ ${Simbody_LIBRARIES})
gz_install_library(gazebo_physics_simbody)
gz_install_includes("physics/simbody" ${headers})
diff --git a/gazebo/physics/simbody/SimbodyBallJoint.hh b/gazebo/physics/simbody/SimbodyBallJoint.hh
index 6d3adb4..75c0696 100644
--- a/gazebo/physics/simbody/SimbodyBallJoint.hh
+++ b/gazebo/physics/simbody/SimbodyBallJoint.hh
@@ -32,7 +32,7 @@ namespace gazebo
/// \{
/// \brief SimbodyBallJoint class models a ball joint in Simbody.
- class GAZEBO_VISIBLE SimbodyBallJoint : public BallJoint<SimbodyJoint>
+ class GZ_PHYSICS_VISIBLE SimbodyBallJoint : public BallJoint<SimbodyJoint>
{
/// \brief Simbody Ball Joint Constructor
public: SimbodyBallJoint(SimTK::MultibodySystem *_world, BasePtr _parent);
diff --git a/gazebo/physics/simbody/SimbodyBoxShape.hh b/gazebo/physics/simbody/SimbodyBoxShape.hh
index 93a0335..65299ed 100644
--- a/gazebo/physics/simbody/SimbodyBoxShape.hh
+++ b/gazebo/physics/simbody/SimbodyBoxShape.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \{
/// \brief Simbody box collision
- class GAZEBO_VISIBLE SimbodyBoxShape : public BoxShape
+ class GZ_PHYSICS_VISIBLE SimbodyBoxShape : public BoxShape
{
/// \brief Constructor
public: SimbodyBoxShape(CollisionPtr _parent) : BoxShape(_parent) {}
diff --git a/gazebo/physics/simbody/SimbodyCollision.cc b/gazebo/physics/simbody/SimbodyCollision.cc
index 36fdc96..ac5e24c 100644
--- a/gazebo/physics/simbody/SimbodyCollision.cc
+++ b/gazebo/physics/simbody/SimbodyCollision.cc
@@ -34,7 +34,6 @@ SimbodyCollision::SimbodyCollision(LinkPtr _parent)
//////////////////////////////////////////////////
SimbodyCollision::~SimbodyCollision()
{
- delete this->collisionShape;
this->collisionShape = NULL;
}
diff --git a/gazebo/physics/simbody/SimbodyCollision.hh b/gazebo/physics/simbody/SimbodyCollision.hh
index 99358c5..851a9dd 100644
--- a/gazebo/physics/simbody/SimbodyCollision.hh
+++ b/gazebo/physics/simbody/SimbodyCollision.hh
@@ -38,7 +38,7 @@ namespace gazebo
/// \{
/// \brief Simbody collisions
- class GAZEBO_VISIBLE SimbodyCollision : public Collision
+ class GZ_PHYSICS_VISIBLE SimbodyCollision : public Collision
{
/// \brief Constructor
public: SimbodyCollision(LinkPtr _parent);
diff --git a/gazebo/physics/simbody/SimbodyCylinderShape.hh b/gazebo/physics/simbody/SimbodyCylinderShape.hh
index 6aa7405..7923490 100644
--- a/gazebo/physics/simbody/SimbodyCylinderShape.hh
+++ b/gazebo/physics/simbody/SimbodyCylinderShape.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \{
/// \brief Cylinder collision
- class GAZEBO_VISIBLE SimbodyCylinderShape : public CylinderShape
+ class GZ_PHYSICS_VISIBLE SimbodyCylinderShape : public CylinderShape
{
/// \brief Constructor
public: SimbodyCylinderShape(CollisionPtr _parent)
diff --git a/gazebo/physics/simbody/SimbodyFixedJoint.cc b/gazebo/physics/simbody/SimbodyFixedJoint.cc
new file mode 100644
index 0000000..a1742f3
--- /dev/null
+++ b/gazebo/physics/simbody/SimbodyFixedJoint.cc
@@ -0,0 +1,102 @@
+/*
+ * 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/common/Assert.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/common/Exception.hh"
+
+#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/simbody/SimbodyLink.hh"
+#include "gazebo/physics/simbody/SimbodyPhysics.hh"
+#include "gazebo/physics/simbody/SimbodyFixedJoint.hh"
+
+using namespace gazebo;
+using namespace physics;
+
+//////////////////////////////////////////////////
+SimbodyFixedJoint::SimbodyFixedJoint(SimTK::MultibodySystem */*_world*/,
+ BasePtr _parent)
+ : FixedJoint<SimbodyJoint>(_parent)
+{
+ this->physicsInitialized = false;
+}
+
+//////////////////////////////////////////////////
+SimbodyFixedJoint::~SimbodyFixedJoint()
+{
+}
+
+//////////////////////////////////////////////////
+void SimbodyFixedJoint::Load(sdf::ElementPtr _sdf)
+{
+ FixedJoint<SimbodyJoint>::Load(_sdf);
+}
+
+//////////////////////////////////////////////////
+void SimbodyFixedJoint::SetVelocity(unsigned int /*_index*/, double /*_angle*/)
+{
+ gzwarn << "SimbodyFixedJoint: called method "
+ << "SetVelocity that is not valid for joints of type fixed.\n";
+}
+
+//////////////////////////////////////////////////
+double SimbodyFixedJoint::GetVelocity(unsigned int /*index*/) const
+{
+ gzwarn << "SimbodyFixedJoint: called method "
+ << "GetVelocity that is not valid for joints of type fixed.\n";
+ return 0.0;
+}
+
+//////////////////////////////////////////////////
+void SimbodyFixedJoint::SetMaxForce(unsigned int /*_index*/,
+ double /*_force*/)
+{
+ gzwarn << "SimbodyFixedJoint: called method "
+ << "SetMaxForce that is not valid for joints of type fixed.\n";
+}
+
+//////////////////////////////////////////////////
+double SimbodyFixedJoint::GetMaxForce(unsigned int /*index*/)
+{
+ gzwarn << "SimbodyFixedJoint: called method "
+ << "GetMaxForce that is not valid for joints of type fixed.\n";
+ return 0.0;
+}
+
+//////////////////////////////////////////////////
+void SimbodyFixedJoint::SetForceImpl(unsigned int /*_index*/,
+ double /*_torque*/)
+{
+ gzwarn << "SimbodyFixedJoint: called method "
+ << "SetForceImpl that is not valid for joints of type fixed.\n";
+}
+
+//////////////////////////////////////////////////
+math::Vector3 SimbodyFixedJoint::GetGlobalAxis(unsigned int /*index*/) const
+{
+ gzwarn << "SimbodyFixedJoint: called method "
+ << "GetGlobalAxis that is not valid for joints of type fixed.\n";
+ return math::Vector3();
+}
+
+//////////////////////////////////////////////////
+math::Angle SimbodyFixedJoint::GetAngleImpl(unsigned int /*_index*/) const
+{
+ gzwarn << "SimbodyFixedJoint: called method "
+ << "GetAngleImpl that is not valid for joints of type fixed.\n";
+ return math::Angle();
+}
diff --git a/gazebo/physics/simbody/SimbodyFixedJoint.hh b/gazebo/physics/simbody/SimbodyFixedJoint.hh
new file mode 100644
index 0000000..b31c193
--- /dev/null
+++ b/gazebo/physics/simbody/SimbodyFixedJoint.hh
@@ -0,0 +1,76 @@
+/*
+ * 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_SIMBODY_FIXEDJOINT_HH_
+#define _GAZEBO_SIMBODY_FIXEDJOINT_HH_
+
+#include <vector>
+
+#include "gazebo/math/Angle.hh"
+#include "gazebo/math/Vector3.hh"
+#include "gazebo/physics/FixedJoint.hh"
+#include "gazebo/physics/simbody/SimbodyJoint.hh"
+#include "gazebo/physics/simbody/SimbodyPhysics.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace physics
+ {
+ /// \ingroup gazebo_physics
+ /// \addtogroup gazebo_physics_simbody Simbody Physics
+ /// \{
+
+ /// \brief A fixed joint rigidly connecting two bodies
+ class GZ_PHYSICS_VISIBLE SimbodyFixedJoint : public FixedJoint<SimbodyJoint>
+ {
+ /// \brief Constructor
+ /// \param[in] world pointer to the simbody world
+ /// \param[in] _parent pointer to the parent Model
+ public: SimbodyFixedJoint(SimTK::MultibodySystem *world, BasePtr _parent);
+
+ /// \brief Destructor
+ public: virtual ~SimbodyFixedJoint();
+
+ // Documentation inherited.
+ protected: virtual void Load(sdf::ElementPtr _sdf);
+
+ // Documentation inherited.
+ public: virtual double GetVelocity(unsigned int _index) const;
+
+ // Documentation inherited.
+ public: virtual void SetVelocity(unsigned int _index, double _angle);
+
+ // Documentation inherited.
+ public: virtual void SetMaxForce(unsigned int _index, double _t);
+
+ // Documentation inherited.
+ public: virtual double GetMaxForce(unsigned int _index);
+
+ // Documentation inherited.
+ public: virtual void SetForceImpl(unsigned int _index, double _torque);
+
+ // Documentation inherited.
+ public: virtual math::Vector3 GetGlobalAxis(unsigned int _index) const;
+
+ // Documentation inherited.
+ public: virtual math::Angle GetAngleImpl(unsigned int _index) const;
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/physics/simbody/SimbodyHeightmapShape.hh b/gazebo/physics/simbody/SimbodyHeightmapShape.hh
index 6e98390..9495d3c 100644
--- a/gazebo/physics/simbody/SimbodyHeightmapShape.hh
+++ b/gazebo/physics/simbody/SimbodyHeightmapShape.hh
@@ -34,7 +34,7 @@ namespace gazebo
/// \{
/// \brief Height map collision.
- class GAZEBO_VISIBLE SimbodyHeightmapShape : public HeightmapShape
+ class GZ_PHYSICS_VISIBLE SimbodyHeightmapShape : public HeightmapShape
{
/// \brief Constructor.
public: SimbodyHeightmapShape(CollisionPtr _parent);
diff --git a/gazebo/physics/simbody/SimbodyHinge2Joint.hh b/gazebo/physics/simbody/SimbodyHinge2Joint.hh
index c01b57b..7b7c85a 100644
--- a/gazebo/physics/simbody/SimbodyHinge2Joint.hh
+++ b/gazebo/physics/simbody/SimbodyHinge2Joint.hh
@@ -34,7 +34,8 @@ namespace gazebo
/// \{
/// \brief A two axis hinge joint
- class GAZEBO_VISIBLE SimbodyHinge2Joint : public Hinge2Joint<SimbodyJoint>
+ class GZ_PHYSICS_VISIBLE SimbodyHinge2Joint :
+ public Hinge2Joint<SimbodyJoint>
{
/// \brief Constructor
public: SimbodyHinge2Joint(SimTK::MultibodySystem *world,
diff --git a/gazebo/physics/simbody/SimbodyHingeJoint.hh b/gazebo/physics/simbody/SimbodyHingeJoint.hh
index 887329d..3e49be0 100644
--- a/gazebo/physics/simbody/SimbodyHingeJoint.hh
+++ b/gazebo/physics/simbody/SimbodyHingeJoint.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \{
/// \brief A single axis hinge joint
- class GAZEBO_VISIBLE SimbodyHingeJoint : public HingeJoint<SimbodyJoint>
+ class GZ_PHYSICS_VISIBLE SimbodyHingeJoint : public HingeJoint<SimbodyJoint>
{
/// Constructor
public: SimbodyHingeJoint(SimTK::MultibodySystem *world, BasePtr _parent);
diff --git a/gazebo/physics/simbody/SimbodyJoint.cc b/gazebo/physics/simbody/SimbodyJoint.cc
index 0740808..8a67f3e 100644
--- a/gazebo/physics/simbody/SimbodyJoint.cc
+++ b/gazebo/physics/simbody/SimbodyJoint.cc
@@ -479,11 +479,9 @@ bool SimbodyJoint::SetParam(const std::string &/*_key*/,
}
//////////////////////////////////////////////////
-double SimbodyJoint::GetParam(const std::string &/*_key*/,
- unsigned int /*_index*/)
+double SimbodyJoint::GetParam(const std::string &_key, unsigned int _index)
{
- gzerr << "Not implement in Simbody\n";
- return 0;
+ return Joint::GetParam(_key, _index);
}
//////////////////////////////////////////////////
diff --git a/gazebo/physics/simbody/SimbodyJoint.hh b/gazebo/physics/simbody/SimbodyJoint.hh
index 3c06bde..84c953f 100644
--- a/gazebo/physics/simbody/SimbodyJoint.hh
+++ b/gazebo/physics/simbody/SimbodyJoint.hh
@@ -34,7 +34,7 @@ namespace gazebo
/// \{
/// \brief Base class for all joints
- class GAZEBO_VISIBLE SimbodyJoint : public Joint
+ class GZ_PHYSICS_VISIBLE SimbodyJoint : public Joint
{
/// \brief Constructor
public: SimbodyJoint(BasePtr _parent);
diff --git a/gazebo/physics/simbody/SimbodyLink.cc b/gazebo/physics/simbody/SimbodyLink.cc
index 9f694c3..7aea53b 100644
--- a/gazebo/physics/simbody/SimbodyLink.cc
+++ b/gazebo/physics/simbody/SimbodyLink.cc
@@ -525,6 +525,14 @@ void SimbodyLink::AddForceAtRelativePosition(const math::Vector3 &/*_force*/,
gzerr << "Not implemented.\n";
}
+//////////////////////////////////////////////////
+void SimbodyLink::AddLinkForce(const math::Vector3 &/*_force*/,
+ const math::Vector3 &/*_offset*/)
+{
+ gzlog << "SimbodyLink::AddLinkForce not yet implemented (issue #1478)."
+ << std::endl;
+}
+
/////////////////////////////////////////////////
void SimbodyLink::AddTorque(const math::Vector3 &/*_torque*/)
{
diff --git a/gazebo/physics/simbody/SimbodyLink.hh b/gazebo/physics/simbody/SimbodyLink.hh
index 5209dc1..9d790c4 100644
--- a/gazebo/physics/simbody/SimbodyLink.hh
+++ b/gazebo/physics/simbody/SimbodyLink.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \{
/// \brief Simbody Link class
- class GAZEBO_VISIBLE SimbodyLink : public Link
+ class GZ_PHYSICS_VISIBLE SimbodyLink : public Link
{
/// \brief Constructor
public: SimbodyLink(EntityPtr _parent);
@@ -125,6 +125,10 @@ namespace gazebo
const math::Vector3 &_force,
const math::Vector3 &_relpos);
+ // Documentation inherited
+ public: virtual void AddLinkForce(const math::Vector3 &_force,
+ const math::Vector3 &_offset = math::Vector3::Zero);
+
// Documentation inherited.
public: virtual void AddTorque(const math::Vector3 &_torque);
diff --git a/gazebo/physics/simbody/SimbodyMesh.hh b/gazebo/physics/simbody/SimbodyMesh.hh
index 4f18aca..9cc26d2 100644
--- a/gazebo/physics/simbody/SimbodyMesh.hh
+++ b/gazebo/physics/simbody/SimbodyMesh.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \{
/// \brief Triangle mesh collision helper class
- class GAZEBO_VISIBLE SimbodyMesh
+ class GZ_PHYSICS_VISIBLE SimbodyMesh
{
/// \brief Constructor
public: SimbodyMesh();
diff --git a/gazebo/physics/simbody/SimbodyMeshShape.hh b/gazebo/physics/simbody/SimbodyMeshShape.hh
index e194f29..ebbae34 100644
--- a/gazebo/physics/simbody/SimbodyMeshShape.hh
+++ b/gazebo/physics/simbody/SimbodyMeshShape.hh
@@ -32,7 +32,7 @@ namespace gazebo
/// \{
/// \brief Triangle mesh collision
- class GAZEBO_VISIBLE SimbodyMeshShape : public MeshShape
+ class GZ_PHYSICS_VISIBLE SimbodyMeshShape : public MeshShape
{
/// \brief Constructor
public: SimbodyMeshShape(CollisionPtr _parent);
diff --git a/gazebo/physics/simbody/SimbodyModel.hh b/gazebo/physics/simbody/SimbodyModel.hh
index 48bf69b..1faefde 100644
--- a/gazebo/physics/simbody/SimbodyModel.hh
+++ b/gazebo/physics/simbody/SimbodyModel.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \class SimbodyModel SimbodyModel.hh physics/physics.hh
/// \brief A model is a collection of links, joints, and plugins.
- class GAZEBO_VISIBLE SimbodyModel : public Model
+ class GZ_PHYSICS_VISIBLE SimbodyModel : public Model
{
/// \brief Constructor.
/// \param[in] _parent Parent object.
diff --git a/gazebo/physics/simbody/SimbodyMultiRayShape.hh b/gazebo/physics/simbody/SimbodyMultiRayShape.hh
index a414b58..341b0f9 100644
--- a/gazebo/physics/simbody/SimbodyMultiRayShape.hh
+++ b/gazebo/physics/simbody/SimbodyMultiRayShape.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \{
/// \brief Simbody specific version of MultiRayShape
- class GAZEBO_VISIBLE SimbodyMultiRayShape : public MultiRayShape
+ class GZ_PHYSICS_VISIBLE SimbodyMultiRayShape : public MultiRayShape
{
/// \brief Constructor
public: SimbodyMultiRayShape(CollisionPtr parent);
diff --git a/gazebo/physics/simbody/SimbodyPhysics.cc b/gazebo/physics/simbody/SimbodyPhysics.cc
index daba003..efda0c4 100644
--- a/gazebo/physics/simbody/SimbodyPhysics.cc
+++ b/gazebo/physics/simbody/SimbodyPhysics.cc
@@ -39,7 +39,9 @@
#include "gazebo/physics/simbody/SimbodySliderJoint.hh"
#include "gazebo/physics/simbody/SimbodyHinge2Joint.hh"
#include "gazebo/physics/simbody/SimbodyScrewJoint.hh"
+#include "gazebo/physics/simbody/SimbodyFixedJoint.hh"
+#include "gazebo/physics/ContactManager.hh"
#include "gazebo/physics/PhysicsTypes.hh"
#include "gazebo/physics/PhysicsFactory.hh"
#include "gazebo/physics/World.hh"
@@ -162,13 +164,13 @@ void SimbodyPhysics::Load(sdf::ElementPtr _sdf)
simbodyContactElem->Get<double>("viscous_friction");
// below are not used yet, but should work it into the system
- this->contactMaterialViscousFriction =
- simbodyContactElem->Get<double>("plastic_coef_restitution");
this->contactMaterialPlasticCoefRestitution =
- simbodyContactElem->Get<double>("plastic_impact_velocity");
+ simbodyContactElem->Get<double>("plastic_coef_restitution");
this->contactMaterialPlasticImpactVelocity =
- simbodyContactElem->Get<double>("override_impact_capture_velocity");
+ simbodyContactElem->Get<double>("plastic_impact_velocity");
this->contactImpactCaptureVelocity =
+ simbodyContactElem->Get<double>("override_impact_capture_velocity");
+ this->contactStictionTransitionVelocity =
simbodyContactElem->Get<double>("override_stiction_transition_velocity");
}
@@ -189,7 +191,10 @@ void SimbodyPhysics::OnRequest(ConstRequestPtr &_msg)
physicsMsg.set_min_step_size(this->GetMaxStepSize());
physicsMsg.set_enable_physics(this->world->GetEnablePhysicsEngine());
- physicsMsg.mutable_gravity()->CopyFrom(msgs::Convert(this->GetGravity()));
+ physicsMsg.mutable_gravity()->CopyFrom(
+ msgs::Convert(this->GetGravity().Ign()));
+ physicsMsg.mutable_magnetic_field()->CopyFrom(
+ msgs::Convert(this->MagneticField()));
physicsMsg.set_real_time_update_rate(this->realTimeUpdateRate);
physicsMsg.set_real_time_factor(this->targetRealTimeFactor);
physicsMsg.set_max_step_size(this->maxStepSize);
@@ -203,11 +208,16 @@ void SimbodyPhysics::OnRequest(ConstRequestPtr &_msg)
/////////////////////////////////////////////////
void SimbodyPhysics::OnPhysicsMsg(ConstPhysicsPtr &_msg)
{
+ // Parent class handles many generic parameters
+ // This should be done first so that the profile settings
+ // can be over-ridden by other message parameters.
+ PhysicsEngine::OnPhysicsMsg(_msg);
+
if (_msg->has_enable_physics())
this->world->EnablePhysicsEngine(_msg->enable_physics());
if (_msg->has_gravity())
- this->SetGravity(msgs::Convert(_msg->gravity()));
+ this->SetGravity(msgs::ConvertIgn(_msg->gravity()));
if (_msg->has_real_time_factor())
this->SetTargetRealTimeFactor(_msg->real_time_factor());
@@ -235,9 +245,6 @@ void SimbodyPhysics::OnPhysicsMsg(ConstPhysicsPtr &_msg)
/// Make sure all models get at least on update cycle.
this->world->EnableAllModels();
-
- // Parent class handles many generic parameters
- PhysicsEngine::OnPhysicsMsg(_msg);
}
//////////////////////////////////////////////////
@@ -403,6 +410,224 @@ void SimbodyPhysics::InitForThread()
//////////////////////////////////////////////////
void SimbodyPhysics::UpdateCollision()
{
+ boost::recursive_mutex::scoped_lock lock(*this->physicsUpdateMutex);
+
+ this->contactManager->ResetCount();
+
+ // Get all contacts from Simbody
+ const SimTK::State &state = this->integ->getState();
+
+ // get contact snapshot
+ const SimTK::ContactSnapshot &contactSnapshot =
+ this->tracker.getActiveContacts(state);
+
+ int numc = contactSnapshot.getNumContacts();
+
+ int count = 0;
+ for (int j = 0; j < numc; ++j)
+ {
+ // get contact stuff from Simbody
+ const SimTK::Contact &simbodyContact = contactSnapshot.getContact(j);
+ {
+ SimTK::ContactSurfaceIndex csi1 = simbodyContact.getSurface1();
+ SimTK::ContactSurfaceIndex csi2 = simbodyContact.getSurface2();
+ const SimTK::ContactSurface &cs1 = this->tracker.getContactSurface(csi1);
+ const SimTK::ContactSurface &cs2 = this->tracker.getContactSurface(csi2);
+
+ /// \TODO: See issue #1584
+ /// \TODO: below, get collision data from simbody contacts
+ Collision *collision1 = NULL;
+ Collision *collision2 = NULL;
+ physics::LinkPtr link1 = NULL;
+ physics::LinkPtr link2 = NULL;
+
+ /// \TODO: get SimTK::ContactGeometry* from ContactForce somehow
+ const SimTK::ContactGeometry &cg1 = cs1.getShape();
+ const SimTK::ContactGeometry &cg2 = cs2.getShape();
+
+ /// \TODO: proof of concept only
+ /// loop through all link->all collisions and find
+ /// this is going to be very very slow, we'll need
+ /// something with a void* pointer in simbody
+ /// to support something like this.
+ physics::Model_V models = this->world->GetModels();
+ for (physics::Model_V::iterator mi = models.begin();
+ mi != models.end(); ++mi)
+ {
+ physics::Link_V links = (*mi)->GetLinks();
+ for (Link_V::iterator li = links.begin(); li != links.end(); ++li)
+ {
+ Collision_V collisions = (*li)->GetCollisions();
+ for (Collision_V::iterator ci = collisions.begin();
+ ci != collisions.end(); ++ci)
+ {
+ /// compare SimbodyCollision::GetCollisionShape() to
+ /// ContactGeometry from SimTK::ContactForce
+ SimbodyCollisionPtr sc =
+ boost::dynamic_pointer_cast<physics::SimbodyCollision>(*ci);
+ if (sc->GetCollisionShape() == &cg1)
+ {
+ collision1 = (*ci).get();
+ link1 = (*li);
+ }
+ else if (sc->GetCollisionShape() == &cg2)
+ {
+ collision2 = (*ci).get();
+ link2 = (*li);
+ }
+ }
+ }
+ }
+
+ // add contacts to the manager. This will return NULL if no one is
+ // listening for contact information.
+ Contact *contactFeedback = this->contactManager->NewContact(collision1,
+ collision2, this->world->GetSimTime());
+
+ if (contactFeedback)
+ {
+ const bool useContactPatch = true;
+ if (useContactPatch)
+ {
+ // get contact patch to get detailed contacts
+ // see https://github.com/simbody/simbody/blob/master/examples/ExampleContactPlayground.cpp#L110
+ SimTK::ContactPatch patch;
+ this->system.realize(state, SimTK::Stage::Velocity);
+ const bool found =
+ this->contact.calcContactPatchDetailsById(
+ state, simbodyContact.getContactId(), patch);
+
+ // loop through details of patch
+ if (found)
+ {
+ for (int i = 0; i < patch.getNumDetails(); ++i)
+ {
+ if (count >= MAX_CONTACT_JOINTS)
+ {
+ gzerr << "max contact count [" << MAX_CONTACT_JOINTS
+ << "] exceeded. truncating info.\n";
+ continue;
+ }
+ // gzerr << "count: " << count << "\n";
+
+ // get detail
+ const SimTK::ContactDetail &detail = patch.getContactDetail(i);
+ // get contact information from simbody and
+ // add them to contactFeedback.
+ // Store the contact depth
+ contactFeedback->depths[count] = detail.getDeformation();
+
+ // Store the contact position
+ contactFeedback->positions[count].Set(
+ detail.getContactPoint()[0],
+ detail.getContactPoint()[1],
+ detail.getContactPoint()[2]);
+
+ // Store the contact normal
+ contactFeedback->normals[count].Set(
+ detail.getContactNormal()[0],
+ detail.getContactNormal()[1],
+ detail.getContactNormal()[2]);
+
+ // Store the contact forces
+ const SimTK::Vec3 f2 = detail.getForceOnSurface2();
+ const SimTK::SpatialVec s2 =
+ SimTK::SpatialVec(SimTK::Vec3(0, 0, 0), f2);
+ /// Get transform from point to CG.
+ /// detail.getContactPoint() returns in body frame
+ /// per gazebo contact feedback convention.
+ const SimTK::Vec3 offset2 = -detail.getContactPoint();
+ SimTK::SpatialVec s2cg = SimTK::shiftForceBy(s2, offset2);
+ SimTK::Vec3 t2cg = s2cg[0];
+ SimTK::Vec3 f2cg = s2cg[1];
+
+ /// shift for body 1
+ /// \TODO: generalize wrench shifting below later and add
+ /// it to JointWrench class for shifting wrenches around
+ /// arbitrarily based on frames.
+ ///
+ /// shift forces to link1 frame without rotating it first
+ math::Pose pose1 = link1->GetWorldPose();
+ math::Pose pose2 = link2->GetWorldPose();
+ const SimTK::Vec3 offset1 = -detail.getContactPoint()
+ + SimbodyPhysics::Vector3ToVec3(pose1.pos - pose2.pos);
+ SimTK::SpatialVec s1cg = SimTK::shiftForceBy(-s2, offset1);
+
+ /// get torque and force components
+ SimTK::Vec3 t1cg = s1cg[0];
+ SimTK::Vec3 f1cg = s1cg[1];
+
+ /* actually don't need to do this? confirm that
+ everything is in the world frame!
+ /// \TODO: rotate it into link 1 frame, there must be
+ /// a clean way to do this in simbody...
+ /// my gazebo way of rotating frames for now, to replace with
+ /// clean simbody function calls.
+ /// rotation from link2 to link1 frame specified in link2 frame
+ math::Quaternion rot21 = (pose1 - pose2).rot;
+ t1cg = SimbodyPhysics::Vector3ToVec3(
+ rot21.RotateVectorReverse(SimbodyPhysics::Vec3ToVector3(t1cg)));
+ f1cg = SimbodyPhysics::Vector3ToVec3(
+ rot21.RotateVectorReverse(SimbodyPhysics::Vec3ToVector3(f1cg)));
+
+ gzerr << "numc: " << j << "\n";
+ gzerr << "count: " << count << "\n";
+ gzerr << "index: " << i << "\n";
+ gzerr << "offset 2: " << detail.getContactPoint() << "\n";
+ gzerr << "s2: " << s2 << "\n";
+ gzerr << "s2cg: " << s2cg << "\n";
+ gzerr << "f2cg: " << f2cg << "\n";
+ gzerr << "t2cg: " << t2cg << "\n";
+ gzerr << "offset 1: " << detail.getContactPoint() << "\n";
+ gzerr << "s1cg: " << s1cg << "\n";
+ gzerr << "f1cg: " << f1cg << "\n";
+ gzerr << "t1cg: " << t1cg << "\n";
+ */
+
+ // copy.
+ contactFeedback->wrench[count].body1Force.Set(
+ f1cg[0], f1cg[1], f1cg[2]);
+ contactFeedback->wrench[count].body2Force.Set(
+ f2cg[0], f2cg[1], f2cg[2]);
+ contactFeedback->wrench[count].body1Torque.Set(
+ t1cg[0], t1cg[1], t1cg[2]);
+ contactFeedback->wrench[count].body2Torque.Set(
+ t2cg[0], t2cg[1], t2cg[2]);
+
+ // Increase the counters
+ ++count;
+ contactFeedback->count = count;
+ }
+ }
+ }
+ else // use single ContactForce
+ {
+ // // get contact information from simbody ContactForce and
+ // // add it to contactFeedback.
+
+ // /// \TODO: confirm the contact depth is zero?
+ // contactFeedback->depths[count] = 0.0;
+
+ // // Store the contact position
+ // contactFeedback->positions[count].Set(
+ // contactForce.getContactPoint()[0],
+ // contactForce.getContactPoint()[1],
+ // contactForce.getContactPoint()[2]);
+
+ // // Store the contact normal
+ // contactFeedback->normals[j].Set(
+ // 0, 0, 0);
+ // // contactForce.getContactNormal()[0],
+ // // contactForce.getContactNormal()[1],
+ // // contactForce.getContactNormal()[2]);
+
+ // // Increase the counters
+ // ++count;
+ // contactFeedback->count = count;
+ }
+ }
+ }
+ }
}
//////////////////////////////////////////////////
@@ -557,6 +782,8 @@ JointPtr SimbodyPhysics::CreateJoint(const std::string &_type,
joint.reset(new SimbodyHinge2Joint(this->dynamicsWorld, _parent));
else if (_type == "screw")
joint.reset(new SimbodyScrewJoint(this->dynamicsWorld, _parent));
+ else if (_type == "fixed")
+ joint.reset(new SimbodyFixedJoint(this->dynamicsWorld, _parent));
else
gzthrow("Unable to create joint of type[" << _type << "]");
@@ -596,6 +823,7 @@ void SimbodyPhysics::CreateMultibodyGraph(
_mbgraph.addJointType(GetTypeString(physics::Base::SLIDER_JOINT), 1);
_mbgraph.addJointType(GetTypeString(physics::Base::UNIVERSAL_JOINT), 2);
_mbgraph.addJointType(GetTypeString(physics::Base::SCREW_JOINT), 1);
+ _mbgraph.addJointType(GetTypeString(physics::Base::FIXED_JOINT), 0);
// Simbody has a Ball constraint that is a good choice if you need to
// break a loop at a ball joint.
@@ -1019,6 +1247,13 @@ void SimbodyPhysics::AddDynamicModelToSimbodySystem(
ballJoint.setDefaultRotation(defR_FM);
mobod = ballJoint;
}
+ else if (type == "fixed")
+ {
+ MobilizedBody::Weld fixedJoint(
+ parentMobod, X_IF0,
+ massProps, X_OM0);
+ mobod = fixedJoint;
+ }
else
{
gzerr << "Simbody joint type [" << type << "] not implemented.\n";
@@ -1131,6 +1366,8 @@ std::string SimbodyPhysics::GetTypeString(physics::Base::EntityType _type)
return "screw";
else if (_type & physics::Base::UNIVERSAL_JOINT)
return "universal";
+ else if (_type & physics::Base::FIXED_JOINT)
+ return "fixed";
gzerr << "Unrecognized joint type\n";
return "UNRECOGNIZED";
@@ -1173,6 +1410,10 @@ void SimbodyPhysics::AddCollisionsToLink(const physics::SimbodyLink *_link,
Transform X_LC =
SimbodyPhysics::Pose2Transform((*ci)->GetRelativePose());
+ // use pointer to store CollisionGeometry
+ SimbodyCollisionPtr sc =
+ boost::dynamic_pointer_cast<physics::SimbodyCollision>(*ci);
+
switch ((*ci)->GetShapeType() & (~physics::Entity::SHAPE))
{
case physics::Entity::PLANE_SHAPE:
@@ -1180,17 +1421,11 @@ void SimbodyPhysics::AddCollisionsToLink(const physics::SimbodyLink *_link,
boost::shared_ptr<physics::PlaneShape> p =
boost::dynamic_pointer_cast<physics::PlaneShape>((*ci)->GetShape());
- // Add a contact surface to represent the ground.
- // Half space normal is -x; must rotate about y to make it +z.
- this->matter.Ground().updBody().addContactSurface(Rotation(Pi/2, YAxis),
- ContactSurface(ContactGeometry::HalfSpace(), material));
-
- Vec3 normal = SimbodyPhysics::Vector3ToVec3(p->GetNormal());
-
// by default, simbody HalfSpace normal is in the -X direction
// rotate it based on normal vector specified by user
// Create a rotation whos x-axis is in the
// negative normal vector direction
+ Vec3 normal = SimbodyPhysics::Vector3ToVec3(p->GetNormal());
Rotation R_XN(-UnitVec3(normal), XAxis);
ContactSurface surface(ContactGeometry::HalfSpace(), material);
@@ -1198,7 +1433,12 @@ void SimbodyPhysics::AddCollisionsToLink(const physics::SimbodyLink *_link,
if (addModelClique)
surface.joinClique(_modelClique);
- _mobod.updBody().addContactSurface(R_XN, surface);
+ int surfNum = _mobod.updBody().addContactSurface(R_XN, surface);
+
+ // store ContactGeometry pointer in SimbodyCollision object
+ SimTK::ContactSurface &contactSurf =
+ _mobod.updBody().updContactSurface(surfNum);
+ sc->SetCollisionShape(&contactSurf.updShape());
}
break;
@@ -1210,7 +1450,12 @@ void SimbodyPhysics::AddCollisionsToLink(const physics::SimbodyLink *_link,
ContactSurface surface(ContactGeometry::Sphere(r), material);
if (addModelClique)
surface.joinClique(_modelClique);
- _mobod.updBody().addContactSurface(X_LC, surface);
+ int surfNum = _mobod.updBody().addContactSurface(X_LC, surface);
+
+ // store ContactGeometry pointer in SimbodyCollision object
+ SimTK::ContactSurface &contactSurf =
+ _mobod.updBody().updContactSurface(surfNum);
+ sc->SetCollisionShape(&contactSurf.updShape());
}
break;
@@ -1235,7 +1480,12 @@ void SimbodyPhysics::AddCollisionsToLink(const physics::SimbodyLink *_link,
if (addModelClique)
surface.joinClique(_modelClique);
- _mobod.updBody().addContactSurface(X_LC, surface);
+ int surfNum = _mobod.updBody().addContactSurface(X_LC, surface);
+
+ // store ContactGeometry pointer in SimbodyCollision object
+ SimTK::ContactSurface &contactSurf =
+ _mobod.updBody().updContactSurface(surfNum);
+ sc->SetCollisionShape(&contactSurf.updShape());
}
break;
@@ -1260,7 +1510,12 @@ void SimbodyPhysics::AddCollisionsToLink(const physics::SimbodyLink *_link,
if (addModelClique)
surface.joinClique(_modelClique);
- _mobod.updBody().addContactSurface(X_LC, surface);
+ int surfNum = _mobod.updBody().addContactSurface(X_LC, surface);
+
+ // store ContactGeometry pointer in SimbodyCollision object
+ SimTK::ContactSurface &contactSurf =
+ _mobod.updBody().updContactSurface(surfNum);
+ sc->SetCollisionShape(&contactSurf.updShape());
}
break;
default:
@@ -1342,99 +1597,102 @@ std::string SimbodyPhysics::GetTypeString(unsigned int _type)
//////////////////////////////////////////////////
boost::any SimbodyPhysics::GetParam(const std::string &_key) const
{
- if (_key == "type")
- {
- gzwarn << "Please use keyword `solver_typ` in the future.\n";
- return this->GetParam("solver_type");
- }
- else if (_key == "solver_type")
+ boost::any value;
+ this->GetParam(_key, value);
+ return value;
+}
+
+//////////////////////////////////////////////////
+bool SimbodyPhysics::GetParam(const std::string &_key, boost::any &_value) const
+{
+ if (_key == "solver_type")
{
- return "Spatial Algebra and Elastic Foundation";
+ _value = std::string("Spatial Algebra and Elastic Foundation");
}
else if (_key == "integrator_type")
{
- return this->integratorType;
+ _value = this->integratorType;
}
else if (_key == "accuracy")
{
if (this->integ)
- return this->integ->getAccuracyInUse();
+ _value = this->integ->getAccuracyInUse();
else
- return 0.0f;
+ _value = 0.0f;
}
else if (_key == "max_transient_velocity")
{
- return this->contact.getTransitionVelocity();
- }
- else if (_key == "max_step_size")
- {
- return this->GetMaxStepSize();
+ _value = this->contact.getTransitionVelocity();
}
else
{
- gzwarn << "key [" << _key
- << "] not supported, returning (int)0." << std::endl;
- return 0;
+ return PhysicsEngine::GetParam(_key, _value);
}
+ return true;
}
//////////////////////////////////////////////////
bool SimbodyPhysics::SetParam(const std::string &_key, const boost::any &_value)
{
- /// \TODO fill this out, see issue #1116
- if (_key == "accuracy")
+ try
{
- int value;
- try
+ if (_key == "accuracy")
{
- value = boost::any_cast<int>(_value);
+ this->integ->setAccuracy(boost::any_cast<double>(_value));
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "max_transient_velocity")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ this->contact.setTransitionVelocity(boost::any_cast<double>(_value));
}
- gzerr << "Setting [" << _key << "] in Simbody to [" << value
- << "] not yet supported.\n";
- return false;
- }
- else if (_key == "max_transient_velocity")
- {
- double value;
- try
+ else if (_key == "stiffness")
{
- value = boost::any_cast<double>(_value);
+ this->contactMaterialStiffness = boost::any_cast<double>(_value);
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "dissipation")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ this->contactMaterialDissipation = boost::any_cast<double>(_value);
}
- gzerr << "Setting [" << _key << "] in Simbody to [" << value
- << "] not yet supported.\n";
- return false;
- }
- else if (_key == "max_step_size")
- {
- double value;
- try
+ else if (_key == "plastic_coef_restitution")
{
- value = boost::any_cast<double>(_value);
+ this->contactMaterialPlasticCoefRestitution =
+ boost::any_cast<double>(_value);
}
- catch(const boost::bad_any_cast &e)
+ else if (_key == "plastic_impact_velocity")
{
- gzerr << "boost any_cast error:" << e.what() << "\n";
- return false;
+ this->contactMaterialPlasticImpactVelocity =
+ boost::any_cast<double>(_value);
+ }
+ else if (_key == "static_friction")
+ {
+ this->contactMaterialStaticFriction = boost::any_cast<double>(_value);
+ }
+ else if (_key == "dynamic_friction")
+ {
+ this->contactMaterialDynamicFriction = boost::any_cast<double>(_value);
+ }
+ else if (_key == "viscous_friction")
+ {
+ this->contactMaterialViscousFriction = boost::any_cast<double>(_value);
+ }
+ else if (_key == "override_impact_capture_velocity")
+ {
+ this->contactMaterialPlasticImpactVelocity =
+ boost::any_cast<double>(_value);
+ }
+ else if (_key == "override_stiction_transition_velocity")
+ {
+ this->contactImpactCaptureVelocity = boost::any_cast<double>(_value);
+ }
+ else
+ {
+ return PhysicsEngine::SetParam(_key, _value);
}
- gzerr << "Setting [" << _key << "] in Simbody to [" << value
- << "] not yet supported.\n";
- return false;
}
- else
+ catch(boost::bad_any_cast &e)
{
- gzwarn << _key << " is not supported in Simbody" << std::endl;
+ gzerr << "SimbodyPhysics::SetParam(" << _key << ") boost::any_cast error: "
+ << e.what() << std::endl;
return false;
}
- // should never get here
- return false;
+ return true;
}
diff --git a/gazebo/physics/simbody/SimbodyPhysics.hh b/gazebo/physics/simbody/SimbodyPhysics.hh
index 2f45600..0e999f4 100644
--- a/gazebo/physics/simbody/SimbodyPhysics.hh
+++ b/gazebo/physics/simbody/SimbodyPhysics.hh
@@ -39,7 +39,7 @@ namespace gazebo
/// \{
/// \brief Simbody physics engine
- class GAZEBO_VISIBLE SimbodyPhysics : public PhysicsEngine
+ class GZ_PHYSICS_VISIBLE SimbodyPhysics : public PhysicsEngine
{
/// \brief Constructor
public: SimbodyPhysics(WorldPtr _world);
@@ -219,6 +219,10 @@ namespace gazebo
public: virtual boost::any GetParam(const std::string &_key) const;
// Documentation inherited
+ public: virtual bool GetParam(const std::string &_key,
+ boost::any &_value) const;
+
+ // Documentation inherited
public: virtual bool SetParam(const std::string &_key,
const boost::any &_value);
diff --git a/gazebo/physics/simbody/SimbodyPlaneShape.hh b/gazebo/physics/simbody/SimbodyPlaneShape.hh
index 2d255a9..2df6373 100644
--- a/gazebo/physics/simbody/SimbodyPlaneShape.hh
+++ b/gazebo/physics/simbody/SimbodyPlaneShape.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \{
/// \brief Simbody collision for an infinite plane.
- class GAZEBO_VISIBLE SimbodyPlaneShape : public PlaneShape
+ class GZ_PHYSICS_VISIBLE SimbodyPlaneShape : public PlaneShape
{
/// \brief Constructor
public: SimbodyPlaneShape(CollisionPtr _parent);
diff --git a/gazebo/physics/simbody/SimbodyPolylineShape.hh b/gazebo/physics/simbody/SimbodyPolylineShape.hh
index fb26bc1..e3572ba 100644
--- a/gazebo/physics/simbody/SimbodyPolylineShape.hh
+++ b/gazebo/physics/simbody/SimbodyPolylineShape.hh
@@ -27,7 +27,7 @@ namespace gazebo
class SimbodyMesh;
/// \brief Simbody polyline shape
- class GAZEBO_VISIBLE SimbodyPolylineShape : public PolylineShape
+ class GZ_PHYSICS_VISIBLE SimbodyPolylineShape : public PolylineShape
{
/// \brief Constructor
/// \param[in] _parent Collision parent.
diff --git a/gazebo/physics/simbody/SimbodyRayShape.hh b/gazebo/physics/simbody/SimbodyRayShape.hh
index 36d7f36..b20f6b4 100644
--- a/gazebo/physics/simbody/SimbodyRayShape.hh
+++ b/gazebo/physics/simbody/SimbodyRayShape.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \{
/// \brief Ray shape for simbody
- class GAZEBO_VISIBLE SimbodyRayShape : public RayShape
+ class GZ_PHYSICS_VISIBLE SimbodyRayShape : public RayShape
{
/// \brief Constructor.
/// \param[in] _physicsEngine Pointer to the physics engine.
diff --git a/gazebo/physics/simbody/SimbodyScrewJoint.cc b/gazebo/physics/simbody/SimbodyScrewJoint.cc
index 477ae95..95977a8 100644
--- a/gazebo/physics/simbody/SimbodyScrewJoint.cc
+++ b/gazebo/physics/simbody/SimbodyScrewJoint.cc
@@ -256,7 +256,7 @@ double SimbodyScrewJoint::GetThreadPitch(unsigned int /*_index*/)
//////////////////////////////////////////////////
double SimbodyScrewJoint::GetThreadPitch()
{
- if (this->physicsInitialized &&
+ if (!this->mobod.isEmptyHandle() && this->physicsInitialized &&
this->simbodyPhysics->simbodyPhysicsInitialized)
{
// downcast mobod to screw mobod first
@@ -315,6 +315,14 @@ bool SimbodyScrewJoint::SetHighStop(
{
if (this->physicsInitialized)
{
+ // check if limitForce is initialized
+ if (this->limitForce[_index].isEmptyHandle())
+ {
+ gzerr << "child link is NULL, force element not initialized, "
+ << "SetHighStop failed. Please file a report on issue tracker.\n";
+ return false;
+ }
+
if (_index == 0)
{
// angular limit is specified
@@ -384,6 +392,14 @@ bool SimbodyScrewJoint::SetLowStop(
{
if (this->physicsInitialized)
{
+ // check if limitForce is initialized
+ if (this->limitForce[_index].isEmptyHandle())
+ {
+ gzerr << "child link is NULL, force element not initialized, "
+ << "SetHighStop failed. Please file a report on issue tracker.\n";
+ return false;
+ }
+
if (_index == 0)
{
// angular limit is specified
diff --git a/gazebo/physics/simbody/SimbodyScrewJoint.hh b/gazebo/physics/simbody/SimbodyScrewJoint.hh
index 308ab19..a1aeb35 100644
--- a/gazebo/physics/simbody/SimbodyScrewJoint.hh
+++ b/gazebo/physics/simbody/SimbodyScrewJoint.hh
@@ -32,7 +32,7 @@ namespace gazebo
/// \{
/// \brief A screw joint
- class GAZEBO_VISIBLE SimbodyScrewJoint : public ScrewJoint<SimbodyJoint>
+ class GZ_PHYSICS_VISIBLE SimbodyScrewJoint : public ScrewJoint<SimbodyJoint>
{
/// \brief Constructor
/// \param[in] _world Pointer to the Simbody world.
diff --git a/gazebo/physics/simbody/SimbodySliderJoint.hh b/gazebo/physics/simbody/SimbodySliderJoint.hh
index 9754b34..7d97ecf 100644
--- a/gazebo/physics/simbody/SimbodySliderJoint.hh
+++ b/gazebo/physics/simbody/SimbodySliderJoint.hh
@@ -34,7 +34,8 @@ namespace gazebo
/// \{
/// \brief A slider joint
- class GAZEBO_VISIBLE SimbodySliderJoint : public SliderJoint<SimbodyJoint>
+ class GZ_PHYSICS_VISIBLE SimbodySliderJoint :
+ public SliderJoint<SimbodyJoint>
{
/// \brief Constructor
/// \param[in] _world Pointer to the Simbody world.
diff --git a/gazebo/physics/simbody/SimbodySphereShape.hh b/gazebo/physics/simbody/SimbodySphereShape.hh
index 97da778..d229c8c 100644
--- a/gazebo/physics/simbody/SimbodySphereShape.hh
+++ b/gazebo/physics/simbody/SimbodySphereShape.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \{
/// \brief Simbody sphere collision
- class GAZEBO_VISIBLE SimbodySphereShape : public SphereShape
+ class GZ_PHYSICS_VISIBLE SimbodySphereShape : public SphereShape
{
/// \brief Constructor
/// \param[in] _parent Collision parent pointer
diff --git a/gazebo/physics/simbody/SimbodyUniversalJoint.hh b/gazebo/physics/simbody/SimbodyUniversalJoint.hh
index 4dce803..7aa3caa 100644
--- a/gazebo/physics/simbody/SimbodyUniversalJoint.hh
+++ b/gazebo/physics/simbody/SimbodyUniversalJoint.hh
@@ -32,7 +32,7 @@ namespace gazebo
/// \{
/// \brief A simbody universal joint class
- class GAZEBO_VISIBLE SimbodyUniversalJoint
+ class GZ_PHYSICS_VISIBLE SimbodyUniversalJoint
: public UniversalJoint<SimbodyJoint>
{
/// \brief Constructor
diff --git a/gazebo/rendering/ApplyWrenchVisual.cc b/gazebo/rendering/ApplyWrenchVisual.cc
new file mode 100644
index 0000000..e491cf9
--- /dev/null
+++ b/gazebo/rendering/ApplyWrenchVisual.cc
@@ -0,0 +1,519 @@
+/*
+ * 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/common/MeshManager.hh"
+
+#include "gazebo/rendering/DynamicLines.hh"
+#include "gazebo/rendering/Scene.hh"
+#include "gazebo/rendering/Visual.hh"
+#include "gazebo/rendering/SelectionObj.hh"
+#include "gazebo/rendering/ApplyWrenchVisualPrivate.hh"
+#include "gazebo/rendering/ApplyWrenchVisual.hh"
+
+using namespace gazebo;
+using namespace rendering;
+
+/////////////////////////////////////////////////
+ApplyWrenchVisual::ApplyWrenchVisual(const std::string &_name,
+ VisualPtr _parentVis)
+ : Visual(*new ApplyWrenchVisualPrivate, _name, _parentVis, false)
+{
+}
+
+/////////////////////////////////////////////////
+ApplyWrenchVisual::~ApplyWrenchVisual()
+{
+ this->Fini();
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchVisual::Fini()
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ if (dPtr->torqueVisual && dPtr->torqueLine)
+ dPtr->torqueVisual->DeleteDynamicLine(dPtr->torqueLine);
+
+ if (!dPtr->scene)
+ return;
+
+ // Destroy objects and nodes
+ Ogre::SceneManager *manager = dPtr->scene->GetManager();
+ if (!manager)
+ return;
+
+ std::vector<std::string> suffixes = {
+ "_FORCE_SHAFT_",
+ "_FORCE_HEAD_",
+ "_TORQUE_TUBE_",
+ "_TORQUE_HEAD_"};
+
+ for (auto suffix : suffixes)
+ {
+ std::string name = this->GetName() + suffix;
+ if (manager->hasEntity(name))
+ manager->destroyMovableObject(manager->getEntity(name));
+
+ name += "NODE_";
+ if (manager->hasSceneNode(name))
+ manager->destroySceneNode(manager->getSceneNode(name));
+ }
+
+ // Remove visuals
+ if (dPtr->forceVisual &&
+ dPtr->scene->GetVisual(dPtr->forceVisual->GetName()))
+ {
+ dPtr->scene->RemoveVisual(dPtr->forceVisual);
+ }
+
+ if (dPtr->torqueVisual &&
+ dPtr->scene->GetVisual(dPtr->torqueVisual->GetName()))
+ {
+ dPtr->scene->RemoveVisual(dPtr->torqueVisual);
+ }
+
+ if (dPtr->rotTool &&
+ dPtr->scene->GetVisual(dPtr->rotTool->GetName()))
+ {
+ dPtr->rotTool->Fini();
+ dPtr->scene->RemoveVisual(dPtr->rotTool);
+ }
+
+ dPtr->forceVisual.reset();
+ dPtr->torqueVisual.reset();
+ dPtr->rotTool.reset();
+
+ Visual::Fini();
+}
+
+///////////////////////////////////////////////////
+void ApplyWrenchVisual::Load()
+{
+ Visual::Load();
+
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ if (!dPtr->scene)
+ {
+ gzerr << "Visual has no scene, not loading." << std::endl;
+ return;
+ }
+
+ dPtr->selectedMaterial = "Gazebo/OrangeTransparentOverlay";
+ dPtr->unselectedMaterial = "Gazebo/DarkOrangeTransparentOverlay";
+
+ // Force visual
+ dPtr->forceVisual.reset(new rendering::Visual(
+ this->GetName() + "_FORCE_VISUAL_", shared_from_this()));
+ dPtr->forceVisual->Load();
+
+ // Force shaft
+ this->InsertMesh("axis_shaft");
+
+ Ogre::MovableObject *shaftObj =
+ (Ogre::MovableObject*)(dPtr->scene->GetManager()->createEntity(
+ this->GetName()+"_FORCE_SHAFT_", "axis_shaft"));
+ shaftObj->getUserObjectBindings().setUserAny(
+ Ogre::Any(std::string(dPtr->forceVisual->GetName())));
+
+ Ogre::SceneNode *shaftNode =
+ dPtr->forceVisual->GetSceneNode()->createChildSceneNode(
+ this->GetName() + "_FORCE_SHAFT_NODE_");
+ shaftNode->attachObject(shaftObj);
+ shaftNode->setPosition(0, 0, 0.1);
+
+ // Force head
+ this->InsertMesh("axis_head");
+
+ Ogre::MovableObject *headObj =
+ (Ogre::MovableObject*)(dPtr->scene->GetManager()->createEntity(
+ this->GetName()+"_FORCE_HEAD_", "axis_head"));
+ headObj->getUserObjectBindings().setUserAny(
+ Ogre::Any(std::string(dPtr->forceVisual->GetName())));
+
+ Ogre::SceneNode *headNode =
+ dPtr->forceVisual->GetSceneNode()->createChildSceneNode(
+ this->GetName() + "_FORCE_HEAD_NODE_");
+ headNode->attachObject(headObj);
+ headNode->setPosition(0, 0, 0.24);
+
+ dPtr->forceVisual->SetMaterial(dPtr->unselectedMaterial);
+ dPtr->forceVisual->GetSceneNode()->setInheritScale(false);
+
+ // Torque visual
+ dPtr->torqueVisual.reset(new rendering::Visual(
+ this->GetName() + "_TORQUE_VISUAL_", shared_from_this()));
+ dPtr->torqueVisual->Load();
+
+ // Torque tube
+ common::MeshManager::Instance()->CreateTube("torque_tube",
+ 0.1, 0.15, 0.05, 2, 32, 1.5*M_PI);
+ this->InsertMesh("torque_tube");
+
+ Ogre::MovableObject *tubeObj =
+ (Ogre::MovableObject*)(dPtr->scene->GetManager()->createEntity(
+ this->GetName()+"_TORQUE_TUBE_", "torque_tube"));
+ tubeObj->getUserObjectBindings().setUserAny(
+ Ogre::Any(std::string(dPtr->torqueVisual->GetName())));
+
+ Ogre::SceneNode *tubeNode =
+ dPtr->torqueVisual->GetSceneNode()->createChildSceneNode(
+ this->GetName() + "_TORQUE_TUBE_NODE_");
+ tubeNode->attachObject(tubeObj);
+
+ // Torque arrow
+ this->InsertMesh("axis_head");
+
+ Ogre::MovableObject *torqueHeadObj =
+ (Ogre::MovableObject*)(dPtr->scene->GetManager()->createEntity(
+ this->GetName()+"_TORQUE_HEAD_", "axis_head"));
+ torqueHeadObj->getUserObjectBindings().setUserAny(
+ Ogre::Any(std::string(dPtr->torqueVisual->GetName())));
+
+ Ogre::SceneNode *torqueHeadNode =
+ dPtr->torqueVisual->GetSceneNode()->createChildSceneNode(
+ this->GetName() + "_TORQUE_HEAD_NODE_");
+ torqueHeadNode->attachObject(torqueHeadObj);
+ torqueHeadNode->setScale(3, 3, 1);
+ torqueHeadNode->setPosition(-0.04, 0.125, 0);
+ math::Quaternion quat(0, -M_PI/2.0, 0);
+ torqueHeadNode->setOrientation(
+ Ogre::Quaternion(quat.w, quat.x, quat.y, quat.z));
+
+ dPtr->torqueVisual->SetMaterial(dPtr->unselectedMaterial);
+ dPtr->torqueVisual->GetSceneNode()->setInheritScale(false);
+
+ // Torque line
+ dPtr->torqueLine = dPtr->torqueVisual->
+ CreateDynamicLine(rendering::RENDERING_LINE_LIST);
+ dPtr->torqueLine->setMaterial(dPtr->unselectedMaterial);
+ dPtr->torqueLine->AddPoint(0, 0, 0);
+ dPtr->torqueLine->AddPoint(0, 0, 0.1);
+
+ // Rotation manipulator
+ dPtr->rotTool.reset(new rendering::SelectionObj(
+ this->GetName() + "__SELECTION_OBJ", shared_from_this()));
+ dPtr->rotTool->Load();
+ dPtr->rotTool->SetMode("rotate");
+ dPtr->rotTool->SetHandleVisible(SelectionObj::ROT_X, false);
+ dPtr->rotTool->SetHandleMaterial(SelectionObj::ROT_Y,
+ "Gazebo/DarkMagentaTransparent");
+ dPtr->rotTool->SetHandleMaterial(SelectionObj::ROT_Z,
+ "Gazebo/DarkMagentaTransparent");
+
+ // Initialize
+ dPtr->forceVector = math::Vector3::Zero;
+ dPtr->torqueVector = math::Vector3::Zero;
+
+ this->SetVisibilityFlags(GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE);
+ this->Resize();
+ this->UpdateForceVisual();
+ this->UpdateTorqueVisual();
+ this->SetMode(Mode::NONE);
+}
+
+///////////////////////////////////////////////////
+math::Quaternion ApplyWrenchVisual::GetQuaternionFromVector(
+ const math::Vector3 &_vec)
+{
+ double roll = 0;
+ double pitch = -atan2(_vec.z, sqrt(pow(_vec.x, 2) + pow(_vec.y, 2)));
+ double yaw = atan2(_vec.y, _vec.x);
+
+ return math::Quaternion(roll, pitch, yaw);
+}
+
+///////////////////////////////////////////////////
+void ApplyWrenchVisual::SetCoM(const math::Vector3 &_comVector)
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ dPtr->comVector = _comVector;
+
+ {
+ // UpdateTorqueVisual changes torqueVisual
+ std::lock_guard<std::mutex> lock(dPtr->mutex);
+
+ this->UpdateTorqueVisual();
+ }
+}
+
+///////////////////////////////////////////////////
+void ApplyWrenchVisual::SetForcePos(const math::Vector3 &_forcePosVector)
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ dPtr->forcePosVector = _forcePosVector;
+
+ {
+ // UpdateForceVisual changes forceVisual
+ std::lock_guard<std::mutex> lock(dPtr->mutex);
+
+ this->UpdateForceVisual();
+ }
+}
+
+///////////////////////////////////////////////////
+void ApplyWrenchVisual::SetForce(const math::Vector3 &_forceVector,
+ const bool _rotatedByMouse)
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ dPtr->forceVector = _forceVector;
+ dPtr->rotatedByMouse = _rotatedByMouse;
+
+ if (_forceVector == math::Vector3::Zero)
+ {
+ if (dPtr->torqueVector == math::Vector3::Zero)
+ this->SetMode(Mode::NONE);
+ else
+ this->SetMode(Mode::TORQUE);
+ }
+ else
+ {
+ this->SetMode(Mode::FORCE);
+ }
+}
+
+///////////////////////////////////////////////////
+void ApplyWrenchVisual::SetTorque(const math::Vector3 &_torqueVector,
+ const bool _rotatedByMouse)
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ dPtr->torqueVector = _torqueVector;
+ dPtr->rotatedByMouse = _rotatedByMouse;
+
+ if (_torqueVector == math::Vector3::Zero)
+ {
+ if (dPtr->forceVector == math::Vector3::Zero)
+ this->SetMode(Mode::NONE);
+ else
+ this->SetMode(Mode::FORCE);
+ }
+ else
+ {
+ this->SetMode(Mode::TORQUE);
+ }
+}
+
+///////////////////////////////////////////////////
+void ApplyWrenchVisual::UpdateForceVisual()
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ if (!dPtr->forceVisual || !dPtr->rotTool)
+ {
+ gzwarn << "No force visual" << std::endl;
+ return;
+ }
+
+ math::Vector3 normVec = dPtr->forceVector;
+ normVec.Normalize();
+
+ // Place it on X axis in case it is zero
+ if (normVec == math::Vector3::Zero)
+ normVec = math::Vector3::UnitX;
+
+ // Set rotation in the vector direction
+ math::Quaternion quat = this->GetQuaternionFromVector(normVec);
+ dPtr->forceVisual->SetRotation(quat * math::Quaternion(
+ math::Vector3(0, M_PI/2.0, 0)));
+
+ // Set arrow tip to forcePosVector
+ dPtr->forceVisual->SetPosition(-normVec * 0.28 *
+ dPtr->forceVisual->GetScale().z + dPtr->forcePosVector);
+
+ // Rotation tool
+ dPtr->rotTool->SetPosition(dPtr->forcePosVector);
+ if (!dPtr->rotatedByMouse)
+ dPtr->rotTool->SetRotation(quat);
+}
+
+///////////////////////////////////////////////////
+void ApplyWrenchVisual::UpdateTorqueVisual()
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ if (!dPtr->torqueVisual || !dPtr->rotTool)
+ {
+ gzwarn << "No torque visual" << std::endl;
+ return;
+ }
+
+ math::Vector3 normVec = dPtr->torqueVector;
+ normVec.Normalize();
+
+ // Place it on X axis in case it is zero
+ if (normVec == math::Vector3::Zero)
+ normVec = math::Vector3::UnitX;
+
+ // Set rotation in the vector direction
+ math::Quaternion quat = this->GetQuaternionFromVector(normVec);
+ dPtr->torqueVisual->SetRotation(quat * math::Quaternion(
+ math::Vector3(0, M_PI/2.0, 0)));
+
+ // Position towards comVector
+ double linkDiagonal = dPtr->parent->GetBoundingBox().GetSize().GetLength();
+ dPtr->torqueVisual->SetPosition(normVec*linkDiagonal*0.75 + dPtr->comVector);
+ dPtr->torqueLine->SetPoint(1,
+ math::Vector3(0, 0, -linkDiagonal*0.75)/dPtr->torqueVisual->GetScale());
+
+ // Rotation tool
+ dPtr->rotTool->SetPosition(dPtr->comVector);
+ if (!dPtr->rotatedByMouse)
+ dPtr->rotTool->SetRotation(quat);
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchVisual::Resize()
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ if (!dPtr->parent || !dPtr->forceVisual || !dPtr->torqueVisual ||
+ !dPtr->rotTool)
+ {
+ gzwarn << "ApplyWrenchVisual is incomplete." << std::endl;
+ return;
+ }
+
+ // Protect force/torque visuals
+ std::lock_guard<std::mutex> lock(dPtr->mutex);
+
+ double linkSize = std::max(0.1,
+ dPtr->parent->GetBoundingBox().GetSize().GetLength());
+
+ // Force visual
+ dPtr->forceVisual->SetScale(math::Vector3(2*linkSize,
+ 2*linkSize,
+ 2*linkSize));
+
+ // Torque visual
+ dPtr->torqueVisual->SetScale(math::Vector3(linkSize,
+ linkSize,
+ linkSize));
+
+ // Rot tool
+ dPtr->rotTool->SetScale(math::Vector3(0.75*linkSize,
+ 0.75*linkSize,
+ 0.75*linkSize));
+}
+
+///////////////////////////////////////////////////
+rendering::VisualPtr ApplyWrenchVisual::GetForceVisual() const
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ if (!dPtr->forceVisual)
+ {
+ gzerr << "Force visual not found, but it should exist." << std::endl;
+ return NULL;
+ }
+
+ std::lock_guard<std::mutex> lock(dPtr->mutex);
+
+ return dPtr->forceVisual;
+}
+
+///////////////////////////////////////////////////
+rendering::VisualPtr ApplyWrenchVisual::GetTorqueVisual() const
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ if (!dPtr->torqueVisual)
+ {
+ gzerr << "Torque visual not found, but it should exist." << std::endl;
+ return NULL;
+ }
+
+ std::lock_guard<std::mutex> lock(dPtr->mutex);
+
+ return dPtr->torqueVisual;
+}
+
+///////////////////////////////////////////////////
+rendering::SelectionObjPtr ApplyWrenchVisual::GetRotTool() const
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ if (!dPtr->rotTool)
+ {
+ gzerr << "Rot tool not found, but it should exist." << std::endl;
+ return NULL;
+ }
+
+ return dPtr->rotTool;
+}
+
+/////////////////////////////////////////////////
+void ApplyWrenchVisual::SetMode(Mode _mode)
+{
+ ApplyWrenchVisualPrivate *dPtr =
+ reinterpret_cast<ApplyWrenchVisualPrivate *>(this->dataPtr);
+
+ if (!dPtr->forceVisual || !dPtr->torqueVisual || !dPtr->rotTool)
+ {
+ gzerr << "Some visual is missing!" << std::endl;
+ return;
+ }
+
+ // Protect force/torque visuals
+ std::lock_guard<std::mutex> lock(dPtr->mutex);
+
+ if (_mode == Mode::FORCE)
+ {
+ dPtr->forceVisual->SetMaterial(dPtr->selectedMaterial);
+ dPtr->torqueVisual->SetMaterial(dPtr->unselectedMaterial);
+
+ dPtr->rotTool->SetHandleVisible(SelectionObj::ROT_Y, true);
+ dPtr->rotTool->SetHandleVisible(SelectionObj::ROT_Z, true);
+
+ this->UpdateForceVisual();
+ }
+ else if (_mode == Mode::TORQUE)
+ {
+ dPtr->torqueVisual->SetMaterial(dPtr->selectedMaterial);
+ dPtr->forceVisual->SetMaterial(dPtr->unselectedMaterial);
+
+ dPtr->rotTool->SetHandleVisible(SelectionObj::ROT_Y, true);
+ dPtr->rotTool->SetHandleVisible(SelectionObj::ROT_Z, true);
+
+ this->UpdateTorqueVisual();
+ }
+ else if (_mode == Mode::NONE)
+ {
+ // Dark visuals
+ dPtr->forceVisual->SetMaterial(dPtr->unselectedMaterial);
+ dPtr->torqueVisual->SetMaterial(dPtr->unselectedMaterial);
+ // hide rot
+ dPtr->rotTool->SetHandleVisible(SelectionObj::ROT_Y, false);
+ dPtr->rotTool->SetHandleVisible(SelectionObj::ROT_Z, false);
+ }
+}
+
diff --git a/gazebo/rendering/ApplyWrenchVisual.hh b/gazebo/rendering/ApplyWrenchVisual.hh
new file mode 100644
index 0000000..2801278
--- /dev/null
+++ b/gazebo/rendering/ApplyWrenchVisual.hh
@@ -0,0 +1,124 @@
+/*
+ * 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_APPLYWRENCHVISUAL_HH_
+#define _GAZEBO_APPLYWRENCHVISUAL_HH_
+
+#include <string>
+
+#include "gazebo/rendering/Visual.hh"
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ /// \addtogroup gazebo_rendering
+ /// \{
+
+ /// \class ApplyWrenchVisual ApplyWrenchVisual.hh rendering/rendering.hh
+ /// \brief Visualization for the apply wrench GUI
+ class GAZEBO_VISIBLE ApplyWrenchVisual : public Visual
+ {
+ /// \enum Modes
+ /// \brief Visual is in one of these modes at each time.
+ public: enum Mode
+ {
+ /// \brief None
+ NONE = 0,
+ /// \brief Force
+ FORCE = 1,
+ /// \brief Torque
+ TORQUE = 2
+ };
+
+ /// \brief Constructor
+ /// \param[in] _name Name of the visual
+ /// \param[in] _parentVis Pointer to the parent visual
+ public: ApplyWrenchVisual(const std::string &_name, VisualPtr _parentVis);
+
+ /// \brief Destructor
+ public: virtual ~ApplyWrenchVisual();
+
+ // Documentation inherited.
+ public: void Load();
+
+ // Documentation inherited.
+ public: virtual void Fini();
+
+ /// \brief Set the CoM vector and update the position of the torque
+ /// visual.
+ /// \param[in] _comVector New vector.
+ public: void SetCoM(const math::Vector3 &_comVector);
+
+ /// \brief Set the force position vector and update the position of the
+ /// force visual.
+ /// \param[in] _forcePosVector New vector.
+ public: void SetForcePos(const math::Vector3 &_forcePosVector);
+
+ /// \brief Update force vector, force text and mode.
+ /// \param[in] _forceVector New vector.
+ /// \param[in] _rotatedByMouse Whether the rotation comes from the mouse
+ /// or not.
+ public: void SetForce(const math::Vector3 &_forceVector,
+ const bool _rotatedByMouse);
+
+ /// \brief Update torque vector, torque text and mode.
+ /// \param[in] _torqueVector New vector.
+ /// \param[in] _rotatedByMouse Whether the rotation comes from the mouse
+ /// or not.
+ public: void SetTorque(const math::Vector3 &_torqueVector,
+ const bool _rotatedByMouse);
+
+ /// \brief Update the force visual according to the force and force
+ /// position vectors.
+ public: void UpdateForceVisual();
+
+ /// \brief Update the torque visual according to the torque and CoM
+ /// vectors.
+ public: void UpdateTorqueVisual();
+
+ /// \brief Resize all children according to target link's size.
+ public: void Resize();
+
+ /// \brief Returns the force visual.
+ /// \return Pointer to force visual.
+ public: rendering::VisualPtr GetForceVisual() const;
+
+ /// \brief Returns the torque visual.
+ /// \return Pointer to torque visual.
+ public: rendering::VisualPtr GetTorqueVisual() const;
+
+ /// \brief Returns the rotation tool.
+ /// \return Pointer to rotation tool.
+ public: rendering::SelectionObjPtr GetRotTool() const;
+
+ /// \brief Set the mode to force, torque or none, update colors
+ /// and visibility accordingly.
+ /// \param[in] _mode New mode.
+ public: void SetMode(Mode _mode);
+
+ /// \brief Get the rotation to point the positive Z axis to the
+ /// given direction.
+ /// \param[in] _dir Direction vector.
+ /// \return Resulting quaternion
+ private: math::Quaternion GetQuaternionFromVector(
+ const math::Vector3 &_dir);
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/rendering/ApplyWrenchVisualPrivate.hh b/gazebo/rendering/ApplyWrenchVisualPrivate.hh
new file mode 100644
index 0000000..16ee068
--- /dev/null
+++ b/gazebo/rendering/ApplyWrenchVisualPrivate.hh
@@ -0,0 +1,72 @@
+/*
+ * 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_APPLYWRENCHVISUAL_PRIVATE_HH_
+#define _GAZEBO_APPLYWRENCHVISUAL_PRIVATE_HH_
+
+#include <string>
+#include <mutex>
+
+#include "gazebo/rendering/VisualPrivate.hh"
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ /// \brief Private data for the Apply Wrench Visual class.
+ class ApplyWrenchVisualPrivate : public VisualPrivate
+ {
+ /// \brief Material for the current mode.
+ public: std::string selectedMaterial;
+
+ /// \brief Material for the unselected mode.
+ public: std::string unselectedMaterial;
+
+ /// \brief Arrow representing force.
+ public: VisualPtr forceVisual;
+
+ /// \brief Tube and line representing torque.
+ public: VisualPtr torqueVisual;
+
+ /// \brief Line connecting the torque visual to the CoM.
+ public: rendering::DynamicLines *torqueLine;
+
+ /// \brief CoM position in link coordinates.
+ public: math::Vector3 comVector;
+
+ /// \brief Force application point in link coordinates.
+ public: math::Vector3 forcePosVector;
+
+ /// \brief Force vector expressed in the link frame.
+ public: math::Vector3 forceVector;
+
+ /// \brief Torque vector expressed in the link frame.
+ public: math::Vector3 torqueVector;
+
+ /// \brief Mutex to protect variables
+ public: std::mutex mutex;
+
+ /// \brief Rotation tool composed of two circles.
+ public: rendering::SelectionObjPtr rotTool;
+
+ /// \brief If true, the rotation tool was rotated by the mouse and
+ /// shouldn't be oriented again according to the vector.
+ public: bool rotatedByMouse;
+ };
+ }
+}
+#endif
diff --git a/gazebo/rendering/ArrowVisual.cc b/gazebo/rendering/ArrowVisual.cc
index c8af0ad..db5212e 100644
--- a/gazebo/rendering/ArrowVisual.cc
+++ b/gazebo/rendering/ArrowVisual.cc
@@ -35,6 +35,7 @@ ArrowVisual::ArrowVisual(const std::string &_name, VisualPtr _vis)
ArrowVisualPrivate *dPtr =
reinterpret_cast<ArrowVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_GUI;
dPtr->headNode = NULL;
dPtr->shaftNode = NULL;
dPtr->rotationNode = NULL;
@@ -101,14 +102,11 @@ void ArrowVisual::ShowShaft(bool _show)
ArrowVisualPrivate *dPtr =
reinterpret_cast<ArrowVisualPrivate *>(this->dataPtr);
+ dPtr->sceneNode->removeChild(dPtr->shaftNode);
if (_show)
{
dPtr->sceneNode->addChild(dPtr->shaftNode);
}
- else
- {
- dPtr->sceneNode->removeChild(dPtr->shaftNode);
- }
}
/////////////////////////////////////////////////
@@ -117,14 +115,11 @@ void ArrowVisual::ShowHead(bool _show)
ArrowVisualPrivate *dPtr =
reinterpret_cast<ArrowVisualPrivate *>(this->dataPtr);
+ dPtr->sceneNode->removeChild(dPtr->headNode);
if (_show)
{
dPtr->sceneNode->addChild(dPtr->headNode);
}
- else
- {
- dPtr->sceneNode->removeChild(dPtr->headNode);
- }
}
/////////////////////////////////////////////////
@@ -133,6 +128,7 @@ void ArrowVisual::ShowRotation(bool _show)
ArrowVisualPrivate *dPtr =
reinterpret_cast<ArrowVisualPrivate *>(this->dataPtr);
+ dPtr->sceneNode->removeChild(dPtr->rotationNode);
if (_show)
{
Ogre::MovableObject *rotationObj = dPtr->rotationNode->getAttachedObject(0);
@@ -145,8 +141,4 @@ void ArrowVisual::ShowRotation(bool _show)
dPtr->rotationNode->setVisible(this->GetVisible());
dPtr->sceneNode->addChild(dPtr->rotationNode);
}
- else
- {
- dPtr->sceneNode->removeChild(dPtr->rotationNode);
- }
}
diff --git a/gazebo/rendering/ArrowVisual.hh b/gazebo/rendering/ArrowVisual.hh
index 511ea1c..2b3265e 100644
--- a/gazebo/rendering/ArrowVisual.hh
+++ b/gazebo/rendering/ArrowVisual.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class ArrowVisual ArrowVisual.hh rendering/rendering.hh
/// \brief Basic arrow visualization
- class GAZEBO_VISIBLE ArrowVisual : public Visual
+ class GZ_RENDERING_VISIBLE ArrowVisual : public Visual
{
/// \brief Constructor
/// \param[in] _name Name of the arrow visual
diff --git a/gazebo/rendering/AxisVisual.cc b/gazebo/rendering/AxisVisual.cc
index 05a7ca8..30e0352 100644
--- a/gazebo/rendering/AxisVisual.cc
+++ b/gazebo/rendering/AxisVisual.cc
@@ -30,9 +30,24 @@ using namespace gazebo;
using namespace rendering;
/////////////////////////////////////////////////
-AxisVisual::AxisVisual(const std::string &_name, VisualPtr _vis)
- : Visual(*new AxisVisualPrivate, _name, _vis, false)
+AxisVisual::AxisVisual(const std::string &_name, VisualPtr _parent)
+ : Visual(*new AxisVisualPrivate, _name, _parent, false)
{
+ AxisVisualPrivate *dPtr =
+ reinterpret_cast<AxisVisualPrivate *>(this->dataPtr);
+
+ dPtr->type = VT_GUI;
+}
+
+//////////////////////////////////////////////////
+AxisVisual::AxisVisual(VisualPrivate &_dataPtr, const std::string &_name,
+ VisualPtr _parent)
+ : Visual(_dataPtr, _name, _parent, false)
+{
+ AxisVisualPrivate *dPtr =
+ reinterpret_cast<AxisVisualPrivate *>(this->dataPtr);
+
+ dPtr->type = VT_GUI;
}
/////////////////////////////////////////////////
diff --git a/gazebo/rendering/AxisVisual.hh b/gazebo/rendering/AxisVisual.hh
index 74c243b..8fa094d 100644
--- a/gazebo/rendering/AxisVisual.hh
+++ b/gazebo/rendering/AxisVisual.hh
@@ -33,12 +33,12 @@ namespace gazebo
/// \class AxisVisual AxisVisual.hh rendering/rendering.hh
/// \brief Basic axis visualization
- class GAZEBO_VISIBLE AxisVisual : public Visual
+ class GZ_RENDERING_VISIBLE AxisVisual : public Visual
{
/// \brief Constructor
/// \param[in] _name Name of the AxisVisual
- /// \param[in] _vis Parent visual
- public: AxisVisual(const std::string &_name, VisualPtr _vis);
+ /// \param[in] _parent Parent visual
+ public: AxisVisual(const std::string &_name, VisualPtr _parent);
/// \brief Destructor
public: virtual ~AxisVisual();
@@ -83,6 +83,14 @@ namespace gazebo
/// \param[in] _axis The number of the axis (0, 1, 2 = x,y,z)
/// \param[in] _visible True to set the axis to be visible.
public: void SetAxisVisible(unsigned int _axis, bool _visible);
+
+ /// \internal
+ /// \brief Constructor used by inherited classes
+ /// \param[in] _dataPtr Pointer to private data.
+ /// \param[in] _name Name of the visual.
+ /// \param[in] _parent Parent of the visual.
+ protected: AxisVisual(VisualPrivate &_dataPtr,
+ const std::string &_name, VisualPtr _parent);
};
/// \}
}
diff --git a/gazebo/rendering/CMakeLists.txt b/gazebo/rendering/CMakeLists.txt
index 36ff408..c55a383 100644
--- a/gazebo/rendering/CMakeLists.txt
+++ b/gazebo/rendering/CMakeLists.txt
@@ -2,17 +2,27 @@ include (${gazebo_cmake_dir}/GazeboUtils.cmake)
include_directories(SYSTEM
${OGRE_INCLUDE_DIRS}
+ ${TBB_INCLUDEDIR}
skyx
skyx/include
)
-if (${OGRE_VERSION} GREATER 1.7.4)
+if (${OGRE_VERSION} VERSION_GREATER 1.7.4)
add_subdirectory(deferred_shading)
endif()
+
add_subdirectory(skyx)
add_subdirectory(selection_buffer)
+if (WIN32)
+ include_directories(${libdl_include_dir})
+endif()
+
+include_directories(${tinyxml_INCLUDE_DIRS})
+link_directories(${tinyxml_LIBRARY_DIRS})
+
set (sources
+ ApplyWrenchVisual.cc
ArrowVisual.cc
AxisVisual.cc
Camera.cc
@@ -28,13 +38,17 @@ set (sources
GpuLaser.cc
Grid.cc
Heightmap.cc
+ InertiaVisual.cc
JointVisual.cc
LaserVisual.cc
+ LinkFrameVisual.cc
SonarVisual.cc
Light.cc
Material.cc
MovableText.cc
OrbitViewController.cc
+ OriginVisual.cc
+ OrthoViewController.cc
Projector.cc
RayQuery.cc
RenderEngine.cc
@@ -56,7 +70,17 @@ set (sources
WrenchVisual.cc
)
+if (NOT USE_EXTERNAL_TINYXML)
+ include_directories (${CMAKE_SOURCE_DIR}/deps/win/tinyxml)
+ set (sources ${sources}
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinystr.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxml.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxmlerror.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxmlparser.cpp)
+endif()
+
set (headers
+ ApplyWrenchVisual.hh
ArrowVisual.hh
AxisVisual.hh
Camera.hh
@@ -72,13 +96,17 @@ set (headers
GpuLaser.hh
Grid.hh
Heightmap.hh
+ InertiaVisual.hh
JointVisual.hh
LaserVisual.hh
+ LinkFrameVisual.hh
SonarVisual.hh
Light.hh
Material.hh
MovableText.hh
OrbitViewController.hh
+ OriginVisual.hh
+ OrthoViewController.hh
Projector.hh
RayQuery.hh
RenderEngine.hh
@@ -111,15 +139,19 @@ if (HAVE_OCULUS)
)
endif ()
-set (gtest_sources
-)
+set (gtest_sources)
gz_build_tests(${gtest_sources})
set (gtest_sources_dri
+ COMVisual_TEST.cc
GpuLaser_TEST.cc
Heightmap_TEST.cc
JointVisual_TEST.cc
+ LinkFrameVisual_TEST.cc
+ OriginVisual_TEST.cc
+ Scene_TEST.cc
+ SelectionObj_TEST.cc
SonarVisual_TEST.cc
TransmitterVisual_TEST.cc
Visual_TEST.cc
@@ -139,6 +171,11 @@ link_directories(
${ogre_library_dirs}
)
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_RENDERING")
+if (WIN32)
+ set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /FIWinsock2.h")
+endif()
+
gz_add_library(gazebo_rendering ${sources})
target_link_libraries(gazebo_rendering
@@ -149,19 +186,18 @@ target_link_libraries(gazebo_rendering
gazebo_transport
${ogre_libraries}
${OPENGL_LIBRARIES}
+ ${tinyxml_LIBRARIES}
)
if (HAVE_OCULUS)
- target_link_libraries(gazebo_rendering
- OculusVR
- )
+ target_link_libraries(gazebo_rendering ${OculusVR_LIBRARIES})
endif()
-if (NOT APPLE)
+if (NOT APPLE AND NOT WIN32)
target_link_libraries(gazebo_rendering X11)
endif()
-if (${OGRE_VERSION} GREATER 1.7.4)
+if (${OGRE_VERSION} VERSION_GREATER 1.7.4)
target_link_libraries(gazebo_rendering gazebo_rendering_deferred)
endif()
diff --git a/gazebo/rendering/COMVisual.cc b/gazebo/rendering/COMVisual.cc
index 942ce0a..52654c4 100644
--- a/gazebo/rendering/COMVisual.cc
+++ b/gazebo/rendering/COMVisual.cc
@@ -18,13 +18,11 @@
* Author: Nate Koenig
*/
-#include "gazebo/common/MeshManager.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Quaternion.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/rendering/DynamicLines.hh"
-#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/COMVisualPrivate.hh"
#include "gazebo/rendering/COMVisual.hh"
@@ -36,6 +34,9 @@ using namespace rendering;
COMVisual::COMVisual(const std::string &_name, VisualPtr _vis)
: Visual(*new COMVisualPrivate, _name, _vis, false)
{
+ COMVisualPrivate *dPtr =
+ reinterpret_cast<COMVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_PHYSICS;
}
/////////////////////////////////////////////////
@@ -47,8 +48,28 @@ COMVisual::~COMVisual()
void COMVisual::Load(sdf::ElementPtr _elem)
{
Visual::Load();
- math::Pose pose = _elem->Get<math::Pose>("origin");
- this->Load(pose);
+
+ COMVisualPrivate *dPtr =
+ reinterpret_cast<COMVisualPrivate *>(this->dataPtr);
+
+ if (_elem->HasAttribute("name"))
+ dPtr->linkName = _elem->Get<std::string>("name");
+
+ if (_elem->HasElement("inertial"))
+ {
+ if (_elem->GetElement("inertial")->HasElement("pose"))
+ {
+ dPtr->inertiaPose =
+ _elem->GetElement("inertial")->Get<math::Pose>("pose");
+ }
+ else if (_elem->GetElement("inertial")->HasElement("mass"))
+ {
+ dPtr->mass =
+ _elem->GetElement("inertial")->Get<double>("mass");
+ }
+ }
+
+ this->Load();
}
/////////////////////////////////////////////////
@@ -56,6 +77,9 @@ void COMVisual::Load(ConstLinkPtr &_msg)
{
Visual::Load();
+ COMVisualPrivate *dPtr =
+ reinterpret_cast<COMVisualPrivate *>(this->dataPtr);
+
math::Vector3 xyz(_msg->inertial().pose().position().x(),
_msg->inertial().pose().position().y(),
_msg->inertial().pose().position().z());
@@ -64,59 +88,76 @@ void COMVisual::Load(ConstLinkPtr &_msg)
_msg->inertial().pose().orientation().y(),
_msg->inertial().pose().orientation().z());
- // Use principal moments of inertia to scale COM visual
- // \todo: rotate COM to match principal axes when product terms are nonzero
- // This can be done with Eigen, or with code from the following paper:
- // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric Matrix
- // http://arxiv.org/abs/1306.6291v3
- double mass = _msg->inertial().mass();
- double Ixx = _msg->inertial().ixx();
- double Iyy = _msg->inertial().iyy();
- double Izz = _msg->inertial().izz();
- math::Vector3 boxScale;
- if (mass < 0 || Ixx < 0 || Iyy < 0 || Izz < 0 ||
- Ixx + Iyy < Izz || Iyy + Izz < Ixx || Izz + Ixx < Iyy)
- {
- // Unrealistic inertia, load with default scale
- gzlog << "The link " << _msg->name() << " has unrealistic inertia, "
- << "unable to visualize box of equivalent inertia." << std::endl;
- this->Load(math::Pose(xyz, q));
- }
- else
- {
- // Compute dimensions of box with uniform density and equivalent inertia.
- boxScale.x = sqrt(6*(Izz + Iyy - Ixx) / mass);
- boxScale.y = sqrt(6*(Izz + Ixx - Iyy) / mass);
- boxScale.z = sqrt(6*(Ixx + Iyy - Izz) / mass);
- this->Load(math::Pose(xyz, q), boxScale);
- }
+ dPtr->inertiaPose = math::Pose(xyz, q);
+
+ dPtr->mass = _msg->inertial().mass();
+ dPtr->linkName = _msg->name();
+
+ this->Load();
}
/////////////////////////////////////////////////
-void COMVisual::Load(const math::Pose &_pose,
- const math::Vector3 &_scale)
+void COMVisual::Load()
{
COMVisualPrivate *dPtr =
reinterpret_cast<COMVisualPrivate *>(this->dataPtr);
- math::Vector3 p1(0, 0, -2*_scale.z);
- math::Vector3 p2(0, 0, 2*_scale.z);
- math::Vector3 p3(0, -2*_scale.y, 0);
- math::Vector3 p4(0, 2*_scale.y, 0);
- math::Vector3 p5(-2*_scale.x, 0, 0);
- math::Vector3 p6(2*_scale.x, 0, 0);
- p1 += _pose.pos;
- p2 += _pose.pos;
- p3 += _pose.pos;
- p4 += _pose.pos;
- p5 += _pose.pos;
- p6 += _pose.pos;
- p1 = _pose.rot.RotateVector(p1);
- p2 = _pose.rot.RotateVector(p2);
- p3 = _pose.rot.RotateVector(p3);
- p4 = _pose.rot.RotateVector(p4);
- p5 = _pose.rot.RotateVector(p5);
- p6 = _pose.rot.RotateVector(p6);
+ if (dPtr->mass < 0)
+ {
+ // Unrealistic mass, load with default mass
+ gzlog << "The link " << dPtr->linkName << " has unrealistic mass, "
+ << "unable to visualize sphere of equivalent mass." << std::endl;
+ dPtr->mass = 1;
+ }
+
+ // Compute radius of sphere with density of lead and equivalent mass.
+ double sphereRadius;
+ double dLead = 11340;
+ sphereRadius = cbrt((0.75 * dPtr->mass) / (M_PI * dLead));
+
+ // Get the link's bounding box
+ VisualPtr vis = this->GetScene()->GetVisual(dPtr->linkName);
+ math::Box box;
+
+ if (vis)
+ box = vis->GetBoundingBox();
+
+ // Mass indicator: equivalent sphere with density of lead
+ this->InsertMesh("unit_sphere");
+
+ Ogre::MovableObject *sphereObj =
+ (Ogre::MovableObject*)(dPtr->scene->GetManager()->createEntity(
+ this->GetName()+"__SPHERE__", "unit_sphere"));
+ sphereObj->setVisibilityFlags(GZ_VISIBILITY_GUI);
+ sphereObj->setCastShadows(false);
+
+ dPtr->sphereNode =
+ dPtr->sceneNode->createChildSceneNode(this->GetName() + "_SPHERE");
+
+ dPtr->sphereNode->attachObject(sphereObj);
+ dPtr->sphereNode->setScale(sphereRadius*2, sphereRadius*2, sphereRadius*2);
+ dPtr->sphereNode->setPosition(dPtr->inertiaPose.pos.x,
+ dPtr->inertiaPose.pos.y, dPtr->inertiaPose.pos.z);
+ dPtr->sphereNode->setOrientation(Ogre::Quaternion(
+ dPtr->inertiaPose.rot.w, dPtr->inertiaPose.rot.x,
+ dPtr->inertiaPose.rot.y, dPtr->inertiaPose.rot.z));
+ dPtr->sphereNode->setInheritScale(false);
+
+ this->SetMaterial("Gazebo/CoM");
+
+ // CoM position indicator
+ math::Vector3 p1(0, 0, box.min.z - dPtr->inertiaPose.pos.z);
+ math::Vector3 p2(0, 0, box.max.z - dPtr->inertiaPose.pos.z);
+ math::Vector3 p3(0, box.min.y - dPtr->inertiaPose.pos.y, 0);
+ math::Vector3 p4(0, box.max.y - dPtr->inertiaPose.pos.y, 0);
+ math::Vector3 p5(box.min.x - dPtr->inertiaPose.pos.x, 0, 0);
+ math::Vector3 p6(box.max.x - dPtr->inertiaPose.pos.x, 0, 0);
+ p1 += dPtr->inertiaPose.pos;
+ p2 += dPtr->inertiaPose.pos;
+ p3 += dPtr->inertiaPose.pos;
+ p4 += dPtr->inertiaPose.pos;
+ p5 += dPtr->inertiaPose.pos;
+ p6 += dPtr->inertiaPose.pos;
dPtr->crossLines = this->CreateDynamicLine(rendering::RENDERING_LINE_LIST);
dPtr->crossLines->setMaterial("Gazebo/Green");
@@ -127,22 +168,14 @@ void COMVisual::Load(const math::Pose &_pose,
dPtr->crossLines->AddPoint(p5);
dPtr->crossLines->AddPoint(p6);
- this->InsertMesh("unit_box");
-
- Ogre::MovableObject *boxObj =
- (Ogre::MovableObject*)(dPtr->scene->GetManager()->createEntity(
- this->GetName()+"__BOX__", "unit_box"));
- boxObj->setVisibilityFlags(GZ_VISIBILITY_GUI);
- ((Ogre::Entity*)boxObj)->setMaterialName("__GAZEBO_TRANS_PURPLE_MATERIAL__");
-
- dPtr->boxNode =
- dPtr->sceneNode->createChildSceneNode(this->GetName() + "_BOX");
+ this->SetVisibilityFlags(GZ_VISIBILITY_GUI);
+}
- dPtr->boxNode->attachObject(boxObj);
- dPtr->boxNode->setScale(_scale.x, _scale.y, _scale.z);
- dPtr->boxNode->setPosition(_pose.pos.x, _pose.pos.y, _pose.pos.z);
- dPtr->boxNode->setOrientation(Ogre::Quaternion(_pose.rot.w, _pose.rot.x,
- _pose.rot.y, _pose.rot.z));
+/////////////////////////////////////////////////
+math::Pose COMVisual::GetInertiaPose() const
+{
+ COMVisualPrivate *dPtr =
+ reinterpret_cast<COMVisualPrivate *>(this->dataPtr);
- this->SetVisibilityFlags(GZ_VISIBILITY_GUI);
+ return dPtr->inertiaPose;
}
diff --git a/gazebo/rendering/COMVisual.hh b/gazebo/rendering/COMVisual.hh
index 50c0d71..8d96b23 100644
--- a/gazebo/rendering/COMVisual.hh
+++ b/gazebo/rendering/COMVisual.hh
@@ -20,11 +20,7 @@
#include <string>
-#include <sdf/sdf.hh>
-
#include "gazebo/math/Pose.hh"
-#include "gazebo/math/Vector3.hh"
-#include "gazebo/msgs/MessageTypes.hh"
#include "gazebo/rendering/Visual.hh"
#include "gazebo/util/system.hh"
@@ -37,7 +33,7 @@ namespace gazebo
/// \class COMVisual COMVisual.hh rendering/rendering.hh
/// \brief Basic Center of Mass visualization
- class GAZEBO_VISIBLE COMVisual : public Visual
+ class GZ_RENDERING_VISIBLE COMVisual : public Visual
{
/// \brief Constructor
/// \param[in] _name Name of the Visual
@@ -56,11 +52,12 @@ namespace gazebo
/// \param[in] _msg Pointer to the message
public: virtual void Load(ConstLinkPtr &_msg);
- /// \brief Load based on a math::Pose
- /// \param[in] _pose Pose of the COM visual
- /// \param[in] _scale Scale factor for the COM visual.
- private: void Load(const math::Pose &_pose,
- const math::Vector3 &_scale = math::Vector3(0.02, 0.02, 0.02));
+ /// \brief Get inertia pose.
+ /// \return Inertia pose in link frame.
+ public: math::Pose GetInertiaPose() const;
+
+ /// \brief Load using previously set member variables.
+ private: void Load();
};
/// \}
}
diff --git a/gazebo/rendering/COMVisualPrivate.hh b/gazebo/rendering/COMVisualPrivate.hh
index c02caa1..2aa1646 100644
--- a/gazebo/rendering/COMVisualPrivate.hh
+++ b/gazebo/rendering/COMVisualPrivate.hh
@@ -36,11 +36,20 @@ namespace gazebo
/// \brief Private data for the COM Visual class
class COMVisualPrivate : public VisualPrivate
{
- /// \brief Lines that make the cross marking the center of mass
+ /// \brief Lines that make the cross marking the center of mass.
public: DynamicLines *crossLines;
- /// \brief Box that make the cross marking the center of mass
- public: Ogre::SceneNode *boxNode;
+ /// \brief Sphere with density of lead and equivalent mass.
+ public: Ogre::SceneNode *sphereNode;
+
+ /// \brief Inertia pose in link frame.
+ public: math::Pose inertiaPose;
+
+ /// \brief Parent link name.
+ public: std::string linkName;
+
+ /// \brief Link mass.
+ public: double mass;
};
}
}
diff --git a/gazebo/rendering/COMVisual_TEST.cc b/gazebo/rendering/COMVisual_TEST.cc
new file mode 100644
index 0000000..b3d18bc
--- /dev/null
+++ b/gazebo/rendering/COMVisual_TEST.cc
@@ -0,0 +1,89 @@
+/*
+ * 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/RenderingIface.hh"
+#include "gazebo/rendering/Scene.hh"
+#include "gazebo/rendering/COMVisual.hh"
+#include "gazebo/test/ServerFixture.hh"
+
+using namespace gazebo;
+class COMVisual_TEST : public ServerFixture
+{
+};
+
+/////////////////////////////////////////////////
+TEST_F(COMVisual_TEST, COMVisualTest)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene("default");
+
+ if (!scene)
+ scene = gazebo::rendering::create_scene("default", false);
+
+ EXPECT_TRUE(scene != NULL);
+
+ // create a default link message
+ gazebo::msgs::LinkPtr linkDefaultMsg;
+ linkDefaultMsg.reset(new gazebo::msgs::Link);
+
+ // create a link visual
+ gazebo::rendering::VisualPtr linkDefaultVis;
+ linkDefaultVis.reset(
+ new gazebo::rendering::Visual("link", scene->GetWorldVisual()));
+
+ // create CoMVisual for the link using msg Load
+ gazebo::rendering::COMVisualPtr comDefaultVis(
+ new gazebo::rendering::COMVisual("_COM_VISUAL_", linkDefaultVis));
+ comDefaultVis->Load(linkDefaultMsg);
+
+ EXPECT_EQ(comDefaultVis->GetInertiaPose().pos, math::Vector3::Zero);
+ EXPECT_EQ(comDefaultVis->GetInertiaPose().rot.GetAsEuler(),
+ math::Vector3::Zero);
+
+ // Create a message and set inertia pose
+ math::Vector3 pos(1, 0, -3);
+ math::Quaternion quat(M_PI/2, 0, -M_PI/5);
+
+ gazebo::msgs::Link linkMsg;
+ linkMsg.set_name("link");
+ msgs::Set(linkMsg.mutable_inertial()->mutable_pose()->mutable_position(),
+ pos.Ign());
+ msgs::Set(linkMsg.mutable_inertial()->mutable_pose()->mutable_orientation(),
+ quat.Ign());
+
+ // create a link visual
+ gazebo::rendering::VisualPtr linkVis;
+ linkVis.reset(
+ new gazebo::rendering::Visual("link", scene->GetWorldVisual()));
+
+ // create CoMVisual for the link using SDF Load
+ gazebo::rendering::COMVisualPtr comVis(
+ new gazebo::rendering::COMVisual("_COM_VISUAL_", linkVis));
+ comVis->Load(msgs::LinkToSDF(linkMsg));
+
+ EXPECT_EQ(comVis->GetInertiaPose().pos, pos);
+ EXPECT_EQ(comVis->GetInertiaPose().rot, quat);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/rendering/Camera.cc b/gazebo/rendering/Camera.cc
index 7bb2483..be9039d 100644
--- a/gazebo/rendering/Camera.cc
+++ b/gazebo/rendering/Camera.cc
@@ -15,11 +15,20 @@
*
*/
-#include <dirent.h>
#include <sstream>
+
#include <boost/filesystem.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
+
// Moved to top to avoid osx compilation errors
#include "gazebo/math/Rand.hh"
@@ -113,22 +122,11 @@ Camera::Camera(const std::string &_name, ScenePtr _scene,
Camera::~Camera()
{
delete [] this->saveFrameBuffer;
+ this->saveFrameBuffer = NULL;
delete [] this->bayerFrameBuffer;
+ this->bayerFrameBuffer = NULL;
- this->sceneNode = NULL;
-
- if (this->renderTexture && this->scene->GetInitialized())
- Ogre::TextureManager::getSingleton().remove(this->renderTexture->getName());
- this->renderTexture = NULL;
- this->renderTarget = NULL;
-
- if (this->camera && this->scene && this->scene->GetManager())
- {
- this->scene->GetManager()->destroyCamera(this->scopedUniqueName);
- this->camera = NULL;
- }
-
- this->connections.clear();
+ this->Fini();
this->sdf->Reset();
this->sdf.reset();
@@ -219,17 +217,29 @@ void Camera::Init()
void Camera::Fini()
{
this->initialized = false;
- this->connections.clear();
this->dataPtr->node.reset();
- RTShaderSystem::DetachViewport(this->viewport, this->scene);
+ if (this->viewport && this->scene)
+ RTShaderSystem::DetachViewport(this->viewport, this->scene);
- if (this->renderTarget && this->scene->GetInitialized())
+ if (this->renderTarget)
this->renderTarget->removeAllViewports();
+ this->renderTarget = NULL;
+ if (this->renderTexture)
+ Ogre::TextureManager::getSingleton().remove(this->renderTexture->getName());
+ this->renderTexture = NULL;
+
+ if (this->camera)
+ {
+ this->scene->GetManager()->destroyCamera(this->scopedUniqueName);
+ this->camera = NULL;
+ }
+
+ this->sceneNode = NULL;
this->viewport = NULL;
- this->renderTarget = NULL;
+ this->scene.reset();
this->connections.clear();
}
@@ -480,7 +490,10 @@ void Camera::PostRender()
{
this->ReadPixelBuffer();
- this->lastRenderWallTime = common::Time::GetWallTime();
+ // Only record last render time if data was actually generated
+ // (If a frame was rendered).
+ if (this->newData)
+ this->lastRenderWallTime = common::Time::GetWallTime();
if (this->newData && (this->captureData || this->captureDataOnce))
{
@@ -647,6 +660,7 @@ void Camera::SetClipDist(float _near, float _far)
void Camera::SetHFOV(math::Angle _angle)
{
this->sdf->GetElement("horizontal_fov")->Set(_angle.Radian());
+ this->UpdateFOV();
}
//////////////////////////////////////////////////
@@ -1051,7 +1065,8 @@ bool Camera::SaveFrame(const unsigned char *_image,
Ogre::Codec::CodecDataPtr codecDataPtr(imgData);
// OGRE 1.9 renames codeToFile to encodeToFile
- #if (OGRE_VERSION < ((1 << 16) | (9 << 8) | 0))
+ // Looks like 1.9RC, which we're using on Windows, doesn't have this change.
+ #if (OGRE_VERSION < ((1 << 16) | (9 << 8) | 0)) || defined(_WIN32)
pCodec->codeToFile(stream, filename, codecDataPtr);
#else
pCodec->encodeToFile(stream, filename, codecDataPtr);
@@ -1260,6 +1275,9 @@ void Camera::CreateCamera()
this->camera = this->scene->GetManager()->createCamera(
this->scopedUniqueName);
+ if (this->sdf->HasElement("projection_type"))
+ this->SetProjectionType(this->sdf->Get<std::string>("projection_type"));
+
this->camera->setFixedYawAxis(false);
this->camera->yaw(Ogre::Degree(-90.0));
this->camera->roll(Ogre::Degree(-90.0));
@@ -1299,6 +1317,9 @@ void Camera::SetRenderTarget(Ogre::RenderTarget *_target)
this->viewport->setShadowsEnabled(true);
this->viewport->setOverlaysEnabled(false);
+ if (this->camera->getProjectionType() == Ogre::PT_ORTHOGRAPHIC)
+ this->scene->SetShadowsEnabled(false);
+
RTShaderSystem::AttachViewport(this->viewport, this->GetScene());
this->viewport->setBackgroundColour(
@@ -1311,6 +1332,7 @@ void Camera::SetRenderTarget(Ogre::RenderTarget *_target)
double hfov = this->GetHFOV().Radian();
double vfov = 2.0 * atan(tan(hfov / 2.0) / ratio);
+
this->camera->setAspectRatio(ratio);
this->camera->setFOVy(Ogre::Radian(vfov));
@@ -1709,3 +1731,79 @@ DistortionPtr Camera::GetDistortion() const
{
return this->dataPtr->distortion;
}
+
+//////////////////////////////////////////////////
+void Camera::UpdateFOV()
+{
+ if (this->viewport)
+ {
+ this->viewport->setDimensions(0, 0, 1, 1);
+ double ratio = static_cast<double>(this->viewport->getActualWidth()) /
+ static_cast<double>(this->viewport->getActualHeight());
+
+ double hfov = this->sdf->Get<double>("horizontal_fov");
+ double vfov = 2.0 * atan(tan(hfov / 2.0) / ratio);
+
+ this->camera->setAspectRatio(ratio);
+ this->camera->setFOVy(Ogre::Radian(vfov));
+
+ delete [] this->saveFrameBuffer;
+ this->saveFrameBuffer = NULL;
+ }
+}
+
+//////////////////////////////////////////////////
+float Camera::GetAvgFPS() const
+{
+ if (this->renderTarget)
+ return this->renderTarget->getAverageFPS();
+ else
+ return 0.0f;
+}
+
+//////////////////////////////////////////////////
+unsigned int Camera::GetTriangleCount() const
+{
+ return this->renderTarget->getTriangleCount();
+}
+
+//////////////////////////////////////////////////
+bool Camera::SetProjectionType(const std::string &_type)
+{
+ bool result = true;
+
+ if (_type == "orthographic")
+ {
+ // Shadows do not work properly with orthographic projection
+ this->scene->SetShadowsEnabled(false);
+ this->camera->setProjectionType(Ogre::PT_ORTHOGRAPHIC);
+ }
+ else if (_type == "perspective")
+ {
+ this->camera->setProjectionType(Ogre::PT_PERSPECTIVE);
+ this->camera->setCustomProjectionMatrix(false);
+ this->scene->SetShadowsEnabled(true);
+ }
+ else
+ {
+ gzerr << "Invalid projection type[" << _type << "]. "
+ << "Valid values are 'perspective' and 'orthographic'.\n";
+ result = false;
+ }
+
+ return result;
+}
+
+//////////////////////////////////////////////////
+std::string Camera::GetProjectionType() const
+{
+ if (this->camera->getProjectionType() == Ogre::PT_ORTHOGRAPHIC)
+ {
+ return "orthographic";
+ }
+ // There are only two types of projection in OGRE.
+ else
+ {
+ return "perspective";
+ }
+}
diff --git a/gazebo/rendering/Camera.hh b/gazebo/rendering/Camera.hh
index 560ad6e..4edf00f 100644
--- a/gazebo/rendering/Camera.hh
+++ b/gazebo/rendering/Camera.hh
@@ -75,7 +75,8 @@ namespace gazebo
/// \brief Basic camera sensor
///
/// This is the base class for all cameras.
- class GAZEBO_VISIBLE Camera : public boost::enable_shared_from_this<Camera>
+ class GZ_RENDERING_VISIBLE Camera :
+ public boost::enable_shared_from_this<Camera>
{
/// \brief Constructor
/// \param[in] _namePrefix Unique prefix name for the camera.
@@ -212,7 +213,7 @@ namespace gazebo
/// \brief Set the clip distances
/// \param[in] _near Near clip distance in meters
/// \param[in] _far Far clip distance in meters
- public: void SetClipDist(float _near, float _far);
+ public: virtual void SetClipDist(float _near, float _far);
/// \brief Set the camera FOV (horizontal)
/// \param[in] _radians Horizontal field of view
@@ -334,11 +335,11 @@ namespace gazebo
/// \brief Get the average FPS
/// \return The average frames per second
- public: virtual float GetAvgFPS() const {return 0;}
+ public: virtual float GetAvgFPS() const;
/// \brief Get the triangle count
/// \return The current triangle count
- public: virtual unsigned int GetTriangleCount() const {return 0;}
+ public: virtual unsigned int GetTriangleCount() const;
/// \brief Set the aspect ratio
/// \param[in] _ratio The aspect ratio (width / height) in pixels
@@ -529,6 +530,18 @@ namespace gazebo
/// \return Distortion model.
public: DistortionPtr GetDistortion() const;
+ /// \brief Set the type of projection used by the camera.
+ /// \param[in] _type The type of projection: "perspective" or
+ /// "orthographic".
+ /// \return True if successful.
+ /// \sa GetProjectionType()
+ public: virtual bool SetProjectionType(const std::string &_type);
+
+ /// \brief Return the projection type as a string.
+ /// \return "perspective" or "orthographic"
+ /// \sa SetProjectionType(const std::string &_type)
+ public: std::string GetProjectionType() const;
+
/// \brief Implementation of the render call
protected: virtual void RenderImpl();
@@ -592,6 +605,9 @@ namespace gazebo
/// completed.
protected: virtual void AnimationComplete();
+ /// \brief Update the camera's field of view.
+ protected: virtual void UpdateFOV();
+
/// \brief if user requests bayer image, post process rgb from ogre
/// to generate bayer formats
/// \param[in] _dst Destination buffer for the image data
diff --git a/gazebo/rendering/CameraPrivate.hh b/gazebo/rendering/CameraPrivate.hh
index 9532c00..6411189 100644
--- a/gazebo/rendering/CameraPrivate.hh
+++ b/gazebo/rendering/CameraPrivate.hh
@@ -34,7 +34,7 @@ namespace gazebo
namespace rendering
{
/// \brief Private data for the Camera class
- class GAZEBO_VISIBLE CameraPrivate
+ class GZ_RENDERING_VISIBLE CameraPrivate
{
/// \brief Visual that the camera is tracking.
public: VisualPtr trackedVisual;
diff --git a/gazebo/rendering/CameraVisual.cc b/gazebo/rendering/CameraVisual.cc
index 11cf7cf..1878f06 100644
--- a/gazebo/rendering/CameraVisual.cc
+++ b/gazebo/rendering/CameraVisual.cc
@@ -14,9 +14,12 @@
* limitations under the License.
*
*/
-/* Desc: Camera 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 "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/DynamicLines.hh"
@@ -32,18 +35,15 @@ using namespace rendering;
CameraVisual::CameraVisual(const std::string &_name, VisualPtr _vis)
: Visual(*new CameraVisualPrivate, _name, _vis)
{
+ CameraVisualPrivate *dPtr =
+ reinterpret_cast<CameraVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_SENSOR;
}
/////////////////////////////////////////////////
CameraVisual::~CameraVisual()
{
- CameraVisualPrivate *dPtr =
- reinterpret_cast<CameraVisualPrivate *>(this->dataPtr);
-
- if (dPtr->scene)
- dPtr->scene->RemoveCamera(dPtr->camera->GetName());
-
- dPtr->camera.reset();
+ this->Fini();
}
/////////////////////////////////////////////////
@@ -52,7 +52,7 @@ void CameraVisual::Load(const msgs::CameraSensor &_msg)
CameraVisualPrivate *dPtr =
reinterpret_cast<CameraVisualPrivate *>(this->dataPtr);
- math::Vector2d imageSize = msgs::Convert(_msg.image_size());
+ math::Vector2d imageSize = msgs::ConvertIgn(_msg.image_size());
double dist = 2.0;
double width = 1.0;
@@ -81,12 +81,16 @@ void CameraVisual::Load(const msgs::CameraSensor &_msg)
plane.normal = Ogre::Vector3::NEGATIVE_UNIT_X;
plane.d = dist;
- Ogre::MeshManager::getSingleton().createPlane(this->GetName() + "__floor",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- plane, width, height, 1, 1, true, 1, 1.0f, 1.0f,
- Ogre::Vector3::UNIT_Z);
+ if (!Ogre::MeshManager::getSingleton().resourceExists(
+ this->GetName() + "__floor"))
+ {
+ Ogre::MeshManager::getSingleton().createPlane(this->GetName() + "__floor",
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ plane, width, height, 1, 1, true, 1, 1.0f, 1.0f,
+ Ogre::Vector3::UNIT_Z);
+ }
- Ogre::Entity* planeEnt =
+ Ogre::Entity *planeEnt =
dPtr->scene->GetManager()->createEntity(this->GetName() + "__plane",
this->GetName() + "__floor");
planeEnt->setMaterialName(this->GetName()+"_RTT_material");
@@ -134,3 +138,24 @@ void CameraVisual::Update()
dPtr->camera->Render();
}
+
+/////////////////////////////////////////////////
+void CameraVisual::Fini()
+{
+ /*CameraVisualPrivate *dPtr =
+ reinterpret_cast<CameraVisualPrivate *>(this->dataPtr);
+ dPtr->connections.clear();
+
+ if (dPtr->scene && dPtr->camera)
+ dPtr->scene->RemoveCamera(dPtr->camera->GetName());
+
+ dPtr->camera.reset();
+
+ */
+ this->DetachObjects();
+ if (this->dataPtr->scene)
+ {
+ this->dataPtr->scene->GetManager()->destroyEntity(
+ this->GetName() + "__plane");
+ }
+}
diff --git a/gazebo/rendering/CameraVisual.hh b/gazebo/rendering/CameraVisual.hh
index 4d9f4fd..1c59e5c 100644
--- a/gazebo/rendering/CameraVisual.hh
+++ b/gazebo/rendering/CameraVisual.hh
@@ -39,7 +39,7 @@ namespace gazebo
/// This class is used to visualize a camera image generated from
/// a CameraSensor. The sensor's image is drawn on a billboard in the 3D
/// environment.
- class GAZEBO_VISIBLE CameraVisual : public Visual
+ class GZ_RENDERING_VISIBLE CameraVisual : public Visual
{
/// \brief Constructor
/// \param[in] _name Name of the Visual
@@ -54,6 +54,9 @@ namespace gazebo
public: void Load(const msgs::CameraSensor &_msg);
using Visual::Load;
+ // Documentation inherited
+ protected: virtual void Fini();
+
/// \brief Update the visual
private: void Update();
};
diff --git a/gazebo/rendering/ContactVisual.cc b/gazebo/rendering/ContactVisual.cc
index 5a837c4..4ab4b10 100644
--- a/gazebo/rendering/ContactVisual.cc
+++ b/gazebo/rendering/ContactVisual.cc
@@ -18,6 +18,12 @@
* 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 "gazebo/common/MeshManager.hh"
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Subscriber.hh"
@@ -41,6 +47,8 @@ ContactVisual::ContactVisual(const std::string &_name, VisualPtr _vis,
ContactVisualPrivate *dPtr =
reinterpret_cast<ContactVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_PHYSICS;
+
dPtr->receivedMsg = false;
dPtr->node = transport::NodePtr(new transport::Node());
@@ -95,13 +103,13 @@ void ContactVisual::Update()
{
for (int j = 0; j < dPtr->contactsMsg->contact(i).position_size(); j++)
{
- math::Vector3 pos = msgs::Convert(
+ math::Vector3 pos = msgs::ConvertIgn(
dPtr->contactsMsg->contact(i).position(j));
- math::Vector3 normal = msgs::Convert(
+ math::Vector3 normal = msgs::ConvertIgn(
dPtr->contactsMsg->contact(i).normal(j));
double depth = dPtr->contactsMsg->contact(i).depth(j);
- math::Vector3 force = msgs::Convert(
+ math::Vector3 force = msgs::ConvertIgn(
dPtr->contactsMsg->contact(i).wrench(j).body_1_wrench().force());
// Scaling factor for the normal line.
diff --git a/gazebo/rendering/ContactVisual.hh b/gazebo/rendering/ContactVisual.hh
index b45d1b5..5566937 100644
--- a/gazebo/rendering/ContactVisual.hh
+++ b/gazebo/rendering/ContactVisual.hh
@@ -36,7 +36,7 @@ namespace gazebo
///
/// This class visualizes contact points by drawing arrows in the 3D
/// environment.
- class GAZEBO_VISIBLE ContactVisual : public Visual
+ class GZ_RENDERING_VISIBLE ContactVisual : public Visual
{
/// \brief Constructor
/// \param[in] _name Name of the ContactVisual
diff --git a/gazebo/rendering/Conversions.hh b/gazebo/rendering/Conversions.hh
index 5f5c761..1ab2227 100644
--- a/gazebo/rendering/Conversions.hh
+++ b/gazebo/rendering/Conversions.hh
@@ -34,7 +34,7 @@ namespace gazebo
/// \brief Conversions Conversions.hh rendering/Conversions.hh
/// \brief A set of utility function to convert between Gazebo and Ogre
/// data types
- class GAZEBO_VISIBLE Conversions
+ class GZ_RENDERING_VISIBLE Conversions
{
/// \brief Return the equivalent ogre color
/// \param[in] _clr Gazebo color to convert
diff --git a/gazebo/rendering/DepthCamera.cc b/gazebo/rendering/DepthCamera.cc
index 100689f..306693e 100644
--- a/gazebo/rendering/DepthCamera.cc
+++ b/gazebo/rendering/DepthCamera.cc
@@ -14,14 +14,15 @@
* limitations under the License.
*
*/
-
-/* Desc: A camera sensor using OpenGL
- * Author: Nate Koenig
- * Date: 15 July 2003
- */
-
-#include <dirent.h>
#include <sstream>
+#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
#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/RTShaderSystem.hh"
diff --git a/gazebo/rendering/DepthCamera.hh b/gazebo/rendering/DepthCamera.hh
index b79a7cb..d8844b3 100644
--- a/gazebo/rendering/DepthCamera.hh
+++ b/gazebo/rendering/DepthCamera.hh
@@ -54,7 +54,7 @@ namespace gazebo
/// \class DepthCamera DepthCamera.hh rendering/rendering.hh
/// \brief Depth camera used to render depth data into an image buffer
- class GAZEBO_VISIBLE DepthCamera : public Camera
+ class GZ_RENDERING_VISIBLE DepthCamera : public Camera
{
/// \brief Constructor
/// \param[in] _namePrefix Unique prefix name for the camera.
diff --git a/gazebo/rendering/Distortion.hh b/gazebo/rendering/Distortion.hh
index 5ef70b9..31170c5 100644
--- a/gazebo/rendering/Distortion.hh
+++ b/gazebo/rendering/Distortion.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \class Distortion Distortion.hh rendering/rendering.hh
/// \brief Camera distortion based on Brown's model. Note that the current
/// implementation only supports barrel distortion.
- class GAZEBO_VISIBLE Distortion
+ class GZ_RENDERING_VISIBLE Distortion
{
/// \brief Constructor
public: Distortion();
diff --git a/gazebo/rendering/DynamicLines.hh b/gazebo/rendering/DynamicLines.hh
index ab2ca5a..2f30d7f 100644
--- a/gazebo/rendering/DynamicLines.hh
+++ b/gazebo/rendering/DynamicLines.hh
@@ -40,7 +40,7 @@ namespace gazebo
/// \class DynamicLines DynamicLines.hh rendering/rendering.hh
/// \brief Class for drawing lines that can change
- class GAZEBO_VISIBLE DynamicLines : public DynamicRenderable
+ class GZ_RENDERING_VISIBLE DynamicLines : public DynamicRenderable
{
/// \brief Constructor
/// \param[in] _opType The type of Line
diff --git a/gazebo/rendering/DynamicRenderable.hh b/gazebo/rendering/DynamicRenderable.hh
index 433f9b5..b709466 100644
--- a/gazebo/rendering/DynamicRenderable.hh
+++ b/gazebo/rendering/DynamicRenderable.hh
@@ -32,7 +32,7 @@ namespace gazebo
/// \class DynamicRenderable DynamicRenderable.hh rendering/rendering.hh
/// \brief Abstract base class providing mechanisms for dynamically
/// growing hardware buffers.
- class GAZEBO_VISIBLE DynamicRenderable : public Ogre::SimpleRenderable
+ class GZ_RENDERING_VISIBLE DynamicRenderable : public Ogre::SimpleRenderable
{
/// \brief Constructor
public: DynamicRenderable();
diff --git a/gazebo/rendering/FPSViewController.cc b/gazebo/rendering/FPSViewController.cc
index e101446..f3f4cd3 100644
--- a/gazebo/rendering/FPSViewController.cc
+++ b/gazebo/rendering/FPSViewController.cc
@@ -14,6 +14,12 @@
* 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 "gazebo/math/Vector2i.hh"
#include "gazebo/common/MouseEvent.hh"
@@ -73,16 +79,16 @@ void FPSViewController::HandleMouseEvent(const common::MouseEvent &_event)
if (!this->enabled)
return;
- math::Vector2i drag = _event.pos - _event.prevPos;
+ ignition::math::Vector2i drag = _event.Pos() - _event.PrevPos();
- if (_event.buttons & common::MouseEvent::LEFT)
+ if (_event.Buttons() & common::MouseEvent::LEFT)
{
- this->camera->Yaw(GZ_DTOR(-drag.x*0.1));
- this->camera->Pitch(GZ_DTOR(drag.y*0.1));
+ this->camera->Yaw(GZ_DTOR(-drag.X()*0.1));
+ this->camera->Pitch(GZ_DTOR(drag.Y()*0.1));
}
- else if (_event.type == common::MouseEvent::SCROLL)
+ else if (_event.Type() == common::MouseEvent::SCROLL)
{
- if (_event.scroll.y < 0)
+ if (_event.Scroll().Y() < 0)
{
this->xVelocityFactor *= 1.05;
this->yVelocityFactor *= 1.05;
diff --git a/gazebo/rendering/FPSViewController.hh b/gazebo/rendering/FPSViewController.hh
index ed8a0c9..fca68f2 100644
--- a/gazebo/rendering/FPSViewController.hh
+++ b/gazebo/rendering/FPSViewController.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class FPSViewController FPSViewController.hh rendering/rendering.hh
/// \brief First Person Shooter style view controller
- class GAZEBO_VISIBLE FPSViewController : public ViewController
+ class GZ_RENDERING_VISIBLE FPSViewController : public ViewController
{
/// \brief Constructor
/// \param[in] Camera to controll
diff --git a/gazebo/rendering/GpuLaser.cc b/gazebo/rendering/GpuLaser.cc
index e14f413..d14d807 100644
--- a/gazebo/rendering/GpuLaser.cc
+++ b/gazebo/rendering/GpuLaser.cc
@@ -15,14 +15,17 @@
*
*/
-/* Desc: A laser sensor using OpenGL
- * Author: Mihai Emanuel Dolha
- * Date: 29 March 2012
- */
-
-#include <dirent.h>
#include <sstream>
+#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
+
#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/common/Assert.hh"
@@ -526,7 +529,7 @@ void GpuLaser::Set1stPassTarget(Ogre::RenderTarget *_target,
this->firstPassViewports[_index]->setShadowsEnabled(false);
this->firstPassViewports[_index]->setSkiesEnabled(false);
this->firstPassViewports[_index]->setBackgroundColour(
- Ogre::ColourValue(this->far, 0.0, 1.0));
+ Ogre::ColourValue(this->farClip, 0.0, 1.0));
this->firstPassViewports[_index]->setVisibilityMask(
GZ_VISIBILITY_ALL & ~(GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE));
}
@@ -786,25 +789,25 @@ void GpuLaser::SetCosVertFOV(double _cvfov)
//////////////////////////////////////////////////
double GpuLaser::GetNearClip() const
{
- return this->near;
+ return this->nearClip;
}
//////////////////////////////////////////////////
double GpuLaser::GetFarClip() const
{
- return this->far;
+ return this->farClip;
}
//////////////////////////////////////////////////
void GpuLaser::SetNearClip(double _near)
{
- this->near = _near;
+ this->nearClip = _near;
}
//////////////////////////////////////////////////
void GpuLaser::SetFarClip(double _far)
{
- this->far = _far;
+ this->farClip = _far;
}
//////////////////////////////////////////////////
diff --git a/gazebo/rendering/GpuLaser.hh b/gazebo/rendering/GpuLaser.hh
index 0ac2498..92ad482 100644
--- a/gazebo/rendering/GpuLaser.hh
+++ b/gazebo/rendering/GpuLaser.hh
@@ -14,13 +14,8 @@
* limitations under the License.
*
*/
-/* Desc: A laser sensor using OpenGL
- * Author: Mihai Emanuel Dolha
- * Date: 29 March 2012
- */
-
-#ifndef _GPULASER_HH_
-#define _GPULASER_HH_
+#ifndef _GAZEBO_RENDERING_GPULASER_HH_
+#define _GAZEBO_RENDERING_GPULASER_HH_
#include <string>
#include <vector>
@@ -65,7 +60,7 @@ namespace gazebo
/// \class GpuLaser GpuLaser.hh rendering/rendering.hh
/// \brief GPU based laser distance sensor
- class GAZEBO_VISIBLE GpuLaser
+ class GZ_RENDERING_VISIBLE GpuLaser
: public Camera, public Ogre::RenderObjectListener
{
/// \brief Constructor
@@ -279,10 +274,10 @@ namespace gazebo
protected: double cvfov;
/// \brief Near clip plane.
- protected: double near;
+ protected: double nearClip;
/// \brief Far clip plane.
- protected: double far;
+ protected: double farClip;
/// \brief True if the sensor is horizontal only.
protected: bool isHorizontal;
diff --git a/gazebo/rendering/GpuLaser_TEST.cc b/gazebo/rendering/GpuLaser_TEST.cc
index fdb8481..b14f72f 100644
--- a/gazebo/rendering/GpuLaser_TEST.cc
+++ b/gazebo/rendering/GpuLaser_TEST.cc
@@ -20,8 +20,9 @@
#include "gazebo/rendering/RenderingIface.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/GpuLaser.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
+using namespace gazebo;
class GpuLaser_TEST : public ServerFixture
{
};
diff --git a/gazebo/rendering/Grid.cc b/gazebo/rendering/Grid.cc
index 95aa23d..d6a5b85 100644
--- a/gazebo/rendering/Grid.cc
+++ b/gazebo/rendering/Grid.cc
@@ -52,7 +52,6 @@ Grid::~Grid()
{
this->scene->GetManager()->destroySceneNode(this->sceneNode->getName());
this->scene->GetManager()->destroyManualObject(this->manualObject);
-
this->material->unload();
}
diff --git a/gazebo/rendering/Grid.hh b/gazebo/rendering/Grid.hh
index 72bc631..477039b 100644
--- a/gazebo/rendering/Grid.hh
+++ b/gazebo/rendering/Grid.hh
@@ -50,7 +50,7 @@ namespace gazebo
///
/// Displays a grid of cells, drawn with lines. A grid with an
/// identity orientation is drawn along the XY plane.
- class GAZEBO_VISIBLE Grid
+ class GZ_RENDERING_VISIBLE Grid
{
/// \brief Constructor
///
diff --git a/gazebo/rendering/Heightmap.cc b/gazebo/rendering/Heightmap.cc
index 8546d00..c11d130 100644
--- a/gazebo/rendering/Heightmap.cc
+++ b/gazebo/rendering/Heightmap.cc
@@ -14,10 +14,12 @@
* limitations under the License.
*
*/
-/* Desc: Heightmap geometry
- * Author: Nate Koenig
- * Date: 12 May 2009
- */
+
+#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.h>
#include <math.h>
@@ -34,6 +36,7 @@
#include "gazebo/rendering/Conversions.hh"
#include "gazebo/rendering/Heightmap.hh"
#include "gazebo/rendering/UserCamera.hh"
+#include "gazebo/rendering/RenderEngine.hh"
using namespace gazebo;
using namespace rendering;
@@ -44,6 +47,13 @@ const double Heightmap::holdRadiusFactor = 1.15;
const boost::filesystem::path Heightmap::pagingDirname = "paging";
const boost::filesystem::path Heightmap::hashFilename = "gzterrain.SHA1";
+static std::string glslVersion = "130";
+static std::string vpInStr = "in";
+static std::string vpOutStr = "out";
+static std::string fpInStr = "in";
+static std::string fpOutStr = "out";
+static std::string textureStr = "texture";
+
//////////////////////////////////////////////////
Heightmap::Heightmap(ScenePtr _scene)
{
@@ -90,17 +100,22 @@ Heightmap::~Heightmap()
//////////////////////////////////////////////////
void Heightmap::LoadFromMsg(ConstVisualPtr &_msg)
{
- this->terrainSize = msgs::Convert(_msg->geometry().heightmap().size());
- this->terrainOrigin = msgs::Convert(_msg->geometry().heightmap().origin());
+ this->terrainSize = msgs::ConvertIgn(_msg->geometry().heightmap().size());
+ this->terrainOrigin = msgs::ConvertIgn(_msg->geometry().heightmap().origin());
for (int i = 0; i < _msg->geometry().heightmap().texture_size(); ++i)
{
- this->diffuseTextures.push_back(common::find_file(
- _msg->geometry().heightmap().texture(i).diffuse()));
- this->normalTextures.push_back(common::find_file(
- _msg->geometry().heightmap().texture(i).normal()));
- this->worldSizes.push_back(
- _msg->geometry().heightmap().texture(i).size());
+ std::string diffusePath = common::find_file(
+ _msg->geometry().heightmap().texture(i).diffuse());
+ std::string normalPath = common::find_file(
+ _msg->geometry().heightmap().texture(i).normal());
+
+ RenderEngine::Instance()->AddResourcePath(diffusePath);
+ RenderEngine::Instance()->AddResourcePath(normalPath);
+
+ this->diffuseTextures.push_back(diffusePath);
+ this->normalTextures.push_back(normalPath);
+ this->worldSizes.push_back(_msg->geometry().heightmap().texture(i).size());
}
for (int i = 0; i < _msg->geometry().heightmap().blend_size(); ++i)
@@ -290,6 +305,26 @@ void Heightmap::Load()
if (this->terrainGlobals != NULL)
return;
+ const Ogre::RenderSystemCapabilities *capabilities;
+ Ogre::RenderSystemCapabilities::ShaderProfiles profiles;
+ Ogre::RenderSystemCapabilities::ShaderProfiles::const_iterator iter;
+
+ capabilities =
+ Ogre::Root::getSingleton().getRenderSystem()->getCapabilities();
+ Ogre::DriverVersion glVersion;
+ glVersion.build = 0;
+ glVersion.major = 3;
+ glVersion.minor = 0;
+ glVersion.release = 0;
+ if (capabilities->isDriverOlderThanVersion(glVersion))
+ {
+ glslVersion = "120";
+ vpInStr = "attribute";
+ vpOutStr = "varying";
+ fpInStr = "varying";
+ textureStr = "texture2D";
+ }
+
// The terraingGroup is composed by a number of terrains (1 by default)
int nTerrains = 1;
@@ -316,7 +351,7 @@ void Heightmap::Load()
geomMsg.ParseFromString(response->serialized_data());
// Copy the height data.
- this->terrainSize = msgs::Convert(geomMsg.heightmap().size());
+ this->terrainSize = msgs::ConvertIgn(geomMsg.heightmap().size());
this->heights.resize(geomMsg.heightmap().heights().size());
memcpy(&this->heights[0], geomMsg.heightmap().heights().data(),
sizeof(this->heights[0])*geomMsg.heightmap().heights().size());
@@ -607,7 +642,7 @@ bool Heightmap::InitBlendMaps(Ogre::Terrain *_terrain)
{
if (!_terrain)
{
- std::cerr << "Invalid terrain\n";
+ gzerr << "Invalid terrain\n";
return false;
}
@@ -944,7 +979,7 @@ GzTerrainMatGen::~GzTerrainMatGen()
GzTerrainMatGen::SM2Profile::SM2Profile(
Ogre::TerrainMaterialGenerator *_parent, const Ogre::String &_name,
const Ogre::String &_desc)
-: TerrainMaterialGeneratorA::SM2Profile::SM2Profile(_parent, _name, _desc)
+: TerrainMaterialGeneratorA::SM2Profile(_parent, _name, _desc)
{
this->mShaderGen = NULL;
}
@@ -1246,7 +1281,7 @@ GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVertexProgram(
Ogre::HighLevelGpuProgramPtr ret =
this->createVertexProgram(_prof, _terrain, _tt);
- Ogre::StringUtil::StrStreamType sourceStr;
+ Ogre::StringStream sourceStr;
this->generateVertexProgramSource(_prof, _terrain, _tt, sourceStr);
ret->setSource(sourceStr.str());
@@ -1264,7 +1299,7 @@ GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFragmentProgram(
Ogre::HighLevelGpuProgramPtr ret = this->createFragmentProgram(_prof,
_terrain, _tt);
- Ogre::StringUtil::StrStreamType sourceStr;
+ Ogre::StringStream sourceStr;
this->generateFragmentProgramSource(_prof, _terrain, _tt, sourceStr);
@@ -1373,7 +1408,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::updateParams(
void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::
generateVertexProgramSource(const SM2Profile *_prof,
const Ogre::Terrain* _terrain, TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream)
+ Ogre::StringStream &_outStream)
{
this->generateVpHeader(_prof, _terrain, _tt, _outStream);
@@ -1397,11 +1432,11 @@ generateVertexProgramSource(const SM2Profile *_prof,
// virtual.
void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpHeader(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
- TechniqueType _tt, Ogre::StringUtil::StrStreamType &_outStream)
+ TechniqueType _tt, Ogre::StringStream &_outStream)
{
bool compression = false;
- _outStream << "#version 130\n\n";
+ _outStream << "#version " << glslVersion << "\n\n";
#if OGRE_VERSION_MAJOR >= 1 && OGRE_VERSION_MINOR >= 8
compression = _terrain->_getUseVertexCompression() &&
@@ -1411,21 +1446,21 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpHeader(
{
// The parameter "in vec4 vertex;" is automatically bound by OGRE.
// The parameter "in vec4 uv0'" is automatically bound by OGRE.
- _outStream << "in vec4 vertex;\n"
- << "in vec4 uv0;\n";
+ _outStream << vpInStr << " vec4 vertex;\n"
+ << vpInStr << " vec4 uv0;\n";
}
else
#endif
{
// The parameter "in vec4 vertex;" is automatically bound by OGRE.
// The parameter "in vec4 uv0'" is automatically bound by OGRE.
- _outStream << "in vec4 vertex;\n"
- << "in vec4 uv0;\n";
+ _outStream << vpInStr << " vec4 vertex;\n"
+ << vpInStr << " vec4 uv0;\n";
}
if (_tt != RENDER_COMPOSITE_MAP)
// The parameter "in vec4 uv1'" is automatically bound by OGRE.
- _outStream << "in vec4 uv1;\n";
+ _outStream << vpInStr << " vec4 uv1;\n";
_outStream <<
"uniform mat4 worldMatrix;\n"
@@ -1454,10 +1489,10 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpHeader(
_outStream << "uniform vec4 uvMul" << i << ";\n";
_outStream <<
- "out vec4 position;\n";
+ vpOutStr << " vec4 position;\n";
unsigned int texCoordSet = 1;
- _outStream << "out vec4 uvMisc;\n";
+ _outStream << vpOutStr << " vec4 uvMisc;\n";
// layer UV's premultiplied, packed as xy/zw
unsigned int numUVSets = numLayers / 2;
@@ -1469,13 +1504,13 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpHeader(
{
for (unsigned int i = 0; i < numUVSets; ++i)
{
- _outStream << "out vec4 layerUV" << i << ";\n";
+ _outStream << vpOutStr << " vec4 layerUV" << i << ";\n";
}
}
if (_prof->getParent()->getDebugLevel() && _tt != RENDER_COMPOSITE_MAP)
{
- _outStream << "out vec2 lodInfo;\n";
+ _outStream << vpOutStr << " vec2 lodInfo;\n";
}
bool fog = _terrain->getSceneManager()->getFogMode() != Ogre::FOG_NONE &&
@@ -1485,7 +1520,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpHeader(
{
_outStream <<
"uniform vec4 fogParams;\n"
- "out float fogVal;\n";
+ << vpOutStr << " float fogVal;\n";
}
if (_prof->isShadowingEnabled(_tt, _terrain))
@@ -1590,7 +1625,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpHeader(
// but is needed because generateVpDynamicShadows() is not declared virtual.
void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpFooter(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
- TechniqueType _tt, Ogre::StringUtil::StrStreamType &_outStream)
+ TechniqueType _tt, Ogre::StringStream &_outStream)
{
_outStream << " gl_Position = viewProjMatrix * worldPos;\n"
<< " uvMisc.xy = uv.xy;\n";
@@ -1621,7 +1656,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpFooter(
void
GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateVpDynamicShadows(
const SM2Profile *_prof, const Ogre::Terrain * /*_terrain*/,
- TechniqueType /*_tt*/, Ogre::StringUtil::StrStreamType &_outStream)
+ TechniqueType /*_tt*/, Ogre::StringStream &_outStream)
{
unsigned int numTextures = 1;
@@ -1711,7 +1746,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::defaultVpParams(
unsigned int GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::
generateVpDynamicShadowsParams(unsigned int _texCoord, const SM2Profile *_prof,
const Ogre::Terrain * /*_terrain*/, TechniqueType /*_tt*/,
- Ogre::StringUtil::StrStreamType &_outStream)
+ Ogre::StringStream &_outStream)
{
// out semantics & params
unsigned int numTextures = 1;
@@ -1723,7 +1758,7 @@ generateVpDynamicShadowsParams(unsigned int _texCoord, const SM2Profile *_prof,
for (unsigned int i = 0; i < numTextures; ++i)
{
- _outStream << "out vec4 lightSpacePos" << i << ";\n"
+ _outStream << vpOutStr << " vec4 lightSpacePos" << i << ";\n"
<< "uniform mat4 texViewProjMatrix" << i << ";\n";
// Don't add depth range params
@@ -1740,9 +1775,9 @@ generateVpDynamicShadowsParams(unsigned int _texCoord, const SM2Profile *_prof,
/////////////////////////////////////////////////
void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
- TechniqueType _tt, Ogre::StringUtil::StrStreamType &_outStream)
+ TechniqueType _tt, Ogre::StringStream &_outStream)
{
- _outStream << "#version 130\n\n";
+ _outStream << "#version " << glslVersion << "\n\n";
_outStream <<
"vec4 expand(vec4 v)\n"
@@ -1761,10 +1796,10 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
this->generateFpDynamicShadowsHelpers(_prof, _terrain, _tt, _outStream);
_outStream <<
- "in vec4 position;\n";
+ fpInStr << " vec4 position;\n";
Ogre::uint texCoordSet = 1;
- _outStream << "in vec4 uvMisc;\n";
+ _outStream << fpInStr << " vec4 uvMisc;\n";
// UV's premultiplied, packed as xy/zw
Ogre::uint maxLayers = _prof->getMaxLayers(_terrain);
@@ -1784,13 +1819,13 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
for (Ogre::uint i = 0; i < numUVSets; ++i)
{
_outStream <<
- "in vec4 layerUV" << i << ";\n";
+ fpInStr << " vec4 layerUV" << i << ";\n";
}
}
if (_prof->getParent()->getDebugLevel() && _tt != RENDER_COMPOSITE_MAP)
{
- _outStream << "in vec2 lodInfo;\n";
+ _outStream << fpInStr << " vec2 lodInfo;\n";
}
bool fog = _terrain->getSceneManager()->getFogMode() != Ogre::FOG_NONE &&
@@ -1800,7 +1835,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
{
_outStream <<
"uniform vec3 fogColour;\n"
- "in float fogVal;\n";
+ << fpInStr << " float fogVal;\n";
}
Ogre::uint currentSamplerIdx = 0;
@@ -1864,19 +1899,25 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
"Try reducing the number of layers.", __FUNCTION__);
}
- _outStream << "out vec4 outputCol;\n";
+ std::string outputColTypeStr = "vec4";
+ if (glslVersion != "120")
+ {
+ _outStream << "out vec4 outputCol;\n";
+ outputColTypeStr = "";
+ }
_outStream <<
"void main()\n"
"{\n"
" float shadow = 1.0;\n"
" vec2 uv = uvMisc.xy;\n"
- " outputCol = vec4(0.0, 0.0, 0.0, 1.0);\n";
+ " " << outputColTypeStr << " outputCol = vec4(0.0, 0.0, 0.0, 1.0);\n";
if (_tt != LOW_LOD)
{
// global normal
- _outStream << " vec3 normal = expand(texture(globalNormal, uv)).xyz;\n";
+ _outStream << " vec3 normal = expand("
+ << textureStr << "(globalNormal, uv)).xyz;\n";
}
_outStream <<
@@ -1892,7 +1933,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
{
// we just do a single calculation from composite map
_outStream <<
- " vec4 composite = texture(compositeMap, uv);\n"
+ " vec4 composite = " << textureStr << "(compositeMap, uv);\n"
" diffuse = composite.xyz;\n";
// TODO - specular; we'll need normals for this!
}
@@ -1902,7 +1943,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpHeader(
for (Ogre::uint i = 0; i < numBlendTextures; ++i)
{
_outStream << " vec4 blendTexVal" << i
- << " = texture(blendTex" << i << ", uv);\n";
+ << " = " << textureStr << "(blendTex" << i << ", uv);\n";
}
if (_prof->isLayerNormalMappingEnabled())
@@ -1966,7 +2007,7 @@ void
GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadowsParams(
Ogre::uint *_texCoord, Ogre::uint *_sampler, const SM2Profile *_prof,
const Ogre::Terrain * /*_terrain*/, TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream)
+ Ogre::StringStream &_outStream)
{
if (_tt == HIGH_LOD)
this->mShadowSamplerStartHi = *_sampler;
@@ -1984,7 +2025,7 @@ GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadowsParams(
for (Ogre::uint i = 0; i < numTextures; ++i)
{
_outStream <<
- "in vec4 lightSpacePos" << i << ";\n" <<
+ "varying vec4 lightSpacePos" << i << ";\n" <<
"uniform sampler2D shadowMap" << i << ";\n";
*_sampler = *_sampler + 1;
@@ -2002,7 +2043,7 @@ GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadowsParams(
void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpLayer(
const SM2Profile *_prof, const Ogre::Terrain * /*_terrain*/,
TechniqueType _tt, Ogre::uint _layer,
- Ogre::StringUtil::StrStreamType &_outStream)
+ Ogre::StringStream &_outStream)
{
Ogre::uint uvIdx = _layer / 2;
Ogre::String uvChannels = (_layer % 2) ? ".zw" : ".xy";
@@ -2026,15 +2067,15 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpLayer(
if (_prof->isLayerParallaxMappingEnabled() && _tt != RENDER_COMPOSITE_MAP)
{
// modify UV - note we have to sample an extra time
- _outStream << " displacement = texture(normtex" << _layer
+ _outStream << " displacement = " << textureStr << "(normtex" << _layer
<< ", uv" << _layer << ").w\n"
" * scaleBiasSpecular.x + scaleBiasSpecular.y;\n";
_outStream << " uv" << _layer << " += TSeyeDir.xy * displacement;\n";
}
// access TS normal map
- _outStream << " TSnormal = expand(texture(normtex" << _layer << ", uv"
- << _layer << ")).xyz;\n";
+ _outStream << " TSnormal = expand(" << textureStr << "(normtex"
+ << _layer << ", uv" << _layer << ")).xyz;\n";
_outStream << " TShalfAngle = normalize(TSlightDir + TSeyeDir);\n";
_outStream << " litResLayer = lit(dot(TSlightDir, TSnormal), "
@@ -2049,7 +2090,8 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpLayer(
// sample diffuse texture
_outStream << " vec4 diffuseSpecTex" << _layer
- << " = texture(difftex" << _layer << ", uv" << _layer << ");\n";
+ << " = " << textureStr << "(difftex" << _layer << ", uv" << _layer
+ << ");\n";
// apply to common
if (!_layer)
@@ -2079,7 +2121,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpLayer(
/////////////////////////////////////////////////
void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpFooter(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
- TechniqueType _tt, Ogre::StringUtil::StrStreamType &_outStream)
+ TechniqueType _tt, Ogre::StringStream &_outStream)
{
if (_tt == LOW_LOD)
{
@@ -2099,13 +2141,14 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpFooter(
_prof->isGlobalColourMapEnabled())
{
// sample colour map and apply to diffuse
- _outStream << " diffuse *= texture(globalColourMap, uv).xyz;\n";
+ _outStream << " diffuse *= " << textureStr
+ << "(globalColourMap, uv).xyz;\n";
}
if (_prof->isLightmapEnabled())
{
// sample lightmap
- _outStream << " shadow = texture(lightMap, uv).x;\n";
+ _outStream << " shadow = " << textureStr << "(lightMap, uv).x;\n";
}
if (_prof->isShadowingEnabled(_tt, _terrain))
@@ -2146,6 +2189,8 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpFooter(
_outStream << " outputCol.xyz = mix(outputCol.xyz, fogColour, fogVal);\n";
}
+ if (glslVersion == "120")
+ _outStream << " gl_FragColor = outputCol;\n";
// Final return
_outStream << "\n}\n";
@@ -2155,7 +2200,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpFooter(
void
GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadowsHelpers(
const SM2Profile *_prof, const Ogre::Terrain * /*_terrain*/,
- TechniqueType /*_tt*/, Ogre::StringUtil::StrStreamType &_outStream)
+ TechniqueType /*_tt*/, Ogre::StringStream &_outStream)
{
// TODO make filtering configurable
_outStream <<
@@ -2187,11 +2232,19 @@ GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadowsHelpers(
" vec4 newUV = offsetSample(uv, vec2(x, y), invShadowMapSize);\n"
" // manually project and assign derivatives\n"
" // to avoid gradient issues inside loops\n"
- " newUV = newUV / newUV.w;\n"
+ " newUV = newUV / newUV.w;\n";
// The following line used to be:
- // " float depth = tex2d(shadowMap, newUV.xy, 1.0, 1.0).x;\n"
- " float depth = textureGrad(shadowMap, newUV.xy, "
- " vec2(1.0, 1.0), vec2(1.0, 1.0)).x;\n"
+ // " float depth = tex2d(shadowMap, newUV.xy).x;\n"
+ if (glslVersion == "120")
+ _outStream <<
+ " float depth = texture2D(shadowMap, newUV.xy).x;\n";
+ else
+ {
+ _outStream <<
+ " float depth = textureGrad(shadowMap, newUV.xy, "
+ " vec2(1.0, 1.0), vec2(1.0, 1.0)).x;\n";
+ }
+ _outStream <<
// " if (depth >= 1.0 || depth >= uv.z)\n"
" if (depth >= 1.0 || depth >= newUV.z)\n"
" shadow += 1.0;\n"
@@ -2205,7 +2258,7 @@ GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadowsHelpers(
_outStream <<
"float calcSimpleShadow(sampler2D shadowMap, vec4 shadowMapPos)\n"
"{\n"
- " return textureProj(shadowMap, shadowMapPos).x;\n"
+ " return " << textureStr << "Proj(shadowMap, shadowMapPos).x;\n"
"}\n";
}
@@ -2284,7 +2337,7 @@ GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadowsHelpers(
/////////////////////////////////////////////////
void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadows(
const SM2Profile *_prof, const Ogre::Terrain * /*_terrain*/,
- TechniqueType /*_tt*/, Ogre::StringUtil::StrStreamType &_outStream)
+ TechniqueType /*_tt*/, Ogre::StringStream &_outStream)
{
if (_prof->getReceiveDynamicShadowsPSSM())
{
@@ -2342,7 +2395,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFpDynamicShadows(
void
GzTerrainMatGen::SM2Profile::ShaderHelperGLSL::generateFragmentProgramSource(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
- TechniqueType _tt, Ogre::StringUtil::StrStreamType &_outStream)
+ TechniqueType _tt, Ogre::StringStream &_outStream)
{
this->generateFpHeader(_prof, _terrain, _tt, _outStream);
@@ -2433,7 +2486,7 @@ GzTerrainMatGen::SM2Profile::ShaderHelperCg::generateVertexProgram(
Ogre::HighLevelGpuProgramPtr ret =
this->createVertexProgram(_prof, _terrain, _tt);
- Ogre::StringUtil::StrStreamType sourceStr;
+ Ogre::StringStream sourceStr;
this->generateVertexProgramSource(_prof, _terrain, _tt, sourceStr);
ret->setSource(sourceStr.str());
@@ -2501,7 +2554,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperCg::defaultVpParams(
void
GzTerrainMatGen::SM2Profile::ShaderHelperCg::generateVpDynamicShadows(
const SM2Profile *_prof, const Ogre::Terrain * /*_terrain*/,
- TechniqueType /*_tt*/, Ogre::StringUtil::StrStreamType &_outStream)
+ TechniqueType /*_tt*/, Ogre::StringStream &_outStream)
{
unsigned int numTextures = 1;
@@ -2537,7 +2590,7 @@ GzTerrainMatGen::SM2Profile::ShaderHelperCg::generateVpDynamicShadows(
unsigned int GzTerrainMatGen::SM2Profile::ShaderHelperCg::
generateVpDynamicShadowsParams(unsigned int _texCoord, const SM2Profile *_prof,
const Ogre::Terrain * /*_terrain*/, TechniqueType /*_tt*/,
- Ogre::StringUtil::StrStreamType &_outStream)
+ Ogre::StringStream &_outStream)
{
// out semantics & params
unsigned int numTextures = 1;
@@ -2571,7 +2624,7 @@ generateVpDynamicShadowsParams(unsigned int _texCoord, const SM2Profile *_prof,
// virtual.
void GzTerrainMatGen::SM2Profile::ShaderHelperCg::generateVpHeader(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
- TechniqueType _tt, Ogre::StringUtil::StrStreamType &_outStream)
+ TechniqueType _tt, Ogre::StringStream &_outStream)
{
_outStream << "void main_vp(\n";
@@ -2753,7 +2806,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperCg::generateVpHeader(
// but is needed because generateVpDynamicShadows() is not declared virtual.
void GzTerrainMatGen::SM2Profile::ShaderHelperCg::generateVpFooter(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
- TechniqueType _tt, Ogre::StringUtil::StrStreamType &_outStream)
+ TechniqueType _tt, Ogre::StringStream &_outStream)
{
_outStream << " oPos = mul(viewProjMatrix, worldPos);\n"
<< " oUVMisc.xy = uv.xy;\n";
@@ -2784,7 +2837,7 @@ void GzTerrainMatGen::SM2Profile::ShaderHelperCg::generateVpFooter(
void GzTerrainMatGen::SM2Profile::ShaderHelperCg::
generateVertexProgramSource(const SM2Profile *_prof,
const Ogre::Terrain* _terrain, TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream)
+ Ogre::StringStream &_outStream)
{
this->generateVpHeader(_prof, _terrain, _tt, _outStream);
@@ -2809,7 +2862,7 @@ GzTerrainMatGen::SM2Profile::ShaderHelperCg::generateFragmentProgram(
Ogre::HighLevelGpuProgramPtr ret = this->createFragmentProgram(_prof,
_terrain, _tt);
- Ogre::StringUtil::StrStreamType sourceStr;
+ Ogre::StringStream sourceStr;
this->generateFragmentProgramSource(_prof, _terrain, _tt, sourceStr);
diff --git a/gazebo/rendering/Heightmap.hh b/gazebo/rendering/Heightmap.hh
index e541743..75ed950 100644
--- a/gazebo/rendering/Heightmap.hh
+++ b/gazebo/rendering/Heightmap.hh
@@ -14,13 +14,9 @@
* limitations under the License.
*
*/
-/* Desc: Heightmap geometry
- * Author: Nate Koenig
- * Date: 12 May 2009
- */
-#ifndef _HEIGHTMAP_HH_
-#define _HEIGHTMAP_HH_
+#ifndef _GAZEBO_RENDERING_HEIGHTMAP_HH_
+#define _GAZEBO_RENDERING_HEIGHTMAP_HH_
#include <string>
#include <vector>
@@ -48,7 +44,7 @@ namespace gazebo
/// \class DummyPageProvider Heightmap.hh rendering/rendering.hh
/// \brief Pretends to provide procedural page content to avoid page loading
- class GAZEBO_VISIBLE DummyPageProvider : public Ogre::PageProvider
+ class GZ_RENDERING_VISIBLE DummyPageProvider : public Ogre::PageProvider
{
/// \brief Give a provider the opportunity to prepare page content
/// procedurally. The parameters are not used.
@@ -85,7 +81,7 @@ namespace gazebo
/// \class Heightmap Heightmap.hh rendering/rendering.hh
/// \brief Rendering a terrain using heightmap information
- class GAZEBO_VISIBLE Heightmap
+ class GZ_RENDERING_VISIBLE Heightmap
{
/// \brief Constructor
/// \param[in] _scene Pointer to the scene that will contain the heightmap
@@ -346,7 +342,7 @@ namespace gazebo
/// \internal
/// \brief Custom terrain material generator for GLSL terrains.
- class GAZEBO_VISIBLE GzTerrainMatGen
+ class GZ_RENDERING_VISIBLE GzTerrainMatGen
: public Ogre::TerrainMaterialGeneratorA
{
/// \brief Constructor
@@ -402,16 +398,16 @@ namespace gazebo
protected: virtual void generateVpHeader(const SM2Profile *_prof,
const Ogre::Terrain *_terrain, TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateVpFooter(const SM2Profile *_prof,
const Ogre::Terrain *_terrain, TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateVertexProgramSource(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void defaultVpParams(const SM2Profile *_prof,
const Ogre::Terrain *_terrain, TechniqueType _tt,
@@ -420,49 +416,49 @@ namespace gazebo
protected: virtual unsigned int generateVpDynamicShadowsParams(
unsigned int _texCoordStart, const SM2Profile *_prof,
const Ogre::Terrain *_terrain, TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateVpDynamicShadows(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateFpHeader(const SM2Profile *_prof,
const Ogre::Terrain *_terrain,
TechniqueType tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateFpLayer(const SM2Profile *_prof,
const Ogre::Terrain *_terrain, TechniqueType tt,
Ogre::uint _layer,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateFpFooter(const SM2Profile *_prof,
const Ogre::Terrain *_terrain,
TechniqueType tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateFpDynamicShadowsParams(
Ogre::uint *_texCoord, Ogre::uint *_sampler,
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateFpDynamicShadowsHelpers(
const SM2Profile *_prof,
const Ogre::Terrain *_terrain,
TechniqueType tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: void generateFpDynamicShadows(const SM2Profile *_prof,
const Ogre::Terrain *_terrain, TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateFragmentProgramSource(
const SM2Profile *_prof,
const Ogre::Terrain *_terrain,
TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void updateVpParams(const SM2Profile *_prof,
const Ogre::Terrain *_terrain, TechniqueType _tt,
@@ -471,6 +467,10 @@ namespace gazebo
private: Ogre::String GetChannel(Ogre::uint _idx);
};
+ // Needed to allow access from ShaderHelperGLSL to protected members
+ // of SM2Profile.
+ friend ShaderHelperGLSL;
+
/// Keeping the CG shader for reference.
/// Utility class to help with generating shaders for Cg / HLSL.
protected: class ShaderHelperCg :
@@ -486,16 +486,16 @@ namespace gazebo
protected: virtual void generateVpHeader(const SM2Profile *_prof,
const Ogre::Terrain *_terrain, TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateVpFooter(const SM2Profile *_prof,
const Ogre::Terrain *_terrain, TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateVertexProgramSource(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void defaultVpParams(const SM2Profile *_prof,
const Ogre::Terrain *_terrain, TechniqueType _tt,
@@ -504,13 +504,18 @@ namespace gazebo
protected: virtual unsigned int generateVpDynamicShadowsParams(
unsigned int _texCoordStart, const SM2Profile *_prof,
const Ogre::Terrain *_terrain, TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
protected: virtual void generateVpDynamicShadows(
const SM2Profile *_prof, const Ogre::Terrain *_terrain,
TechniqueType _tt,
- Ogre::StringUtil::StrStreamType &_outStream);
+ Ogre::StringStream &_outStream);
};
+
+ // Needed to allow access from ShaderHelperCg to protected members
+ // of SM2Profile.
+ friend ShaderHelperCg;
+
#ifdef __clang__
#pragma clang diagnostic pop
#endif // ifdef __clang__
diff --git a/gazebo/rendering/Heightmap_TEST.cc b/gazebo/rendering/Heightmap_TEST.cc
index bda53c6..c5d0165 100644
--- a/gazebo/rendering/Heightmap_TEST.cc
+++ b/gazebo/rendering/Heightmap_TEST.cc
@@ -17,7 +17,7 @@
#include <gtest/gtest.h>
#include <boost/assign/list_of.hpp>
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class Heightmap_TEST : public ServerFixture
@@ -31,6 +31,7 @@ TEST_F(Heightmap_TEST, splitTerrain)
Load("worlds/empty.world");
gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene("default");
+ ASSERT_TRUE(scene != NULL);
scene = gazebo::rendering::create_scene("default", false);
diff --git a/gazebo/rendering/InertiaVisual.cc b/gazebo/rendering/InertiaVisual.cc
new file mode 100644
index 0000000..411d3b4
--- /dev/null
+++ b/gazebo/rendering/InertiaVisual.cc
@@ -0,0 +1,149 @@
+/*
+ * 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/Vector3.hh"
+#include "gazebo/math/Quaternion.hh"
+#include "gazebo/math/Pose.hh"
+
+#include "gazebo/rendering/DynamicLines.hh"
+#include "gazebo/rendering/Scene.hh"
+#include "gazebo/rendering/InertiaVisualPrivate.hh"
+#include "gazebo/rendering/InertiaVisual.hh"
+
+using namespace gazebo;
+using namespace rendering;
+
+/////////////////////////////////////////////////
+InertiaVisual::InertiaVisual(const std::string &_name, VisualPtr _vis)
+ : Visual(*new InertiaVisualPrivate, _name, _vis, false)
+{
+ InertiaVisualPrivate *dPtr =
+ reinterpret_cast<InertiaVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_PHYSICS;
+}
+
+/////////////////////////////////////////////////
+InertiaVisual::~InertiaVisual()
+{
+}
+
+/////////////////////////////////////////////////
+void InertiaVisual::Load(sdf::ElementPtr _elem)
+{
+ Visual::Load();
+ math::Pose pose = _elem->Get<math::Pose>("origin");
+ this->Load(pose);
+}
+
+/////////////////////////////////////////////////
+void InertiaVisual::Load(ConstLinkPtr &_msg)
+{
+ Visual::Load();
+
+ math::Vector3 xyz(_msg->inertial().pose().position().x(),
+ _msg->inertial().pose().position().y(),
+ _msg->inertial().pose().position().z());
+ math::Quaternion q(_msg->inertial().pose().orientation().w(),
+ _msg->inertial().pose().orientation().x(),
+ _msg->inertial().pose().orientation().y(),
+ _msg->inertial().pose().orientation().z());
+
+ // Use principal moments of inertia to scale Inertia visual
+ // \todo: rotate to match principal axes when product terms are nonzero
+ // This can be done with Eigen, or with code from the following paper:
+ // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric Matrix
+ // http://arxiv.org/abs/1306.6291v3
+ double mass = _msg->inertial().mass();
+ double Ixx = _msg->inertial().ixx();
+ double Iyy = _msg->inertial().iyy();
+ double Izz = _msg->inertial().izz();
+ math::Vector3 boxScale;
+ if (mass < 0 || Ixx < 0 || Iyy < 0 || Izz < 0 ||
+ Ixx + Iyy < Izz || Iyy + Izz < Ixx || Izz + Ixx < Iyy)
+ {
+ // Unrealistic inertia, load with default scale
+ gzlog << "The link " << _msg->name() << " has unrealistic inertia, "
+ << "unable to visualize box of equivalent inertia." << std::endl;
+ this->Load(math::Pose(xyz, q));
+ }
+ else
+ {
+ // Compute dimensions of box with uniform density and equivalent inertia.
+ boxScale.x = sqrt(6*(Izz + Iyy - Ixx) / mass);
+ boxScale.y = sqrt(6*(Izz + Ixx - Iyy) / mass);
+ boxScale.z = sqrt(6*(Ixx + Iyy - Izz) / mass);
+
+ this->Load(math::Pose(xyz, q), boxScale);
+ }
+}
+
+/////////////////////////////////////////////////
+void InertiaVisual::Load(const math::Pose &_pose,
+ const math::Vector3 &_scale)
+{
+ InertiaVisualPrivate *dPtr =
+ reinterpret_cast<InertiaVisualPrivate *>(this->dataPtr);
+
+ // Inertia position indicator
+ math::Vector3 p1(0, 0, -2*_scale.z);
+ math::Vector3 p2(0, 0, 2*_scale.z);
+ math::Vector3 p3(0, -2*_scale.y, 0);
+ math::Vector3 p4(0, 2*_scale.y, 0);
+ math::Vector3 p5(-2*_scale.x, 0, 0);
+ math::Vector3 p6(2*_scale.x, 0, 0);
+ p1 += _pose.pos;
+ p2 += _pose.pos;
+ p3 += _pose.pos;
+ p4 += _pose.pos;
+ p5 += _pose.pos;
+ p6 += _pose.pos;
+ p1 = _pose.rot.RotateVector(p1);
+ p2 = _pose.rot.RotateVector(p2);
+ p3 = _pose.rot.RotateVector(p3);
+ p4 = _pose.rot.RotateVector(p4);
+ p5 = _pose.rot.RotateVector(p5);
+ p6 = _pose.rot.RotateVector(p6);
+
+ dPtr->crossLines = this->CreateDynamicLine(rendering::RENDERING_LINE_LIST);
+ dPtr->crossLines->setMaterial("Gazebo/Green");
+ dPtr->crossLines->AddPoint(p1);
+ dPtr->crossLines->AddPoint(p2);
+ dPtr->crossLines->AddPoint(p3);
+ dPtr->crossLines->AddPoint(p4);
+ dPtr->crossLines->AddPoint(p5);
+ dPtr->crossLines->AddPoint(p6);
+
+ // Inertia indicator: equivalent box of uniform density
+ this->InsertMesh("unit_box");
+
+ Ogre::MovableObject *boxObj =
+ (Ogre::MovableObject*)(dPtr->scene->GetManager()->createEntity(
+ this->GetName()+"__BOX__", "unit_box"));
+ boxObj->setVisibilityFlags(GZ_VISIBILITY_GUI);
+ ((Ogre::Entity*)boxObj)->setMaterialName("__GAZEBO_TRANS_PURPLE_MATERIAL__");
+
+ dPtr->boxNode =
+ dPtr->sceneNode->createChildSceneNode(this->GetName() + "_BOX");
+
+ dPtr->boxNode->attachObject(boxObj);
+ dPtr->boxNode->setScale(_scale.x, _scale.y, _scale.z);
+ dPtr->boxNode->setPosition(_pose.pos.x, _pose.pos.y, _pose.pos.z);
+ dPtr->boxNode->setOrientation(Ogre::Quaternion(_pose.rot.w, _pose.rot.x,
+ _pose.rot.y, _pose.rot.z));
+
+ this->SetVisibilityFlags(GZ_VISIBILITY_GUI);
+}
diff --git a/gazebo/rendering/InertiaVisual.hh b/gazebo/rendering/InertiaVisual.hh
new file mode 100644
index 0000000..2b1f2f1
--- /dev/null
+++ b/gazebo/rendering/InertiaVisual.hh
@@ -0,0 +1,63 @@
+/*
+ * 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_INERTIAVISUAL_HH_
+#define _GAZEBO_INERTIAVISUAL_HH_
+
+#include <string>
+
+#include "gazebo/math/Pose.hh"
+#include "gazebo/rendering/Visual.hh"
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ /// \addtogroup gazebo_rendering Rendering
+ /// \{
+
+ /// \class InertiaVisual InertiaVisual.hh rendering/rendering.hh
+ /// \brief Basic Inertia visualization
+ class GZ_RENDERING_VISIBLE InertiaVisual : public Visual
+ {
+ /// \brief Constructor
+ /// \param[in] _name Name of the Visual
+ /// \param[in] _vis Parent Visual
+ public: InertiaVisual(const std::string &_name, VisualPtr _vis);
+
+ /// \brief Destructor
+ public: virtual ~InertiaVisual();
+
+ /// \brief Load the Visual from an SDF pointer
+ /// \param[in] _elem SDF Element pointer
+ public: virtual void Load(sdf::ElementPtr _elem);
+ using Visual::Load;
+
+ /// \brief Load from a message
+ /// \param[in] _msg Pointer to the message
+ public: virtual void Load(ConstLinkPtr &_msg);
+
+ /// \brief Load based on a math::Pose
+ /// \param[in] _pose Pose of the Inertia visual
+ /// \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));
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/rendering/InertiaVisualPrivate.hh b/gazebo/rendering/InertiaVisualPrivate.hh
new file mode 100644
index 0000000..6ed3db1
--- /dev/null
+++ b/gazebo/rendering/InertiaVisualPrivate.hh
@@ -0,0 +1,45 @@
+/*
+ * 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_INERTIAVISUAL_PRIVATE_HH_
+#define _GAZEBO_INERTIAVISUAL_PRIVATE_HH_
+
+#include "gazebo/rendering/VisualPrivate.hh"
+
+namespace ogre
+{
+ class SceneNode;
+}
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ class DynamicLines;
+
+ /// \brief Private data for the Inertia Visual class
+ class InertiaVisualPrivate : public VisualPrivate
+ {
+ /// \brief Lines that make the cross marking the center of mass.
+ public: DynamicLines *crossLines;
+
+ /// \brief Box with uniform density and equivalent inertia.
+ public: Ogre::SceneNode *boxNode;
+ };
+ }
+}
+#endif
diff --git a/gazebo/rendering/JointVisual.cc b/gazebo/rendering/JointVisual.cc
index 1509da6..3c9eeff 100644
--- a/gazebo/rendering/JointVisual.cc
+++ b/gazebo/rendering/JointVisual.cc
@@ -30,6 +30,9 @@ using namespace rendering;
JointVisual::JointVisual(const std::string &_name, VisualPtr _vis)
: Visual(*new JointVisualPrivate, _name, _vis, false)
{
+ JointVisualPrivate *dPtr =
+ reinterpret_cast<JointVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_PHYSICS;
}
/////////////////////////////////////////////////
@@ -53,8 +56,12 @@ void JointVisual::Load(ConstJointPtr &_msg)
new AxisVisual(this->GetName() + "_AXIS", shared_from_this()));
dPtr->axisVisual->Load();
- this->SetPosition(msgs::Convert(_msg->pose().position()));
- this->SetRotation(msgs::Convert(_msg->pose().orientation()));
+ ignition::math::Pose3d pose;
+ if (_msg->has_pose())
+ pose = msgs::ConvertIgn(_msg->pose());
+
+ this->SetPosition(pose.Pos());
+ this->SetRotation(pose.Rot());
if (_msg->has_axis2())
{
@@ -70,12 +77,11 @@ void JointVisual::Load(ConstJointPtr &_msg)
JointVisualPtr jointVis;
jointVis.reset(new JointVisual(this->GetName() + "_parent_", parentVis));
- jointVis->Load(_msg,
- msgs::Convert(_msg->pose()) + this->GetParent()->GetWorldPose());
+ jointVis->Load(_msg, pose + this->GetParent()->GetWorldPose().Ign());
// attach axis2 to this visual
msgs::Axis axis2Msg = _msg->axis2();
- this->CreateAxis(msgs::Convert(axis2Msg.xyz()),
+ dPtr->arrowVisual = this->CreateAxis(msgs::ConvertIgn(axis2Msg.xyz()),
axis2Msg.use_parent_model_frame(), _msg->type());
dPtr->parentAxisVis = jointVis;
@@ -85,10 +91,20 @@ void JointVisual::Load(ConstJointPtr &_msg)
// for all other joint types:
// axis1 is attached to child link
msgs::Axis axis1Msg = _msg->axis1();
- this->CreateAxis(msgs::Convert(axis1Msg.xyz()),
+ dPtr->arrowVisual = this->CreateAxis(msgs::ConvertIgn(axis1Msg.xyz()),
axis1Msg.use_parent_model_frame(), _msg->type());
}
+ // Scale according to the link it is attached to
+ double linkSize = std::max(0.1,
+ dPtr->parent->GetBoundingBox().GetSize().GetLength());
+ dPtr->scaleToLink = math::Vector3(linkSize * 0.7,
+ linkSize * 0.7,
+ linkSize * 0.7);
+ this->SetScale(dPtr->scaleToLink);
+ if (dPtr->parentAxisVis)
+ dPtr->parentAxisVis->SetScale(dPtr->scaleToLink);
+
this->GetSceneNode()->setInheritScale(false);
this->SetVisibilityFlags(GZ_VISIBILITY_GUI);
}
@@ -96,10 +112,13 @@ void JointVisual::Load(ConstJointPtr &_msg)
/////////////////////////////////////////////////
void JointVisual::Load(ConstJointPtr &_msg, const math::Pose &_worldPose)
{
+ JointVisualPrivate *dPtr =
+ reinterpret_cast<JointVisualPrivate *>(this->dataPtr);
+
Visual::Load();
msgs::Axis axis1Msg = _msg->axis1();
- this->CreateAxis(msgs::Convert(axis1Msg.xyz()),
+ dPtr->arrowVisual = this->CreateAxis(msgs::ConvertIgn(axis1Msg.xyz()),
axis1Msg.use_parent_model_frame(), _msg->type());
// joint pose is always relative to the child link so update axis pose
@@ -110,12 +129,9 @@ void JointVisual::Load(ConstJointPtr &_msg, const math::Pose &_worldPose)
}
/////////////////////////////////////////////////
-void JointVisual::CreateAxis(const math::Vector3 &_axis, bool _useParentFrame,
- msgs::Joint::Type _type)
+ArrowVisualPtr JointVisual::CreateAxis(const math::Vector3 &_axis,
+ bool _useParentFrame, msgs::Joint::Type _type)
{
- JointVisualPrivate *dPtr =
- reinterpret_cast<JointVisualPrivate *>(this->dataPtr);
-
ArrowVisualPtr axis;
std::stringstream nameStr;
@@ -125,6 +141,18 @@ void JointVisual::CreateAxis(const math::Vector3 &_axis, bool _useParentFrame,
axis->Load();
axis->SetMaterial("Gazebo/YellowTransparent");
+ this->UpdateAxis(axis, _axis, _useParentFrame, _type);
+
+ return axis;
+}
+
+/////////////////////////////////////////////////
+void JointVisual::UpdateAxis(ArrowVisualPtr _arrowVisual,
+ const math::Vector3 &_axis, bool _useParentFrame, msgs::Joint::Type _type)
+{
+ JointVisualPrivate *dPtr =
+ reinterpret_cast<JointVisualPrivate *>(this->dataPtr);
+
// Get rotation to axis vector
math::Vector3 axisDir = _axis;
math::Vector3 u = axisDir.Normalize();
@@ -137,7 +165,7 @@ void JointVisual::CreateAxis(const math::Vector3 &_axis, bool _useParentFrame,
quat.SetFromAxis(u.GetPerpendicular(), angle);
else
quat.SetFromAxis((v.Cross(u)).Normalize(), angle);
- axis->SetRotation(quat);
+ _arrowVisual->SetRotation(quat);
if (_useParentFrame)
{
@@ -146,42 +174,175 @@ void JointVisual::CreateAxis(const math::Vector3 &_axis, bool _useParentFrame,
VisualPtr model = this->GetRootVisual();
math::Quaternion quatFromModel =
model->GetWorldPose().rot.GetInverse()*this->GetWorldPose().rot;
- axis->SetRotation(quatFromModel.GetInverse()*axis->GetRotation());
+ _arrowVisual->SetRotation(quatFromModel.GetInverse() *
+ _arrowVisual->GetRotation());
}
- if (_type == msgs::Joint::REVOLUTE || _type == msgs::Joint::REVOLUTE2
- || _type == msgs::Joint::UNIVERSAL || _type == msgs::Joint::GEARBOX)
- axis->ShowRotation(true);
+ _arrowVisual->ShowRotation(_type == msgs::Joint::REVOLUTE ||
+ _type == msgs::Joint::REVOLUTE2 ||
+ _type == msgs::Joint::UNIVERSAL ||
+ _type == msgs::Joint::GEARBOX);
- math::Quaternion axisWorldRotation = axis->GetWorldPose().rot;
+ if (dPtr->axisVisual)
+ {
+ _arrowVisual->SetVisible(true);
+ }
+ else
+ {
+ return;
+ }
+
+ // Hide existing arrow head if it overlaps with the axis
+ math::Quaternion axisWorldRotation = _arrowVisual->GetWorldPose().rot;
math::Quaternion jointWorldRotation = this->GetWorldPose().rot;
- // hide the existing axis's arrow head if it overlaps with the one we are
- // creating
+ dPtr->axisVisual->ShowAxisHead(0, true);
+ dPtr->axisVisual->ShowAxisHead(1, true);
+ dPtr->axisVisual->ShowAxisHead(2, true);
+ _arrowVisual->ShowShaft(true);
+
math::Vector3 axisWorld = axisWorldRotation*math::Vector3::UnitZ;
if (axisWorld == jointWorldRotation*math::Vector3::UnitX)
{
- if (dPtr->axisVisual)
- {
- dPtr->axisVisual->ShowAxisHead(0, false);
- axis->ShowShaft(false);
- }
+ dPtr->axisVisual->ShowAxisHead(0, false);
+ _arrowVisual->ShowShaft(false);
}
else if (axisWorld == jointWorldRotation*math::Vector3::UnitY)
{
- if (dPtr->axisVisual)
+ dPtr->axisVisual->ShowAxisHead(1, false);
+ _arrowVisual->ShowShaft(false);
+ }
+ else if (axisWorld == jointWorldRotation*math::Vector3::UnitZ)
+ {
+ dPtr->axisVisual->ShowAxisHead(2, false);
+ _arrowVisual->ShowShaft(false);
+ }
+}
+
+/////////////////////////////////////////////////
+void JointVisual::UpdateFromMsg(ConstJointPtr &_msg)
+{
+ JointVisualPrivate *dPtr =
+ reinterpret_cast<JointVisualPrivate *>(this->dataPtr);
+
+ if (_msg->has_pose())
+ {
+ // Avoid position changing when parent is scaled
+ this->SetPosition(msgs::ConvertIgn(_msg->pose().position()) /
+ this->GetParent()->GetScale().Ign());
+ this->SetRotation(msgs::ConvertIgn(_msg->pose().orientation()));
+ }
+
+ ArrowVisualPtr axis2Visual = NULL;
+ if (dPtr->parentAxisVis)
+ {
+ axis2Visual = dPtr->parentAxisVis->GetArrowVisual();
+ }
+
+ // Show XYZ heads
+ if (dPtr->axisVisual)
+ {
+ dPtr->axisVisual->ShowAxisHead(0, true);
+ dPtr->axisVisual->ShowAxisHead(1, true);
+ dPtr->axisVisual->ShowAxisHead(2, true);
+ }
+
+ msgs::Axis axis1Msg;
+ msgs::Axis axis2Msg;
+ // Now has 2 axes
+ if (_msg->has_axis2())
+ {
+ axis1Msg = _msg->axis1();
+ axis2Msg = _msg->axis2();
+ // Previously already had 2 axes
+ if (axis2Visual)
{
- dPtr->axisVisual->ShowAxisHead(1, false);
- axis->ShowShaft(false);
+ this->UpdateAxis(dPtr->arrowVisual, msgs::ConvertIgn(axis1Msg.xyz()),
+ axis1Msg.use_parent_model_frame(), _msg->type());
+ this->UpdateAxis(axis2Visual, msgs::ConvertIgn(axis2Msg.xyz()),
+ axis2Msg.use_parent_model_frame(), _msg->type());
+ // joint pose is always relative to the child link
+ dPtr->parentAxisVis->SetWorldPose(msgs::ConvertIgn(_msg->pose()) +
+ this->GetParent()->GetWorldPose().Ign());
+ }
+ else
+ {
+ VisualPtr parentVis;
+ if (_msg->has_parent() && _msg->parent() == "world")
+ parentVis = this->GetScene()->GetWorldVisual();
+ else if (_msg->has_parent_id())
+ parentVis = this->GetScene()->GetVisual(_msg->parent_id());
+
+ JointVisualPtr jointVis;
+ jointVis.reset(new JointVisual(this->GetName() + "_parent_", parentVis));
+ jointVis->Load(_msg,
+ msgs::ConvertIgn(_msg->pose()) +
+ this->GetParent()->GetWorldPose().Ign());
+
+ dPtr->parentAxisVis = jointVis;
+ dPtr->parentAxisVis->SetScale(dPtr->scaleToLink);
+
+ // Previously had 1 axis, which becomes axis 2 now
+ if (dPtr->arrowVisual)
+ {
+ this->UpdateAxis(dPtr->arrowVisual, msgs::ConvertIgn(axis2Msg.xyz()),
+ axis2Msg.use_parent_model_frame(), _msg->type());
+ }
+ // Previously had no axis
+ else
+ {
+ dPtr->arrowVisual = this->CreateAxis(msgs::ConvertIgn(axis2Msg.xyz()),
+ axis2Msg.use_parent_model_frame(), _msg->type());
+ }
}
}
- else if (axisWorld == jointWorldRotation*math::Vector3::UnitZ)
+ // Now has 1 axis
+ else if (_msg->has_axis1())
{
- if (dPtr->axisVisual)
+ // Hide axis 2
+ if (axis2Visual)
+ axis2Visual->SetVisible(false);
+
+ axis1Msg = _msg->axis1();
+ // Previously had at least 1 axis
+ if (dPtr->arrowVisual)
+ {
+ this->UpdateAxis(dPtr->arrowVisual, msgs::ConvertIgn(axis1Msg.xyz()),
+ axis1Msg.use_parent_model_frame(), _msg->type());
+ }
+ // Previously had no axis
+ else
{
- dPtr->axisVisual->ShowAxisHead(2, false);
- axis->ShowShaft(false);
+ dPtr->arrowVisual = this->CreateAxis(msgs::ConvertIgn(axis1Msg.xyz()),
+ axis1Msg.use_parent_model_frame(), _msg->type());
}
}
+ // Now has no axis
+ else if (_msg->has_type())
+ {
+ // Hide axes 1 and 2
+ if (dPtr->arrowVisual)
+ dPtr->arrowVisual->SetVisible(false);
+ if (axis2Visual)
+ axis2Visual->SetVisible(false);
+ }
+}
+
+/////////////////////////////////////////////////
+JointVisualPtr JointVisual::GetParentAxisVisual() const
+{
+ JointVisualPrivate *dPtr =
+ reinterpret_cast<JointVisualPrivate *>(this->dataPtr);
+
+ return dPtr->parentAxisVis;
+}
+
+/////////////////////////////////////////////////
+ArrowVisualPtr JointVisual::GetArrowVisual() const
+{
+ JointVisualPrivate *dPtr =
+ reinterpret_cast<JointVisualPrivate *>(this->dataPtr);
+
+ return dPtr->arrowVisual;
}
/////////////////////////////////////////////////
diff --git a/gazebo/rendering/JointVisual.hh b/gazebo/rendering/JointVisual.hh
index d8fed03..5cd49d5 100644
--- a/gazebo/rendering/JointVisual.hh
+++ b/gazebo/rendering/JointVisual.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class JointVisual JointVisual.hh rendering/rendering.hh
/// \brief Visualization for joints
- class GAZEBO_VISIBLE JointVisual : public Visual
+ class GZ_RENDERING_VISIBLE JointVisual : public Visual
{
/// \brief Constructor
/// \param[in] _name Name of the visual
@@ -61,11 +61,35 @@ namespace gazebo
/// \param[in] _useParentFrame True to use parent frame instead of the
/// joint frame.
/// \param[in] _type Type of axis.
- public: void CreateAxis(const math::Vector3 &_axis, bool _useParentFrame,
- msgs::Joint::Type _type);
+ /// \returns Newly created arrow visual.
+ public: ArrowVisualPtr CreateAxis(const math::Vector3 &_axis,
+ bool _useParentFrame, msgs::Joint::Type _type);
// Documentation Inherited.
public: void SetVisible(bool _visible, bool _cascade = true);
+
+ /// \brief Update the joint visual based on a message.
+ /// \param[in] _msg Joint message
+ public: void UpdateFromMsg(ConstJointPtr &_msg);
+
+ /// \brief Update an axis' arrow visual.
+ /// \param[in] _arrowVisual Arrow visual to be updated.
+ /// \param[in] _axis Axis vector.
+ /// \param[in] _useParentFrame True to use parent frame instead of the
+ /// joint frame.
+ /// \param[in] _type Type of axis.
+ public: void UpdateAxis(ArrowVisualPtr _arrowVisual,
+ const math::Vector3 &_axis, bool _useParentFrame,
+ msgs::Joint::Type _type);
+
+ /// \brief Get the JointVisual which is attached to the parent link.
+ /// returns Parent axis visual.
+ public: JointVisualPtr GetParentAxisVisual() const;
+
+ /// \brief Get the arrow visual which represents the axis attached to the
+ /// child link.
+ /// returns Arrow visual.
+ public: ArrowVisualPtr GetArrowVisual() const;
};
/// \}
}
diff --git a/gazebo/rendering/JointVisualPrivate.hh b/gazebo/rendering/JointVisualPrivate.hh
index 4f6abc3..6b33fb4 100644
--- a/gazebo/rendering/JointVisualPrivate.hh
+++ b/gazebo/rendering/JointVisualPrivate.hh
@@ -29,11 +29,20 @@ namespace gazebo
/// \brief Private data for the Joint Visual class.
class JointVisualPrivate : public VisualPrivate
{
- /// \brief The visual used to draw the joint.
+ /// \brief The joint's XYZ frame visual.
public: AxisVisualPtr axisVisual;
- /// \brief Second joint visual for hinge2 and universal joints.
+ /// \brief The visual representing the one joint axis. There can be only
+ /// one axis visual per joint visual, so joints with two axes have a 2nd
+ /// JointVisual with its own arrowVisual.
+ public: ArrowVisualPtr arrowVisual;
+
+ /// \brief Second joint visual for hinge2 and universal joints. It is a
+ /// simplified visual without an XYZ frame.
public: JointVisualPtr parentAxisVis;
+
+ /// \brief Scale based on the size of the joint's child link.
+ public: math::Vector3 scaleToLink;
};
}
}
diff --git a/gazebo/rendering/JointVisual_TEST.cc b/gazebo/rendering/JointVisual_TEST.cc
index abd6d0b..e5fe16f 100644
--- a/gazebo/rendering/JointVisual_TEST.cc
+++ b/gazebo/rendering/JointVisual_TEST.cc
@@ -20,8 +20,9 @@
#include "gazebo/rendering/RenderingIface.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/JointVisual.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
+using namespace gazebo;
class JointVisual_TEST : public ServerFixture
{
};
@@ -52,12 +53,13 @@ TEST_F(JointVisual_TEST, JointVisualTest)
jointMsg->set_child_id(childVis->GetId());
jointMsg->set_name("test_joint");
jointMsg->set_id(11111);
- msgs::Set(jointMsg->mutable_pose(), math::Pose(1, 2, 3, 1.57, 1.57, 0));
+ msgs::Set(jointMsg->mutable_pose(),
+ ignition::math::Pose3d(1, 2, 3, 1.57, 1.57, 0));
jointMsg->set_type(msgs::Joint::REVOLUTE2);
jointMsg->add_angle(1.2);
{
msgs::Axis *axis1 = jointMsg->mutable_axis1();
- msgs::Set(axis1->mutable_xyz(), gazebo::math::Vector3(0, 1, 0));
+ msgs::Set(axis1->mutable_xyz(), ignition::math::Vector3d(0, 1, 0));
axis1->set_limit_lower(-1.2);
axis1->set_limit_upper(2.3);
axis1->set_limit_effort(6);
@@ -69,7 +71,7 @@ TEST_F(JointVisual_TEST, JointVisualTest)
jointMsg->add_angle(-1.2);
{
msgs::Axis *axis2 = jointMsg->mutable_axis2();
- msgs::Set(axis2->mutable_xyz(), gazebo::math::Vector3(0, 0, 1));
+ msgs::Set(axis2->mutable_xyz(), ignition::math::Vector3d(0, 0, 1));
axis2->set_limit_lower(-1.2);
axis2->set_limit_upper(-0.3);
axis2->set_limit_effort(3);
@@ -85,6 +87,87 @@ TEST_F(JointVisual_TEST, JointVisualTest)
new gazebo::rendering::JointVisual(
"model_GUIONLY_joint_vis", childVis));
jointVis->Load(jointMsg);
+
+ // pose matches the message's pose
+ EXPECT_EQ(jointVis->GetPose(), math::Pose(1, 2, 3, 1.57, 1.57, 0));
+
+ // has axis 1 and it is visible
+ EXPECT_TRUE(jointVis->GetArrowVisual() != NULL);
+ EXPECT_TRUE(jointVis->GetArrowVisual()->GetVisible());
+
+ // has axis 2 and it is visible
+ EXPECT_TRUE(jointVis->GetParentAxisVisual() != NULL);
+ EXPECT_TRUE(jointVis->GetParentAxisVisual()->GetArrowVisual() != NULL);
+ EXPECT_TRUE(jointVis->GetParentAxisVisual()->GetArrowVisual()->GetVisible());
+
+ // update pose from a message
+ jointMsg.reset(new gazebo::msgs::Joint);
+ jointMsg->set_name("test_joint");
+ msgs::Set(jointMsg->mutable_pose(),
+ ignition::math::Pose3d(3, 2, 1, 0, 1.57, 0));
+ jointVis->UpdateFromMsg(jointMsg);
+
+ // pose properly updated
+ EXPECT_EQ(jointVis->GetPose(), math::Pose(3, 2, 1, 0, 1.57, 0));
+
+ // axis 1 still visible
+ EXPECT_TRUE(jointVis->GetArrowVisual() != NULL);
+ EXPECT_TRUE(jointVis->GetArrowVisual()->GetVisible());
+
+ // axis 2 still visible
+ EXPECT_TRUE(jointVis->GetParentAxisVisual() != NULL);
+ EXPECT_TRUE(jointVis->GetParentAxisVisual()->GetArrowVisual() != NULL);
+ EXPECT_TRUE(jointVis->GetParentAxisVisual()->GetArrowVisual()->GetVisible());
+
+ // update joint type and axis from a message
+ jointMsg.reset(new gazebo::msgs::Joint);
+ jointMsg->set_name("test_joint");
+ jointMsg->set_type(msgs::Joint::REVOLUTE);
+ jointMsg->add_angle(2.5);
+ {
+ msgs::Axis *axis1 = jointMsg->mutable_axis1();
+ msgs::Set(axis1->mutable_xyz(), ignition::math::Vector3d(1, 0, 0));
+ axis1->set_limit_lower(-1.2);
+ axis1->set_limit_upper(2.3);
+ axis1->set_limit_effort(6);
+ axis1->set_limit_velocity(1);
+ axis1->set_damping(true);
+ axis1->set_friction(true);
+ axis1->set_use_parent_model_frame(false);
+ }
+ jointVis->UpdateFromMsg(jointMsg);
+
+ // pose hasn't changed
+ EXPECT_EQ(jointVis->GetPose(), math::Pose(3, 2, 1, 0, 1.57, 0));
+
+ // axis 1 still visible
+ EXPECT_TRUE(jointVis->GetArrowVisual() != NULL);
+ EXPECT_TRUE(jointVis->GetArrowVisual()->GetVisible());
+
+ // axis 2 still there but not visible
+ EXPECT_TRUE(jointVis->GetParentAxisVisual() != NULL);
+ EXPECT_TRUE(jointVis->GetParentAxisVisual()->GetArrowVisual() != NULL);
+ EXPECT_FALSE(jointVis->GetParentAxisVisual()->GetArrowVisual()->GetVisible());
+
+ // update joint type and pose from a message
+ jointMsg.reset(new gazebo::msgs::Joint);
+ jointMsg->set_name("test_joint");
+ msgs::Set(jointMsg->mutable_pose(),
+ ignition::math::Pose3d(0, -2, 1, -1.57, 1.57, 0));
+ jointMsg->set_type(msgs::Joint::BALL);
+ jointVis->UpdateFromMsg(jointMsg);
+
+ // new pose
+ EXPECT_EQ(jointVis->GetPose(), math::Pose(0, -2, 1, -1.57, 1.57, 0));
+
+ // axis 1 still there but not visible
+ EXPECT_TRUE(jointVis->GetArrowVisual() != NULL);
+ EXPECT_FALSE(jointVis->GetArrowVisual()->GetVisible());
+
+ // axis 2 still there but not visible
+ EXPECT_TRUE(jointVis->GetParentAxisVisual() != NULL);
+ EXPECT_TRUE(jointVis->GetParentAxisVisual()->GetArrowVisual() != NULL);
+ EXPECT_FALSE(jointVis->GetParentAxisVisual()->GetArrowVisual()->GetVisible());
}
/////////////////////////////////////////////////
diff --git a/gazebo/rendering/LaserVisual.cc b/gazebo/rendering/LaserVisual.cc
index 651884f..5d5cb77 100644
--- a/gazebo/rendering/LaserVisual.cc
+++ b/gazebo/rendering/LaserVisual.cc
@@ -14,10 +14,12 @@
* limitations under the License.
*
*/
-/* Desc: Laser Visualization Class
- * Author: Nate Koenig
- * Date: 14 Dec 2007
- */
+
+#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 "gazebo/common/MeshManager.hh"
#include "gazebo/transport/transport.hh"
@@ -39,6 +41,8 @@ LaserVisual::LaserVisual(const std::string &_name, VisualPtr _vis,
LaserVisualPrivate *dPtr =
reinterpret_cast<LaserVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_SENSOR;
+
dPtr->receivedMsg = false;
dPtr->node = transport::NodePtr(new transport::Node());
@@ -57,12 +61,18 @@ LaserVisual::~LaserVisual()
LaserVisualPrivate *dPtr =
reinterpret_cast<LaserVisualPrivate *>(this->dataPtr);
- for (unsigned int i = 0; i < dPtr->rayFans.size(); ++i)
- {
- this->DeleteDynamicLine(dPtr->rayFans[i]);
- dPtr->rayFans[i] = NULL;
- }
+ for (auto ray : dPtr->rayFans)
+ this->DeleteDynamicLine(ray);
+
+ for (auto ray : dPtr->noHitRayFans)
+ this->DeleteDynamicLine(ray);
+
+ for (auto ray : dPtr->rayLines)
+ this->DeleteDynamicLine(ray);
+
dPtr->rayFans.clear();
+ dPtr->noHitRayFans.clear();
+ dPtr->rayLines.clear();
}
/////////////////////////////////////////////////
@@ -97,18 +107,14 @@ void LaserVisual::Update()
dPtr->receivedMsg = false;
- double angle = dPtr->laserMsg->scan().angle_min();
double verticalAngle = dPtr->laserMsg->scan().vertical_angle_min();
- double r;
- math::Vector3 pt;
- math::Pose offset = msgs::Convert(dPtr->laserMsg->scan().world_pose()) -
- this->GetWorldPose();
+ ignition::math::Pose3d offset =
+ msgs::ConvertIgn(dPtr->laserMsg->scan().world_pose()) -
+ this->GetWorldPose().Ign();
unsigned int vertCount = dPtr->laserMsg->scan().has_vertical_count() ?
dPtr->laserMsg->scan().vertical_count() : 1u;
- math::Quaternion ray;
- math::Vector3 axis;
for (unsigned int j = 0; j < vertCount; ++j)
{
if (j+1 > dPtr->rayFans.size())
@@ -117,23 +123,68 @@ void LaserVisual::Update()
this->CreateDynamicLine(rendering::RENDERING_TRIANGLE_FAN));
dPtr->rayFans[j]->setMaterial("Gazebo/BlueLaser");
dPtr->rayFans[j]->AddPoint(math::Vector3(0, 0, 0));
+
+ // No hit ray fans display rays that do not hit obstacles.
+ dPtr->noHitRayFans.push_back(
+ this->CreateDynamicLine(rendering::RENDERING_TRIANGLE_FAN));
+ dPtr->noHitRayFans[j]->setMaterial("Gazebo/LightBlueLaser");
+ dPtr->noHitRayFans[j]->AddPoint(math::Vector3(0, 0, 0));
+
+ dPtr->rayLines.push_back(
+ this->CreateDynamicLine(rendering::RENDERING_LINE_LIST));
+ dPtr->rayLines[j]->setMaterial("Gazebo/BlueLaser");
+
this->SetVisibilityFlags(GZ_VISIBILITY_GUI);
}
- dPtr->rayFans[j]->SetPoint(0, offset.pos);
+ dPtr->rayFans[j]->SetPoint(0, offset.Pos());
+ dPtr->noHitRayFans[j]->SetPoint(0, offset.Pos());
- angle = dPtr->laserMsg->scan().angle_min();
+ double angle = dPtr->laserMsg->scan().angle_min();
unsigned int count = dPtr->laserMsg->scan().count();
for (unsigned int i = 0; i < count; ++i)
{
- r = dPtr->laserMsg->scan().ranges(j*count + i);
- ray.SetFromEuler(math::Vector3(0.0, -verticalAngle, angle));
- axis = offset.rot * ray * math::Vector3(1.0, 0.0, 0.0);
- pt = (axis * r) + offset.pos;
-
+ double r = dPtr->laserMsg->scan().ranges(j*count + i);
+ ignition::math::Quaterniond ray(
+ ignition::math::Vector3d(0.0, -verticalAngle, angle));
+ ignition::math::Vector3d axis = offset.Rot() * ray *
+ ignition::math::Vector3d(1.0, 0.0, 0.0);
+
+ double hitRange = std::isinf(r) ? 0 : r;
+ ignition::math::Vector3d pt = (axis * hitRange) + offset.Pos();
+
+ double noHitRange =
+ std::isinf(r) ? dPtr->laserMsg->scan().range_max() : hitRange;
+ ignition::math::Vector3d noHitPt = (axis * noHitRange) + offset.Pos();
+
+ // Draw the lines that represent each simulated ray
+ if (i >= dPtr->rayLines[j]->GetPointCount()/2)
+ {
+ dPtr->rayLines[j]->AddPoint(offset.Pos());
+ if (std::isinf(r))
+ dPtr->rayLines[j]->AddPoint(noHitPt);
+ else
+ dPtr->rayLines[j]->AddPoint(pt);
+ }
+ else
+ {
+ dPtr->rayLines[j]->SetPoint(i*2, offset.Pos());
+ if (std::isinf(r))
+ dPtr->rayLines[j]->SetPoint(i*2+1, noHitPt);
+ else
+ dPtr->rayLines[j]->SetPoint(i*2+1, pt);
+ }
+
+ // Draw the triangle fan that fill in the gaps for the laser rays
if (i+1 >= dPtr->rayFans[j]->GetPointCount())
+ {
dPtr->rayFans[j]->AddPoint(pt);
+ dPtr->noHitRayFans[j]->AddPoint(noHitPt);
+ }
else
+ {
dPtr->rayFans[j]->SetPoint(i+1, pt);
+ dPtr->noHitRayFans[j]->SetPoint(i+1, noHitPt);
+ }
angle += dPtr->laserMsg->scan().angle_step();
}
diff --git a/gazebo/rendering/LaserVisual.hh b/gazebo/rendering/LaserVisual.hh
index 5795f65..eaf7388 100644
--- a/gazebo/rendering/LaserVisual.hh
+++ b/gazebo/rendering/LaserVisual.hh
@@ -34,7 +34,7 @@ namespace gazebo
/// \class LaserVisual LaserVisual.hh rendering/rendering.hh
/// \brief Visualization for laser data.
- class GAZEBO_VISIBLE LaserVisual : public Visual
+ class GZ_RENDERING_VISIBLE LaserVisual : public Visual
{
/// \brief Constructor.
/// \param[in] _name Name of the visual.
diff --git a/gazebo/rendering/LaserVisualPrivate.hh b/gazebo/rendering/LaserVisualPrivate.hh
index 02e3632..5cd69c3 100644
--- a/gazebo/rendering/LaserVisualPrivate.hh
+++ b/gazebo/rendering/LaserVisualPrivate.hh
@@ -39,9 +39,15 @@ namespace gazebo
/// \brief Subscription to the laser data.
public: transport::SubscriberPtr laserScanSub;
- /// \brief Renders the laser data.
+ /// \brief Renders the laser data as a triangle fan.
public: std::vector<DynamicLines *> rayFans;
+ /// \brief Renders laser data for rays that do not hit obstacles.
+ public: std::vector<DynamicLines *> noHitRayFans;
+
+ /// \brief Renders the laser data as a line list.
+ public: std::vector<DynamicLines *> rayLines;
+
/// \brief Mutex to protect the contact message.
public: boost::mutex mutex;
diff --git a/gazebo/rendering/Light.cc b/gazebo/rendering/Light.cc
index 57bbd60..de0f747 100644
--- a/gazebo/rendering/Light.cc
+++ b/gazebo/rendering/Light.cc
@@ -151,8 +151,8 @@ void Light::UpdateFromMsg(ConstLightPtr &_msg)
if (_msg->has_pose())
{
- this->SetPosition(msgs::Convert(_msg->pose().position()));
- this->SetRotation(msgs::Convert(_msg->pose().orientation()));
+ this->SetPosition(msgs::ConvertIgn(_msg->pose().position()));
+ this->SetRotation(msgs::ConvertIgn(_msg->pose().orientation()));
}
}
@@ -165,8 +165,8 @@ void Light::LoadFromMsg(const msgs::Light &_msg)
if (_msg.has_pose())
{
- this->SetPosition(msgs::Convert(_msg.pose().position()));
- this->SetRotation(msgs::Convert(_msg.pose().orientation()));
+ this->SetPosition(msgs::ConvertIgn(_msg.pose().position()));
+ this->SetRotation(msgs::ConvertIgn(_msg.pose().orientation()));
}
}
@@ -590,11 +590,12 @@ void Light::FillMsg(msgs::Light &_msg) const
else if (lightType == "directional")
_msg.set_type(msgs::Light::DIRECTIONAL);
- msgs::Set(_msg.mutable_pose()->mutable_position(), this->GetPosition());
- msgs::Set(_msg.mutable_pose()->mutable_orientation(), this->GetRotation());
+ msgs::Set(_msg.mutable_pose()->mutable_position(), this->GetPosition().Ign());
+ msgs::Set(_msg.mutable_pose()->mutable_orientation(),
+ this->GetRotation().Ign());
msgs::Set(_msg.mutable_diffuse(), this->GetDiffuseColor());
msgs::Set(_msg.mutable_specular(), this->GetSpecularColor());
- msgs::Set(_msg.mutable_direction(), this->GetDirection());
+ msgs::Set(_msg.mutable_direction(), this->GetDirection().Ign());
_msg.set_cast_shadows(this->light->getCastShadows());
diff --git a/gazebo/rendering/Light.hh b/gazebo/rendering/Light.hh
index 31c95f3..6d11b1f 100644
--- a/gazebo/rendering/Light.hh
+++ b/gazebo/rendering/Light.hh
@@ -49,7 +49,8 @@ namespace gazebo
/// class encapsulates all three. Point lights are light light bulbs,
/// spot lights project a cone of light, and directional lights are light
/// sun light.
- class GAZEBO_VISIBLE Light : public boost::enable_shared_from_this<Light>
+ class GZ_RENDERING_VISIBLE Light :
+ public boost::enable_shared_from_this<Light>
{
/// \brief Constructor.
/// \param[in] _scene Pointer to the scene that contains the Light.
diff --git a/gazebo/rendering/LinkFrameVisual.cc b/gazebo/rendering/LinkFrameVisual.cc
new file mode 100644
index 0000000..1920402
--- /dev/null
+++ b/gazebo/rendering/LinkFrameVisual.cc
@@ -0,0 +1,91 @@
+/*
+ * 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/rendering/ogre_gazebo.h"
+#include "gazebo/rendering/AxisVisual.hh"
+#include "gazebo/rendering/LinkFrameVisualPrivate.hh"
+#include "gazebo/rendering/LinkFrameVisual.hh"
+
+using namespace gazebo;
+using namespace rendering;
+
+/////////////////////////////////////////////////
+LinkFrameVisual::LinkFrameVisual(const std::string &_name, VisualPtr _parent)
+ : AxisVisual(*new LinkFrameVisualPrivate, _name, _parent)
+{
+ LinkFrameVisualPrivate *dPtr =
+ reinterpret_cast<LinkFrameVisualPrivate *>(this->dataPtr);
+
+ dPtr->type = VT_PHYSICS;
+ dPtr->highlightedTransp = 0.01;
+ dPtr->nonHighlightedTransp = 0.8;
+}
+
+/////////////////////////////////////////////////
+void LinkFrameVisual::Load()
+{
+ LinkFrameVisualPrivate *dPtr =
+ reinterpret_cast<LinkFrameVisualPrivate *>(this->dataPtr);
+
+ AxisVisual::Load();
+
+ double linkSize = std::max(0.1,
+ dPtr->parent->GetBoundingBox().GetSize().GetLength());
+ linkSize = std::min(linkSize, 1.0);
+ dPtr->scaleToLink = math::Vector3(linkSize * 0.7,
+ linkSize * 0.7,
+ linkSize * 0.7);
+
+ // Scale according to the link it is attached to
+ if (dPtr->scaleToLink != math::Vector3::Zero)
+ this->SetScale(dPtr->scaleToLink);
+
+ // Don't scale when link is scaled
+ this->GetSceneNode()->setInheritScale(false);
+
+ this->ShowAxisHead(0, false);
+ this->ShowAxisHead(1, false);
+ this->ShowAxisHead(2, false);
+ this->SetTransparency(dPtr->nonHighlightedTransp);
+ this->SetCastShadows(false);
+}
+
+//////////////////////////////////////////////////
+void LinkFrameVisual::SetHighlighted(bool _highlighted)
+{
+ LinkFrameVisualPrivate *dPtr =
+ reinterpret_cast<LinkFrameVisualPrivate *>(this->dataPtr);
+
+ if (_highlighted)
+ {
+ this->SetTransparency(dPtr->highlightedTransp);
+ }
+ else
+ {
+ this->SetTransparency(dPtr->nonHighlightedTransp);
+ }
+}
+
+//////////////////////////////////////////////////
+bool LinkFrameVisual::GetHighlighted()
+{
+ LinkFrameVisualPrivate *dPtr =
+ reinterpret_cast<LinkFrameVisualPrivate *>(this->dataPtr);
+
+ return math::equal(this->GetTransparency(), dPtr->highlightedTransp);
+}
+
diff --git a/gazebo/rendering/LinkFrameVisual.hh b/gazebo/rendering/LinkFrameVisual.hh
new file mode 100644
index 0000000..e61af67
--- /dev/null
+++ b/gazebo/rendering/LinkFrameVisual.hh
@@ -0,0 +1,58 @@
+/*
+ * 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_LINK_FRAME_VISUAL_HH_
+#define _GAZEBO_LINK_FRAME_VISUAL_HH_
+
+#include <string>
+
+#include "gazebo/math/Vector3.hh"
+#include "gazebo/rendering/AxisVisual.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ /// \addtogroup gazebo_rendering Rendering
+ /// \{
+
+ /// \class LinkFrameVisual LinkFrameVisual.hh rendering/rendering.hh
+ /// \brief Visualization for link frames.
+ class GZ_RENDERING_VISIBLE LinkFrameVisual : public AxisVisual
+ {
+ /// \brief Constructor
+ /// \param[in] _name Name of the LinkFrameVisual
+ /// \param[in] _parent Parent visual
+ public: LinkFrameVisual(const std::string &_name, VisualPtr _parent);
+
+ /// \brief Destructor
+ public: virtual ~LinkFrameVisual() = default;
+
+ // Documentation inherited
+ public: virtual void Load();
+
+ // Documentation inherited
+ public: virtual void SetHighlighted(bool _highlighted);
+
+ // Documentation inherited
+ public: bool GetHighlighted();
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/rendering/LinkFrameVisualPrivate.hh b/gazebo/rendering/LinkFrameVisualPrivate.hh
new file mode 100644
index 0000000..e2c4038
--- /dev/null
+++ b/gazebo/rendering/LinkFrameVisualPrivate.hh
@@ -0,0 +1,43 @@
+/*
+ * 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_LINK_FRAME_VISUAL_PRIVATE_HH_
+#define _GAZEBO_LINK_FRAME_VISUAL_PRIVATE_HH_
+
+#include <string>
+
+#include "gazebo/rendering/AxisVisualPrivate.hh"
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ /// \brief Private data for the LinkFrame Visual class.
+ class LinkFrameVisualPrivate : public AxisVisualPrivate
+ {
+ /// \brief Scale based on the size of the parent link.
+ public: math::Vector3 scaleToLink;
+
+ /// \brief Transparency when highlighted.
+ public: float highlightedTransp;
+
+ /// \brief Transparency when not highlighted.
+ public: float nonHighlightedTransp;
+ };
+ }
+}
+#endif
diff --git a/gazebo/rendering/LinkFrameVisual_TEST.cc b/gazebo/rendering/LinkFrameVisual_TEST.cc
new file mode 100644
index 0000000..7b97ed2
--- /dev/null
+++ b/gazebo/rendering/LinkFrameVisual_TEST.cc
@@ -0,0 +1,89 @@
+/*
+ * 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 axispliance 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/RenderingIface.hh"
+#include "gazebo/rendering/Scene.hh"
+#include "gazebo/rendering/LinkFrameVisual.hh"
+#include "gazebo/test/ServerFixture.hh"
+
+using namespace gazebo;
+class LinkFrameVisual_TEST : public ServerFixture
+{
+};
+
+/////////////////////////////////////////////////
+TEST_F(LinkFrameVisual_TEST, LinkFrameTest)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene("default");
+
+ if (!scene)
+ scene = gazebo::rendering::create_scene("default", false);
+
+ EXPECT_TRUE(scene != NULL);
+
+ // create a link visual
+ gazebo::rendering::VisualPtr linkVis;
+ linkVis.reset(
+ new gazebo::rendering::Visual("link", scene->GetWorldVisual()));
+
+ // create frame visual for the link
+ gazebo::rendering::LinkFrameVisualPtr linkFrameVis(
+ new gazebo::rendering::LinkFrameVisual("_LINK_FRAME_VISUAL_",
+ linkVis));
+ linkFrameVis->Load();
+
+ // Check that it was added to the scene (by Load)
+ EXPECT_EQ(scene->GetVisual("_LINK_FRAME_VISUAL_"), linkFrameVis);
+
+ // Check that it has type physics
+ EXPECT_EQ(linkFrameVis->GetType(), gazebo::rendering::Visual::VT_PHYSICS);
+
+ // Check that the link visual is the parent
+ EXPECT_EQ(linkFrameVis->GetParent(), linkVis);
+
+ // Check that the pose within the link visual (local pose) is zero
+ EXPECT_EQ(linkFrameVis->GetPose(), gazebo::math::Pose::Zero);
+
+ // Check that frame is not highlighted
+ EXPECT_FALSE(linkFrameVis->GetHighlighted());
+
+ // Set highlighted
+ linkFrameVis->SetHighlighted(true);
+
+ // Check that frame is highlighted
+ EXPECT_TRUE(linkFrameVis->GetHighlighted());
+
+ // Remove it from the scene (Fini is called)
+ scene->RemoveVisual(linkFrameVis);
+
+ // Check that it was removed
+ EXPECT_TRUE(scene->GetVisual("_LINK_FRAME_VISUAL_") == NULL);
+
+ // Reset pointer
+ linkFrameVis.reset();
+ EXPECT_TRUE(linkFrameVis == NULL);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/rendering/Material.cc b/gazebo/rendering/Material.cc
index 06126f8..0162d8f 100644
--- a/gazebo/rendering/Material.cc
+++ b/gazebo/rendering/Material.cc
@@ -17,6 +17,7 @@
#include "gazebo/common/Color.hh"
#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/common/Console.hh"
+#include "gazebo/rendering/Conversions.hh"
#include "gazebo/rendering/Material.hh"
using namespace gazebo;
@@ -157,6 +158,7 @@ void Material::Update(const gazebo::common::Material *_mat)
common::Color specular = _mat->GetSpecular();
common::Color emissive = _mat->GetEmissive();
+
pass->setLightingEnabled(_mat->GetLighting());
pass->setDiffuse(diffuse.r, diffuse.g, diffuse.b, diffuse.a);
pass->setAmbient(ambient.r, ambient.g, ambient.b);
@@ -182,3 +184,36 @@ void Material::Update(const gazebo::common::Material *_mat)
texState->setName(_mat->GetTextureImage());
}
}
+
+//////////////////////////////////////////////////
+bool Material::GetMaterialAsColor(const std::string &_materialName,
+ common::Color &_ambient, common::Color &_diffuse,
+ common::Color &_specular, common::Color &_emissive)
+{
+ Ogre::MaterialPtr matPtr;
+
+ if (Ogre::MaterialManager::getSingleton().resourceExists(_materialName))
+ {
+ matPtr = Ogre::MaterialManager::getSingleton().getByName(_materialName,
+ "General");
+
+ if (matPtr.isNull())
+ return false;
+
+ Ogre::Technique *technique = matPtr->getTechnique(0);
+ if (technique)
+ {
+ Ogre::Pass *pass = technique->getPass(0);
+ if (pass)
+ {
+ _ambient = Conversions::Convert(pass->getAmbient());
+ _diffuse = Conversions::Convert(pass->getDiffuse());
+ _specular = Conversions::Convert(pass->getSpecular());
+ _emissive = Conversions::Convert(pass->getSelfIllumination());
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
diff --git a/gazebo/rendering/Material.hh b/gazebo/rendering/Material.hh
index bc733e5..3202eff 100644
--- a/gazebo/rendering/Material.hh
+++ b/gazebo/rendering/Material.hh
@@ -17,6 +17,8 @@
#ifndef _RENDERING_MATERIAL_HH_
#define _RENDERING_MATERIAL_HH_
+#include <string>
+
#include "gazebo/common/Material.hh"
#include "gazebo/util/system.hh"
@@ -27,7 +29,7 @@ namespace gazebo
{
/// \class Material Material.hh rendering/rendering.hh
/// \brief An internal class used by Visuals to add materials to Ogre.
- class GAZEBO_VISIBLE Material
+ class GZ_RENDERING_VISIBLE Material
{
/// \brief Create all the default materials
public: static void CreateMaterials();
@@ -35,6 +37,17 @@ namespace gazebo
/// \brief Update the Ogre materials from a Gazebo material.
/// \param[in] _mat The Gazebo material to add to the Ogre system.
public: static void Update(const gazebo::common::Material *_mat);
+
+ /// \brief Get the color of the material.
+ /// \param[in] _materialName Name of the material.
+ /// \param[out] _ambient Ambient color of the material.
+ /// \param[out] _diffuse Diffuse color of the material.
+ /// \param[out] _specular Specular color of the material.
+ /// \param[out] _emissive Emissive color of the material.
+ /// \return True if the material found, false otherwise.
+ public: static bool GetMaterialAsColor(const std::string &_materialName,
+ common::Color &_ambient, common::Color &_diffuse,
+ common::Color &_specular, common::Color &_emissive);
};
}
}
diff --git a/gazebo/rendering/MovableText.cc b/gazebo/rendering/MovableText.cc
index 4b2520c..602293f 100644
--- a/gazebo/rendering/MovableText.cc
+++ b/gazebo/rendering/MovableText.cc
@@ -24,6 +24,11 @@
* @update 2007 by independentCreations see independentCreations at gmail.com
*/
+#ifdef _WIN32
+ // Ensure that windows types like LONGLONG are defined
+ #include <windows.h>
+#endif
+
#include <boost/thread/recursive_mutex.hpp>
#include "gazebo/common/common.hh"
diff --git a/gazebo/rendering/MovableText.hh b/gazebo/rendering/MovableText.hh
index 80d28d1..0217a25 100644
--- a/gazebo/rendering/MovableText.hh
+++ b/gazebo/rendering/MovableText.hh
@@ -46,7 +46,7 @@ namespace gazebo
/// \class MovableText MovableText.hh rendering/rendering.hh
/// \brief Movable text
- class GAZEBO_VISIBLE MovableText
+ class GZ_RENDERING_VISIBLE MovableText
: public Ogre::MovableObject, public Ogre::Renderable
{
/// \enum HorizAlign
diff --git a/gazebo/rendering/OculusCamera.cc b/gazebo/rendering/OculusCamera.cc
index 4f87b16..5c3ae89 100644
--- a/gazebo/rendering/OculusCamera.cc
+++ b/gazebo/rendering/OculusCamera.cc
@@ -15,7 +15,6 @@
*
*/
-#include <OVR.h>
#include <sstream>
#include <string>
@@ -26,91 +25,121 @@
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Events.hh"
+#include "gazebo/rendering/skyx/include/SkyX.h"
#include "gazebo/rendering/selection_buffer/SelectionBuffer.hh"
#include "gazebo/rendering/RenderEngine.hh"
#include "gazebo/rendering/Conversions.hh"
-#include "gazebo/rendering/WindowManager.hh"
-#include "gazebo/rendering/FPSViewController.hh"
-#include "gazebo/rendering/OrbitViewController.hh"
#include "gazebo/rendering/RenderTypes.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/RTShaderSystem.hh"
#include "gazebo/rendering/Camera.hh"
#include "gazebo/rendering/Visual.hh"
-#include "gazebo/rendering/DynamicLines.hh"
+#include "gazebo/rendering/OculusCameraPrivate.hh"
#include "gazebo/rendering/OculusCamera.hh"
-
using namespace gazebo;
using namespace rendering;
-const float g_defaultNearClip = 0.1f;
-const float g_defaultFarClip = 5000.0f;
-const float g_defaultIPD = 0.064f;
-const float g_defautlProjectionCenterOffset = 0.14529906f;
-const float g_defaultDistortion[4] = {1.0f, 0.22f, 0.24f, 0};
+const float g_defaultNearClip = 0.01f;
+const float g_defaultFarClip = 500.0f;
//////////////////////////////////////////////////
OculusCamera::OculusCamera(const std::string &_name, ScenePtr _scene)
- : Camera(_name, _scene)
+ : Camera(_name, _scene), dataPtr(new OculusCameraPrivate)
{
- // Oculus is not ready yet.
- this->ready = false;
-
- // Set default OculusCamera render rate to 30Hz
- this->SetRenderRate(30.0);
-
- OVR::System::Init(OVR::Log::ConfigureDefaultLog(OVR::LogMask_None));
-
- this->deviceManager = OVR::DeviceManager::Create();
- if (!this->deviceManager)
+ ovr_Initialize();
+ this->dataPtr->hmd = ovrHmd_Create(0);
+ if (!this->dataPtr->hmd)
+ {
+ gzerr << "Oculus Rift not detected. "
+ << "Oculus error["
+ << ovrHmd_GetLastError(NULL) << "]. "
+ << "Is the oculusd service running?\n"
+ << "Did you copy the udev rules from the oculussdk repo?\n"
+ << "See: http://gazebosim.org/tutorials?tut=oculus"
+ << std::endl;
return;
+ }
- this->stereoConfig = new OVR::Util::Render::StereoConfig();
- if (!this->stereoConfig)
+ if (this->dataPtr->hmd->ProductName[0] == '\0')
+ {
+ gzerr << "Oculus Rift detected, display not enabled. "
+ << "Oculus error["
+ << ovrHmd_GetLastError(NULL) << "]. "
+ << std::endl;
return;
+ }
- this->centerOffset = this->stereoConfig->GetProjectionCenterOffset();
-
- this->hmd =
- this->deviceManager->EnumerateDevices<OVR::HMDDevice>().CreateDevice();
- if (this->hmd)
+ // These are the suggested refresh rates for dk1 and dk2
+ // dk1: 60Hz
+ // dk2: 75Hz
+ switch (this->dataPtr->hmd->Type)
{
- OVR::HMDInfo devinfo;
- this->hmd->GetDeviceInfo(&devinfo);
- this->stereoConfig->SetHMDInfo(devinfo);
- this->sensor = this->hmd->GetSensor();
- }
- else
+ case ovrHmd_DK1:
+ // A little bit extra for safety
+ this->SetRenderRate(70.0);
+ break;
+ case ovrHmd_DK2:
+ // A little bit extra for safety
+ this->SetRenderRate(80.0);
+ break;
+ case ovrHmd_None:
+ gzerr << "Unable to handle Oculus with type 'None'\n";
+ return;
+ case ovrHmd_DKHD:
+ gzerr << "Unable to handle Oculus with type 'DKHD'\n";
+ return;
+ case ovrHmd_Other:
+ gzerr << "Unable to handle Oculus with type 'Other'\n";
+ return;
+ default:
+ gzerr << "Unknown Oculus type '" << this->dataPtr->hmd->Type << "'\n";
+ return;
+ };
+
+ // Log some useful information
+ gzmsg << "Oculus Rift found." << std::endl;
+ gzmsg << "\tType: " << this->dataPtr->hmd->Type << std::endl;
+ gzmsg << "\tProduct Name: " << this->dataPtr->hmd->ProductName << std::endl;
+ gzmsg << "\tProduct ID: " << this->dataPtr->hmd->ProductId << std::endl;
+ gzmsg << "\tFirmware: " << this->dataPtr->hmd->FirmwareMajor << "."
+ << this->dataPtr->hmd->FirmwareMinor << std::endl;
+ gzmsg << "\tResolution: " << this->dataPtr->hmd->Resolution.w << "x"
+ << this->dataPtr->hmd->Resolution.h << std::endl;
+ gzmsg << "\tPosition tracking: "
+ << (this->dataPtr->hmd->TrackingCaps & ovrTrackingCap_Position)
+ << std::endl;
+
+ // Start the sensor which informs of the Rift's pose and motion
+ if (!ovrHmd_ConfigureTracking(this->dataPtr->hmd, ovrTrackingCap_Orientation
+ | ovrTrackingCap_MagYawCorrection | ovrTrackingCap_Position, 0))
{
- this->sensor =
- this->deviceManager->EnumerateDevices<OVR::SensorDevice>().CreateDevice();
+ gzerr << "Tracking capability not found. "
+ << "The Oculus viewpoint will not move.\n";
}
- if (!this->sensor)
- return;
-
- this->sensorFusion = new OVR::SensorFusion();
- if (!sensorFusion)
- return;
-
- this->sensorFusion->AttachToSensor(this->sensor);
- this->sensorFusion->SetPredictionEnabled(true);
-
- this->node = transport::NodePtr(new transport::Node());
- this->node->Init();
+ this->dataPtr->node = transport::NodePtr(new transport::Node());
+ this->dataPtr->node->Init();
- this->controlSub = this->node->Subscribe("~/world_control",
+ this->dataPtr->controlSub = this->dataPtr->node->Subscribe("~/world_control",
&OculusCamera::OnControl, this);
// Oculus is now ready.
- this->ready = true;
+ this->dataPtr->ready = true;
}
//////////////////////////////////////////////////
OculusCamera::~OculusCamera()
{
+ RenderEngine::Instance()->root->destroySceneManager(
+ this->dataPtr->externalSceneManager);
+
+ ovrHmd_Destroy(this->dataPtr->hmd);
+ ovr_Shutdown();
+
this->connections.clear();
+ delete this->dataPtr;
+ this->dataPtr = NULL;
}
//////////////////////////////////////////////////
@@ -144,25 +173,23 @@ void OculusCamera::Init()
Camera::Init();
- this->SetHFOV(GZ_DTOR(60));
-
// Oculus
{
- this->rightCamera = this->scene->GetManager()->createCamera(
+ this->dataPtr->rightCamera = this->scene->GetManager()->createCamera(
"OculusUserRight");
- this->rightCamera->pitch(Ogre::Degree(90));
+ this->dataPtr->rightCamera->pitch(Ogre::Degree(90));
// Don't yaw along variable axis, causes leaning
- this->rightCamera->setFixedYawAxis(true, Ogre::Vector3::UNIT_Z);
- this->rightCamera->setDirection(1, 0, 0);
+ this->dataPtr->rightCamera->setFixedYawAxis(true, Ogre::Vector3::UNIT_Z);
+ this->dataPtr->rightCamera->setDirection(1, 0, 0);
- this->sceneNode->attachObject(this->rightCamera);
+ this->sceneNode->attachObject(this->dataPtr->rightCamera);
- this->rightCamera->setAutoAspectRatio(false);
+ this->dataPtr->rightCamera->setAutoAspectRatio(false);
this->camera->setAutoAspectRatio(false);
- this->rightCamera->setNearClipDistance(g_defaultNearClip);
- this->rightCamera->setFarClipDistance(g_defaultFarClip);
+ this->dataPtr->rightCamera->setNearClipDistance(g_defaultNearClip);
+ this->dataPtr->rightCamera->setFarClipDistance(g_defaultFarClip);
this->camera->setNearClipDistance(g_defaultNearClip);
this->camera->setFarClipDistance(g_defaultFarClip);
@@ -191,6 +218,17 @@ void OculusCamera::Init()
}
//////////////////////////////////////////////////
+void OculusCamera::RenderImpl()
+{
+ ovrHmd_BeginFrameTiming(this->dataPtr->hmd, this->dataPtr->frameIndex);
+ this->dataPtr->renderTextureLeft->getBuffer()->getRenderTarget()->update();
+ this->dataPtr->renderTextureRight->getBuffer()->getRenderTarget()->update();
+ this->renderTarget->update();
+ ovrHmd_EndFrameTiming(this->dataPtr->hmd);
+ this->dataPtr->frameIndex++;
+}
+
+//////////////////////////////////////////////////
void OculusCamera::Update()
{
if (!this->Ready())
@@ -198,10 +236,37 @@ void OculusCamera::Update()
Camera::Update();
- OVR::Quatf q = this->sensorFusion->GetPredictedOrientation();
+ ovrFrameTiming frameTiming = ovrHmd_GetFrameTiming(this->dataPtr->hmd,
+ this->dataPtr->frameIndex);
+
+ ovrTrackingState ts = ovrHmd_GetTrackingState(
+ this->dataPtr->hmd, frameTiming.ScanoutMidpointSeconds);
+
+ // Only doing orientation tracking for now. Position tracking is an option
+ // for dk2
+ if (ts.StatusFlags & ovrStatus_OrientationTracked)
+ {
+ if (this->dataPtr->oculusTrackingWarned)
+ {
+ gzmsg << "Oculus: Head tracking enabled.\n";
+ this->dataPtr->oculusTrackingWarned = false;
+ }
- // Set the orientation, and correct for the oculus coordinate system
- this->sceneNode->setOrientation(Ogre::Quaternion(q.w, -q.z, -q.x, q.y));
+ ovrPosef ovrpose = ts.HeadPose.ThePose;
+ this->sceneNode->setOrientation(Ogre::Quaternion(
+ ovrpose.Orientation.w,
+ -ovrpose.Orientation.z,
+ -ovrpose.Orientation.x,
+ ovrpose.Orientation.y));
+ }
+ else if (!this->dataPtr->oculusTrackingWarned)
+ {
+ gzwarn << "Oculus: No head tracking.\n\t"
+ << "If you do not see a following message about 'Head tracking enabled'"
+ << ", then try rebooting while leaving the Oculus turned on."
+ << std::endl;
+ this->dataPtr->oculusTrackingWarned = true;
+ }
this->sceneNode->needUpdate();
}
@@ -209,13 +274,12 @@ void OculusCamera::Update()
//////////////////////////////////////////////////
void OculusCamera::ResetSensor()
{
- this->sensorFusion->Reset();
}
//////////////////////////////////////////////////
bool OculusCamera::Ready()
{
- return this->ready;
+ return this->dataPtr->ready;
}
//////////////////////////////////////////////////
@@ -288,7 +352,7 @@ void OculusCamera::Resize(unsigned int /*_w*/, unsigned int /*_h*/)
if (this->viewport)
{
this->viewport->setDimensions(0, 0, 0.5, 1);
- this->rightViewport->setDimensions(0.5, 0, 0.5, 1);
+ this->dataPtr->rightViewport->setDimensions(0.5, 0, 0.5, 1);
delete [] this->saveFrameBuffer;
this->saveFrameBuffer = NULL;
@@ -296,20 +360,6 @@ void OculusCamera::Resize(unsigned int /*_w*/, unsigned int /*_h*/)
}
//////////////////////////////////////////////////
-float OculusCamera::GetAvgFPS() const
-{
- return RenderEngine::Instance()->GetWindowManager()->GetAvgFPS(
- this->windowId);
-}
-
-//////////////////////////////////////////////////
-unsigned int OculusCamera::GetTriangleCount() const
-{
- return RenderEngine::Instance()->GetWindowManager()->GetTriangleCount(
- this->windowId);
-}
-
-//////////////////////////////////////////////////
bool OculusCamera::MoveToPosition(const math::Pose &_pose, double _time)
{
return Camera::MoveToPosition(_pose, _time);
@@ -401,24 +451,89 @@ void OculusCamera::MoveToVisual(VisualPtr _visual)
//////////////////////////////////////////////////
void OculusCamera::SetRenderTarget(Ogre::RenderTarget *_target)
{
- Camera::SetRenderTarget(_target);
+ this->renderTarget = _target;
+ this->Oculus();
- this->viewport->setDimensions(0, 0, 0.5, 1.0);
+ Ogre::RenderTexture *rt =
+ this->dataPtr->renderTextureLeft->getBuffer()->getRenderTarget();
- this->rightViewport =
- this->renderTarget->addViewport(this->rightCamera, 1,
- 0.5f, 0, 0.5f, 1.0f);
- this->rightViewport->setBackgroundColour(
+ rt->addViewport(this->camera);
+ rt->getViewport(0)->setClearEveryFrame(true);
+ rt->getViewport(0)->setOverlaysEnabled(false);
+ rt->getViewport(0)->setShadowsEnabled(true);
+ rt->getViewport(0)->setBackgroundColour(
Conversions::Convert(this->scene->GetBackgroundColor()));
-
- RTShaderSystem::AttachViewport(this->rightViewport, this->GetScene());
-
- this->viewport->setVisibilityMask(GZ_VISIBILITY_ALL);
- this->rightViewport->setVisibilityMask(GZ_VISIBILITY_ALL);
+ rt->getViewport(0)->setVisibilityMask(GZ_VISIBILITY_ALL &
+ ~(GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE));
+ RTShaderSystem::AttachViewport(rt->getViewport(0), this->GetScene());
+
+ if (this->GetScene()->GetSkyX() != NULL)
+ rt->addListener(this->GetScene()->GetSkyX());
+
+ rt = this->dataPtr->renderTextureRight->getBuffer()->getRenderTarget();
+ rt->addViewport(this->dataPtr->rightCamera);
+ rt->getViewport(0)->setClearEveryFrame(true);
+ rt->getViewport(0)->setShadowsEnabled(true);
+ rt->getViewport(0)->setOverlaysEnabled(false);
+ rt->getViewport(0)->setBackgroundColour(
+ Conversions::Convert(this->scene->GetBackgroundColor()));
+ rt->getViewport(0)->setVisibilityMask(GZ_VISIBILITY_ALL &
+ ~(GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE));
+ RTShaderSystem::AttachViewport(rt->getViewport(0), this->GetScene());
+
+ if (this->GetScene()->GetSkyX() != NULL)
+ rt->addListener(this->GetScene()->GetSkyX());
+
+ ovrFovPort fovLeft = this->dataPtr->hmd->DefaultEyeFov[ovrEye_Left];
+ ovrFovPort fovRight = this->dataPtr->hmd->DefaultEyeFov[ovrEye_Right];
+
+ float combinedTanHalfFovHorizontal =
+ std::max(fovLeft.LeftTan, fovLeft.RightTan);
+ float combinedTanHalfFovVertical = std::max(fovLeft.UpTan, fovLeft.DownTan);
+
+ float aspectRatio = combinedTanHalfFovHorizontal / combinedTanHalfFovVertical;
+
+ this->camera->setAspectRatio(aspectRatio);
+ this->dataPtr->rightCamera->setAspectRatio(aspectRatio);
+
+ ovrMatrix4f projL = ovrMatrix4f_Projection(fovLeft, 0.001, 500.0, true);
+ ovrMatrix4f projR = ovrMatrix4f_Projection(fovRight, 0.001, 500.0, true);
+
+ this->camera->setCustomProjectionMatrix(true,
+ Ogre::Matrix4(
+ projL.M[0][0], projL.M[0][1], projL.M[0][2], projL.M[0][3],
+ projL.M[1][0], projL.M[1][1], projL.M[1][2], projL.M[1][3],
+ projL.M[2][0], projL.M[2][1], projL.M[2][2], projL.M[2][3],
+ projL.M[3][0], projL.M[3][1], projL.M[3][2], projL.M[3][3]));
+
+ this->dataPtr->rightCamera->setCustomProjectionMatrix(true,
+ Ogre::Matrix4(
+ projR.M[0][0], projR.M[0][1], projR.M[0][2], projR.M[0][3],
+ projR.M[1][0], projR.M[1][1], projR.M[1][2], projR.M[1][3],
+ projR.M[2][0], projR.M[2][1], projR.M[2][2], projR.M[2][3],
+ projR.M[3][0], projR.M[3][1], projR.M[3][2], projR.M[3][3]));
+
+ // This seems like a mistake, but it's here on purpose.
+ // Problem: Shadows get rendered incorrectly with
+ // setCustomProjectionMatrix.
+ // Solution (HACK): Pass false to setCustomProjectionMatrix. Found this
+ // solution here: http://www.ogre3d.org/forums/viewtopic.php?f=2&t=60904
+ // and here: http://www.ogre3d.org/forums/viewtopic.php?f=2&t=78461
+ this->camera->setCustomProjectionMatrix(false,
+ Ogre::Matrix4(
+ projL.M[0][0], projL.M[0][1], projL.M[0][2], projL.M[0][3],
+ projL.M[1][0], projL.M[1][1], projL.M[1][2], projL.M[1][3],
+ projL.M[2][0], projL.M[2][1], projL.M[2][2], projL.M[2][3],
+ projL.M[3][0], projL.M[3][1], projL.M[3][2], projL.M[3][3]));
+
+ this->dataPtr->rightCamera->setCustomProjectionMatrix(false,
+ Ogre::Matrix4(
+ projR.M[0][0], projR.M[0][1], projR.M[0][2], projR.M[0][3],
+ projR.M[1][0], projR.M[1][1], projR.M[1][2], projR.M[1][3],
+ projR.M[2][0], projR.M[2][1], projR.M[2][2], projR.M[2][3],
+ projR.M[3][0], projR.M[3][1], projR.M[3][2], projR.M[3][3]));
this->initialized = true;
-
- this->Oculus();
}
//////////////////////////////////////////////////
@@ -427,103 +542,211 @@ void OculusCamera::Oculus()
if (!this->Ready())
return;
+ // Create a separate scene manager to holds a distorted mesh and a camera.
+ // The distorted mesh receives the left and right camera images, and the
+ // camera in the externalSceneManager renders the distorted meshes.
+ this->dataPtr->externalSceneManager =
+ RenderEngine::Instance()->root->createSceneManager(Ogre::ST_GENERIC);
+ this->dataPtr->externalSceneManager->setAmbientLight(
+ Ogre::ColourValue(0.5, 0.5, 0.5));
+
+ // Get the texture sizes
+ ovrSizei textureSizeLeft = ovrHmd_GetFovTextureSize(this->dataPtr->hmd,
+ ovrEye_Left, this->dataPtr->hmd->DefaultEyeFov[0], 1.0f);
+ ovrSizei textureSizeRight = ovrHmd_GetFovTextureSize(this->dataPtr->hmd,
+ ovrEye_Right, this->dataPtr->hmd->DefaultEyeFov[1], 1.0f);
+
+ // Create the left and right render textures.
+ this->dataPtr->renderTextureLeft =
+ Ogre::TextureManager::getSingleton().createManual(
+ "OculusRiftRenderTextureLeft",
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ Ogre::TEX_TYPE_2D,
+ textureSizeLeft.w,
+ textureSizeLeft.h,
+ 0,
+ Ogre::PF_R8G8B8,
+ Ogre::TU_RENDERTARGET);
+
+ this->dataPtr->renderTextureRight =
+ Ogre::TextureManager::getSingleton().createManual(
+ "OculusRiftRenderTextureRight",
+ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ Ogre::TEX_TYPE_2D,
+ textureSizeRight.w,
+ textureSizeRight.h,
+ 0,
+ Ogre::PF_R8G8B8,
+ Ogre::TU_RENDERTARGET);
+
+ // Create the left and right materials.
Ogre::MaterialPtr matLeft =
- Ogre::MaterialManager::getSingleton().getByName("Ogre/Compositor/Oculus");
- Ogre::MaterialPtr matRight = matLeft->clone("Ogre/Compositor/Oculus/Right");
-
- Ogre::GpuProgramParametersSharedPtr pParamsLeft =
- matLeft->getTechnique(0)->getPass(0)->getFragmentProgramParameters();
- Ogre::GpuProgramParametersSharedPtr pParamsRight =
- matRight->getTechnique(0)->getPass(0)->getFragmentProgramParameters();
- Ogre::Vector4 hmdwarp;
-
- if (this->stereoConfig)
+ Ogre::MaterialManager::getSingleton().getByName("Oculus/LeftEye");
+ Ogre::MaterialPtr matRight =
+ Ogre::MaterialManager::getSingleton().getByName("Oculus/RightEye");
+
+ // Attach materials to the render textures.
+ matLeft->getTechnique(0)->getPass(0)->getTextureUnitState(0)->setTexture(
+ this->dataPtr->renderTextureLeft);
+ matRight->getTechnique(0)->getPass(0)->getTextureUnitState(0)->setTexture(
+ this->dataPtr->renderTextureRight);
+
+ ovrFovPort fovLeft = this->dataPtr->hmd->DefaultEyeFov[ovrEye_Left];
+ ovrFovPort fovRight = this->dataPtr->hmd->DefaultEyeFov[ovrEye_Right];
+
+ // Get eye description information
+ ovrEyeRenderDesc eyeRenderDesc[2];
+ eyeRenderDesc[0] = ovrHmd_GetRenderDesc(
+ this->dataPtr->hmd, ovrEye_Left, fovLeft);
+ eyeRenderDesc[1] = ovrHmd_GetRenderDesc(
+ this->dataPtr->hmd, ovrEye_Right, fovRight);
+
+ double combinedTanHalfFovHorizontal = std::max(
+ std::max(fovLeft.LeftTan, fovLeft.RightTan),
+ std::max(fovRight.LeftTan, fovRight.RightTan));
+ double combinedTanHalfFovVertical = std::max(
+ std::max(fovLeft.UpTan, fovLeft.DownTan),
+ std::max(fovRight.UpTan, fovRight.DownTan));
+
+ // Hold some values that are needed when creating the distortion meshes.
+ ovrVector2f uvScaleOffset[2];
+ ovrRecti viewports[2];
+ viewports[0].Pos.x = 0;
+ viewports[0].Pos.y = 0;
+ viewports[0].Size.w = textureSizeLeft.w;
+ viewports[0].Size.h = textureSizeLeft.h;
+ viewports[1].Pos.x = textureSizeLeft.w;
+ viewports[1].Pos.y = 0;
+ viewports[1].Size.w = textureSizeRight.w;
+ viewports[1].Size.h = textureSizeRight.h;
+
+ // Create a scene node in the external scene to hold the distortion
+ // meshes.
+ Ogre::SceneNode *meshNode =
+ this->dataPtr->externalSceneManager->getRootSceneNode()
+ ->createChildSceneNode();
+
+ // Create the Distortion Meshes:
+ for (int eyeIndex = 0; eyeIndex < 2; ++eyeIndex)
{
- hmdwarp = Ogre::Vector4(this->stereoConfig->GetDistortionK(0),
- this->stereoConfig->GetDistortionK(1),
- this->stereoConfig->GetDistortionK(2),
- this->stereoConfig->GetDistortionK(3));
- }
- else
- {
- hmdwarp = Ogre::Vector4(g_defaultDistortion[0],
- g_defaultDistortion[1],
- g_defaultDistortion[2],
- g_defaultDistortion[3]);
- }
+ ovrDistortionMesh meshData;
- pParamsLeft->setNamedConstant("HmdWarpParam", hmdwarp);
- pParamsRight->setNamedConstant("HmdWarpParam", hmdwarp);
+ // Make the FOV symmetrical. Refer to Section 8.5.2 of the oculus SDF
+ // developers manual.
+ eyeRenderDesc[eyeIndex].Fov.RightTan = combinedTanHalfFovHorizontal;
+ eyeRenderDesc[eyeIndex].Fov.LeftTan = combinedTanHalfFovHorizontal;
+ eyeRenderDesc[eyeIndex].Fov.UpTan = combinedTanHalfFovVertical;
+ eyeRenderDesc[eyeIndex].Fov.DownTan = combinedTanHalfFovVertical;
- Ogre::Vector4 hmdchrom;
- if (this->stereoConfig)
- {
- hmdchrom = Ogre::Vector4(
- this->stereoConfig->GetHMDInfo().ChromaAbCorrection);
- }
- else
- {
- hmdchrom = Ogre::Vector4(0.996, -0.004, 1.014, 0.0f);
- }
-
- pParamsLeft->setNamedConstant("ChromAbParam", hmdchrom);
- pParamsRight->setNamedConstant("ChromAbParam", hmdchrom);
-
- pParamsLeft->setNamedConstant("LensCenter", 0.5f +
- (this->stereoConfig->GetProjectionCenterOffset()/2.0f));
+ ovrHmd_CreateDistortionMesh(
+ this->dataPtr->hmd,
+ eyeRenderDesc[eyeIndex].Eye,
+ eyeRenderDesc[eyeIndex].Fov,
+ 0,
+ &meshData);
- pParamsRight->setNamedConstant("LensCenter", 0.5f -
- (this->stereoConfig->GetProjectionCenterOffset()/2.0f));
+ Ogre::GpuProgramParametersSharedPtr params;
- Ogre::CompositorPtr comp =
- Ogre::CompositorManager::getSingleton().getByName("OculusRight");
- comp->getTechnique(0)->getOutputTargetPass()->getPass(0)->setMaterialName(
- "Ogre/Compositor/Oculus/Right");
+ if (eyeIndex == 0)
+ {
+ ovrHmd_GetRenderScaleAndOffset(eyeRenderDesc[eyeIndex].Fov,
+ textureSizeLeft, viewports[eyeIndex], uvScaleOffset);
- for (int i = 0; i < 2; ++i)
- {
- Ogre::Camera *cam = i == 0 ? this->camera : this->rightCamera;
+ params =
+ matLeft->getTechnique(0)->getPass(0)->getVertexProgramParameters();
+ }
+ else
+ {
+ ovrHmd_GetRenderScaleAndOffset(eyeRenderDesc[eyeIndex].Fov,
+ textureSizeRight, viewports[eyeIndex], uvScaleOffset);
+ params =
+ matRight->getTechnique(0)->getPass(0)->getVertexProgramParameters();
+ }
- int idx = i * 2 - 1;
- if (this->stereoConfig)
+#if OGRE_VERSION_MAJOR == 1 && OGRE_VERSION_MINOR >= 9
+ params->setNamedConstant("eyeToSourceUVScale",
+ Ogre::Vector2(uvScaleOffset[0].x, uvScaleOffset[0].y));
+ params->setNamedConstant("eyeToSourceUVOffset",
+ Ogre::Vector2(uvScaleOffset[1].x, uvScaleOffset[1].y));
+#else
+ params->setNamedConstant("eyeToSourceUVScale",
+ Ogre::Vector4(uvScaleOffset[0].x, uvScaleOffset[0].y, 0, 0));
+ params->setNamedConstant("eyeToSourceUVOffset",
+ Ogre::Vector4(uvScaleOffset[1].x, uvScaleOffset[1].y, 0, 0));
+#endif
+ Ogre::ManualObject *externalObj;
+
+ // create ManualObject
+ if (eyeIndex == 0)
{
- // Setup cameras.
- cam->setNearClipDistance(this->stereoConfig->GetEyeToScreenDistance());
- cam->setFarClipDistance(g_defaultFarClip);
- cam->setPosition(0, idx * this->stereoConfig->GetIPD() * 0.5f * -1.0, 0);
- cam->setAspectRatio(this->stereoConfig->GetAspect());
- cam->setFOVy(Ogre::Radian(this->stereoConfig->GetYFOVRadians()));
-
- // Oculus requires offset projection, create a custom projection matrix
- Ogre::Matrix4 proj = Ogre::Matrix4::IDENTITY;
- proj.setTrans(Ogre::Vector3(
- -this->stereoConfig->GetProjectionCenterOffset() * idx, 0, 0));
- cam->setCustomProjectionMatrix(true, proj * cam->getProjectionMatrix());
+ externalObj =
+ this->dataPtr->externalSceneManager->createManualObject(
+ "OculusRiftRenderObjectLeft");
+ externalObj->begin("Oculus/LeftEye",
+ Ogre::RenderOperation::OT_TRIANGLE_LIST);
}
else
{
- cam->setNearClipDistance(g_defaultNearClip);
- cam->setFarClipDistance(g_defaultFarClip);
- cam->setPosition(idx * g_defaultIPD * 0.5f, 0, 0);
+ externalObj =
+ this->dataPtr->externalSceneManager->createManualObject(
+ "OculusRiftRenderObjectRight");
+ externalObj->begin("Oculus/RightEye",
+ Ogre::RenderOperation::OT_TRIANGLE_LIST);
}
- if (i == 0)
+ for (unsigned int i = 0; i < meshData.VertexCount; ++i)
{
- this->compositors[i] =
- Ogre::CompositorManager::getSingleton().addCompositor(
- this->viewport, "OculusLeft");
- if (!this->compositors[i])
- gzerr << "Invalid compositor\n";
- this->compositors[i]->setEnabled(true);
+ ovrDistortionVertex v = meshData.pVertexData[i];
+ externalObj->position(v.ScreenPosNDC.x, v.ScreenPosNDC.y, 0);
+ externalObj->textureCoord(v.TanEyeAnglesR.x, v.TanEyeAnglesR.y);
+ externalObj->textureCoord(v.TanEyeAnglesG.x, v.TanEyeAnglesG.y);
+ externalObj->textureCoord(v.TanEyeAnglesB.x, v.TanEyeAnglesB.y);
+
+ float vig = std::max(v.VignetteFactor, 0.0f);
+ externalObj->colour(vig, vig, vig, vig);
}
- else
+
+ for (unsigned int i = 0; i < meshData.IndexCount; ++i)
{
- this->compositors[i] =
- Ogre::CompositorManager::getSingleton().addCompositor(
- this->rightViewport, "OculusRight");
- this->compositors[i]->setEnabled(true);
+ externalObj->index(meshData.pIndexData[i]);
}
+
+ // Manual render object complete
+ externalObj->end();
+
+ // Attach externalObj object to the node
+ meshNode->attachObject(externalObj);
+
+ // Free up memory
+ ovrHmd_DestroyDistortionMesh(&meshData);
}
+
+ // Position the node in the scene
+ meshNode->setPosition(0, 0, -1);
+ meshNode->setScale(1, 1, -1);
+
+ // Create the external camera
+ this->dataPtr->externalCamera =
+ this->dataPtr->externalSceneManager->createCamera(
+ "_OculusRiftExternalCamera_INTERNAL_");
+ this->dataPtr->externalCamera->setFarClipDistance(50);
+ this->dataPtr->externalCamera->setNearClipDistance(0.001);
+ this->dataPtr->externalCamera->setProjectionType(Ogre::PT_ORTHOGRAPHIC);
+ this->dataPtr->externalCamera->setOrthoWindow(2, 2);
+ this->dataPtr->externalSceneManager->getRootSceneNode()->attachObject(
+ this->dataPtr->externalCamera);
+
+ // Create the external viewport
+ this->dataPtr->externalViewport = this->renderTarget->addViewport(
+ this->dataPtr->externalCamera);
+ this->dataPtr->externalViewport->setBackgroundColour(
+ Ogre::ColourValue::Black);
+ this->dataPtr->externalViewport->setOverlaysEnabled(true);
+
+ // Set up IPD in meters:
+ float ipd = ovrHmd_GetFloat(this->dataPtr->hmd, OVR_KEY_IPD, 0.064f);
+ this->camera->setPosition(-ipd * 0.5, 0, 0);
+ this->dataPtr->rightCamera->setPosition(ipd * 0.5, 0, 0);
}
/////////////////////////////////////////////////
@@ -534,7 +757,7 @@ void OculusCamera::AdjustAspect(double _v)
for (int i = 0; i < 2; ++i)
{
- Ogre::Camera *cam = i == 0 ? this->camera : this->rightCamera;
+ Ogre::Camera *cam = i == 0 ? this->camera : this->dataPtr->rightCamera;
cam->setAspectRatio(cam->getAspectRatio() + _v);
}
}
diff --git a/gazebo/rendering/OculusCamera.hh b/gazebo/rendering/OculusCamera.hh
index 5c11b68..34d7f31 100644
--- a/gazebo/rendering/OculusCamera.hh
+++ b/gazebo/rendering/OculusCamera.hh
@@ -19,49 +19,21 @@
#define _GAZEBO_OCULUS_CAMERA_HH_
#include <string>
-#include <vector>
#include "gazebo/rendering/Camera.hh"
-#include "gazebo/rendering/RenderTypes.hh"
-#include "gazebo/common/CommonTypes.hh"
-
-namespace OVR
-{
- class HMDDevice;
- class SensorFusion;
- class DeviceManager;
- class SensorDevice;
-
- namespace Util
- {
- class MagCalibration;
- namespace Render
- {
- class StereoConfig;
- }
- }
-}
-
-namespace Ogre
-{
- class CompositorInstance;
-}
namespace gazebo
{
namespace rendering
{
- class OrbitViewController;
- class FPSViewController;
- class Visual;
- class SelectionBuffer;
+ class OculusCameraPrivate;
/// \addtogroup gazebo_rendering
/// \{
/// \class OculusCamera OculusCamera.hh rendering/rendering.hh
/// \brief A camera used for user visualization of a scene
- class GAZEBO_VISIBLE OculusCamera : public Camera
+ class GZ_RENDERING_VISIBLE OculusCamera : public Camera
{
/// \brief Constructor
/// \param[in] _name Name of the camera.
@@ -95,14 +67,6 @@ namespace gazebo
/// \param[in] _h Height of the camera image.
public: void Resize(unsigned int _w, unsigned int _h);
- /// \brief Get the average frames per second
- /// \return The average rendering frames per second
- public: float GetAvgFPS() const;
-
- /// \brief Get the triangle count.
- /// \return The number of triangles currently being rendered.
- public: unsigned int GetTriangleCount() const;
-
/// \brief Move the camera to focus on a visual.
/// \param[in] _visual Visual to move the camera to.
public: void MoveToVisual(VisualPtr _visual);
@@ -138,6 +102,9 @@ namespace gazebo
/// \return True when Oculus is ready to use.
public: bool Ready();
+ // Documentation inherited
+ protected: virtual void RenderImpl();
+
/// \brief Set the camera to be attached to a visual.
///
/// This causes the camera to move in relation to the specified visual.
@@ -167,44 +134,9 @@ namespace gazebo
/// \brief Apply distorsion to the render target.
private: void Oculus();
- /// \brief Ogre camera for the right Oculus screen.
- protected: Ogre::Camera *rightCamera;
-
- /// \brief View poer for the right camera.
- protected: Ogre::Viewport *rightViewport;
-
- /// \brief Ogre Compositors
- private: Ogre::CompositorInstance *compositors[2];
-
- /// \brief Oculus deviceManager. Manages when the devices are inserted,
- /// removed, or the number of devices present.
- private: OVR::DeviceManager *deviceManager;
-
- /// \brief An Oculus Head-Mounted display.
- private: OVR::HMDDevice *hmd;
-
- /// \brief Maintains a scene stereo state.
- private: OVR::Util::Render::StereoConfig *stereoConfig;
-
- /// \brief An interface to sensor data
- private: OVR::SensorDevice *sensor;
-
- /// \brief Accumulates sensor notification messages to keep track of
- /// orientation.
- private: OVR::SensorFusion *sensorFusion;
-
- /// \brief Horizontal projection center offset as a distance away from the
- /// one-eye [-1,1] unit viewport.
- private: float centerOffset;
-
- /// \brief Transport node for using gazebo pub/sub.
- private: transport::NodePtr node;
-
- /// \brief Subscriber used to receive updates on world_control topic.
- private: transport::SubscriberPtr controlSub;
-
- /// \brief True when Oculus is connected and ready to use.
- private: bool ready;
+ /// \internal
+ /// \brief Pointer to private data.
+ private: OculusCameraPrivate *dataPtr;
};
/// \}
}
diff --git a/gazebo/rendering/OculusCameraPrivate.hh b/gazebo/rendering/OculusCameraPrivate.hh
new file mode 100644
index 0000000..6d7eaab
--- /dev/null
+++ b/gazebo/rendering/OculusCameraPrivate.hh
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2012-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_RENDERING_OCULUS_CAMERA_PRIVATE_HH_
+#define _GAZEBO_RENDERING_OCULUS_CAMERA_PRIVATE_HH_
+
+#include <OVR_CAPI.h>
+#include "gazebo/util/system.hh"
+
+namespace Ogre
+{
+ class Camera;
+ class Viewport;
+ class SceneManager;
+}
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ /// \brief Private data for the Oculus Camera class
+ class GZ_RENDERING_VISIBLE OculusCameraPrivate
+ {
+ /// \brief Constructor
+ public: OculusCameraPrivate()
+ : rightCamera(NULL),
+ externalCamera(NULL),
+ rightViewport(NULL),
+ externalViewport(NULL),
+ externalSceneManager(NULL),
+ node(NULL),
+ controlSub(NULL),
+ ready(false),
+ frameIndex(1),
+ oculusTrackingWarned(false) {}
+
+ /// \brief Ogre camera for the right Oculus screen.
+ public: Ogre::Camera *rightCamera;
+
+ /// \brief Camera in the external scene that render the distortion
+ /// meshes.
+ public: Ogre::Camera *externalCamera;
+
+ /// \brief View port for the right camera.
+ public: Ogre::Viewport *rightViewport;
+
+ /// \brief View port for the external camera.
+ public: Ogre::Viewport *externalViewport;
+
+ /// \brief External scene manager. This holds the distortion meshes
+ /// and the external camera.
+ public: Ogre::SceneManager *externalSceneManager;
+
+ /// \brief An Oculus Head-Mounted display.
+ public: ovrHmd hmd;
+
+ /// \brief Transport node for using gazebo pub/sub.
+ public: transport::NodePtr node;
+
+ /// \brief Subscriber used to receive updates on world_control topic.
+ public: transport::SubscriberPtr controlSub;
+
+ /// \brief True when Oculus is connected and ready to use.
+ public: bool ready;
+
+ /// \brief Right camera render texture.
+ public: Ogre::TexturePtr renderTextureRight;
+
+ /// \brief Left camera render texture.
+ public: Ogre::TexturePtr renderTextureLeft;
+
+ /// \brief Frame index used for oculus timing
+ public: unsigned int frameIndex;
+
+ /// \brief Flag used to prevent multiple messages from being
+ /// displayed.
+ public: bool oculusTrackingWarned;
+ };
+ }
+}
+#endif
diff --git a/gazebo/rendering/OrbitViewController.cc b/gazebo/rendering/OrbitViewController.cc
index 8682960..93476a8 100644
--- a/gazebo/rendering/OrbitViewController.cc
+++ b/gazebo/rendering/OrbitViewController.cc
@@ -42,7 +42,7 @@ OrbitViewController::OrbitViewController(UserCameraPtr _camera)
this->refVisual.reset(new Visual("OrbitViewController",
this->camera->GetScene()));
- this->refVisual->Init();
+ this->refVisual->Load();
this->refVisual->AttachMesh("unit_sphere");
this->refVisual->SetScale(math::Vector3(0.2, 0.2, 0.1));
this->refVisual->SetCastShadows(false);
@@ -58,10 +58,11 @@ OrbitViewController::~OrbitViewController()
}
//////////////////////////////////////////////////
-void OrbitViewController::Init(const math::Vector3 &_focalPoint)
+void OrbitViewController::Init(const math::Vector3 &_focalPoint,
+ const double _yaw, const double _pitch)
{
- this->yaw = 0;
- this->pitch = 0;
+ this->yaw = _yaw;
+ this->pitch = _pitch;
this->focalPoint = _focalPoint;
this->distance = this->camera->GetWorldPosition().Distance(this->focalPoint);
@@ -148,7 +149,7 @@ void OrbitViewController::HandleMouseEvent(const common::MouseEvent &_event)
if (!this->enabled)
return;
- math::Vector2i drag = _event.pos - _event.prevPos;
+ math::Vector2i drag = _event.Pos() - _event.PrevPos();
math::Vector3 directionVec(0, 0, 0);
@@ -157,14 +158,14 @@ void OrbitViewController::HandleMouseEvent(const common::MouseEvent &_event)
// If the event is the initial press of a mouse button, then update
// the focal point and distance.
- if (_event.pressPos == _event.pos)
+ if (_event.PressPos() == _event.Pos())
{
if (!this->camera->GetScene()->GetFirstContact(
- this->camera, _event.pressPos, this->focalPoint))
+ this->camera, _event.PressPos(), this->focalPoint))
{
math::Vector3 origin, dir;
this->camera->GetCameraToViewportRay(
- _event.pressPos.x, _event.pressPos.y, origin, dir);
+ _event.PressPos().X(), _event.PressPos().Y(), origin, dir);
this->focalPoint = origin + dir * 10.0;
}
@@ -179,13 +180,13 @@ void OrbitViewController::HandleMouseEvent(const common::MouseEvent &_event)
this->refVisual->SetVisible(true);
// Middle mouse button or Shift + Left button is used to Orbit.
- if (_event.dragging &&
- (_event.buttons & common::MouseEvent::MIDDLE ||
- (_event.buttons & common::MouseEvent::LEFT && _event.shift)))
+ if (_event.Dragging() &&
+ (_event.Buttons() & common::MouseEvent::MIDDLE ||
+ (_event.Buttons() & common::MouseEvent::LEFT && _event.Shift())))
{
// Compute the delta yaw and pitch.
- double dy = this->NormalizeYaw(drag.x * _event.moveScale * -0.4);
- double dp = this->NormalizePitch(drag.y * _event.moveScale * 0.4);
+ double dy = this->NormalizeYaw(drag.x * _event.MoveScale() * -0.4);
+ double dp = this->NormalizePitch(drag.y * _event.MoveScale() * 0.4);
// Limit rotation to pitch only if the "y" key is pressed.
if (!this->key.empty() && this->key == "y")
@@ -197,7 +198,7 @@ void OrbitViewController::HandleMouseEvent(const common::MouseEvent &_event)
this->Orbit(dy, dp);
}
// The left mouse button is used to translate the camera.
- else if ((_event.buttons & common::MouseEvent::LEFT) && _event.dragging)
+ else if ((_event.Buttons() & common::MouseEvent::LEFT) && _event.Dragging())
{
this->distance =
this->camera->GetWorldPose().pos.Distance(this->focalPoint);
@@ -212,7 +213,7 @@ void OrbitViewController::HandleMouseEvent(const common::MouseEvent &_event)
double factor = 2.0;
// The control key increases zoom speed by a factor of two.
- if (_event.control)
+ if (_event.Control())
factor *= 2.0;
// If the "x", "y", or "z" key is pressed, then lock translation to the
@@ -249,21 +250,21 @@ void OrbitViewController::HandleMouseEvent(const common::MouseEvent &_event)
}
}
// The right mouse button is used to zoom the camera.
- else if ((_event.buttons & common::MouseEvent::RIGHT) && _event.dragging)
+ else if ((_event.Buttons() & common::MouseEvent::RIGHT) && _event.Dragging())
{
double fovY = this->camera->GetVFOV().Radian();
this->Zoom((-drag.y / static_cast<float>(height)) *
this->distance * tan(fovY / 2.0) * 6.0);
}
// The scroll wheel controls zoom.
- else if (_event.type == common::MouseEvent::SCROLL)
+ else if (_event.Type() == common::MouseEvent::SCROLL)
{
if (!this->camera->GetScene()->GetFirstContact(
- this->camera, _event.pos, this->focalPoint))
+ this->camera, _event.Pos(), this->focalPoint))
{
math::Vector3 origin, dir;
this->camera->GetCameraToViewportRay(
- _event.pos.x, _event.pos.y, origin, dir);
+ _event.Pos().X(), _event.Pos().Y(), origin, dir);
this->focalPoint = origin + dir * 10.0;
}
@@ -273,11 +274,11 @@ void OrbitViewController::HandleMouseEvent(const common::MouseEvent &_event)
int factor = 80;
// The control key increases zoom speed by a factor of two.
- if (_event.control)
+ if (_event.Control())
factor *= 2;
// This assumes that _event.scroll.y is -1 or +1
- this->Zoom(-(_event.scroll.y * factor) * _event.moveScale *
+ this->Zoom(-(_event.Scroll().Y() * factor) * _event.MoveScale() *
(this->distance / 5.0));
}
else
@@ -423,3 +424,15 @@ void OrbitViewController::Orbit(double _dy, double _dp)
this->init = false;
this->UpdateRefVisual();
}
+
+/////////////////////////////////////////////////
+double OrbitViewController::Yaw() const
+{
+ return this->yaw;
+}
+
+/////////////////////////////////////////////////
+double OrbitViewController::Pitch() const
+{
+ return this->pitch;
+}
diff --git a/gazebo/rendering/OrbitViewController.hh b/gazebo/rendering/OrbitViewController.hh
index a0ffeaa..ff1ea42 100644
--- a/gazebo/rendering/OrbitViewController.hh
+++ b/gazebo/rendering/OrbitViewController.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class OrbitViewController OrbitVeiwController.hh rendering/rendering.hh
/// \brief Orbit view controller
- class GAZEBO_VISIBLE OrbitViewController : public ViewController
+ class GZ_RENDERING_VISIBLE OrbitViewController : public ViewController
{
/// \brief Constructor.
/// \param[in] _camera Pointer to the camera to control.
@@ -45,9 +45,9 @@ namespace gazebo
/// \brief Initialize the controller.
public: virtual void Init();
- /// \brief Initialze the controller with a focal point.
- /// \param[in] _focalPoint Point to look at.
- public: virtual void Init(const math::Vector3 &_focalPoint);
+ // Documentation inherited
+ public: virtual void Init(const math::Vector3 &_focalPoint,
+ const double _yaw = 0, const double _pitch = 0);
/// \brief Update.
public: virtual void Update();
@@ -78,57 +78,61 @@ namespace gazebo
// Documentation inherited from parent
public: virtual void HandleKeyPressEvent(const std::string &_key);
+ public: double Pitch() const;
+
+ public: double Yaw() const;
+
/// \brief Translate the focal point in the local coordinate frame.
/// \param[in] _vec Direction and amount to translate the camera.
- private: void TranslateLocal(const math::Vector3 &_vec);
+ protected: void TranslateLocal(const math::Vector3 &_vec);
/// \brief Translate the focal point in the global coordinate frame.
/// \param[in] _vec Direction and amount to translate the camera.
- private: void TranslateGlobal(const math::Vector3 &_vec);
+ protected: void TranslateGlobal(const math::Vector3 &_vec);
/// \brief Zoom the camera.
/// \paramp[in] _amount Zoom quatity.
- private: void Zoom(float _amount);
+ protected: void Zoom(float _amount);
/// \brief Normalize yaw value.
/// \paramp[in] _v Normalize a yaw value.
/// \return The normalized value.
- private: double NormalizeYaw(double _v);
+ protected: double NormalizeYaw(double _v);
/// \brief Normalize pitch value.
/// \paramp[in] _v Normalize a pitch value.
/// \return The normalized value.
- private: double NormalizePitch(double _v);
+ protected: double NormalizePitch(double _v);
/// \brief Update the reference visual.
- private: void UpdateRefVisual();
+ protected: void UpdateRefVisual();
/// \brief Update the camera's pose based on a rotation update.
/// \param[in] _dy Delta yaw movement.
/// \param[in] _dp Delta pitch movement.
- private: void Orbit(double _dy, double _dp);
+ protected: void Orbit(double _dy, double _dp);
/// \brief Yaw value.
- private: float yaw;
+ protected: float yaw;
/// \brief Pitch value.
- private: float pitch;
+ protected: float pitch;
/// \brief Distance to the focal point.
- private: float distance;
-
- /// \brief The focal point.
- private: math::Vector3 focalPoint;
+ protected: float distance;
/// \brief A reference visual.
- private: VisualPtr refVisual;
+ protected: VisualPtr refVisual;
/// \brief Key that is currently pressed.
- private: std::string key;
+ protected: std::string key;
/// \brief A flag used to inidicate that the view controller has just
/// been initialized.
- private: bool init;
+ protected: bool init;
+
+ /// \brief The focal point.
+ protected: math::Vector3 focalPoint;
};
/// \}
}
diff --git a/gazebo/rendering/OriginVisual.cc b/gazebo/rendering/OriginVisual.cc
new file mode 100644
index 0000000..b68fb5e
--- /dev/null
+++ b/gazebo/rendering/OriginVisual.cc
@@ -0,0 +1,76 @@
+/*
+ * 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/Vector3.hh"
+
+#include "gazebo/rendering/DynamicLines.hh"
+#include "gazebo/rendering/OriginVisualPrivate.hh"
+#include "gazebo/rendering/OriginVisual.hh"
+
+using namespace gazebo;
+using namespace rendering;
+
+/////////////////////////////////////////////////
+OriginVisual::OriginVisual(const std::string &_name, VisualPtr _vis)
+ : Visual(*new OriginVisualPrivate, _name, _vis, false)
+{
+ OriginVisualPrivate *dPtr =
+ reinterpret_cast<OriginVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_GUI;
+}
+
+/////////////////////////////////////////////////
+OriginVisual::~OriginVisual()
+{
+ OriginVisualPrivate *dPtr =
+ reinterpret_cast<OriginVisualPrivate *>(this->dataPtr);
+
+ this->DeleteDynamicLine(dPtr->xLine);
+ this->DeleteDynamicLine(dPtr->yLine);
+ this->DeleteDynamicLine(dPtr->zLine);
+}
+
+/////////////////////////////////////////////////
+void OriginVisual::Load()
+{
+ Visual::Load();
+
+ OriginVisualPrivate *dPtr =
+ reinterpret_cast<OriginVisualPrivate *>(this->dataPtr);
+
+ dPtr->length = 1000;
+
+ dPtr->xLine = this->CreateDynamicLine(
+ rendering::RENDERING_LINE_LIST);
+ dPtr->xLine->setMaterial("Gazebo/Red");
+ dPtr->xLine->AddPoint(math::Vector3::Zero);
+ dPtr->xLine->AddPoint(math::Vector3::UnitX*dPtr->length);
+
+ dPtr->yLine = this->CreateDynamicLine(
+ rendering::RENDERING_LINE_LIST);
+ dPtr->yLine->setMaterial("Gazebo/Green");
+ dPtr->yLine->AddPoint(math::Vector3::Zero);
+ dPtr->yLine->AddPoint(math::Vector3::UnitY*dPtr->length);
+
+ dPtr->zLine = this->CreateDynamicLine(
+ rendering::RENDERING_LINE_LIST);
+ dPtr->zLine->setMaterial("Gazebo/Blue");
+ dPtr->zLine->AddPoint(math::Vector3::Zero);
+ dPtr->zLine->AddPoint(math::Vector3::UnitZ*dPtr->length);
+
+ this->SetVisibilityFlags(GZ_VISIBILITY_GUI);
+}
diff --git a/gazebo/rendering/OriginVisual.hh b/gazebo/rendering/OriginVisual.hh
new file mode 100644
index 0000000..6a695a7
--- /dev/null
+++ b/gazebo/rendering/OriginVisual.hh
@@ -0,0 +1,51 @@
+/*
+ * 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_ORIGIN_VISUAL_HH_
+#define _GAZEBO_ORIGIN_VISUAL_HH_
+
+#include <string>
+
+#include "gazebo/rendering/Visual.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ /// \addtogroup gazebo_rendering Rendering
+ /// \{
+
+ /// \class OriginVisual OriginVisual.hh rendering/rendering.hh
+ /// \brief Basic world origin visualization
+ class GZ_RENDERING_VISIBLE OriginVisual : public Visual
+ {
+ /// \brief Constructor
+ /// \param[in] _name Name of the Visual
+ /// \param[in] _vis Parent Visual
+ public: OriginVisual(const std::string &_name, VisualPtr _vis);
+
+ /// \brief Destructor
+ public: virtual ~OriginVisual();
+
+ /// \brief Load the visual with default parameters
+ public: virtual void Load();
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/rendering/OriginVisualPrivate.hh b/gazebo/rendering/OriginVisualPrivate.hh
new file mode 100644
index 0000000..7ad716d
--- /dev/null
+++ b/gazebo/rendering/OriginVisualPrivate.hh
@@ -0,0 +1,53 @@
+/*
+ * 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_ORIGIN_VISUAL_PRIVATE_HH_
+#define _GAZEBO_ORIGIN_VISUAL_PRIVATE_HH_
+
+#include <string>
+
+#include "gazebo/rendering/VisualPrivate.hh"
+
+namespace ogre
+{
+ class SceneNode;
+}
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ class DynamicLines;
+
+ /// \brief Private data for the Origin Visual class
+ class OriginVisualPrivate : public VisualPrivate
+ {
+ /// \brief Length of lines
+ public: double length;
+
+ /// \brief Line on the X axis.
+ public: DynamicLines *xLine;
+
+ /// \brief Line on the Y axis.
+ public: DynamicLines *yLine;
+
+ /// \brief Line on the Z axis.
+ public: DynamicLines *zLine;
+ };
+ }
+}
+#endif
diff --git a/gazebo/rendering/OriginVisual_TEST.cc b/gazebo/rendering/OriginVisual_TEST.cc
new file mode 100644
index 0000000..6f1dc94
--- /dev/null
+++ b/gazebo/rendering/OriginVisual_TEST.cc
@@ -0,0 +1,65 @@
+/*
+ * 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/RenderingIface.hh"
+#include "gazebo/rendering/Scene.hh"
+#include "gazebo/rendering/OriginVisual.hh"
+#include "gazebo/test/ServerFixture.hh"
+
+using namespace gazebo;
+class OriginVisual_TEST : public ServerFixture
+{
+};
+
+TEST_F(OriginVisual_TEST, Load)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene("default");
+
+ if (!scene)
+ scene = gazebo::rendering::create_scene("default", false);
+
+ EXPECT_TRUE(scene != NULL);
+
+ // Create and load visual
+ rendering::OriginVisualPtr origin;
+ origin.reset(new rendering::OriginVisual("origin", scene->GetWorldVisual()));
+ origin->Load();
+ EXPECT_TRUE(origin != NULL);
+
+ // Check that it was added to the scene (by Load)
+ EXPECT_EQ(scene->GetVisual("origin"), origin);
+
+ // Remove it from the scene (Fini is called)
+ scene->RemoveVisual(origin);
+
+ // Check that it was removed
+ EXPECT_TRUE(scene->GetVisual("origin") == NULL);
+
+ // Reset pointer
+ origin.reset();
+ EXPECT_TRUE(origin == NULL);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/rendering/OrthoViewController.cc b/gazebo/rendering/OrthoViewController.cc
new file mode 100644
index 0000000..02b1b8f
--- /dev/null
+++ b/gazebo/rendering/OrthoViewController.cc
@@ -0,0 +1,314 @@
+/*
+ * 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/rendering/ogre_gazebo.h"
+#include "gazebo/common/MouseEvent.hh"
+
+#include "gazebo/rendering/Conversions.hh"
+#include "gazebo/rendering/Scene.hh"
+#include "gazebo/rendering/Visual.hh"
+#include "gazebo/rendering/UserCamera.hh"
+#include "gazebo/rendering/OrbitViewController.hh"
+#include "gazebo/rendering/OrthoViewControllerPrivate.hh"
+#include "gazebo/rendering/OrthoViewController.hh"
+
+#define TYPE_STRING "ortho"
+
+using namespace gazebo;
+using namespace rendering;
+
+//////////////////////////////////////////////////
+OrthoViewController::OrthoViewController(UserCameraPtr _camera)
+ : OrbitViewController(_camera), dataPtr(new OrthoViewControllerPrivate)
+{
+ this->typeString = TYPE_STRING;
+ this->init = false;
+}
+
+//////////////////////////////////////////////////
+OrthoViewController::~OrthoViewController()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+
+ this->refVisual.reset();
+}
+
+//////////////////////////////////////////////////
+void OrthoViewController::Init()
+{
+ this->dataPtr->scale = 100;
+ this->distance = 1000.0/this->dataPtr->scale;
+
+ int width = this->camera->GetViewportWidth();
+ int height = this->camera->GetViewportHeight();
+
+ if (width > 0 && height > 0)
+ {
+ // set up the view projection
+ this->Zoom(1.0);
+ }
+
+ OrbitViewController::Init();
+}
+
+//////////////////////////////////////////////////
+void OrthoViewController::Init(const math::Vector3 &_focalPoint,
+ double _yaw, double _pitch)
+{
+ this->dataPtr->scale = 100;
+
+ int width = this->camera->GetViewportWidth();
+ int height = this->camera->GetViewportHeight();
+
+ if (width > 0 && height > 0)
+ {
+ // set up the view projection
+ this->Zoom(1.0);
+ }
+
+ OrbitViewController::Init(_focalPoint, _yaw, _pitch);
+}
+
+//////////////////////////////////////////////////
+std::string OrthoViewController::GetTypeString()
+{
+ return TYPE_STRING;
+}
+
+//////////////////////////////////////////////////
+void OrthoViewController::HandleMouseEvent(const common::MouseEvent &_event)
+{
+ if (!this->enabled)
+ return;
+
+ math::Vector2i drag = _event.Pos() - _event.PrevPos();
+
+ math::Vector3 directionVec(0, 0, 0);
+
+ int width = this->camera->GetViewportWidth();
+ int height = this->camera->GetViewportHeight();
+ double orthoWidth = width/this->dataPtr->scale;
+ double orthoHeight = height/this->dataPtr->scale;
+
+ // If the event is the initial press of a mouse button, then update
+ // the focal point and distance.
+ if (_event.PressPos() == _event.Pos())
+ {
+ if (!this->camera->GetScene()->GetFirstContact(
+ this->camera, _event.PressPos(), this->focalPoint))
+ {
+ math::Vector3 origin, dir;
+ this->camera->GetCameraToViewportRay(
+ _event.PressPos().X(), _event.PressPos().Y(), origin, dir);
+ this->focalPoint = origin + dir * 10.0;
+ }
+
+ // pseudo distance
+ this->distance = 1000.0/this->dataPtr->scale;
+
+ this->yaw = this->camera->GetWorldRotation().GetAsEuler().z;
+ this->pitch = this->camera->GetWorldRotation().GetAsEuler().y;
+ }
+
+ // Turn on the reference visual.
+ this->refVisual->SetVisible(true);
+
+ // Middle mouse button or Shift + Left button is used to Orbit.
+ if (_event.Dragging() &&
+ (_event.Buttons() & common::MouseEvent::MIDDLE ||
+ (_event.Buttons() & common::MouseEvent::LEFT && _event.Shift())))
+ {
+ // Compute the delta yaw and pitch.
+ double dy = this->NormalizeYaw(drag.x * _event.MoveScale() * -0.4);
+ double dp = this->NormalizePitch(drag.y * _event.MoveScale() * 0.4);
+
+ // Limit rotation to pitch only if the "y" key is pressed.
+ if (!this->key.empty() && this->key == "y")
+ dy = 0.0;
+ // Limit rotation to yaw if the "z" key is pressed.
+ else if (!this->key.empty() && this->key == "z")
+ dp = 0.0;
+
+ this->Orbit(dy, dp);
+ }
+ // The left mouse button is used to translate the camera.
+ else if ((_event.Buttons() & common::MouseEvent::LEFT) && _event.Dragging())
+ {
+ math::Vector3 translation;
+
+ double factor = 1.0;
+
+ // The control key increases zoom speed by a factor of two.
+ if (_event.Control())
+ factor *= 2.0;
+
+ // If the "x", "y", or "z" key is pressed, then lock translation to the
+ // indicated axis.
+ if (!this->key.empty())
+ {
+ if (this->key == "x")
+ translation.Set((drag.y / static_cast<float>(height)) *
+ orthoHeight * factor, 0.0, 0.0);
+ else if (this->key == "y")
+ translation.Set(0.0, (drag.x / static_cast<float>(width)) *
+ orthoWidth * factor, 0.0);
+ else if (this->key == "z")
+ translation.Set(0.0, 0.0, (drag.y / static_cast<float>(height)) *
+ orthoHeight * factor);
+ else
+ gzerr << "Unable to handle key [" << this->key << "] in orbit view "
+ << "controller.\n";
+
+ // Translate in the global coordinate frame
+ this->TranslateGlobal(translation);
+ }
+ else
+ {
+ // Translate in the "y" "z" plane.
+ translation.Set(0.0,
+ (drag.x / static_cast<float>(width)) * orthoWidth * factor,
+ (drag.y / static_cast<float>(height)) * orthoHeight * factor);
+
+ // Translate in the local coordinate frame
+ this->TranslateLocal(translation);
+ }
+ }
+ // The right mouse button is used to zoom the camera.
+ else if ((_event.Buttons() & common::MouseEvent::RIGHT) && _event.Dragging())
+ {
+ double amount = 1.0 + (drag.y / static_cast<float>(height));
+ this->Zoom(amount, _event.PressPos());
+ }
+ // The scroll wheel controls zoom.
+ else if (_event.Type() == common::MouseEvent::SCROLL)
+ {
+ if (!this->camera->GetScene()->GetFirstContact(
+ this->camera, _event.Pos(), this->focalPoint))
+ {
+ math::Vector3 origin, dir;
+ this->camera->GetCameraToViewportRay(
+ _event.Pos().X(), _event.Pos().Y(), origin, dir);
+ this->focalPoint = origin + dir * 10.0;
+ }
+
+ // pseudo distance
+ this->distance = 1000.0/this->dataPtr->scale;
+
+ double factor = 1.0;
+
+ // The control key increases zoom speed by a factor of two.
+ if (_event.Control())
+ factor *= 2;
+
+ // This assumes that _event.scroll.y is -1 or +1
+ double zoomFactor = 10;
+ double amount = 1.0 + _event.Scroll().Y() * factor * _event.MoveScale()
+ * zoomFactor;
+ this->Zoom(amount, _event.Pos());
+ }
+ else
+ this->refVisual->SetVisible(false);
+}
+
+//////////////////////////////////////////////////
+void OrthoViewController::Zoom(const float _amount,
+ const math::Vector2i &_screenPos)
+{
+ // Zoom to mouse cursor position
+ // Three step process:
+ // Translate mouse point to center of screen
+ // Zoom by changing the orthographic window size
+ // Translate back to mouse cursor position
+
+ math::Vector3 translation;
+ int width = this->camera->GetViewportWidth();
+ int height = this->camera->GetViewportHeight();
+
+ double orthoWidth = width / this->dataPtr->scale;
+ double orthoHeight = height / this->dataPtr->scale;
+
+ translation.Set(0.0,
+ ((width/2.0 - _screenPos.x) / static_cast<float>(width))
+ * orthoWidth,
+ ((height/2.0 - _screenPos.y) / static_cast<float>(height))
+ * orthoHeight);
+ this->TranslateLocal(translation);
+
+ this->dataPtr->scale /= _amount;
+
+ // build custom projection matrix from custom near and far clipping planes,
+ // had to set a negative near clippping plane to workaround a camera
+ // culling issue in orthographic view.
+ Ogre::Matrix4 proj;
+ proj = this->BuildScaledOrthoMatrix(
+ -width / this->dataPtr->scale / 2.0,
+ width / this->dataPtr->scale / 2.0,
+ -height / this->dataPtr->scale / 2.0,
+ height / this->dataPtr->scale / 2.0,
+ -500, 500);
+
+ this->camera->GetOgreCamera()->setCustomProjectionMatrix(true, proj);
+
+ double newOrthoWidth = width / this->dataPtr->scale;
+ double newOrthoHeight = height / this->dataPtr->scale;
+
+ translation.Set(0.0,
+ ((_screenPos.x - width/2.0) / static_cast<float>(width))
+ * newOrthoWidth,
+ ((_screenPos.y - height/2.0) / static_cast<float>(height))
+ * newOrthoHeight);
+
+ this->TranslateLocal(translation);
+
+ this->UpdateRefVisual();
+}
+
+//////////////////////////////////////////////////
+void OrthoViewController::Resize(
+ const unsigned int _width, const unsigned int _height)
+{
+ Ogre::Matrix4 proj = this->BuildScaledOrthoMatrix(
+ _width / this->dataPtr->scale / -2.0,
+ _width / this->dataPtr->scale / 2.0,
+ _height / this->dataPtr->scale / -2.0,
+ _height / this->dataPtr->scale / 2.0,
+ -500, 500);
+
+ this->camera->GetOgreCamera()->setCustomProjectionMatrix(true, proj);
+}
+
+//////////////////////////////////////////////////
+Ogre::Matrix4 OrthoViewController::BuildScaledOrthoMatrix(
+ const float _left, const float _right,
+ const float _bottom, const float _top,
+ const float _near, const float _far) const
+{
+ float invw = 1.0f / (_right - _left);
+ float invh = 1.0f / (_top - _bottom);
+ float invd = 1.0f / (_far - _near);
+
+ Ogre::Matrix4 proj = Ogre::Matrix4::ZERO;
+ proj[0][0] = 2.0f * invw;
+ proj[0][3] = -(_right + _left) * invw;
+ proj[1][1] = 2.0f * invh;
+ proj[1][3] = -(_top + _bottom) * invh;
+ proj[2][2] = -2.0f * invd;
+ proj[2][3] = -(_far + _near) * invd;
+ proj[3][3] = 1.0f;
+
+ return proj;
+}
diff --git a/gazebo/rendering/OrthoViewController.hh b/gazebo/rendering/OrthoViewController.hh
new file mode 100644
index 0000000..e10cdee
--- /dev/null
+++ b/gazebo/rendering/OrthoViewController.hh
@@ -0,0 +1,91 @@
+/*
+ * 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_ORTHOVIEWCONTROLLER_HH_
+#define _GAZEBO_ORTHOVIEWCONTROLLER_HH_
+
+#include <string>
+
+#include "gazebo/rendering/ViewController.hh"
+#include "gazebo/math/Vector2i.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ // Forward declare private data pointer.
+ class OrthoViewControllerPrivate;
+
+ /// \addtogroup gazebo_rendering
+ /// \{
+
+ /// \class OrthoViewController OrbitVeiwController.hh rendering/rendering.hh
+ /// \brief Orthographic view controller
+ class GAZEBO_VISIBLE OrthoViewController : public OrbitViewController
+ {
+ /// \brief Constructor.
+ /// \param[in] _camera Pointer to the camera to control.
+ public: OrthoViewController(UserCameraPtr _camera);
+
+ /// \brief Destructor.
+ public: virtual ~OrthoViewController();
+
+ /// \brief Initialize the controller.
+ public: virtual void Init();
+
+ // Documentation inherited
+ public: virtual void Init(const math::Vector3 &_focalPoint,
+ const double _yaw = 0, const double _pitch = 0);
+
+ /// \brief Get the type name of this view controller.
+ /// \return The view controller name: "ortho".
+ public: static std::string GetTypeString();
+
+ /// \brief Handle a mouse event.
+ /// \param[in] _event The mouse event.
+ public: virtual void HandleMouseEvent(const common::MouseEvent &_event);
+
+ /// \brief Zoom the camera.
+ /// \param[in] _amount Zoom quantity.
+ /// \param[in] _point Position on screen to zoom to
+ private: void Zoom(const float _amount,
+ const math::Vector2i &_point = math::Vector2i(0, 0));
+
+ // Documentation inherited
+ public: virtual void Resize(const unsigned int _width,
+ const unsigned int _height);
+
+ /// \brief Build a custom scaled orthographic projection matrix.
+ /// \param[in] _left Left position
+ /// \param[in] _right Right position
+ /// \param[in] _bottom Bottom position
+ /// \param[in] _top Top position
+ /// \param[in] _near Near clip distance
+ /// \param[in] _far Far clip distance
+ private: Ogre::Matrix4 BuildScaledOrthoMatrix(
+ const float _left, const float _right,
+ const float _bottom, const float _top,
+ const float _near, const float _far) const;
+
+
+ /// \brief Private data pointer
+ private: OrthoViewControllerPrivate *dataPtr;
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/rendering/OrthoViewControllerPrivate.hh b/gazebo/rendering/OrthoViewControllerPrivate.hh
new file mode 100644
index 0000000..5b49185
--- /dev/null
+++ b/gazebo/rendering/OrthoViewControllerPrivate.hh
@@ -0,0 +1,33 @@
+/*
+ * 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_ORTHOVIEWCONTROLLER_PRIVATE_HH_
+#define _GAZEBO_ORTHOVIEWCONTROLLER_PRIVATE_HH_
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ /// \internal
+ /// \brief OrthoViewController private data.
+ class OrthoViewControllerPrivate
+ {
+ /// \brief Scale used for zooming within the orthographic view
+ public: double scale;
+ };
+ }
+}
+#endif
diff --git a/gazebo/rendering/Projector.cc b/gazebo/rendering/Projector.cc
index 90cdd14..080022d 100644
--- a/gazebo/rendering/Projector.cc
+++ b/gazebo/rendering/Projector.cc
@@ -126,14 +126,14 @@ void Projector::Load(sdf::ElementPtr _sdf)
/////////////////////////////////////////////////
void Projector::Load(const msgs::Projector &_msg)
{
- math::Pose pose;
+ ignition::math::Pose3d pose;
std::string textureName;
double nearClip = 0.1;
double farClip = 10.0;
double fov = M_PI * 0.25;
if (_msg.has_pose())
- pose = msgs::Convert(_msg.pose());
+ pose = msgs::ConvertIgn(_msg.pose());
if (_msg.has_texture())
textureName = _msg.texture();
diff --git a/gazebo/rendering/Projector.hh b/gazebo/rendering/Projector.hh
index 2b379ab..6c844d2 100644
--- a/gazebo/rendering/Projector.hh
+++ b/gazebo/rendering/Projector.hh
@@ -43,7 +43,7 @@ namespace gazebo
/// \class Projector Projector.hh rendering/rendering.hh
/// \brief Projects a material onto surface, light a light projector.
- class GAZEBO_VISIBLE Projector
+ class GZ_RENDERING_VISIBLE Projector
{
/// \brief Constructor.
/// \param[in] _parent Name of the parent visual.
diff --git a/gazebo/rendering/RFIDTagVisual.cc b/gazebo/rendering/RFIDTagVisual.cc
index dd2db64..703e1de 100644
--- a/gazebo/rendering/RFIDTagVisual.cc
+++ b/gazebo/rendering/RFIDTagVisual.cc
@@ -19,6 +19,12 @@
* Date: 6th December 2011
*/
+#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 "gazebo/transport/transport.hh"
#include "gazebo/rendering/Conversions.hh"
#include "gazebo/rendering/Scene.hh"
@@ -37,6 +43,8 @@ RFIDTagVisual::RFIDTagVisual(const std::string &_name, VisualPtr _vis,
RFIDTagVisualPrivate *dPtr =
reinterpret_cast<RFIDTagVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_SENSOR;
+
dPtr->node = transport::NodePtr(new transport::Node());
dPtr->node->Init(dPtr->scene->GetName());
diff --git a/gazebo/rendering/RFIDTagVisual.hh b/gazebo/rendering/RFIDTagVisual.hh
index 6a9d605..e98bb8a 100644
--- a/gazebo/rendering/RFIDTagVisual.hh
+++ b/gazebo/rendering/RFIDTagVisual.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class RFIDTagVisual RFIDTagVisual.hh rendering/rendering.hh
/// \brief Visualization for RFID tags sensor
- class GAZEBO_VISIBLE RFIDTagVisual : public Visual
+ class GZ_RENDERING_VISIBLE RFIDTagVisual : public Visual
{
/// \brief Constructor
/// \param[in] _name Name of the visual.
diff --git a/gazebo/rendering/RFIDVisual.cc b/gazebo/rendering/RFIDVisual.cc
index 42b65de..6c63d46 100644
--- a/gazebo/rendering/RFIDVisual.cc
+++ b/gazebo/rendering/RFIDVisual.cc
@@ -19,6 +19,12 @@
* Date:
*/
+#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 "gazebo/transport/transport.hh"
#include "gazebo/rendering/Conversions.hh"
#include "gazebo/rendering/Scene.hh"
@@ -38,6 +44,8 @@ RFIDVisual::RFIDVisual(const std::string &_name, VisualPtr _vis,
RFIDVisualPrivate *dPtr =
reinterpret_cast<RFIDVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_SENSOR;
+
dPtr->node = transport::NodePtr(new transport::Node());
dPtr->node->Init(dPtr->scene->GetName());
diff --git a/gazebo/rendering/RFIDVisual.hh b/gazebo/rendering/RFIDVisual.hh
index 0eb42d7..18a1042 100644
--- a/gazebo/rendering/RFIDVisual.hh
+++ b/gazebo/rendering/RFIDVisual.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class RFIDVisual RFIDVisual.hh rendering/rendering.hh
/// \brief Visualization for RFID sensor.
- class GAZEBO_VISIBLE RFIDVisual : public Visual
+ class GZ_RENDERING_VISIBLE RFIDVisual : public Visual
{
/// \brief Constructor.
/// \param[in] _name Name of the Visual.
diff --git a/gazebo/rendering/RTShaderSystem.cc b/gazebo/rendering/RTShaderSystem.cc
index 53c1514..b7d812f 100644
--- a/gazebo/rendering/RTShaderSystem.cc
+++ b/gazebo/rendering/RTShaderSystem.cc
@@ -15,6 +15,12 @@
*
*/
+#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 <sys/stat.h>
#include <boost/bind.hpp>
@@ -94,7 +100,8 @@ void RTShaderSystem::Fini()
// Finalize RTShader system.
if (this->shaderGenerator != NULL)
{
-#if (OGRE_VERSION < ((1 << 16) | (9 << 8) | 0))
+ // On Windows, we're using 1.9RC1, which doesn't have a bunch of changes.
+#if (OGRE_VERSION < ((1 << 16) | (9 << 8) | 0)) || defined(_WIN32)
Ogre::RTShader::ShaderGenerator::finalize();
#else
Ogre::RTShader::ShaderGenerator::destroy();
@@ -152,6 +159,22 @@ void RTShaderSystem::RemoveScene(ScenePtr _scene)
}
//////////////////////////////////////////////////
+void RTShaderSystem::RemoveScene(const std::string &_scene)
+{
+ if (!this->initialized)
+ return;
+
+ for (auto iter : this->scenes)
+ {
+ if (iter->GetName() == _scene)
+ {
+ this->RemoveScene(iter);
+ return;
+ }
+ }
+}
+
+//////////////////////////////////////////////////
void RTShaderSystem::AttachEntity(Visual *vis)
{
if (!this->initialized)
@@ -386,7 +409,11 @@ bool RTShaderSystem::GetPaths(std::string &coreLibsPath, std::string &cachePath)
stream << tmpdir << "/gazebo-" << user << "-rtshaderlibcache" << "/";
cachePath = stream.str();
// Create the directory
+#ifdef _WIN32
+ if (mkdir(cachePath.c_str()) != 0)
+#else
if (mkdir(cachePath.c_str(), S_IRUSR | S_IWUSR | S_IXUSR) != 0)
+#endif
{
if (errno != EEXIST)
{
@@ -487,7 +514,7 @@ void RTShaderSystem::ApplyShadows(ScenePtr _scene)
}
double shadowFarDistance = 500;
- double cameraNearClip = 0.1;
+ double cameraNearClip = 0.01;
sceneMgr->setShadowFarDistance(shadowFarDistance);
Ogre::PSSMShadowCameraSetup *cameraSetup =
diff --git a/gazebo/rendering/RTShaderSystem.hh b/gazebo/rendering/RTShaderSystem.hh
index 0bf5fc7..c2deea3 100644
--- a/gazebo/rendering/RTShaderSystem.hh
+++ b/gazebo/rendering/RTShaderSystem.hh
@@ -48,7 +48,8 @@ namespace gazebo
///
/// This class allows Gazebo to generate per-pixel shaders for every
/// material at run-time.
- class GAZEBO_VISIBLE RTShaderSystem : public SingletonT<RTShaderSystem>
+ class GZ_RENDERING_VISIBLE RTShaderSystem :
+ public SingletonT<RTShaderSystem>
{
/// \enum LightingModel.
/// \brief The type of lighting.
@@ -89,6 +90,10 @@ namespace gazebo
/// \param[in] The scene to remove
public: void RemoveScene(ScenePtr _scene);
+ /// \brief Remove a scene
+ /// \param[in] Name of the scene to remove.
+ public: void RemoveScene(const std::string &_scene);
+
/// \brief Update the shaders. This should not be called frequently.
public: void UpdateShaders();
diff --git a/gazebo/rendering/RayQuery.cc b/gazebo/rendering/RayQuery.cc
index 2bb9e5a..026a630 100644
--- a/gazebo/rendering/RayQuery.cc
+++ b/gazebo/rendering/RayQuery.cc
@@ -75,9 +75,12 @@ bool RayQuery::SelectMeshTriangle(int _x, int _y, VisualPtr _visual,
const common::SubMesh *submesh = mesh->GetSubMesh(j);
for (unsigned int k = 0; k < submesh->GetIndexCount(); k += 3)
{
- math::Vector3 vertexA = submesh->GetVertex(submesh->GetIndex(k));
- math::Vector3 vertexB = submesh->GetVertex(submesh->GetIndex(k+1));
- math::Vector3 vertexC = submesh->GetVertex(submesh->GetIndex(k+2));
+ ignition::math::Vector3d vertexA =
+ submesh->Vertex(submesh->GetIndex(k));
+ ignition::math::Vector3d vertexB =
+ submesh->Vertex(submesh->GetIndex(k+1));
+ ignition::math::Vector3d vertexC =
+ submesh->Vertex(submesh->GetIndex(k+2));
Ogre::Vector3 worldVertexA = transform * Conversions::Convert(vertexA);
Ogre::Vector3 worldVertexB = transform * Conversions::Convert(vertexB);
diff --git a/gazebo/rendering/RayQuery.hh b/gazebo/rendering/RayQuery.hh
index 1452952..e9ba487 100644
--- a/gazebo/rendering/RayQuery.hh
+++ b/gazebo/rendering/RayQuery.hh
@@ -39,7 +39,7 @@ namespace gazebo
/// \class RayQuery RayQuery.hh rendering/rendering.hh
/// \brief A Ray Query class used for retrieving mesh data of a visual,
/// adapted from Ogre3D wiki.
- class GAZEBO_VISIBLE RayQuery
+ class GZ_RENDERING_VISIBLE RayQuery
{
/// \brief Constructor
/// \param[in] _camera Pointer to camera used for ray casting.
diff --git a/gazebo/rendering/RenderEngine.cc b/gazebo/rendering/RenderEngine.cc
index 8ea14d1..064f90c 100644
--- a/gazebo/rendering/RenderEngine.cc
+++ b/gazebo/rendering/RenderEngine.cc
@@ -15,22 +15,30 @@
*
*/
-#ifdef __APPLE__
+#include <string>
+#include <iostream>
+#include <boost/filesystem.hpp>
+#include <sys/types.h>
+
+#ifdef __APPLE__
# include <QtCore/qglobal.h>
#endif
-#ifndef Q_OS_MAC // Not Apple
+// Not Apple or Windows
+#if not defined( Q_OS_MAC) && not defined(_WIN32)
# include <X11/Xlib.h>
# include <X11/Xutil.h>
# include <GL/glx.h>
#endif
-#include <sys/types.h>
-#include <dirent.h>
-#include <string>
-#include <iostream>
-
-#include <boost/filesystem.hpp>
+#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
#include "gazebo/rendering/ogre_gazebo.h"
@@ -141,10 +149,12 @@ void RenderEngine::Load()
this->SetupResources();
}
- std::stringstream stream;
- stream << (int32_t)this->dummyWindowId;
+ // Create a 1x1 render window so that we can grab a GL context. Based on
+ // testing, this is a hard requirement by Apple. We also need it to
+ // properly initialize GLWidget and UserCameras. See the GLWidget
+ // constructor.
+ this->windowManager->CreateWindow(std::to_string(this->dummyWindowId), 1, 1);
- this->windowManager->CreateWindow(stream.str(), 1, 1);
this->CheckSystemCapabilities();
}
@@ -285,7 +295,12 @@ void RenderEngine::PostRender()
void RenderEngine::Init()
{
if (this->renderPathType == NONE)
+ {
+ gzwarn << "Cannot initialize render engine since "
+ << "render path type is NONE. Ignore this warning if"
+ << "rendering has been turned off on purpose.\n";
return;
+ }
this->initialized = false;
@@ -371,7 +386,8 @@ void RenderEngine::Fini()
this->scenes[i].reset();
this->scenes.clear();
-#ifndef Q_OS_MAC
+ // Not Apple or Windows
+# if not defined( Q_OS_MAC) && not defined(_WIN32)
if (this->dummyDisplay)
{
glXDestroyContext(static_cast<Display*>(this->dummyDisplay),
@@ -381,7 +397,7 @@ void RenderEngine::Fini()
XCloseDisplay(static_cast<Display*>(this->dummyDisplay));
this->dummyDisplay = NULL;
}
-#endif
+# endif
this->initialized = false;
}
@@ -671,7 +687,7 @@ bool RenderEngine::CreateContext()
{
bool result = true;
-#ifdef Q_OS_MAC
+#if defined Q_OS_MAC || _WIN32
this->dummyDisplay = 0;
#else
try
diff --git a/gazebo/rendering/RenderEngine.hh b/gazebo/rendering/RenderEngine.hh
index e932b1c..f04ae97 100644
--- a/gazebo/rendering/RenderEngine.hh
+++ b/gazebo/rendering/RenderEngine.hh
@@ -29,6 +29,7 @@
#include "gazebo/common/SingletonT.hh"
#include "gazebo/common/Event.hh"
#include "gazebo/transport/TransportTypes.hh"
+#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/RenderTypes.hh"
#include "gazebo/util/system.hh"
@@ -52,7 +53,7 @@ namespace gazebo
/// \brief Adaptor to Ogre3d
///
/// Provides the interface to load, initialize the rendering engine.
- class GAZEBO_VISIBLE RenderEngine : public SingletonT<RenderEngine>
+ class GZ_RENDERING_VISIBLE RenderEngine : public SingletonT<RenderEngine>
{
/// \enum RenderPathType
/// \brief The type of rendering path used by the rendering engine.
diff --git a/gazebo/rendering/RenderEvents.cc b/gazebo/rendering/RenderEvents.cc
index d230173..0606577 100644
--- a/gazebo/rendering/RenderEvents.cc
+++ b/gazebo/rendering/RenderEvents.cc
@@ -22,3 +22,5 @@ using namespace rendering;
event::EventT<void (const std::string &)> Events::createScene;
event::EventT<void (const std::string &)> Events::removeScene;
+event::EventT<void (const int32_t)> Events::toggleLayer;
+event::EventT<void (const int32_t)> Events::newLayer;
diff --git a/gazebo/rendering/RenderEvents.hh b/gazebo/rendering/RenderEvents.hh
index 16815ac..8e13119 100644
--- a/gazebo/rendering/RenderEvents.hh
+++ b/gazebo/rendering/RenderEvents.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class Events Events.hh rendering/rendering.hh
/// \brief Base class for rendering events
- class GAZEBO_VISIBLE Events
+ class GZ_RENDERING_VISIBLE Events
{
/// \brief Connect to a scene created event.
/// \param[in] _subscriber Callback to trigger when event occurs.
@@ -59,11 +59,43 @@ namespace gazebo
event::ConnectionPtr _connection)
{removeScene.Disconnect(_connection);}
+ /// \brief Connect to a layer toggle event.
+ /// \param[in] _subscriber Callback to trigger when event occurs.
+ /// \return Pointer the connection. This must stay in scope.
+ public: template<typename T>
+ static event::ConnectionPtr ConnectToggleLayer(T _subscriber)
+ {return toggleLayer.Connect(_subscriber);}
+
+ /// \brief Disconnect from a layer toggle event.
+ /// \param[in] _connection The connection to disconnect.
+ public: static void DisconnectToggleLayer(
+ event::ConnectionPtr _connection)
+ {toggleLayer.Disconnect(_connection);}
+
+ /// \brief Connect to a new layer event.
+ /// \param[in] _subscriber Callback to trigger when event occurs.
+ /// \return Pointer the connection. This must stay in scope.
+ public: template<typename T>
+ static event::ConnectionPtr ConnectNewLayer(T _subscriber)
+ {return newLayer.Connect(_subscriber);}
+
+ /// \brief Disconnect from a new layer event.
+ /// \param[in] _connection The connection to disconnect.
+ public: static void DisconnectNewLayer(
+ event::ConnectionPtr _connection)
+ {newLayer.Disconnect(_connection);}
+
/// \brief The event used to trigger a create scene event.
public: static event::EventT<void (const std::string &)> createScene;
- /// \brief The event used to trigger a remve scene event.
+ /// \brief The event used to trigger a remove scene event.
public: static event::EventT<void (const std::string &)> removeScene;
+
+ /// \brief The event used to turn on/off a layer.
+ public: static event::EventT<void (const int32_t)> toggleLayer;
+
+ /// \brief The event used to indicate that a new layer is present.
+ public: static event::EventT<void (const int32_t)> newLayer;
};
/// \}
}
diff --git a/gazebo/rendering/RenderTypes.hh b/gazebo/rendering/RenderTypes.hh
index 2f9112b..a5194e1 100644
--- a/gazebo/rendering/RenderTypes.hh
+++ b/gazebo/rendering/RenderTypes.hh
@@ -58,8 +58,12 @@ namespace gazebo
class ArrowVisual;
class ContactVisual;
class COMVisual;
+ class InertiaVisual;
class RFIDVisual;
class RFIDTagVisual;
+ class ApplyWrenchVisual;
+ class OriginVisual;
+ class LinkFrameVisual;
class WindowManager;
class SelectionObj;
class RayQuery;
@@ -137,6 +141,10 @@ namespace gazebo
/// \brief Shared pointer to COMVisual
typedef boost::shared_ptr<COMVisual> COMVisualPtr;
+ /// \def InertiaVisualPtr
+ /// \brief Shared pointer to InertiaVisual
+ typedef boost::shared_ptr<InertiaVisual> InertiaVisualPtr;
+
/// \def RFIDVisual
/// \brief Shared pointer to RFIDVisual
typedef boost::shared_ptr<RFIDVisual> RFIDVisualPtr;
@@ -145,6 +153,18 @@ namespace gazebo
/// \brief Shared pointer to RFIDTagVisual
typedef boost::shared_ptr<RFIDTagVisual> RFIDTagVisualPtr;
+ /// \def ApplyWrenchVisualPtr
+ /// \brief Shared pointer to ApplyWrenchVisual
+ typedef boost::shared_ptr<ApplyWrenchVisual> ApplyWrenchVisualPtr;
+
+ /// \def OriginVisualPtr
+ /// \brief Shared pointer to OriginVisual
+ typedef boost::shared_ptr<OriginVisual> OriginVisualPtr;
+
+ /// \def LinkFrameVisualPtr
+ /// \brief Shared pointer to LinkFrameVisual
+ typedef boost::shared_ptr<LinkFrameVisual> LinkFrameVisualPtr;
+
/// \def WindowManager
/// \brief Shared pointer to WindowManager
typedef boost::shared_ptr<WindowManager> WindowManagerPtr;
diff --git a/gazebo/rendering/RenderingIface.hh b/gazebo/rendering/RenderingIface.hh
index 7fa193b..7a68673 100644
--- a/gazebo/rendering/RenderingIface.hh
+++ b/gazebo/rendering/RenderingIface.hh
@@ -29,34 +29,34 @@ namespace gazebo
/// \{
/// \brief load rendering engine.
- GAZEBO_VISIBLE
+ GZ_RENDERING_VISIBLE
bool load();
/// \brief init rendering engine.
- GAZEBO_VISIBLE
+ GZ_RENDERING_VISIBLE
bool init();
/// \brief teardown rendering engine.
- GAZEBO_VISIBLE
+ GZ_RENDERING_VISIBLE
bool fini();
/// \brief get pointer to rendering::Scene by name.
/// \param[in] _name Name of the scene to retreive.
- GAZEBO_VISIBLE
+ GZ_RENDERING_VISIBLE
rendering::ScenePtr get_scene(const std::string &_name = "");
/// \brief create rendering::Scene by name.
/// \param[in] _name Name of the scene to create.
/// \param[in] _enableVisualizations True enables visualization
/// elements such as laser lines.
- GAZEBO_VISIBLE
+ GZ_RENDERING_VISIBLE
rendering::ScenePtr create_scene(const std::string &_name,
bool _enableVisualizations,
bool _isServer = false);
/// \brief remove a rendering::Scene by name
/// \param[in] _name The name of the scene to remove.
- GAZEBO_VISIBLE
+ GZ_RENDERING_VISIBLE
void remove_scene(const std::string &_name);
/// \}
diff --git a/gazebo/rendering/Road2d.cc b/gazebo/rendering/Road2d.cc
index f693525..e093cfe 100644
--- a/gazebo/rendering/Road2d.cc
+++ b/gazebo/rendering/Road2d.cc
@@ -15,6 +15,12 @@
*
*/
+#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 <algorithm>
#include "gazebo/transport/transport.hh"
@@ -210,7 +216,7 @@ void Road2d::Segment::Load(msgs::Road _msg)
for (int i = 0; i < _msg.point_size(); ++i)
{
- this->points.push_back(msgs::Convert(_msg.point(i)));
+ this->points.push_back(msgs::ConvertIgn(_msg.point(i)));
}
this->mRenderOp.vertexData = new Ogre::VertexData;
@@ -248,7 +254,7 @@ void Road2d::Segment::Load(msgs::Road _msg)
float *vertices = static_cast<float*>(
vBuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));
- math::Vector3 pA, pB, tangent;
+ ignition::math::Vector3d pA, pB, tangent;
math::Box bounds;
bounds.min.Set(GZ_DBL_MAX, GZ_DBL_MAX, GZ_DBL_MAX);
@@ -305,16 +311,16 @@ void Road2d::Segment::Load(msgs::Road _msg)
// The tangent is used to calculate the two verteces to either side of
// the point. The vertices define the triangle mesh of the road
- double theta = atan2(tangent.x, -tangent.y);
+ double theta = atan2(tangent.X(), -tangent.Y());
pA = pB = this->points[i];
double w = (this->width * factor) * 0.5;
- pA.x += cos(theta) * w;
- pA.y += sin(theta) * w;
+ pA.X() += cos(theta) * w;
+ pA.Y() += sin(theta) * w;
- pB.x -= cos(theta) * w;
- pB.y -= sin(theta) * w;
+ pB.X() -= cos(theta) * w;
+ pB.Y() -= sin(theta) * w;
bounds.min.SetToMin(pA);
bounds.min.SetToMin(pB);
@@ -323,9 +329,9 @@ void Road2d::Segment::Load(msgs::Road _msg)
bounds.max.SetToMax(pB);
// Position
- *vertices++ = pA.x;
- *vertices++ = pA.y;
- *vertices++ = pA.z;
+ *vertices++ = pA.X();
+ *vertices++ = pA.Y();
+ *vertices++ = pA.Z();
// Normal
*vertices++ = 0;
@@ -337,9 +343,9 @@ void Road2d::Segment::Load(msgs::Road _msg)
*vertices++ = texCoord;
// Position
- *vertices++ = pB.x;
- *vertices++ = pB.y;
- *vertices++ = pB.z;
+ *vertices++ = pB.X();
+ *vertices++ = pB.Y();
+ *vertices++ = pB.Z();
// Normal
*vertices++ = 0;
diff --git a/gazebo/rendering/Road2d.hh b/gazebo/rendering/Road2d.hh
index a2efae6..21bf42d 100644
--- a/gazebo/rendering/Road2d.hh
+++ b/gazebo/rendering/Road2d.hh
@@ -40,7 +40,7 @@ namespace gazebo
/// \class Road Road.hh rendering/rendering.hh
/// \brief Used to render a strip of road.
- class GAZEBO_VISIBLE Road2d
+ class GZ_RENDERING_VISIBLE Road2d
{
/// \brief Constructor
public: Road2d();
@@ -79,7 +79,7 @@ namespace gazebo
public: std::string name;
/// \brief Point that make up the middle of the road.
- public: std::vector<math::Vector3> points;
+ public: std::vector<ignition::math::Vector3d> points;
/// \brief Width of the road.
public: double width;
diff --git a/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc
index 7a2d538..571d754 100644
--- a/gazebo/rendering/Scene.cc
+++ b/gazebo/rendering/Scene.cc
@@ -34,6 +34,8 @@
#include "gazebo/rendering/CameraVisual.hh"
#include "gazebo/rendering/JointVisual.hh"
#include "gazebo/rendering/COMVisual.hh"
+#include "gazebo/rendering/InertiaVisual.hh"
+#include "gazebo/rendering/LinkFrameVisual.hh"
#include "gazebo/rendering/ContactVisual.hh"
#include "gazebo/rendering/Conversions.hh"
#include "gazebo/rendering/Light.hh"
@@ -44,7 +46,7 @@
#include "gazebo/rendering/DepthCamera.hh"
#include "gazebo/rendering/GpuLaser.hh"
#include "gazebo/rendering/Grid.hh"
-#include "gazebo/rendering/DynamicLines.hh"
+#include "gazebo/rendering/OriginVisual.hh"
#include "gazebo/rendering/RFIDVisual.hh"
#include "gazebo/rendering/RFIDTagVisual.hh"
#include "gazebo/rendering/VideoVisual.hh"
@@ -94,6 +96,8 @@ Scene::Scene(const std::string &_name, bool _enableVisualizations,
this->dataPtr->initialized = false;
this->dataPtr->showCOMs = false;
+ this->dataPtr->showInertias = false;
+ this->dataPtr->showLinkFrames = false;
this->dataPtr->showCollisions = false;
this->dataPtr->showJoints = false;
this->dataPtr->transparent = false;
@@ -116,6 +120,10 @@ Scene::Scene(const std::string &_name, bool _enableVisualizations,
this->dataPtr->connections.push_back(
event::Events::ConnectPreRender(boost::bind(&Scene::PreRender, this)));
+ this->dataPtr->connections.push_back(
+ rendering::Events::ConnectToggleLayer(
+ boost::bind(&Scene::ToggleLayer, this, _1)));
+
this->dataPtr->sensorSub = this->dataPtr->node->Subscribe("~/sensor",
&Scene::OnSensorMsg, this, true);
this->dataPtr->visSub =
@@ -143,8 +151,6 @@ Scene::Scene(const std::string &_name, bool _enableVisualizations,
this->dataPtr->skeletonPoseSub =
this->dataPtr->node->Subscribe("~/skeleton_pose/info",
&Scene::OnSkeletonPoseMsg, this);
- this->dataPtr->selectionSub = this->dataPtr->node->Subscribe("~/selection",
- &Scene::OnSelectionMsg, this);
this->dataPtr->skySub =
this->dataPtr->node->Subscribe("~/sky", &Scene::OnSkyMsg, this);
this->dataPtr->modelInfoSub = this->dataPtr->node->Subscribe("~/model/info",
@@ -156,10 +162,6 @@ Scene::Scene(const std::string &_name, bool _enableVisualizations,
this->dataPtr->requestSub = this->dataPtr->node->Subscribe("~/request",
&Scene::OnRequest, this);
- // \TODO: This causes the Scene to occasionally miss the response to
- // scene_info
- // this->responsePub =
- this->dataPtr->node->Advertise<msgs::Response>("~/response");
this->dataPtr->responseSub = this->dataPtr->node->Subscribe("~/response",
&Scene::OnResponse, this, true);
this->dataPtr->sceneSub =
@@ -185,30 +187,71 @@ void Scene::Clear()
this->dataPtr->poseMsgs.clear();
this->dataPtr->sceneMsgs.clear();
this->dataPtr->jointMsgs.clear();
- this->dataPtr->joints.clear();
this->dataPtr->linkMsgs.clear();
- this->dataPtr->cameras.clear();
- this->dataPtr->userCameras.clear();
- this->dataPtr->lights.clear();
+ this->dataPtr->sensorMsgs.clear();
+
+ this->dataPtr->poseSub.reset();
+ this->dataPtr->jointSub.reset();
+ this->dataPtr->sensorSub.reset();
+ this->dataPtr->sceneSub.reset();
+ this->dataPtr->skeletonPoseSub.reset();
+ this->dataPtr->visSub.reset();
+ this->dataPtr->skySub.reset();
+ this->dataPtr->lightSub.reset();
+ this->dataPtr->requestSub.reset();
+ this->dataPtr->responseSub.reset();
+ this->dataPtr->modelInfoSub.reset();
+ this->dataPtr->lightPub.reset();
+ this->dataPtr->responsePub.reset();
+ this->dataPtr->requestPub.reset();
+
+ this->dataPtr->joints.clear();
delete this->dataPtr->terrain;
this->dataPtr->terrain = NULL;
- // Erase the world visual
- this->dataPtr->visuals.erase(this->dataPtr->visuals.begin());
-
while (!this->dataPtr->visuals.empty())
- if (this->dataPtr->visuals.begin()->second)
- this->RemoveVisual(this->dataPtr->visuals.begin()->second);
+ this->RemoveVisual(this->dataPtr->visuals.begin()->first);
+
this->dataPtr->visuals.clear();
+ if (this->dataPtr->originVisual)
+ {
+ this->dataPtr->originVisual->Fini();
+ this->dataPtr->originVisual.reset();
+ }
+
+ if (this->dataPtr->worldVisual)
+ {
+ this->dataPtr->worldVisual->Fini();
+ this->dataPtr->worldVisual.reset();
+ }
+
+ while (!this->dataPtr->lights.empty())
+ if (this->dataPtr->lights.begin()->second)
+ this->RemoveLight(this->dataPtr->lights.begin()->second);
+ this->dataPtr->lights.clear();
+
for (uint32_t i = 0; i < this->dataPtr->grids.size(); ++i)
delete this->dataPtr->grids[i];
this->dataPtr->grids.clear();
- this->dataPtr->sensorMsgs.clear();
+ for (unsigned int i = 0; i < this->dataPtr->cameras.size(); ++i)
+ this->dataPtr->cameras[i]->Fini();
+ this->dataPtr->cameras.clear();
+
+ for (unsigned int i = 0; i < this->dataPtr->userCameras.size(); ++i)
+ this->dataPtr->userCameras[i]->Fini();
+ this->dataPtr->userCameras.clear();
+
+ delete this->dataPtr->skyx;
+ this->dataPtr->skyx = NULL;
+
+ RTShaderSystem::Instance()->RemoveScene(this->GetName());
RTShaderSystem::Instance()->Clear();
+ this->dataPtr->connections.clear();
+
this->dataPtr->initialized = false;
}
@@ -216,53 +259,14 @@ void Scene::Clear()
Scene::~Scene()
{
delete this->dataPtr->requestMsg;
+ this->dataPtr->requestMsg = NULL;
delete this->dataPtr->receiveMutex;
- delete this->dataPtr->raySceneQuery;
-
- this->dataPtr->node->Fini();
- this->dataPtr->node.reset();
- this->dataPtr->visSub.reset();
- this->dataPtr->lightSub.reset();
- this->dataPtr->poseSub.reset();
- this->dataPtr->jointSub.reset();
- this->dataPtr->skeletonPoseSub.reset();
- this->dataPtr->selectionSub.reset();
-
- Visual_M::iterator iter;
- this->dataPtr->visuals.clear();
- this->dataPtr->jointMsgs.clear();
- this->dataPtr->joints.clear();
- this->dataPtr->linkMsgs.clear();
- this->dataPtr->sceneMsgs.clear();
- this->dataPtr->poseMsgs.clear();
- this->dataPtr->lightMsgs.clear();
- this->dataPtr->visualMsgs.clear();
-
- this->dataPtr->worldVisual.reset();
- this->dataPtr->selectionMsg.reset();
- this->dataPtr->lights.clear();
-
- // Remove a scene
- RTShaderSystem::Instance()->RemoveScene(shared_from_this());
+ this->dataPtr->receiveMutex = NULL;
- for (uint32_t i = 0; i < this->dataPtr->grids.size(); ++i)
- delete this->dataPtr->grids[i];
- this->dataPtr->grids.clear();
-
- for (unsigned int i = 0; i < this->dataPtr->cameras.size(); ++i)
- this->dataPtr->cameras[i].reset();
- this->dataPtr->cameras.clear();
-
- for (unsigned int i = 0; i < this->dataPtr->userCameras.size(); ++i)
- this->dataPtr->userCameras[i].reset();
- this->dataPtr->userCameras.clear();
+ // raySceneQuery deletion handled by ogre
+ this->dataPtr->raySceneQuery= NULL;
- if (this->dataPtr->manager)
- {
- RenderEngine::Instance()->root->destroySceneManager(this->dataPtr->manager);
- this->dataPtr->manager = NULL;
- }
- this->dataPtr->connections.clear();
+ this->Clear();
this->dataPtr->sdf->Reset();
this->dataPtr->sdf.reset();
@@ -354,6 +358,11 @@ void Scene::Init()
// Force shadows on.
this->SetShadowsEnabled(true);
+ // Create origin visual
+ this->dataPtr->originVisual.reset(new OriginVisual("__WORLD_ORIGIN__",
+ this->dataPtr->worldVisual));
+ this->dataPtr->originVisual->Load();
+
this->dataPtr->requestPub->WaitForConnection();
this->dataPtr->requestMsg = msgs::CreateRequest("scene_info");
this->dataPtr->requestPub->Publish(*this->dataPtr->requestMsg);
@@ -610,9 +619,11 @@ uint32_t Scene::GetOculusCameraCount() const
#endif
//////////////////////////////////////////////////
-UserCameraPtr Scene::CreateUserCamera(const std::string &_name)
+UserCameraPtr Scene::CreateUserCamera(const std::string &_name,
+ bool _stereoEnabled)
{
- UserCameraPtr camera(new UserCamera(_name, shared_from_this()));
+ UserCameraPtr camera(new UserCamera(_name, shared_from_this(),
+ _stereoEnabled));
camera->Load();
camera->Init();
this->dataPtr->userCameras.push_back(camera);
@@ -1466,6 +1477,9 @@ bool Scene::ProcessSceneMsg(ConstScenePtr &_msg)
if (_msg->has_grid())
this->SetGrid(_msg->grid());
+ if (_msg->has_origin_visual())
+ this->ShowOrigin(_msg->origin_visual());
+
// Process the sky message.
if (_msg->has_sky())
{
@@ -1511,7 +1525,6 @@ bool Scene::ProcessSceneMsg(ConstScenePtr &_msg)
elem->Get<double>("start"),
elem->Get<double>("end"));
}
-
return true;
}
@@ -1521,11 +1534,11 @@ bool Scene::ProcessModelMsg(const msgs::Model &_msg)
std::string modelName, linkName;
modelName = _msg.name() + "::";
- for (int j = 0; j < _msg.visual_size(); j++)
+ for (int j = 0; j < _msg.visual_size(); ++j)
{
boost::shared_ptr<msgs::Visual> vm(new msgs::Visual(
_msg.visual(j)));
- this->dataPtr->visualMsgs.push_back(vm);
+ this->dataPtr->modelVisualMsgs.push_back(vm);
}
// Set the scale of the model visual
@@ -1540,16 +1553,16 @@ bool Scene::ProcessModelMsg(const msgs::Model &_msg)
vm->mutable_scale()->set_x(_msg.scale().x());
vm->mutable_scale()->set_y(_msg.scale().y());
vm->mutable_scale()->set_z(_msg.scale().z());
- this->dataPtr->visualMsgs.push_back(vm);
+ this->dataPtr->modelVisualMsgs.push_back(vm);
}
- for (int j = 0; j < _msg.joint_size(); j++)
+ for (int j = 0; j < _msg.joint_size(); ++j)
{
boost::shared_ptr<msgs::Joint> jm(new msgs::Joint(
_msg.joint(j)));
this->dataPtr->jointMsgs.push_back(jm);
- for (int k = 0; k < _msg.joint(j).sensor_size(); k++)
+ for (int k = 0; k < _msg.joint(j).sensor_size(); ++k)
{
boost::shared_ptr<msgs::Sensor> sm(new msgs::Sensor(
_msg.joint(j).sensor(k)));
@@ -1557,7 +1570,7 @@ bool Scene::ProcessModelMsg(const msgs::Model &_msg)
}
}
- for (int j = 0; j < _msg.link_size(); j++)
+ for (int j = 0; j < _msg.link_size(); ++j)
{
linkName = modelName + _msg.link(j).name();
@@ -1584,25 +1597,33 @@ bool Scene::ProcessModelMsg(const msgs::Model &_msg)
this->dataPtr->linkMsgs.push_back(lm);
}
- for (int k = 0; k < _msg.link(j).visual_size(); k++)
+ if (_msg.link(j).visual_size() > 0)
+ {
+ // note: the first visual in the link is the link visual
+ msgs::VisualPtr vm(new msgs::Visual(
+ _msg.link(j).visual(0)));
+ this->dataPtr->linkVisualMsgs.push_back(vm);
+ }
+
+ for (int k = 1; k < _msg.link(j).visual_size(); ++k)
{
boost::shared_ptr<msgs::Visual> vm(new msgs::Visual(
_msg.link(j).visual(k)));
this->dataPtr->visualMsgs.push_back(vm);
}
- for (int k = 0; k < _msg.link(j).collision_size(); k++)
+ for (int k = 0; k < _msg.link(j).collision_size(); ++k)
{
for (int l = 0;
l < _msg.link(j).collision(k).visual_size(); l++)
{
boost::shared_ptr<msgs::Visual> vm(new msgs::Visual(
_msg.link(j).collision(k).visual(l)));
- this->dataPtr->visualMsgs.push_back(vm);
+ this->dataPtr->collisionVisualMsgs.push_back(vm);
}
}
- for (int k = 0; k < _msg.link(j).sensor_size(); k++)
+ for (int k = 0; k < _msg.link(j).sensor_size(); ++k)
{
boost::shared_ptr<msgs::Sensor> sm(new msgs::Sensor(
_msg.link(j).sensor(k)));
@@ -1690,7 +1711,10 @@ void Scene::PreRender()
ModelMsgs_L modelMsgsCopy;
SensorMsgs_L sensorMsgsCopy;
LightMsgs_L lightMsgsCopy;
+ VisualMsgs_L modelVisualMsgsCopy;
+ VisualMsgs_L linkVisualMsgsCopy;
VisualMsgs_L visualMsgsCopy;
+ VisualMsgs_L collisionVisualMsgsCopy;
JointMsgs_L jointMsgsCopy;
LinkMsgs_L linkMsgsCopy;
@@ -1714,12 +1738,27 @@ void Scene::PreRender()
std::back_inserter(lightMsgsCopy));
this->dataPtr->lightMsgs.clear();
+ std::copy(this->dataPtr->modelVisualMsgs.begin(),
+ this->dataPtr->modelVisualMsgs.end(),
+ std::back_inserter(modelVisualMsgsCopy));
+ this->dataPtr->modelVisualMsgs.clear();
+
+ std::copy(this->dataPtr->linkVisualMsgs.begin(),
+ this->dataPtr->linkVisualMsgs.end(),
+ std::back_inserter(linkVisualMsgsCopy));
+ this->dataPtr->linkVisualMsgs.clear();
+
this->dataPtr->visualMsgs.sort(VisualMessageLessOp);
std::copy(this->dataPtr->visualMsgs.begin(),
this->dataPtr->visualMsgs.end(),
std::back_inserter(visualMsgsCopy));
this->dataPtr->visualMsgs.clear();
+ std::copy(this->dataPtr->collisionVisualMsgs.begin(),
+ this->dataPtr->collisionVisualMsgs.end(),
+ std::back_inserter(collisionVisualMsgsCopy));
+ this->dataPtr->collisionVisualMsgs.clear();
+
std::copy(this->dataPtr->jointMsgs.begin(), this->dataPtr->jointMsgs.end(),
std::back_inserter(jointMsgsCopy));
this->dataPtr->jointMsgs.clear();
@@ -1770,15 +1809,49 @@ void Scene::PreRender()
++lightIter;
}
+ // Process the model visual messages.
+ for (visualIter = modelVisualMsgsCopy.begin();
+ visualIter != modelVisualMsgsCopy.end();)
+ {
+ if (this->ProcessVisualMsg(*visualIter, Visual::VT_MODEL))
+ modelVisualMsgsCopy.erase(visualIter++);
+ else
+ ++visualIter;
+ }
+
+ // Process the link visual messages.
+ for (visualIter = linkVisualMsgsCopy.begin();
+ visualIter != linkVisualMsgsCopy.end();)
+ {
+ if (this->ProcessVisualMsg(*visualIter, Visual::VT_LINK))
+ linkVisualMsgsCopy.erase(visualIter++);
+ else
+ ++visualIter;
+ }
+
// Process the visual messages.
for (visualIter = visualMsgsCopy.begin(); visualIter != visualMsgsCopy.end();)
{
- if (this->ProcessVisualMsg(*visualIter))
+ Visual::VisualType visualType = Visual::VT_VISUAL;
+ if ((*visualIter)->has_type())
+ visualType = Visual::ConvertVisualType((*visualIter)->type());
+
+ if (this->ProcessVisualMsg(*visualIter, visualType))
visualMsgsCopy.erase(visualIter++);
else
++visualIter;
}
+ // Process the collision visual messages.
+ for (visualIter = collisionVisualMsgsCopy.begin();
+ visualIter != collisionVisualMsgsCopy.end();)
+ {
+ if (this->ProcessVisualMsg(*visualIter, Visual::VT_COLLISION))
+ collisionVisualMsgsCopy.erase(visualIter++);
+ else
+ ++visualIter;
+ }
+
// Process the joint messages.
for (jointIter = jointMsgsCopy.begin(); jointIter != jointMsgsCopy.end();)
{
@@ -1820,9 +1893,18 @@ void Scene::PreRender()
std::copy(lightMsgsCopy.begin(), lightMsgsCopy.end(),
std::front_inserter(this->dataPtr->lightMsgs));
+ std::copy(modelVisualMsgsCopy.begin(), modelVisualMsgsCopy.end(),
+ std::front_inserter(this->dataPtr->modelVisualMsgs));
+
+ std::copy(linkVisualMsgsCopy.begin(), linkVisualMsgsCopy.end(),
+ std::front_inserter(this->dataPtr->linkVisualMsgs));
+
std::copy(visualMsgsCopy.begin(), visualMsgsCopy.end(),
std::front_inserter(this->dataPtr->visualMsgs));
+ std::copy(collisionVisualMsgsCopy.begin(), collisionVisualMsgsCopy.end(),
+ std::front_inserter(this->dataPtr->collisionVisualMsgs));
+
std::copy(jointMsgsCopy.begin(), jointMsgsCopy.end(),
std::front_inserter(this->dataPtr->jointMsgs));
@@ -1847,7 +1929,7 @@ void Scene::PreRender()
|| this->dataPtr->selectionMode != "move" ||
iter->first != this->dataPtr->selectedVis->GetId())
{
- math::Pose pose = msgs::Convert(pIter->second);
+ ignition::math::Pose3d pose = msgs::ConvertIgn(pIter->second);
GZ_ASSERT(iter->second, "Visual pointer is NULL");
iter->second->SetPose(pose);
PoseMsgs_M::iterator prev = pIter++;
@@ -1879,7 +1961,7 @@ void Scene::PreRender()
this->dataPtr->selectionMode != "move" ||
iter->first != this->dataPtr->selectedVis->GetId())
{
- math::Pose pose = msgs::Convert(pose_msg);
+ ignition::math::Pose3d pose = msgs::ConvertIgn(pose_msg);
iter2->second->SetPose(pose);
}
}
@@ -1899,15 +1981,6 @@ void Scene::PreRender()
// official time stamp of approval
this->dataPtr->sceneSimTimePosesApplied =
this->dataPtr->sceneSimTimePosesReceived;
-
- if (this->dataPtr->selectionMsg)
- {
- if (!this->dataPtr->selectedVis ||
- this->dataPtr->selectionMsg->name() !=
- this->dataPtr->selectedVis->GetName())
- this->SelectVisual(this->dataPtr->selectionMsg->name(), "normal");
- this->dataPtr->selectionMsg.reset();
- }
}
}
@@ -2002,7 +2075,7 @@ bool Scene::ProcessSensorMsg(ConstSensorPtr &_msg)
// parentVis' children list so that it can be properly deleted.
parentVis->AttachVisual(cameraVis);
- cameraVis->SetPose(msgs::Convert(_msg->pose()));
+ cameraVis->SetPose(msgs::ConvertIgn(_msg->pose()));
cameraVis->SetId(_msg->id());
cameraVis->Load(_msg->camera());
this->dataPtr->visuals[cameraVis->GetId()] = cameraVis;
@@ -2083,6 +2156,16 @@ bool Scene::ProcessLinkMsg(ConstLinkPtr &_msg)
this->CreateCOMVisual(_msg, linkVis);
}
+ if (!this->GetVisual(_msg->name() + "_INERTIA_VISUAL__"))
+ {
+ this->CreateInertiaVisual(_msg, linkVis);
+ }
+
+ if (!this->GetVisual(_msg->name() + "_LINK_FRAME_VISUAL__"))
+ {
+ this->CreateLinkFrameVisual(_msg, linkVis);
+ }
+
for (int i = 0; i < _msg->projector_size(); ++i)
{
std::string pname = _msg->name() + "::" + _msg->projector(i).name();
@@ -2142,6 +2225,8 @@ void Scene::OnResponse(ConstResponsePtr &_msg)
msgs::Scene sceneMsg;
sceneMsg.ParseFromString(_msg->serialized_data());
boost::shared_ptr<msgs::Scene> sm(new msgs::Scene(sceneMsg));
+
+ boost::mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
this->dataPtr->sceneMsgs.push_back(sm);
this->dataPtr->requestMsg = NULL;
}
@@ -2274,7 +2359,7 @@ void Scene::ProcessRequestMsg(ConstRequestPtr &_msg)
if (vis)
vis->ShowCOM(true);
else
- gzerr << "Unable to find joint visual[" << _msg->data() << "]\n";
+ gzerr << "Unable to find COM visual[" << _msg->data() << "]\n";
}
}
else if (_msg->request() == "hide_com")
@@ -2288,6 +2373,54 @@ void Scene::ProcessRequestMsg(ConstRequestPtr &_msg)
vis->ShowCOM(false);
}
}
+ else if (_msg->request() == "show_inertia")
+ {
+ if (_msg->data() == "all")
+ this->ShowInertias(true);
+ else
+ {
+ VisualPtr vis = this->GetVisual(_msg->data());
+ if (vis)
+ vis->ShowInertia(true);
+ else
+ gzerr << "Unable to find inertia visual[" << _msg->data() << "]\n";
+ }
+ }
+ else if (_msg->request() == "hide_inertia")
+ {
+ if (_msg->data() == "all")
+ this->ShowInertias(false);
+ else
+ {
+ VisualPtr vis = this->GetVisual(_msg->data());
+ if (vis)
+ vis->ShowInertia(false);
+ }
+ }
+ else if (_msg->request() == "show_link_frame")
+ {
+ if (_msg->data() == "all")
+ this->ShowLinkFrames(true);
+ else
+ {
+ VisualPtr vis = this->GetVisual(_msg->data());
+ if (vis)
+ vis->ShowLinkFrame(true);
+ else
+ gzerr << "Unable to find link frame visual[" << _msg->data() << "]\n";
+ }
+ }
+ else if (_msg->request() == "hide_link_frame")
+ {
+ if (_msg->data() == "all")
+ this->ShowLinkFrames(false);
+ else
+ {
+ VisualPtr vis = this->GetVisual(_msg->data());
+ if (vis)
+ vis->ShowLinkFrame(false);
+ }
+ }
else if (_msg->request() == "set_transparent")
{
if (_msg->data() == "all")
@@ -2342,7 +2475,7 @@ void Scene::ProcessRequestMsg(ConstRequestPtr &_msg)
}
/////////////////////////////////////////////////
-bool Scene::ProcessVisualMsg(ConstVisualPtr &_msg)
+bool Scene::ProcessVisualMsg(ConstVisualPtr &_msg, Visual::VisualType _type)
{
bool result = false;
Visual_M::iterator iter = this->dataPtr->visuals.end();
@@ -2435,6 +2568,8 @@ bool Scene::ProcessVisualMsg(ConstVisualPtr &_msg)
{
result = true;
visual->LoadFromMsg(_msg);
+ visual->SetType(_type);
+
this->dataPtr->visuals[visual->GetId()] = visual;
if (visual->GetName().find("__COLLISION_VISUAL__") != std::string::npos ||
visual->GetName().find("__SKELETON_VISUAL__") != std::string::npos)
@@ -2444,12 +2579,12 @@ bool Scene::ProcessVisualMsg(ConstVisualPtr &_msg)
}
visual->ShowCOM(this->dataPtr->showCOMs);
+ visual->ShowInertia(this->dataPtr->showInertias);
+ visual->ShowLinkFrame(this->dataPtr->showLinkFrames);
visual->ShowCollision(this->dataPtr->showCollisions);
visual->ShowJoints(this->dataPtr->showJoints);
visual->SetTransparency(this->dataPtr->transparent ? 0.5 : 0.0);
visual->SetWireframe(this->dataPtr->wireframe);
-
- visual->UpdateFromMsg(_msg);
}
}
@@ -2533,12 +2668,6 @@ bool Scene::ProcessLightMsg(ConstLightPtr &_msg)
}
/////////////////////////////////////////////////
-void Scene::OnSelectionMsg(ConstSelectionPtr &_msg)
-{
- this->dataPtr->selectionMsg = _msg;
-}
-
-/////////////////////////////////////////////////
void Scene::OnModelMsg(ConstModelPtr &_msg)
{
boost::mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
@@ -2551,6 +2680,9 @@ void Scene::OnSkyMsg(ConstSkyPtr &_msg)
if (!this->dataPtr->skyx)
return;
+ Ogre::Root::getSingletonPtr()->addFrameListener(this->dataPtr->skyx);
+ this->dataPtr->skyx->update(0);
+
this->dataPtr->skyx->setVisible(true);
SkyX::VClouds::VClouds *vclouds =
@@ -2688,15 +2820,26 @@ void Scene::SetSky()
// vclouds->getLightningManager()->setLightningTimeMultiplier(
// preset.vcLightningsTM);
- Ogre::Root::getSingletonPtr()->addFrameListener(this->dataPtr->skyx);
-
- this->dataPtr->skyx->update(0);
this->dataPtr->skyx->setVisible(false);
}
/////////////////////////////////////////////////
void Scene::SetShadowsEnabled(bool _value)
{
+ // If a usercamera is set to stereo mode, then turn off shadows.
+ // If a usercamera uses orthographic projection, then turn off shadows.
+ // Our shadow mapping technique disables stereo.
+ bool shadowOverride = true;
+ for (std::vector<UserCameraPtr>::iterator iter =
+ this->dataPtr->userCameras.begin();
+ iter != this->dataPtr->userCameras.end() && shadowOverride; ++iter)
+ {
+ shadowOverride = !(*iter)->StereoEnabled() &&
+ (*iter)->GetProjectionType() != "orthographic";
+ }
+
+ _value = _value && shadowOverride;
+
this->dataPtr->sdf->GetElement("shadows")->Set(_value);
if (RenderEngine::Instance()->GetRenderPathType() == RenderEngine::DEFERRED)
@@ -2756,26 +2899,28 @@ void Scene::AddVisual(VisualPtr _vis)
if (this->dataPtr->visuals.find(_vis->GetId()) !=
this->dataPtr->visuals.end())
{
- gzerr << "Duplicate visuals detected[" << _vis->GetName() << "]\n";
+ gzwarn << "Duplicate visuals detected[" << _vis->GetName() << "]\n";
}
this->dataPtr->visuals[_vis->GetId()] = _vis;
}
/////////////////////////////////////////////////
-void Scene::RemoveVisual(VisualPtr _vis)
+void Scene::RemoveVisual(uint32_t _id)
{
- if (_vis)
+ // Delete the visual
+ auto iter = this->dataPtr->visuals.find(_id);
+ if (iter != this->dataPtr->visuals.end())
{
+ VisualPtr vis = iter->second;
// Remove all projectors attached to the visual
- std::map<std::string, Projector *>::iterator piter =
- this->dataPtr->projectors.begin();
+ auto piter = this->dataPtr->projectors.begin();
while (piter != this->dataPtr->projectors.end())
{
// Check to see if the projector is a child of the visual that is
// being removed.
if (piter->second->GetParent()->GetRootVisual()->GetName() ==
- _vis->GetRootVisual()->GetName())
+ vis->GetRootVisual()->GetName())
{
delete piter->second;
this->dataPtr->projectors.erase(piter++);
@@ -2784,21 +2929,38 @@ void Scene::RemoveVisual(VisualPtr _vis)
++piter;
}
- // Delete the visual
- Visual_M::iterator iter = this->dataPtr->visuals.find(_vis->GetId());
- if (iter != this->dataPtr->visuals.end())
- {
- iter->second->Fini();
- this->dataPtr->visuals.erase(iter);
- }
+ this->RemoveVisualizations(vis);
+ vis->Fini();
+ this->dataPtr->visuals.erase(iter);
if (this->dataPtr->selectedVis && this->dataPtr->selectedVis->GetId() ==
- _vis->GetId())
+ vis->GetId())
this->dataPtr->selectedVis.reset();
}
}
/////////////////////////////////////////////////
+void Scene::RemoveVisual(VisualPtr _vis)
+{
+ this->RemoveVisual(_vis->GetId());
+}
+
+/////////////////////////////////////////////////
+void Scene::SetVisualId(VisualPtr _vis, uint32_t _id)
+{
+ if (!_vis)
+ return;
+
+ auto iter = this->dataPtr->visuals.find(_vis->GetId());
+ if (iter != this->dataPtr->visuals.end())
+ {
+ this->dataPtr->visuals.erase(_vis->GetId());
+ this->dataPtr->visuals[_id] = _vis;
+ _vis->SetId(_id);
+ }
+}
+
+/////////////////////////////////////////////////
void Scene::AddLight(LightPtr _light)
{
std::string n = this->StripSceneName(_light->GetName());
@@ -2842,10 +3004,16 @@ void Scene::SetGrid(bool _enabled)
}
}
+/////////////////////////////////////////////////
+void Scene::ShowOrigin(bool _show)
+{
+ this->dataPtr->originVisual->SetVisible(_show);
+}
+
//////////////////////////////////////////////////
std::string Scene::StripSceneName(const std::string &_name) const
{
- if (_name.find(this->GetName() + "::") == 0)
+ if (_name.find(this->GetName() + "::") != std::string::npos)
return _name.substr(this->GetName().size() + 2);
else
return _name;
@@ -2879,13 +3047,59 @@ void Scene::CreateCOMVisual(sdf::ElementPtr _elem, VisualPtr _linkVisual)
}
/////////////////////////////////////////////////
+void Scene::CreateInertiaVisual(ConstLinkPtr &_msg, VisualPtr _linkVisual)
+{
+ InertiaVisualPtr inertiaVis(new InertiaVisual(_msg->name() +
+ "_INERTIA_VISUAL__", _linkVisual));
+ inertiaVis->Load(_msg);
+ inertiaVis->SetVisible(this->dataPtr->showInertias);
+ this->dataPtr->visuals[inertiaVis->GetId()] = inertiaVis;
+}
+
+/////////////////////////////////////////////////
+void Scene::CreateInertiaVisual(sdf::ElementPtr _elem, VisualPtr _linkVisual)
+{
+ InertiaVisualPtr inertiaVis(new InertiaVisual(_linkVisual->GetName() +
+ "_INERTIA_VISUAL__", _linkVisual));
+ inertiaVis->Load(_elem);
+ inertiaVis->SetVisible(false);
+ this->dataPtr->visuals[inertiaVis->GetId()] = inertiaVis;
+}
+
+/////////////////////////////////////////////////
+void Scene::CreateLinkFrameVisual(ConstLinkPtr &_msg, VisualPtr _linkVisual)
+{
+ LinkFrameVisualPtr linkFrameVis(new LinkFrameVisual(_msg->name() +
+ "_LINK_FRAME_VISUAL__", _linkVisual));
+ linkFrameVis->Load();
+ linkFrameVis->SetVisible(this->dataPtr->showLinkFrames);
+ this->dataPtr->visuals[linkFrameVis->GetId()] = linkFrameVis;
+}
+
+/////////////////////////////////////////////////
+void Scene::RemoveVisualizations(rendering::VisualPtr _vis)
+{
+ std::vector<VisualPtr> toRemove;
+ for (unsigned int i = 0; i < _vis->GetChildCount(); ++i)
+ {
+ rendering::VisualPtr childVis = _vis->GetChild(i);
+ Visual::VisualType visType = childVis->GetType();
+ if (visType == Visual::VT_PHYSICS || visType == Visual::VT_SENSOR)
+ {
+ toRemove.push_back(childVis);
+ }
+ }
+ for (auto vis : toRemove)
+ this->RemoveVisual(vis);
+}
+
+/////////////////////////////////////////////////
void Scene::SetWireframe(bool _show)
{
this->dataPtr->wireframe = _show;
- for (Visual_M::iterator iter = this->dataPtr->visuals.begin();
- iter != this->dataPtr->visuals.end(); ++iter)
+ for (auto visual : this->dataPtr->visuals)
{
- iter->second->SetWireframe(_show);
+ visual.second->SetWireframe(_show);
}
if (this->dataPtr->terrain)
@@ -2896,10 +3110,14 @@ void Scene::SetWireframe(bool _show)
void Scene::SetTransparent(bool _show)
{
this->dataPtr->transparent = _show;
- for (Visual_M::iterator iter = this->dataPtr->visuals.begin();
- iter != this->dataPtr->visuals.end(); ++iter)
+ for (auto visual : this->dataPtr->visuals)
{
- iter->second->SetTransparency(_show ? 0.5 : 0.0);
+ if (visual.second->GetType() != Visual::VT_GUI &&
+ visual.second->GetType() != Visual::VT_PHYSICS &&
+ visual.second->GetType() != Visual::VT_SENSOR)
+ {
+ visual.second->SetTransparency(_show ? 0.5 : 0.0);
+ }
}
}
@@ -2907,10 +3125,29 @@ void Scene::SetTransparent(bool _show)
void Scene::ShowCOMs(bool _show)
{
this->dataPtr->showCOMs = _show;
- for (Visual_M::iterator iter = this->dataPtr->visuals.begin();
- iter != this->dataPtr->visuals.end(); ++iter)
+ for (auto visual : this->dataPtr->visuals)
+ {
+ visual.second->ShowCOM(_show);
+ }
+}
+
+/////////////////////////////////////////////////
+void Scene::ShowInertias(bool _show)
+{
+ this->dataPtr->showInertias = _show;
+ for (auto visual : this->dataPtr->visuals)
+ {
+ visual.second->ShowInertia(_show);
+ }
+}
+
+/////////////////////////////////////////////////
+void Scene::ShowLinkFrames(bool _show)
+{
+ this->dataPtr->showLinkFrames = _show;
+ for (auto visual : this->dataPtr->visuals)
{
- iter->second->ShowCOM(_show);
+ visual.second->ShowLinkFrame(_show);
}
}
@@ -2918,10 +3155,9 @@ void Scene::ShowCOMs(bool _show)
void Scene::ShowCollisions(bool _show)
{
this->dataPtr->showCollisions = _show;
- for (Visual_M::iterator iter = this->dataPtr->visuals.begin();
- iter != this->dataPtr->visuals.end(); ++iter)
+ for (auto visual : this->dataPtr->visuals)
{
- iter->second->ShowCollision(_show);
+ visual.second->ShowCollision(_show);
}
}
@@ -2929,10 +3165,9 @@ void Scene::ShowCollisions(bool _show)
void Scene::ShowJoints(bool _show)
{
this->dataPtr->showJoints = _show;
- for (Visual_M::iterator iter = this->dataPtr->visuals.begin();
- iter != this->dataPtr->visuals.end(); ++iter)
+ for (auto visual : this->dataPtr->visuals)
{
- iter->second->ShowJoints(_show);
+ visual.second->ShowJoints(_show);
}
}
@@ -3031,3 +3266,12 @@ void Scene::RemoveProjectors()
}
this->dataPtr->projectors.clear();
}
+
+/////////////////////////////////////////////////
+void Scene::ToggleLayer(const int32_t _layer)
+{
+ for (auto visual : this->dataPtr->visuals)
+ {
+ visual.second->ToggleLayer(_layer);
+ }
+}
diff --git a/gazebo/rendering/Scene.hh b/gazebo/rendering/Scene.hh
index 9e700ad..f3ae10f 100644
--- a/gazebo/rendering/Scene.hh
+++ b/gazebo/rendering/Scene.hh
@@ -31,6 +31,7 @@
#include "gazebo/math/Vector2i.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/rendering/RenderTypes.hh"
+#include "gazebo/rendering/Visual.hh"
#include "gazebo/transport/TransportTypes.hh"
#include "gazebo/util/system.hh"
@@ -65,7 +66,8 @@ namespace gazebo
/// \brief Representation of an entire scene graph.
///
/// Maintains all the Visuals, Lights, and Cameras for a World.
- class GAZEBO_VISIBLE Scene : public boost::enable_shared_from_this<Scene>
+ class GZ_RENDERING_VISIBLE Scene :
+ public boost::enable_shared_from_this<Scene>
{
public: enum SkyXMode {
GZ_SKYX_ALL = 0x0FFFFFFF,
@@ -197,8 +199,11 @@ namespace gazebo
///
/// A user camera is one design for use with a GUI.
/// \param[in] _name Name of the UserCamera.
+ /// \param[in] _stereoEnabled True to enable stereo rendering. This is
+ /// here for compatibility with 3D monitors/TVs.
/// \return A pointer to the new UserCamera.
- public: UserCameraPtr CreateUserCamera(const std::string &_name);
+ public: UserCameraPtr CreateUserCamera(const std::string &_name,
+ bool _stereoEnabled = false);
/// \brief Get the number of user cameras in this scene
/// \return The number of user cameras.
@@ -357,6 +362,17 @@ namespace gazebo
/// \param[in] _vis Visual to remove.
public: void RemoveVisual(VisualPtr _vis);
+ /// \brief Remove a visual from the scene.
+ /// \param[in] _id Id of the visual to remove.
+ public: void RemoveVisual(uint32_t _id);
+
+ /// \internal
+ /// \brief Set the id of a visual. Internally used when visual ids'
+ /// are required to be updated from visual msgs.
+ /// \param[in] _vis Pointer to visual.
+ /// \param[in] _id New id to set to.
+ public: void SetVisualId(VisualPtr _vis, uint32_t _id);
+
/// \brief Add a light to the scene
/// \param[in] _light Light to add.
public: void AddLight(LightPtr _light);
@@ -369,6 +385,10 @@ namespace gazebo
/// \param[in] _enabled Set to true to turn on the grid
public: void SetGrid(bool _enabled);
+ /// \brief Show/hide the world origin indicator.
+ /// \param[in] _show True to show the origin.
+ public: void ShowOrigin(bool _show);
+
/// \brief Get the top level world visual.
/// \return Pointer to the world visual.
public: VisualPtr GetWorldVisual() const;
@@ -402,6 +422,14 @@ namespace gazebo
/// \param[in] _show True to enable center of mass visualization.
public: void ShowCOMs(bool _show);
+ /// \brief Enable or disable inertia visualization.
+ /// \param[in] _show True to enable inertia visualization.
+ public: void ShowInertias(bool _show);
+
+ /// \brief Enable or disable link frame visualization.
+ /// \param[in] _show True to enable link frame visualization.
+ public: void ShowLinkFrames(bool _show);
+
/// \brief Enable or disable joint visualization.
/// \param[in] _show True to enable joint visualization.
public: void ShowJoints(bool _show);
@@ -450,6 +478,12 @@ namespace gazebo
/// \brief Remove all projectors.
public: void RemoveProjectors();
+ /// \brief Toggle layer visilibility. This will process all visuals.
+ /// If a visual is on the specified layer its visiblity will be
+ /// toggled. Visuals with a negative layer index are always visible.
+ /// \param[in] _layer Index of the layer to toggle.
+ public: void ToggleLayer(const int32_t _layer);
+
/// \brief Helper function to setup the sky.
private: void SetSky();
@@ -538,7 +572,10 @@ namespace gazebo
/// \brief Process a visual message.
/// \param[in] _msg The message data.
- private: bool ProcessVisualMsg(ConstVisualPtr &_msg);
+ /// \param[in] _type Type of visual.
+ /// \return True if message is processed successfully.
+ private: bool ProcessVisualMsg(ConstVisualPtr &_msg,
+ Visual::VisualType _type = Visual::VT_ENTITY);
/// \brief Light message callback.
/// \param[in] _msg The message data.
@@ -552,10 +589,6 @@ namespace gazebo
/// \param[in] _msg The message data.
private: void ProcessRequestMsg(ConstRequestPtr &_msg);
- /// \brief Selection message callback.
- /// \param[in] _msg The message data.
- private: void OnSelectionMsg(ConstSelectionPtr &_msg);
-
/// \brief Sky message callback.
/// \param[in] _msg The message data.
private: void OnSkyMsg(ConstSkyPtr &_msg);
@@ -583,6 +616,29 @@ namespace gazebo
private: void CreateCOMVisual(sdf::ElementPtr _elem,
VisualPtr _linkVisual);
+ /// \brief Create a new inertia visual.
+ /// \param[in] _msg Message containing the link data.
+ /// \param[in] _linkVisual Pointer to the link's visual.
+ private: void CreateInertiaVisual(ConstLinkPtr &_msg,
+ VisualPtr _linkVisual);
+
+ /// \brief Create an inertia visual using SDF data.
+ /// \param[in] _elem SDF element data.
+ /// \param[in] _linkVisual Pointer to the link's visual.
+ private: void CreateInertiaVisual(sdf::ElementPtr _elem,
+ VisualPtr _linkVisual);
+
+ /// \brief Create a new link frame visual.
+ /// \param[in] _msg Message containing the link data.
+ /// \param[in] _linkVisual Pointer to the link's visual.
+ private: void CreateLinkFrameVisual(ConstLinkPtr &_msg,
+ VisualPtr _linkVisual);
+
+ /// \brief Helper function to remove all visualizations attached to a
+ /// visual.
+ /// \param[in] _vis Visual that the visualizations are attached to.
+ private: void RemoveVisualizations(VisualPtr _vis);
+
/// \internal
/// \brief Pointer to private data.
private: ScenePrivate *dataPtr;
diff --git a/gazebo/rendering/ScenePrivate.hh b/gazebo/rendering/ScenePrivate.hh
index cbe2bbf..70e7be5 100644
--- a/gazebo/rendering/ScenePrivate.hh
+++ b/gazebo/rendering/ScenePrivate.hh
@@ -157,9 +157,18 @@ namespace gazebo
/// \brief String form of the id.
public: std::string idString;
+ /// \brief List of model visual messages to process.
+ public: VisualMsgs_L modelVisualMsgs;
+
+ /// \brief List of link visual messages to process.
+ public: VisualMsgs_L linkVisualMsgs;
+
/// \brief List of visual messages to process.
public: VisualMsgs_L visualMsgs;
+ /// \brief List of collision visual messages to process.
+ public: VisualMsgs_L collisionVisualMsgs;
+
/// \brief List of light message to process.
public: LightMsgs_L lightMsgs;
@@ -193,9 +202,6 @@ namespace gazebo
/// \brief List of skeleton message to process.
public: SkeletonPoseMsgs_L skeletonPoseMsgs;
- /// \brief A message used to select an object.
- public: boost::shared_ptr<msgs::Selection const> selectionMsg;
-
/// \brief Mutex to lock the various message buffers.
public: boost::mutex *receiveMutex;
@@ -226,9 +232,6 @@ namespace gazebo
/// \brief Subscribe to joint updates.
public: transport::SubscriberPtr jointSub;
- /// \brief Subscribe to selection updates.
- public: transport::SubscriberPtr selectionSub;
-
/// \brief Subscribe to reponses.
public: transport::SubscriberPtr responseSub;
@@ -256,6 +259,9 @@ namespace gazebo
/// \brief The top level in our tree of visuals
public: VisualPtr worldVisual;
+ /// \brief Visual representing the world origin frame.
+ public: OriginVisualPtr originVisual;
+
/// \brief Pointer to a visual selected by a user via the GUI.
public: VisualPtr selectedVis;
@@ -287,6 +293,12 @@ namespace gazebo
/// \brief True when all COMs should be visualized.
public: bool showCOMs;
+ /// \brief True when all inertias should be visualized.
+ public: bool showInertias;
+
+ /// \brief True when all link frames should be visualized.
+ public: bool showLinkFrames;
+
/// \brief True when all collisions should be visualized.
public: bool showCollisions;
diff --git a/gazebo/rendering/Scene_TEST.cc b/gazebo/rendering/Scene_TEST.cc
new file mode 100644
index 0000000..12b44be
--- /dev/null
+++ b/gazebo/rendering/Scene_TEST.cc
@@ -0,0 +1,322 @@
+/*
+ * 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/Scene.hh"
+#include "gazebo/test/ServerFixture.hh"
+
+
+using namespace gazebo;
+class Scene_TEST : public ServerFixture
+{
+};
+
+/////////////////////////////////////////////////
+TEST_F(Scene_TEST, AddRemoveVisuals)
+{
+ Load("worlds/empty.world");
+
+ // Get the scene
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ // Check that it has two visuals, the world and origin visuals
+ EXPECT_EQ(scene->GetVisualCount(), 2u);
+ EXPECT_TRUE(scene->GetVisual("__world_node__") != NULL);
+
+ // Add a visual and check that it has been added
+ rendering::VisualPtr visual1;
+ visual1.reset(new rendering::Visual("visual1", scene));
+ scene->AddVisual(visual1);
+ EXPECT_EQ(scene->GetVisualCount(), 3u);
+ EXPECT_TRUE(scene->GetVisual("visual1") != NULL);
+
+ // Add a visual and check that it has been added
+ rendering::VisualPtr visual2;
+ visual2.reset(new rendering::Visual("visual2", scene));
+ scene->AddVisual(visual2);
+ EXPECT_EQ(scene->GetVisualCount(), 4u);
+ EXPECT_TRUE(scene->GetVisual("visual2") != NULL);
+
+ // Remove a visual and check that it has been removed
+ scene->RemoveVisual(visual1);
+ EXPECT_EQ(scene->GetVisualCount(), 3u);
+ EXPECT_FALSE(scene->GetVisual("visual1"));
+}
+
+/////////////////////////////////////////////////
+TEST_F(Scene_TEST, RemoveModelVisual)
+{
+ // Load a world containing 3 simple shapes
+ Load("worlds/shapes.world");
+
+ // FIXME need a camera otherwise test produces a gl vertex buffer error
+ math::Pose cameraStartPose(0, 0, 0, 0, 0, 0);
+ std::string cameraName = "test_camera";
+ SpawnCamera("test_camera_model", cameraName,
+ cameraStartPose.pos, cameraStartPose.rot.GetAsEuler());
+
+ // Get the scene
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ // Wait until all models are inserted
+ int sleep = 0;
+ int maxSleep = 10;
+ rendering::VisualPtr box, sphere, cylinder;
+ while ((!box || !sphere || !cylinder) && sleep < maxSleep)
+ {
+ box = scene->GetVisual("box");
+ cylinder = scene->GetVisual("cylinder");
+ sphere = scene->GetVisual("sphere");
+ common::Time::MSleep(1000);
+ sleep++;
+ }
+
+ // Check that the model visuals were properly added
+ ASSERT_TRUE(box != NULL);
+ ASSERT_TRUE(sphere != NULL);
+ ASSERT_TRUE(cylinder != NULL);
+
+ // Check that the link visuals were properly added
+ EXPECT_TRUE(scene->GetVisual("box::link") != NULL);
+ EXPECT_TRUE(scene->GetVisual("cylinder::link") != NULL);
+ EXPECT_TRUE(scene->GetVisual("sphere::link") != NULL);
+
+ // Check that the "visual visuals" were properly added
+ EXPECT_TRUE(scene->GetVisual("box::link::visual") != NULL);
+ EXPECT_TRUE(scene->GetVisual("cylinder::link::visual") != NULL);
+ EXPECT_TRUE(scene->GetVisual("sphere::link::visual") != NULL);
+
+ // Check that the collision visuals were properly added
+ EXPECT_TRUE(scene->GetVisual("box::link::collision") != NULL);
+ EXPECT_TRUE(scene->GetVisual("cylinder::link::collision") != NULL);
+ EXPECT_TRUE(scene->GetVisual("sphere::link::collision") != NULL);
+
+ // Check that the inertia visuals were properly added
+ EXPECT_TRUE(scene->GetVisual("box::link_INERTIA_VISUAL__") != NULL);
+ EXPECT_TRUE(scene->GetVisual("cylinder::link_INERTIA_VISUAL__") != NULL);
+ EXPECT_TRUE(scene->GetVisual("sphere::link_INERTIA_VISUAL__") != NULL);
+
+ // Check that the COM visuals were properly added
+ EXPECT_TRUE(scene->GetVisual("box::link_COM_VISUAL__") != NULL);
+ EXPECT_TRUE(scene->GetVisual("cylinder::link_COM_VISUAL__") != NULL);
+ EXPECT_TRUE(scene->GetVisual("sphere::link_COM_VISUAL__") != NULL);
+
+ // Check that the link frame visuals were properly added
+ EXPECT_TRUE(scene->GetVisual("box::link_LINK_FRAME_VISUAL__") != NULL);
+ EXPECT_TRUE(scene->GetVisual("cylinder::link_LINK_FRAME_VISUAL__") != NULL);
+ EXPECT_TRUE(scene->GetVisual("sphere::link_LINK_FRAME_VISUAL__") != NULL);
+
+ // Send request to delete the box model
+ transport::NodePtr node = transport::NodePtr(new transport::Node());
+ node->Init();
+ transport::requestNoReply(node, "entity_delete", "box");
+
+ sleep = 0;
+ while (box && sleep < maxSleep)
+ {
+ box = scene->GetVisual("box");
+ common::Time::MSleep(1000);
+ sleep++;
+ }
+ ASSERT_TRUE(box == NULL);
+
+ // Check that the link visuals were properly removed
+ EXPECT_TRUE(scene->GetVisual("box::link") == NULL);
+
+ // Check that the "visual visuals" were properly removed
+ EXPECT_TRUE(scene->GetVisual("box::link::visual") == NULL);
+
+ // Check that the collision visuals were properly removed
+ EXPECT_TRUE(scene->GetVisual("box::link::collision") == NULL);
+
+ // Check that the inertia visuals were properly removed
+ EXPECT_TRUE(scene->GetVisual("box::link_INERTIA_VISUAL__") == NULL);
+
+ // Check that the COM visuals were properly removed
+ EXPECT_TRUE(scene->GetVisual("box::link_COM_VISUAL__") == NULL);
+
+ // Check that the link frame visuals were properly removed
+ EXPECT_TRUE(scene->GetVisual("box::link_LINK_FRAME_VISUAL__") == NULL);
+}
+
+/////////////////////////////////////////////////
+TEST_F(Scene_TEST, VisualType)
+{
+ // Load a world containing 3 simple shapes
+ Load("worlds/shapes.world");
+
+ // FIXME need a camera otherwise test produces a gl vertex buffer error
+ math::Pose cameraStartPose(0, 0, 0, 0, 0, 0);
+ std::string cameraName = "test_camera";
+ SpawnCamera("test_camera_model", cameraName,
+ cameraStartPose.pos, cameraStartPose.rot.GetAsEuler());
+
+ // Get the scene
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ // Spawn another box. The model loading process is slightly different so
+ // check that its visuals also have the correct visual type.
+ SpawnBox("new_box", math::Vector3(1, 1, 1), math::Vector3(10, 10, 1),
+ math::Vector3(0, 0, 0));
+
+ // Wait until all models are inserted
+ int sleep = 0;
+ int maxSleep = 10;
+ rendering::VisualPtr box, sphere, cylinder, newBox;
+ while ((!box || !sphere || !cylinder || !newBox) && sleep < maxSleep)
+ {
+ box = scene->GetVisual("box");
+ cylinder = scene->GetVisual("cylinder");
+ sphere = scene->GetVisual("sphere");
+ newBox = scene->GetVisual("new_box");
+ common::Time::MSleep(1000);
+ sleep++;
+ }
+
+ // Check that the model visuals were properly added
+ ASSERT_TRUE(box != NULL);
+ ASSERT_TRUE(sphere != NULL);
+ ASSERT_TRUE(cylinder != NULL);
+ ASSERT_TRUE(newBox != NULL);
+
+ // Verify type is VT_MODEL
+ EXPECT_TRUE(box->GetType() == rendering::Visual::VT_MODEL);
+ EXPECT_TRUE(sphere->GetType() == rendering::Visual::VT_MODEL);
+ EXPECT_TRUE(cylinder->GetType() == rendering::Visual::VT_MODEL);
+ EXPECT_TRUE(newBox->GetType() == rendering::Visual::VT_MODEL);
+
+ // Check that the link visuals were properly added
+ rendering::VisualPtr boxLink = scene->GetVisual("box::link");
+ rendering::VisualPtr sphereLink = scene->GetVisual("sphere::link");
+ rendering::VisualPtr cylinderLink = scene->GetVisual("cylinder::link");
+ rendering::VisualPtr newBoxLink = scene->GetVisual("new_box::body");
+ EXPECT_TRUE(boxLink != NULL);
+ EXPECT_TRUE(sphereLink != NULL);
+ EXPECT_TRUE(cylinderLink != NULL);
+ EXPECT_TRUE(newBoxLink != NULL);
+
+ // Verify type is VT_LINK
+ EXPECT_TRUE(boxLink->GetType() == rendering::Visual::VT_LINK);
+ EXPECT_TRUE(sphereLink->GetType() == rendering::Visual::VT_LINK);
+ EXPECT_TRUE(cylinderLink->GetType() == rendering::Visual::VT_LINK);
+ EXPECT_TRUE(newBoxLink->GetType() == rendering::Visual::VT_LINK);
+
+ // Check that the visual visuals were properly added
+ rendering::VisualPtr boxVisual = scene->GetVisual("box::link::visual");
+ rendering::VisualPtr sphereVisual = scene->GetVisual("sphere::link::visual");
+ rendering::VisualPtr cylinderVisual =
+ scene->GetVisual("cylinder::link::visual");
+ rendering::VisualPtr newBoxVisual = scene->GetVisual("new_box::body::visual");
+ EXPECT_TRUE(boxVisual != NULL);
+ EXPECT_TRUE(sphereVisual != NULL);
+ EXPECT_TRUE(cylinderVisual != NULL);
+ EXPECT_TRUE(newBoxVisual != NULL);
+
+ // Verify type is VT_VISUAL
+ EXPECT_TRUE(boxVisual->GetType() == rendering::Visual::VT_VISUAL);
+ EXPECT_TRUE(sphereVisual->GetType() == rendering::Visual::VT_VISUAL);
+ EXPECT_TRUE(cylinderVisual->GetType() == rendering::Visual::VT_VISUAL);
+ EXPECT_TRUE(newBoxVisual->GetType() == rendering::Visual::VT_VISUAL);
+
+ // Check that the collision visuals were properly added
+ rendering::VisualPtr boxCollision = scene->GetVisual("box::link::collision");
+ rendering::VisualPtr sphereCollision =
+ scene->GetVisual("sphere::link::collision");
+ rendering::VisualPtr cylinderCollision =
+ scene->GetVisual("cylinder::link::collision");
+ rendering::VisualPtr newBoxCollision =
+ scene->GetVisual("new_box::body::geom");
+ EXPECT_TRUE(boxCollision != NULL);
+ EXPECT_TRUE(sphereCollision != NULL);
+ EXPECT_TRUE(cylinderCollision != NULL);
+ EXPECT_TRUE(newBoxCollision != NULL);
+
+ // Verify type is VT_COLLISION
+ EXPECT_TRUE(boxCollision->GetType() == rendering::Visual::VT_COLLISION);
+ EXPECT_TRUE(sphereCollision->GetType() == rendering::Visual::VT_COLLISION);
+ EXPECT_TRUE(cylinderCollision->GetType() == rendering::Visual::VT_COLLISION);
+ EXPECT_TRUE(newBoxCollision->GetType() == rendering::Visual::VT_COLLISION);
+
+ // Check that the inertia visuals were properly added
+ rendering::VisualPtr boxInertia =
+ scene->GetVisual("box::link_INERTIA_VISUAL__");
+ rendering::VisualPtr sphereInertia =
+ scene->GetVisual("sphere::link_INERTIA_VISUAL__");
+ rendering::VisualPtr cylinderInertia =
+ scene->GetVisual("cylinder::link_INERTIA_VISUAL__");
+ rendering::VisualPtr newBoxInertia =
+ scene->GetVisual("new_box::body_INERTIA_VISUAL__");
+ EXPECT_TRUE(boxInertia != NULL);
+ EXPECT_TRUE(sphereInertia != NULL);
+ EXPECT_TRUE(cylinderInertia != NULL);
+ EXPECT_TRUE(newBoxInertia != NULL);
+
+ // Verify type is VT_PHYSICS
+ EXPECT_TRUE(boxInertia->GetType() == rendering::Visual::VT_PHYSICS);
+ EXPECT_TRUE(sphereInertia->GetType() == rendering::Visual::VT_PHYSICS);
+ EXPECT_TRUE(cylinderInertia->GetType() == rendering::Visual::VT_PHYSICS);
+ EXPECT_TRUE(newBoxInertia->GetType() == rendering::Visual::VT_PHYSICS);
+
+ // Check that the COM visuals were properly added
+ rendering::VisualPtr boxCOM = scene->GetVisual("box::link_COM_VISUAL__");
+ rendering::VisualPtr sphereCOM =
+ scene->GetVisual("sphere::link_COM_VISUAL__");
+ rendering::VisualPtr cylinderCOM =
+ scene->GetVisual("cylinder::link_COM_VISUAL__");
+ rendering::VisualPtr newBoxCOM =
+ scene->GetVisual("new_box::body_COM_VISUAL__");
+ EXPECT_TRUE(boxCOM != NULL);
+ EXPECT_TRUE(sphereCOM != NULL);
+ EXPECT_TRUE(cylinderCOM != NULL);
+ EXPECT_TRUE(newBoxCOM != NULL);
+
+ // Verify type is VT_PHYSICS
+ EXPECT_TRUE(boxCOM->GetType() == rendering::Visual::VT_PHYSICS);
+ EXPECT_TRUE(cylinderCOM->GetType() == rendering::Visual::VT_PHYSICS);
+ EXPECT_TRUE(cylinderCOM->GetType() == rendering::Visual::VT_PHYSICS);
+ EXPECT_TRUE(newBoxCOM->GetType() == rendering::Visual::VT_PHYSICS);
+
+ // Check that the link frame visuals were properly added
+ rendering::VisualPtr boxLinkFrame =
+ scene->GetVisual("box::link_LINK_FRAME_VISUAL__");
+ rendering::VisualPtr sphereLinkFrame =
+ scene->GetVisual("sphere::link_LINK_FRAME_VISUAL__");
+ rendering::VisualPtr cylinderLinkFrame =
+ scene->GetVisual("cylinder::link_LINK_FRAME_VISUAL__");
+ rendering::VisualPtr newBoxLinkFrame =
+ scene->GetVisual("new_box::body_LINK_FRAME_VISUAL__");
+ EXPECT_TRUE(boxLinkFrame != NULL);
+ EXPECT_TRUE(sphereLinkFrame != NULL);
+ EXPECT_TRUE(cylinderLinkFrame != NULL);
+ EXPECT_TRUE(newBoxLinkFrame != NULL);
+
+ // Verify type is VT_PHYSICS
+ EXPECT_TRUE(boxLinkFrame->GetType() == rendering::Visual::VT_PHYSICS);
+ EXPECT_TRUE(cylinderLinkFrame->GetType() == rendering::Visual::VT_PHYSICS);
+ EXPECT_TRUE(cylinderLinkFrame->GetType() == rendering::Visual::VT_PHYSICS);
+ EXPECT_TRUE(newBoxLinkFrame->GetType() == rendering::Visual::VT_PHYSICS);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/rendering/SelectionObj.cc b/gazebo/rendering/SelectionObj.cc
index a7c24e7..87a2a58 100644
--- a/gazebo/rendering/SelectionObj.cc
+++ b/gazebo/rendering/SelectionObj.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/common/MeshManager.hh"
#include "gazebo/gui/GuiIface.hh"
@@ -36,6 +42,8 @@ SelectionObj::SelectionObj(const std::string &_name, VisualPtr _vis)
SelectionObjPrivate *dPtr =
reinterpret_cast<SelectionObjPrivate *>(this->dataPtr);
+ dPtr->type = VT_GUI;
+
dPtr->state = SELECTION_NONE;
dPtr->mode = SELECTION_NONE;
@@ -62,25 +70,70 @@ SelectionObj::~SelectionObj()
/////////////////////////////////////////////////
void SelectionObj::Load()
{
- SelectionObjPrivate *dPtr =
- reinterpret_cast<SelectionObjPrivate *>(this->dataPtr);
-
Visual::Load();
this->CreateRotateVisual();
this->CreateTranslateVisual();
this->CreateScaleVisual();
- this->SetVisibilityFlags(GZ_VISIBILITY_GUI);
+ this->SetVisibilityFlags(GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE);
- dPtr->transVisual->SetVisible(false);
- dPtr->rotVisual->SetVisible(false);
- dPtr->scaleVisual->SetVisible(false);
+ this->SetHandleVisible(TRANS, false);
+ this->SetHandleVisible(ROT, false);
+ this->SetHandleVisible(SCALE, false);
this->GetSceneNode()->setInheritScale(false);
}
/////////////////////////////////////////////////
+void SelectionObj::Fini()
+{
+ SelectionObjPrivate *dPtr =
+ reinterpret_cast<SelectionObjPrivate *>(this->dataPtr);
+
+ // Destroy objects and nodes created by this visual
+ if (!dPtr->scene)
+ return;
+ Ogre::SceneManager *manager = dPtr->scene->GetManager();
+ if (!manager)
+ return;
+
+ // transVisual / rotVisual / scaleVisual
+ for (unsigned int i = 0; i < this->GetChildCount(); ++i)
+ {
+ if (!this->GetChild(i))
+ continue;
+
+ // transXVisual / transYVisual / transZVisual
+ for (unsigned int j = 0; j < this->GetChild(i)->GetChildCount(); ++j)
+ {
+ if (!this->GetChild(i)->GetChild(j) ||
+ !this->GetChild(i)->GetChild(j)->GetSceneNode())
+ {
+ continue;
+ }
+
+ // transShaftXNode / transHeadXNode
+ for (auto k = this->GetChild(i)->GetChild(j)->GetSceneNode()->
+ numChildren()-1; k >= 0; --k)
+ {
+ Ogre::SceneNode *toDestroy =
+ reinterpret_cast<Ogre::SceneNode *>(
+ this->GetChild(i)->
+ GetChild(j)->
+ GetSceneNode()->
+ getChild(k));
+
+ manager->destroyMovableObject(toDestroy->getAttachedObject(0));
+ manager->destroySceneNode(toDestroy);
+ }
+ }
+ }
+
+ Visual::Fini();
+}
+
+/////////////////////////////////////////////////
void SelectionObj::Attach(rendering::VisualPtr _vis)
{
SelectionObjPrivate *dPtr =
@@ -123,15 +176,15 @@ void SelectionObj::UpdateSize()
// overlays for big objects.
if (math::equal(max, dPtr->maxScale))
{
- dPtr->rotXVisual->SetMaterial(dPtr->xAxisMatOverlay, false);
- dPtr->rotYVisual->SetMaterial(dPtr->yAxisMatOverlay, false);
- dPtr->rotZVisual->SetMaterial(dPtr->zAxisMatOverlay, false);
+ this->SetHandleMaterial(ROT_X, dPtr->xAxisMatOverlay, false);
+ this->SetHandleMaterial(ROT_Y, dPtr->yAxisMatOverlay, false);
+ this->SetHandleMaterial(ROT_Z, dPtr->zAxisMatOverlay, false);
}
else
{
- dPtr->rotXVisual->SetMaterial(dPtr->xAxisMat, false);
- dPtr->rotYVisual->SetMaterial(dPtr->yAxisMat, false);
- dPtr->rotZVisual->SetMaterial(dPtr->zAxisMat, false);
+ this->SetHandleMaterial(ROT_X, dPtr->xAxisMat, false);
+ this->SetHandleMaterial(ROT_Y, dPtr->yAxisMat, false);
+ this->SetHandleMaterial(ROT_Z, dPtr->zAxisMat, false);
}
this->SetScale(math::Vector3(max, max, max));
}
@@ -173,16 +226,11 @@ void SelectionObj::SetMode(SelectionMode _mode)
dPtr->mode = _mode;
- dPtr->transVisual->SetVisible(false);
- dPtr->rotVisual->SetVisible(false);
- dPtr->scaleVisual->SetVisible(false);
+ this->SetHandleVisible(TRANS, false);
+ this->SetHandleVisible(ROT, false);
+ this->SetHandleVisible(SCALE, false);
- if (dPtr->mode == TRANS)
- dPtr->transVisual->SetVisible(true);
- else if (dPtr->mode == ROT)
- dPtr->rotVisual->SetVisible(true);
- else if (dPtr->mode == SCALE)
- dPtr->scaleVisual->SetVisible(true);
+ this->SetHandleVisible(dPtr->mode, true);
}
/////////////////////////////////////////////////
@@ -317,6 +365,7 @@ void SelectionObj::CreateTranslateVisual()
dPtr->transVisual.reset(new rendering::Visual(
this->GetName() + "__SELECTION_OBJ_TRANS__",
shared_from_this()));
+ dPtr->transVisual->Load();
dPtr->transXVisual.reset(
new rendering::Visual(
@@ -415,9 +464,9 @@ void SelectionObj::CreateTranslateVisual()
dPtr->transYVisual->SetRotation(
math::Quaternion(math::Vector3(1, 0, 0), GZ_DTOR(-90)));
- dPtr->transXVisual->SetMaterial(dPtr->xAxisMatOverlay);
- dPtr->transYVisual->SetMaterial(dPtr->yAxisMatOverlay);
- dPtr->transZVisual->SetMaterial(dPtr->zAxisMatOverlay);
+ this->SetHandleMaterial(TRANS_X, dPtr->xAxisMatOverlay);
+ this->SetHandleMaterial(TRANS_Y, dPtr->yAxisMatOverlay);
+ this->SetHandleMaterial(TRANS_Z, dPtr->zAxisMatOverlay);
dPtr->transVisual->SetScale(math::Vector3(5.0, 5.0, 5.0));
@@ -427,11 +476,6 @@ void SelectionObj::CreateTranslateVisual()
GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE);
dPtr->transZVisual->SetVisibilityFlags(
GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE);
-
- // Add to scene so they are selectable by the mouse
- dPtr->scene->AddVisual(dPtr->transXVisual);
- dPtr->scene->AddVisual(dPtr->transYVisual);
- dPtr->scene->AddVisual(dPtr->transZVisual);
}
/////////////////////////////////////////////////
@@ -444,6 +488,7 @@ void SelectionObj::CreateRotateVisual()
dPtr->rotVisual.reset(new rendering::Visual(
this->GetName() + "__SELECTION_OBJ_ROT__",
shared_from_this()));
+ dPtr->rotVisual->Load();
dPtr->rotXVisual.reset(
new rendering::Visual(
@@ -502,9 +547,9 @@ void SelectionObj::CreateRotateVisual()
// By default the visuals are not overlays like translation or scale visuals.
// This is so that the rings does not block the object it's attached too,
// and also gives with better depth perception.
- dPtr->rotXVisual->SetMaterial(dPtr->xAxisMat);
- dPtr->rotYVisual->SetMaterial(dPtr->yAxisMat);
- dPtr->rotZVisual->SetMaterial(dPtr->zAxisMat);
+ this->SetHandleMaterial(ROT_X, dPtr->xAxisMat);
+ this->SetHandleMaterial(ROT_Y, dPtr->yAxisMat);
+ this->SetHandleMaterial(ROT_Z, dPtr->zAxisMat);
dPtr->rotVisual->SetScale(math::Vector3(1.0, 1.0, 1.0));
@@ -514,11 +559,6 @@ void SelectionObj::CreateRotateVisual()
GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE);
dPtr->rotZVisual->SetVisibilityFlags(
GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE);
-
- // Add to scene so they are selectable by the mouse
- dPtr->scene->AddVisual(dPtr->rotXVisual);
- dPtr->scene->AddVisual(dPtr->rotYVisual);
- dPtr->scene->AddVisual(dPtr->rotZVisual);
}
/////////////////////////////////////////////////
@@ -531,6 +571,7 @@ void SelectionObj::CreateScaleVisual()
dPtr->scaleVisual.reset(new rendering::Visual(
this->GetName() + "__SELECTION_OBJ_SCALE__",
shared_from_this()));
+ dPtr->scaleVisual->Load();
dPtr->scaleXVisual.reset(
new rendering::Visual(
@@ -628,9 +669,9 @@ void SelectionObj::CreateScaleVisual()
dPtr->scaleYVisual->SetRotation(
math::Quaternion(math::Vector3(1, 0, 0), GZ_DTOR(-90)));
- dPtr->scaleXVisual->SetMaterial(dPtr->xAxisMatOverlay);
- dPtr->scaleYVisual->SetMaterial(dPtr->yAxisMatOverlay);
- dPtr->scaleZVisual->SetMaterial(dPtr->zAxisMatOverlay);
+ this->SetHandleMaterial(SCALE_X, dPtr->xAxisMatOverlay);
+ this->SetHandleMaterial(SCALE_Y, dPtr->yAxisMatOverlay);
+ this->SetHandleMaterial(SCALE_Z, dPtr->zAxisMatOverlay);
dPtr->scaleVisual->SetScale(math::Vector3(5.0, 5.0, 5.0));
@@ -640,9 +681,85 @@ void SelectionObj::CreateScaleVisual()
GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE);
dPtr->scaleZVisual->SetVisibilityFlags(
GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE);
+}
+
+/////////////////////////////////////////////////
+void SelectionObj::SetHandleVisible(SelectionMode _mode, bool _visible)
+{
+ SelectionObjPrivate *dPtr =
+ reinterpret_cast<SelectionObjPrivate *>(this->dataPtr);
+
+ if (_mode == TRANS || _mode == TRANS_X)
+ dPtr->transXVisual->SetVisible(_visible);
+ if (_mode == TRANS || _mode == TRANS_Y)
+ dPtr->transYVisual->SetVisible(_visible);
+ if (_mode == TRANS || _mode == TRANS_Z)
+ dPtr->transZVisual->SetVisible(_visible);
+ if (_mode == ROT || _mode == ROT_X)
+ dPtr->rotXVisual->SetVisible(_visible);
+ if (_mode == ROT || _mode == ROT_Y)
+ dPtr->rotYVisual->SetVisible(_visible);
+ if (_mode == ROT || _mode == ROT_Z)
+ dPtr->rotZVisual->SetVisible(_visible);
+ if (_mode == SCALE || _mode == SCALE_X)
+ dPtr->scaleXVisual->SetVisible(_visible);
+ if (_mode == SCALE || _mode == SCALE_Y)
+ dPtr->scaleYVisual->SetVisible(_visible);
+ if (_mode == SCALE || _mode == SCALE_Z)
+ dPtr->scaleZVisual->SetVisible(_visible);
+}
+
+/////////////////////////////////////////////////
+bool SelectionObj::GetHandleVisible(SelectionMode _mode) const
+{
+ SelectionObjPrivate *dPtr =
+ reinterpret_cast<SelectionObjPrivate *>(this->dataPtr);
+
+ if (_mode == TRANS || _mode == TRANS_X)
+ return dPtr->transXVisual->GetVisible();
+ if (_mode == TRANS_Y)
+ return dPtr->transYVisual->GetVisible();
+ if (_mode == TRANS_Z)
+ return dPtr->transZVisual->GetVisible();
+ if (_mode == ROT || _mode == ROT_X)
+ return dPtr->rotXVisual->GetVisible();
+ if (_mode == ROT_Y)
+ return dPtr->rotYVisual->GetVisible();
+ if (_mode == ROT_Z)
+ return dPtr->rotZVisual->GetVisible();
+ if (_mode == SCALE || _mode == SCALE_X)
+ return dPtr->scaleXVisual->GetVisible();
+ if (_mode == SCALE_Y)
+ return dPtr->scaleYVisual->GetVisible();
+ if (_mode == SCALE_Z)
+ return dPtr->scaleZVisual->GetVisible();
+
+ return false;
+}
+
+/////////////////////////////////////////////////
+void SelectionObj::SetHandleMaterial(SelectionMode _mode, const std::string
+ &_material, bool _unique)
+{
+ SelectionObjPrivate *dPtr =
+ reinterpret_cast<SelectionObjPrivate *>(this->dataPtr);
- // Add to scene so they are selectable by the mouse
- dPtr->scene->AddVisual(dPtr->scaleXVisual);
- dPtr->scene->AddVisual(dPtr->scaleYVisual);
- dPtr->scene->AddVisual(dPtr->scaleZVisual);
+ if (_mode == TRANS || _mode == TRANS_X)
+ dPtr->transXVisual->SetMaterial(_material, _unique);
+ if (_mode == TRANS || _mode == TRANS_Y)
+ dPtr->transYVisual->SetMaterial(_material, _unique);
+ if (_mode == TRANS || _mode == TRANS_Z)
+ dPtr->transZVisual->SetMaterial(_material, _unique);
+ if (_mode == ROT || _mode == ROT_X)
+ dPtr->rotXVisual->SetMaterial(_material, _unique);
+ if (_mode == ROT || _mode == ROT_Y)
+ dPtr->rotYVisual->SetMaterial(_material, _unique);
+ if (_mode == ROT || _mode == ROT_Z)
+ dPtr->rotZVisual->SetMaterial(_material, _unique);
+ if (_mode == SCALE || _mode == SCALE_X)
+ dPtr->scaleXVisual->SetMaterial(_material, _unique);
+ if (_mode == SCALE || _mode == SCALE_Y)
+ dPtr->scaleYVisual->SetMaterial(_material, _unique);
+ if (_mode == SCALE || _mode == SCALE_Z)
+ dPtr->scaleZVisual->SetMaterial(_material, _unique);
}
diff --git a/gazebo/rendering/SelectionObj.hh b/gazebo/rendering/SelectionObj.hh
index 27fb6bb..1262a8a 100644
--- a/gazebo/rendering/SelectionObj.hh
+++ b/gazebo/rendering/SelectionObj.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class SelectionObj SelectionObj.hh
/// \brief Interactive selection object for models and links
- class GAZEBO_VISIBLE SelectionObj : public Visual
+ class GZ_RENDERING_VISIBLE SelectionObj : public Visual
{
/// \enum Manipulation modes
/// \brief Unique identifiers for manipulation modes.
@@ -77,6 +77,9 @@ namespace gazebo
/// \brief Load
public: void Load();
+ // Documentation inherited.
+ public: virtual void Fini();
+
/// \brief Attach the selection object to the given visual
/// \param[in] _vis Pointer to visual to which the selection object
/// will be attached.
@@ -117,6 +120,26 @@ namespace gazebo
/// \brief Update selection object size to match the parent visual.
public: void UpdateSize();
+ /// \brief Set the visibility for a specific handle or handle group.
+ /// \param[in] _mode Manipulation mode corresponding to the handle.
+ /// \param[in] _visible Whether to show or not.
+ public: void SetHandleVisible(SelectionMode _mode, bool _visible);
+
+ /// \brief Get the visibility for a specific handle. If a handle group
+ /// is specified, the X component's visibility is returned.
+ /// \param[in] _mode Manipulation mode corresponding to the handle.
+ /// \return Whether it is visible or not.
+ public: bool GetHandleVisible(SelectionMode _mode) const;
+
+ /// \brief Set the material for a specific handle or handle group.
+ /// \param[in] _mode Manipulation mode corresponding to the handle.
+ /// \param[in] _material Material name.
+ /// \param[in] _unique True to make the material unique, which
+ /// allows the material to change without changing materials that
+ /// originally had the same name.
+ public: void SetHandleMaterial(SelectionMode _mode, const std::string
+ &_material, bool _unique = true);
+
/// \brief Helper function to create scale visuals.
private: void CreateScaleVisual();
diff --git a/gazebo/rendering/SelectionObj_TEST.cc b/gazebo/rendering/SelectionObj_TEST.cc
new file mode 100644
index 0000000..929a580
--- /dev/null
+++ b/gazebo/rendering/SelectionObj_TEST.cc
@@ -0,0 +1,176 @@
+/*
+ * 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/RenderingIface.hh"
+#include "gazebo/rendering/RenderTypes.hh"
+#include "gazebo/rendering/Scene.hh"
+#include "gazebo/rendering/Visual.hh"
+#include "gazebo/rendering/SelectionObj.hh"
+#include "gazebo/test/ServerFixture.hh"
+
+using namespace gazebo;
+class SelectionObj_TEST : public ServerFixture
+{
+};
+
+/////////////////////////////////////////////////
+TEST_F(SelectionObj_TEST, SelectionObjTest)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene("default");
+
+ if (!scene)
+ scene = gazebo::rendering::create_scene("default", false);
+
+ EXPECT_TRUE(scene != NULL);
+
+ // Test calling constructor and Load functions and make sure
+ // there are no segfaults
+ rendering::SelectionObjPtr obj;
+ obj.reset(new rendering::SelectionObj("obj", scene->GetWorldVisual()));
+ obj->Load();
+
+ // Selection none = no handles visible
+ obj->SetMode(rendering::SelectionObj::SELECTION_NONE);
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_X));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_Y));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_Z));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT_X));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT_Y));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT_Z));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_X));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_Y));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_Z));
+
+ // Translate group visible
+ obj->SetMode(rendering::SelectionObj::TRANS);
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::TRANS));
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_X));
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_Y));
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_Z));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT_X));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT_Y));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT_Z));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_X));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_Y));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_Z));
+
+ // Rotate group visible
+ obj->SetMode(rendering::SelectionObj::ROT);
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_X));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_Y));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_Z));
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::ROT));
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::ROT_X));
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::ROT_Y));
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::ROT_Z));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_X));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_Y));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_Z));
+
+ // Scale group visible
+ obj->SetMode(rendering::SelectionObj::SCALE);
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_X));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_Y));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_Z));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT_X));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT_Y));
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT_Z));
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::SCALE));
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_X));
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_Y));
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_Z));
+
+ // Set child handles
+ obj->SetHandleVisible(rendering::SelectionObj::TRANS_X, true);
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::TRANS_X));
+
+ obj->SetHandleVisible(rendering::SelectionObj::ROT_Y, false);
+ EXPECT_FALSE(obj->GetHandleVisible(rendering::SelectionObj::ROT_Y));
+
+ obj->SetHandleVisible(rendering::SelectionObj::SCALE_Z, true);
+ EXPECT_TRUE(obj->GetHandleVisible(rendering::SelectionObj::SCALE_Z));
+}
+
+/////////////////////////////////////////////////
+TEST_F(SelectionObj_TEST, LoadFini)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene("default");
+
+ if (!scene)
+ scene = gazebo::rendering::create_scene("default", false);
+
+ EXPECT_TRUE(scene != NULL);
+
+ // Create and load visual
+ rendering::SelectionObjPtr obj;
+ obj.reset(new rendering::SelectionObj("obj", scene->GetWorldVisual()));
+ obj->Load();
+ EXPECT_TRUE(obj != NULL);
+
+ // Check that it was added to the scene (by Load)
+ EXPECT_EQ(scene->GetVisual("obj"), obj);
+
+ // Remove it from the scene (Fini is called)
+ scene->RemoveVisual(obj);
+
+ // Check that it was removed
+ EXPECT_TRUE(scene->GetVisual("obj") == NULL);
+
+ // Reset pointer
+ obj.reset();
+ EXPECT_TRUE(obj == NULL);
+
+ // Create another visual with the same name
+ rendering::SelectionObjPtr obj2;
+ obj2.reset(new rendering::SelectionObj("obj", scene->GetWorldVisual()));
+ obj2->Load();
+ EXPECT_TRUE(obj2 != NULL);
+
+ // Check that the scene returns the new visual
+ EXPECT_EQ(scene->GetVisual("obj"), obj2);
+
+ // Remove it from the scene (Fini is called)
+ scene->RemoveVisual(obj2);
+
+ // Check that it was removed
+ EXPECT_TRUE(scene->GetVisual("obj") == NULL);
+
+ // Reset pointer
+ obj2.reset();
+ EXPECT_TRUE(obj2 == NULL);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/rendering/SonarVisual.cc b/gazebo/rendering/SonarVisual.cc
index 9554a1b..cca7caa 100644
--- a/gazebo/rendering/SonarVisual.cc
+++ b/gazebo/rendering/SonarVisual.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/common/MeshManager.hh"
#include "gazebo/transport/transport.hh"
@@ -35,6 +41,8 @@ SonarVisual::SonarVisual(const std::string &_name, VisualPtr _vis,
SonarVisualPrivate *dPtr =
reinterpret_cast<SonarVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_SENSOR;
+
dPtr->receivedMsg = false;
dPtr->node = transport::NodePtr(new transport::Node());
@@ -130,12 +138,14 @@ void SonarVisual::Update()
dPtr->sonarRay->SetPoint(0, math::Vector3(0, 0, rangeDelta * 0.5));
}
- math::Pose pose = msgs::Convert(dPtr->sonarMsg->sonar().world_pose());
+ ignition::math::Pose3d pose =
+ msgs::ConvertIgn(dPtr->sonarMsg->sonar().world_pose());
this->SetPose(pose);
if (dPtr->sonarMsg->sonar().has_contact())
{
- math::Vector3 pos = msgs::Convert(dPtr->sonarMsg->sonar().contact());
+ ignition::math::Vector3d pos =
+ msgs::ConvertIgn(dPtr->sonarMsg->sonar().contact());
dPtr->sonarRay->SetPoint(1, pos);
}
else
diff --git a/gazebo/rendering/SonarVisual.hh b/gazebo/rendering/SonarVisual.hh
index 6b26724..995f02e 100644
--- a/gazebo/rendering/SonarVisual.hh
+++ b/gazebo/rendering/SonarVisual.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class SonarVisual SonarVisual.hh rendering/rendering.hh
/// \brief Visualization for sonar data.
- class GAZEBO_VISIBLE SonarVisual : public Visual
+ class GZ_RENDERING_VISIBLE SonarVisual : public Visual
{
/// \brief Constructor.
/// \param[in] _name Name of the visual.
diff --git a/gazebo/rendering/SonarVisual_TEST.cc b/gazebo/rendering/SonarVisual_TEST.cc
index 9b6ee24..7b74baa 100644
--- a/gazebo/rendering/SonarVisual_TEST.cc
+++ b/gazebo/rendering/SonarVisual_TEST.cc
@@ -20,8 +20,9 @@
#include "gazebo/rendering/RenderingIface.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/SonarVisual.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
+using namespace gazebo;
class SonarVisual_TEST : public ServerFixture
{
};
diff --git a/gazebo/rendering/TransmitterVisual.cc b/gazebo/rendering/TransmitterVisual.cc
index 2c26168..e86481e 100644
--- a/gazebo/rendering/TransmitterVisual.cc
+++ b/gazebo/rendering/TransmitterVisual.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/transport/transport.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/DynamicLines.hh"
@@ -32,6 +38,8 @@ TransmitterVisual::TransmitterVisual(const std::string &_name, VisualPtr _vis,
TransmitterVisualPrivate *dPtr =
reinterpret_cast<TransmitterVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_SENSOR;
+
dPtr->isFirst = true;
dPtr->receivedMsg = false;
diff --git a/gazebo/rendering/TransmitterVisual.hh b/gazebo/rendering/TransmitterVisual.hh
index f286e04..19b5d45 100644
--- a/gazebo/rendering/TransmitterVisual.hh
+++ b/gazebo/rendering/TransmitterVisual.hh
@@ -32,7 +32,7 @@ namespace gazebo
/// \class TransmitterVisual TransmitterVisual.hh rendering/rendering.hh
/// \brief Visualization for the wireless propagation data.
- class GAZEBO_VISIBLE TransmitterVisual : public Visual
+ class GZ_RENDERING_VISIBLE TransmitterVisual : public Visual
{
/// \brief Constructor.
/// \param[in] _name Name of the visual.
diff --git a/gazebo/rendering/TransmitterVisual_TEST.cc b/gazebo/rendering/TransmitterVisual_TEST.cc
index 67fd50d..b7b3963 100644
--- a/gazebo/rendering/TransmitterVisual_TEST.cc
+++ b/gazebo/rendering/TransmitterVisual_TEST.cc
@@ -20,8 +20,9 @@
#include "gazebo/rendering/RenderingIface.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/TransmitterVisual.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
+using namespace gazebo;
class TransmitterVisual_TEST : public ServerFixture
{
};
diff --git a/gazebo/rendering/UserCamera.cc b/gazebo/rendering/UserCamera.cc
index 40b3759..b3c9c85 100644
--- a/gazebo/rendering/UserCamera.cc
+++ b/gazebo/rendering/UserCamera.cc
@@ -25,29 +25,38 @@
#include "gazebo/rendering/WindowManager.hh"
#include "gazebo/rendering/FPSViewController.hh"
#include "gazebo/rendering/OrbitViewController.hh"
+#include "gazebo/rendering/OrthoViewController.hh"
#include "gazebo/rendering/RenderTypes.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/UserCameraPrivate.hh"
+#include "gazebo/rendering/Conversions.hh"
#include "gazebo/rendering/UserCamera.hh"
using namespace gazebo;
using namespace rendering;
//////////////////////////////////////////////////
-UserCamera::UserCamera(const std::string &_name, ScenePtr _scene)
+UserCamera::UserCamera(const std::string &_name, ScenePtr _scene,
+ bool _stereoEnabled)
: Camera(_name, _scene),
dataPtr(new UserCameraPrivate)
{
this->dataPtr->orbitViewController = NULL;
+ this->dataPtr->orthoViewController = NULL;
this->dataPtr->fpsViewController = NULL;
this->dataPtr->viewController = NULL;
this->dataPtr->selectionBuffer = NULL;
this->dataPtr->joyTwistControl = true;
this->dataPtr->joystickButtonToggleLast = false;
this->dataPtr->joyPoseControl = true;
+ this->dataPtr->rightCamera = NULL;
+ this->dataPtr->rightViewport = NULL;
+ this->dataPtr->stereoEnabled = _stereoEnabled;
- // Set default UserCamera render rate to 30Hz
- this->SetRenderRate(30.0);
+ // Set default UserCamera render rate to 120Hz when stereo rendering is
+ // enabled. Otherwise use 60Hz.
+ // Some padding is added for safety.
+ this->SetRenderRate(_stereoEnabled ? 130.0 : 70.0);
this->SetUseSDFPose(false);
}
@@ -56,6 +65,7 @@ UserCamera::UserCamera(const std::string &_name, ScenePtr _scene)
UserCamera::~UserCamera()
{
delete this->dataPtr->orbitViewController;
+ delete this->dataPtr->orthoViewController;
delete this->dataPtr->fpsViewController;
this->connections.clear();
@@ -76,12 +86,17 @@ void UserCamera::Load()
Camera::Load();
this->dataPtr->node = transport::NodePtr(new transport::Node());
this->dataPtr->node->Init();
+
this->dataPtr->joySubTwist =
this->dataPtr->node->Subscribe("~/user_camera/joy_twist",
&UserCamera::OnJoyTwist, this);
+
this->dataPtr->joySubPose =
this->dataPtr->node->Subscribe("~/user_camera/joy_pose",
&UserCamera::OnJoyPose, this);
+
+ this->dataPtr->posePub =
+ this->dataPtr->node->Advertise<msgs::Pose>("~/user_camera/pose", 1, 30.0);
}
//////////////////////////////////////////////////
@@ -89,6 +104,8 @@ void UserCamera::Init()
{
this->dataPtr->orbitViewController = new OrbitViewController(
boost::dynamic_pointer_cast<UserCamera>(shared_from_this()));
+ this->dataPtr->orthoViewController = new OrthoViewController(
+ boost::dynamic_pointer_cast<UserCamera>(shared_from_this()));
this->dataPtr->fpsViewController = new FPSViewController(
boost::dynamic_pointer_cast<UserCamera>(shared_from_this()));
this->dataPtr->viewController = this->dataPtr->orbitViewController;
@@ -98,6 +115,23 @@ void UserCamera::Init()
// Don't yaw along variable axis, causes leaning
this->camera->setFixedYawAxis(true, Ogre::Vector3::UNIT_Z);
this->camera->setDirection(1, 0, 0);
+ this->camera->setAutoAspectRatio(false);
+
+ // Right camera
+ if (this->dataPtr->stereoEnabled)
+ {
+ this->dataPtr->rightCamera = this->scene->GetManager()->createCamera(
+ "StereoUserRight");
+ this->dataPtr->rightCamera->pitch(Ogre::Degree(90));
+
+ // Don't yaw along variable axis, causes leaning
+ this->dataPtr->rightCamera->setFixedYawAxis(true, Ogre::Vector3::UNIT_Z);
+ this->dataPtr->rightCamera->setDirection(1, 0, 0);
+
+ this->dataPtr->rightCamera->setAutoAspectRatio(false);
+
+ this->sceneNode->attachObject(this->dataPtr->rightCamera);
+ }
this->SetHFOV(GZ_DTOR(60));
@@ -170,6 +204,19 @@ void UserCamera::Init()
}
//////////////////////////////////////////////////
+void UserCamera::SetDefaultPose(const math::Pose &_pose)
+{
+ this->dataPtr->defaultPose = _pose;
+ this->SetWorldPose(_pose);
+}
+
+//////////////////////////////////////////////////
+math::Pose UserCamera::DefaultPose() const
+{
+ return this->dataPtr->defaultPose;
+}
+
+//////////////////////////////////////////////////
void UserCamera::SetWorldPose(const math::Pose &_pose)
{
Camera::SetWorldPose(_pose);
@@ -183,6 +230,9 @@ void UserCamera::Update()
if (this->dataPtr->viewController)
this->dataPtr->viewController->Update();
+
+ // publish camera pose
+ this->dataPtr->posePub->Publish(msgs::Convert(this->GetWorldPose().Ign()));
}
//////////////////////////////////////////////////
@@ -206,9 +256,6 @@ void UserCamera::Fini()
//////////////////////////////////////////////////
void UserCamera::HandleMouseEvent(const common::MouseEvent &_evt)
{
- if (this->dataPtr->selectionBuffer)
- this->dataPtr->selectionBuffer->Update();
-
// DEBUG: this->dataPtr->selectionBuffer->ShowOverlay(true);
// Don't update the camera if it's being animated.
@@ -306,34 +353,74 @@ bool UserCamera::TrackVisualImpl(VisualPtr _visual)
}
//////////////////////////////////////////////////
-void UserCamera::SetViewController(const std::string &type)
+void UserCamera::SetViewController(const std::string &_type)
{
- if (this->dataPtr->viewController->GetTypeString() == type)
+ if (_type.empty() ||
+ this->dataPtr->viewController->GetTypeString() == _type)
+ {
return;
+ }
+
+ std::string vc = this->dataPtr->viewController->GetTypeString();
- if (type == OrbitViewController::GetTypeString())
+ if (_type == OrbitViewController::GetTypeString())
+ {
this->dataPtr->viewController = this->dataPtr->orbitViewController;
- else if (type == FPSViewController::GetTypeString())
+ this->dataPtr->viewController->Init();
+
+ this->dataPtr->prevViewControllerName = vc;
+ }
+ else if (_type == OrthoViewController::GetTypeString())
+ {
+ this->dataPtr->viewController = this->dataPtr->orthoViewController;
+ if (vc == "orbit")
+ {
+ this->dataPtr->viewController->Init(
+ this->dataPtr->orbitViewController->GetFocalPoint(),
+ this->dataPtr->orbitViewController->Yaw(),
+ this->dataPtr->orbitViewController->Pitch());
+ }
+ else
+ this->dataPtr->viewController->Init();
+
+ this->dataPtr->prevViewControllerName = vc;
+ }
+ else if (_type == FPSViewController::GetTypeString())
+ {
this->dataPtr->viewController = this->dataPtr->fpsViewController;
- else
- gzthrow("Invalid view controller type: " + type);
+ this->dataPtr->viewController->Init();
- this->dataPtr->viewController->Init();
+ this->dataPtr->prevViewControllerName = vc;
+ }
+ else
+ {
+ gzerr << "Invalid view controller type[" << _type << "]. "
+ << "The view controller is not changed.\n";
+ }
}
//////////////////////////////////////////////////
-void UserCamera::SetViewController(const std::string &type,
- const math::Vector3 &_pos)
+void UserCamera::SetViewController(const std::string &_type,
+ const math::Vector3 &_pos)
{
- if (this->dataPtr->viewController->GetTypeString() == type)
+ if (_type.empty() ||
+ this->dataPtr->viewController->GetTypeString() == _type)
+ {
return;
+ }
+
+ std::string vc = this->dataPtr->viewController->GetTypeString();
- if (type == OrbitViewController::GetTypeString())
+ if (_type == OrbitViewController::GetTypeString())
this->dataPtr->viewController = this->dataPtr->orbitViewController;
- else if (type == FPSViewController::GetTypeString())
+ else if (_type == OrthoViewController::GetTypeString())
+ this->dataPtr->viewController = this->dataPtr->orthoViewController;
+ else if (_type == FPSViewController::GetTypeString())
this->dataPtr->viewController = this->dataPtr->fpsViewController;
else
- gzthrow("Invalid view controller type: " + type);
+ gzthrow("Invalid view controller type: " + _type);
+
+ this->dataPtr->prevViewControllerName = vc;
this->dataPtr->viewController->Init(_pos);
}
@@ -351,22 +438,28 @@ unsigned int UserCamera::GetImageHeight() const
}
//////////////////////////////////////////////////
-void UserCamera::Resize(unsigned int /*_w*/, unsigned int /*_h*/)
+void UserCamera::Resize(unsigned int _w, unsigned int _h)
+{
+ this->UpdateFOV();
+ this->dataPtr->viewController->Resize(_w, _h);
+}
+
+//////////////////////////////////////////////////
+void UserCamera::UpdateFOV()
{
- if (this->viewport)
+ Camera::UpdateFOV();
+
+ if (this->dataPtr->stereoEnabled && this->viewport)
{
- this->viewport->setDimensions(0, 0, 1, 1);
double ratio = static_cast<double>(this->viewport->getActualWidth()) /
- static_cast<double>(this->viewport->getActualHeight());
+ static_cast<double>(this->viewport->getActualHeight());
double hfov =
this->sdf->Get<double>("horizontal_fov");
double vfov = 2.0 * atan(tan(hfov / 2.0) / ratio);
- this->camera->setAspectRatio(ratio);
- this->camera->setFOVy(Ogre::Radian(vfov));
- delete [] this->saveFrameBuffer;
- this->saveFrameBuffer = NULL;
+ this->dataPtr->rightCamera->setAspectRatio(ratio);
+ this->dataPtr->rightCamera->setFOVy(Ogre::Radian(vfov));
}
}
@@ -378,20 +471,6 @@ void UserCamera::SetViewportDimensions(float /*x_*/, float /*y_*/,
}
//////////////////////////////////////////////////
-float UserCamera::GetAvgFPS() const
-{
- return RenderEngine::Instance()->GetWindowManager()->GetAvgFPS(
- this->windowId);
-}
-
-//////////////////////////////////////////////////
-unsigned int UserCamera::GetTriangleCount() const
-{
- return RenderEngine::Instance()->GetWindowManager()->GetTriangleCount(
- this->windowId);
-}
-
-//////////////////////////////////////////////////
void UserCamera::ToggleShowVisual()
{
// this->visual->ToggleVisible();
@@ -517,6 +596,33 @@ void UserCamera::SetRenderTarget(Ogre::RenderTarget *_target)
{
Camera::SetRenderTarget(_target);
+ // Setup stereo rendering viewports
+ if (this->dataPtr->stereoEnabled)
+ {
+ float focalLength = 1.0;
+
+ // Defaulting to 0.03m stereo baseline.
+ Ogre::Vector2 offset(0.03f, 0.0f);
+
+ this->camera->setFocalLength(focalLength);
+ this->camera->setFrustumOffset(offset);
+
+ this->dataPtr->rightCamera->setFocalLength(focalLength);
+ this->dataPtr->rightCamera->setFrustumOffset(-offset);
+
+ this->dataPtr->rightViewport =
+ this->renderTarget->addViewport(this->dataPtr->rightCamera, 1);
+ this->dataPtr->rightViewport->setBackgroundColour(
+ Conversions::Convert(this->scene->GetBackgroundColor()));
+
+#if OGRE_VERSION_MAJOR > 1 || OGRE_VERSION_MINOR > 9
+ this->viewport->setDrawBuffer(Ogre::CBT_BACK_LEFT);
+ this->dataPtr->rightViewport->setDrawBuffer(Ogre::CBT_BACK_RIGHT);
+#endif
+
+ this->dataPtr->rightViewport->setVisibilityMask(GZ_VISIBILITY_ALL);
+ }
+
this->viewport->setVisibilityMask(GZ_VISIBILITY_ALL);
this->initialized = true;
@@ -540,9 +646,6 @@ VisualPtr UserCamera::GetVisual(const math::Vector2i &_mousePos,
if (!this->dataPtr->selectionBuffer)
return result;
- // Update the selection buffer
- this->dataPtr->selectionBuffer->Update();
-
Ogre::Entity *entity =
this->dataPtr->selectionBuffer->OnSelectionClick(_mousePos.x, _mousePos.y);
@@ -621,7 +724,7 @@ std::string UserCamera::GetViewControllerTypeString()
void UserCamera::OnJoyTwist(ConstJoystickPtr &_msg)
{
// Scaling factor applied to rotations.
- static math::Vector3 rpyFactor(0, 0.01, 0.05);
+ static ignition::math::Vector3d rpyFactor(0, 0.01, 0.05);
// toggle using joystick to move camera
if (this->dataPtr->joystickButtonToggleLast == false &&
@@ -654,14 +757,16 @@ void UserCamera::OnJoyTwist(ConstJoystickPtr &_msg)
if (_msg->has_translation())
{
const double transRotRatio = 0.05;
- math::Vector3 trans = msgs::Convert(_msg->translation()) * transRotRatio;
+ ignition::math::Vector3d trans =
+ msgs::ConvertIgn(_msg->translation()) * transRotRatio;
pose.pos = pose.rot.RotateVector(trans) + pose.pos;
}
// Get the jostick RPY. We are disabling rotation around x.
if (_msg->has_rotation())
{
- math::Vector3 rot = msgs::Convert(_msg->rotation()) * rpyFactor;
+ ignition::math::Vector3d rot =
+ msgs::ConvertIgn(_msg->rotation()) * rpyFactor;
pose.rot.SetFromEuler(pose.rot.GetAsEuler() + rot);
}
@@ -678,8 +783,73 @@ void UserCamera::OnJoyPose(ConstPosePtr &_msg)
if (_msg->has_position() && _msg->has_orientation())
{
// Get the XYZ
- math::Pose pose(msgs::Convert(_msg->position()),
- msgs::Convert(_msg->orientation()));
+ ignition::math::Pose3d pose(msgs::ConvertIgn(_msg->position()),
+ msgs::ConvertIgn(_msg->orientation()));
this->SetWorldPose(pose);
}
}
+
+//////////////////////////////////////////////////
+void UserCamera::SetClipDist(float _near, float _far)
+{
+ Camera::SetClipDist(_near, _far);
+
+ // Update right camera, if it exists.
+ if (this->dataPtr->stereoEnabled)
+ {
+ this->dataPtr->rightCamera->setNearClipDistance(
+ this->camera->getNearClipDistance());
+ this->dataPtr->rightCamera->setFarClipDistance(
+ this->camera->getFarClipDistance());
+ this->dataPtr->rightCamera->setRenderingDistance(
+ this->camera->getRenderingDistance());
+ }
+}
+
+//////////////////////////////////////////////////
+bool UserCamera::StereoEnabled() const
+{
+ return this->dataPtr->stereoEnabled;
+}
+
+//////////////////////////////////////////////////
+void UserCamera::EnableStereo(bool _enable)
+{
+#if OGRE_VERSION_MAJOR > 1 || OGRE_VERSION_MINOR > 9
+ if (this->dataPtr->rightViewport)
+ {
+ if (_enable)
+ {
+ this->dataPtr->rightViewport->setDrawBuffer(Ogre::CBT_BACK_RIGHT);
+ this->dataPtr->rightViewport->setAutoUpdated(true);
+ this->viewport->setDrawBuffer(Ogre::CBT_BACK_LEFT);
+ }
+ else
+ {
+ this->dataPtr->rightViewport->setAutoUpdated(false);
+ this->dataPtr->rightViewport->setDrawBuffer(Ogre::CBT_BACK);
+ this->viewport->setDrawBuffer(Ogre::CBT_BACK);
+ }
+ }
+ else
+ {
+ gzwarn << "Tried to enable/disable stereo. "
+ << "However, stereo is turned off via the gui.ini file.\n";
+ }
+#else
+ gzwarn << "Tried to EnableStereo("
+ << _enable
+ << "). However, Ogre version >= 1.10.0 is required.\n";
+#endif
+}
+
+/////////////////////////////////////////////////
+bool UserCamera::SetProjectionType(const std::string &_type)
+{
+ if (_type == "orthographic")
+ this->SetViewController("ortho");
+ else if (!this->dataPtr->prevViewControllerName.empty())
+ this->SetViewController(this->dataPtr->prevViewControllerName);
+
+ return Camera::SetProjectionType(_type);
+}
diff --git a/gazebo/rendering/UserCamera.hh b/gazebo/rendering/UserCamera.hh
index 171aac4..5ca78b7 100644
--- a/gazebo/rendering/UserCamera.hh
+++ b/gazebo/rendering/UserCamera.hh
@@ -36,12 +36,15 @@ namespace gazebo
/// \class UserCamera UserCamera.hh rendering/rendering.hh
/// \brief A camera used for user visualization of a scene
- class GAZEBO_VISIBLE UserCamera : public Camera
+ class GZ_RENDERING_VISIBLE UserCamera : public Camera
{
/// \brief Constructor
/// \param[in] _name Name of the camera.
/// \param[in] _scene Scene to put the camera in.
- public: UserCamera(const std::string &_name, ScenePtr _scene);
+ /// \param[in] _stereoEnabled True to enable stereo rendering. This is
+ /// here for compatibility with 3D monitors/TVs.
+ public: UserCamera(const std::string &_name, ScenePtr _scene,
+ bool _stereoEnabled = false);
/// \brief Destructor
public: virtual ~UserCamera();
@@ -50,6 +53,9 @@ namespace gazebo
/// \param[in] _sdf Parameters for the camera.
public: void Load(sdf::ElementPtr _sdf);
+ // Documentation inherited
+ public: virtual void SetClipDist(float _near, float _far);
+
/// \brief Generic load function
public: void Load();
@@ -69,6 +75,15 @@ namespace gazebo
/// \param[in] _pose New pose of the camera.
public: virtual void SetWorldPose(const math::Pose &_pose);
+ /// \brief Set the default pose in the world coordinate frame and set
+ /// that as the current camera world pose.
+ /// \param[in] _pose New default pose of the camera.
+ public: void SetDefaultPose(const math::Pose &_pose);
+
+ /// \brief Get the default pose in the world coordinate frame.
+ /// \return Default pose of the camera.
+ public: math::Pose DefaultPose() const;
+
/// \brief Handle a mouse event.
/// \param[in] _evt The mouse event.
public: void HandleMouseEvent(const common::MouseEvent &_evt);
@@ -108,14 +123,6 @@ namespace gazebo
public: void SetViewportDimensions(float _x, float _y,
float _w, float _h);
- /// \brief Get the average frames per second
- /// \return The average rendering frames per second
- public: float GetAvgFPS() const;
-
- /// \brief Get the triangle count.
- /// \return The number of triangles currently being rendered.
- public: unsigned int GetTriangleCount() const;
-
/// \brief Move the camera to focus on a visual.
/// \param[in] _visual Visual to move the camera to.
public: void MoveToVisual(VisualPtr _visual);
@@ -183,6 +190,22 @@ namespace gazebo
/// gz topic ~/user_camera/joy_pose.
public: void SetJoyPoseControl(bool _value);
+ /// \brief Get whether stereo is enabled.
+ /// \return True if stereo is enabled.
+ public: bool StereoEnabled() const;
+
+ /// \brief Turn on/off stereo rendering. Stereo must be initially
+ /// enable in the ~/.gazebo/gui.ini file using:
+ ///
+ /// [rendering]
+ /// stereo=1
+ ///
+ /// \param[in] _enable True to turn on stereo, false to turn off.
+ public: void EnableStereo(bool _enable);
+
+ // Documentation inherited.
+ public: virtual bool SetProjectionType(const std::string &_type);
+
/// \brief Set the camera to be attached to a visual.
///
/// This causes the camera to move in relation to the specified visual.
@@ -208,6 +231,8 @@ namespace gazebo
/// \return True if the camera is now tracking the visual.
protected: virtual bool TrackVisualImpl(VisualPtr _visual);
+ // Documentation inherited.
+ protected: virtual void UpdateFOV();
/// \brief Toggle whether to show the visual.
private: void ToggleShowVisual();
diff --git a/gazebo/rendering/UserCameraPrivate.hh b/gazebo/rendering/UserCameraPrivate.hh
index 471c256..1b5c44d 100644
--- a/gazebo/rendering/UserCameraPrivate.hh
+++ b/gazebo/rendering/UserCameraPrivate.hh
@@ -15,8 +15,10 @@
*
*/
-#ifndef _USERCAMERA_PRIVATE_HH_
-#define _USERCAMERA_PRIVATE_HH_
+#ifndef _GAZEBO_USERCAMERA_PRIVATE_HH_
+#define _GAZEBO_USERCAMERA_PRIVATE_HH_
+
+#include <string>
namespace gazebo
{
@@ -37,9 +39,15 @@ namespace gazebo
/// \brief The currently active view controller.
public: ViewController *viewController;
+ /// \brief The previously used view controller.
+ public: std::string prevViewControllerName;
+
/// \brief An orbit view controller.
public: OrbitViewController *orbitViewController;
+ /// \brief An orthographic view controller.
+ public: OrthoViewController *orthoViewController;
+
/// \brief A FPS view controller.
public: FPSViewController *fpsViewController;
@@ -61,6 +69,21 @@ namespace gazebo
/// \brief Used to detect joystick button release
public: bool joystickButtonToggleLast;
+
+ /// \brief An optional Ogre camera for stereo rendering.
+ public: Ogre::Camera *rightCamera;
+
+ /// \brief An optional viewport for stereo rendering.
+ public: Ogre::Viewport *rightViewport;
+
+ /// \brief Publishes user camera world pose
+ public: transport::PublisherPtr posePub;
+
+ /// \brief True if stereo rendering should be enabled.
+ public: bool stereoEnabled;
+
+ /// \brief Default camera pose.
+ public: math::Pose defaultPose;
};
}
}
diff --git a/gazebo/rendering/VideoVisual.cc b/gazebo/rendering/VideoVisual.cc
index c2785e3..162641e 100644
--- a/gazebo/rendering/VideoVisual.cc
+++ b/gazebo/rendering/VideoVisual.cc
@@ -32,6 +32,7 @@ VideoVisual::VideoVisual(const std::string &_name, VisualPtr _parent)
VideoVisualPrivate *dPtr =
reinterpret_cast<VideoVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_GUI;
dPtr->video = new common::Video();
dPtr->video->Load("/home/nkoenig/Videos/pr2_risotto/risotto_robot.mp4");
diff --git a/gazebo/rendering/VideoVisual.hh b/gazebo/rendering/VideoVisual.hh
index 9c2a32b..740d8c6 100644
--- a/gazebo/rendering/VideoVisual.hh
+++ b/gazebo/rendering/VideoVisual.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class VideoVisual VideoVisual.hh rendering/rendering.hh
/// \brief A visual element that displays a video as a texture
- class GAZEBO_VISIBLE VideoVisual : public Visual
+ class GZ_RENDERING_VISIBLE VideoVisual : public Visual
{
/// \brief Constructor
/// \param[in] _name Name of the video visual.
diff --git a/gazebo/rendering/ViewController.cc b/gazebo/rendering/ViewController.cc
index 6d5cc26..a7ce5c9 100644
--- a/gazebo/rendering/ViewController.cc
+++ b/gazebo/rendering/ViewController.cc
@@ -14,6 +14,13 @@
* 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 "gazebo/rendering/Camera.hh"
#include "gazebo/rendering/ViewController.hh"
@@ -33,7 +40,8 @@ ViewController::~ViewController()
}
//////////////////////////////////////////////////
-void ViewController::Init(const math::Vector3 &/*_focalPoint*/)
+void ViewController::Init(const math::Vector3 &/*_focalPoint*/,
+ double /*_yaw*/, double /*_pitch*/)
{
}
@@ -49,5 +57,8 @@ void ViewController::SetEnabled(bool _value)
this->enabled = _value;
}
-
-
+//////////////////////////////////////////////////
+void ViewController::Resize(const unsigned int /*_width*/,
+ const unsigned int /*_height*/)
+{
+}
diff --git a/gazebo/rendering/ViewController.hh b/gazebo/rendering/ViewController.hh
index b3ec9d9..1edc0f1 100644
--- a/gazebo/rendering/ViewController.hh
+++ b/gazebo/rendering/ViewController.hh
@@ -31,7 +31,7 @@ namespace gazebo
/// \class ViewController ViewController.hh rendering/rendering.hh
/// \brief Base class for view controllers.
- class GAZEBO_VISIBLE ViewController
+ class GZ_RENDERING_VISIBLE ViewController
{
/// \brief Constructor
/// \param[in] _camera The user camera to controll.
@@ -45,12 +45,21 @@ namespace gazebo
/// \brief Initialize with a focus point.
/// \param[in] _focalPoint The point to look at.
- public: virtual void Init(const math::Vector3 &_focalPoint);
+ /// \param[in] _yaw Initial yaw angle.
+ /// \param[in] _pitch Initial pitch angle.
+ public: virtual void Init(const math::Vector3 &_focalPoint,
+ const double _yaw = 0, const double _pitch = 0);
/// \brief Update the controller, which should update the position
/// of the Camera.
public: virtual void Update() = 0;
+ /// \brief Called by the UserCamera when a resize event occurs.
+ /// \param[in] _width New width
+ /// \param[in] _height New height
+ public: virtual void Resize(const unsigned int _width,
+ const unsigned int _height);
+
/// \brief Set whether the controller is enabled.
/// \param[in] _value True if the controller is enabled.
public: void SetEnabled(bool _value);
diff --git a/gazebo/rendering/Visual.cc b/gazebo/rendering/Visual.cc
index e3e3bc3..840cdd4 100644
--- a/gazebo/rendering/Visual.cc
+++ b/gazebo/rendering/Visual.cc
@@ -28,6 +28,7 @@
#include "gazebo/common/Mesh.hh"
#include "gazebo/common/Plugin.hh"
#include "gazebo/common/Skeleton.hh"
+#include "gazebo/rendering/RenderEvents.hh"
#include "gazebo/rendering/WireBox.hh"
#include "gazebo/rendering/Conversions.hh"
#include "gazebo/rendering/DynamicLines.hh"
@@ -81,6 +82,7 @@ void Visual::Init(const std::string &_name, ScenePtr _scene,
this->dataPtr->id = this->dataPtr->visualIdCount--;
this->dataPtr->boundingBox = NULL;
this->dataPtr->useRTShader = _useRTShader;
+ this->dataPtr->visibilityFlags = GZ_VISIBILITY_ALL;
this->dataPtr->sdf.reset(new sdf::Element);
sdf::initFile("visual.sdf", this->dataPtr->sdf);
@@ -90,6 +92,10 @@ void Visual::Init(const std::string &_name, ScenePtr _scene,
this->dataPtr->animState = NULL;
this->dataPtr->skeleton = NULL;
this->dataPtr->initialized = false;
+ this->dataPtr->lighting = true;
+ this->dataPtr->castShadows = true;
+ this->dataPtr->visible = true;
+ this->dataPtr->layer = -1;
std::string uniqueName = this->GetName();
int index = 0;
@@ -125,6 +131,9 @@ void Visual::Init(const std::string &_name, VisualPtr _parent,
this->dataPtr->animState = NULL;
this->dataPtr->initialized = false;
this->dataPtr->lighting = true;
+ this->dataPtr->castShadows = true;
+ this->dataPtr->visible = true;
+ this->dataPtr->layer = -1;
Ogre::SceneNode *pnode = NULL;
if (_parent)
@@ -201,19 +210,7 @@ void Visual::Fini()
if (this->dataPtr->parent)
this->dataPtr->parent->DetachVisual(this->GetName());
- // Detach all children
- std::vector<VisualPtr>::iterator iter;
- for (iter = this->dataPtr->children.begin();
- iter != this->dataPtr->children.end(); ++iter)
- {
- this->dataPtr->sceneNode->removeChild((*iter)->GetSceneNode());
- (*iter)->dataPtr->parent.reset();
- (*iter).reset();
- }
-
- this->dataPtr->children.clear();
-
- if (this->dataPtr->sceneNode != NULL)
+ if (this->dataPtr->sceneNode)
{
this->dataPtr->sceneNode->detachAllObjects();
this->dataPtr->scene->GetManager()->destroySceneNode(
@@ -228,6 +225,7 @@ void Visual::Fini()
}
RTShaderSystem::Instance()->DetachEntity(this);
+ this->dataPtr->scene.reset();
}
/////////////////////////////////////////////////
@@ -235,7 +233,6 @@ VisualPtr Visual::Clone(const std::string &_name, VisualPtr _newParent)
{
VisualPtr result(new Visual(_name, _newParent));
result->Load(this->dataPtr->sdf);
-
std::vector<VisualPtr>::iterator iter;
for (iter = this->dataPtr->children.begin();
iter != this->dataPtr->children.end(); ++iter)
@@ -246,6 +243,8 @@ VisualPtr Visual::Clone(const std::string &_name, VisualPtr _newParent)
if (_newParent == this->dataPtr->scene->GetWorldVisual())
result->SetWorldPose(this->GetWorldPose());
result->ShowCollision(false);
+
+ result->SetName(_name);
return result;
}
@@ -282,11 +281,13 @@ void Visual::DestroyAllAttachedMovableObjects(Ogre::SceneNode *_sceneNode)
//////////////////////////////////////////////////
void Visual::Init()
{
+ this->dataPtr->type = VT_ENTITY;
this->dataPtr->transparency = 0.0;
this->dataPtr->isStatic = false;
this->dataPtr->visible = true;
this->dataPtr->ribbonTrail = NULL;
this->dataPtr->staticGeom = NULL;
+ this->dataPtr->layer = -1;
if (this->dataPtr->useRTShader)
RTShaderSystem::Instance()->AttachEntity(this);
@@ -297,143 +298,7 @@ void Visual::Init()
//////////////////////////////////////////////////
void Visual::LoadFromMsg(const boost::shared_ptr< msgs::Visual const> &_msg)
{
- sdf::ElementPtr geomElem = this->dataPtr->sdf->GetElement("geometry");
- geomElem->ClearElements();
-
- if (_msg->has_geometry())
- {
- if (_msg->geometry().type() == msgs::Geometry::BOX)
- {
- sdf::ElementPtr elem = geomElem->AddElement("box");
- elem->GetElement("size")->Set(
- msgs::Convert(_msg->geometry().box().size()));
- }
- else if (_msg->geometry().type() == msgs::Geometry::SPHERE)
- {
- sdf::ElementPtr elem = geomElem->AddElement("sphere");
- elem->GetElement("radius")->Set(_msg->geometry().sphere().radius());
- }
- else if (_msg->geometry().type() == msgs::Geometry::CYLINDER)
- {
- sdf::ElementPtr elem = geomElem->AddElement("cylinder");
- elem->GetElement("radius")->Set(_msg->geometry().cylinder().radius());
- elem->GetElement("length")->Set(_msg->geometry().cylinder().length());
- }
- else if (_msg->geometry().type() == msgs::Geometry::PLANE)
- {
- math::Plane plane = msgs::Convert(_msg->geometry().plane());
- sdf::ElementPtr elem = geomElem->AddElement("plane");
- elem->GetElement("normal")->Set(plane.normal);
- elem->GetElement("size")->Set(plane.size);
- }
- else if (_msg->geometry().type() == msgs::Geometry::POLYLINE)
- {
- sdf::ElementPtr elem = geomElem->AddElement("polyline");
- elem->GetElement("height")->Set(_msg->geometry().polyline().height());
- for (int i = 0; i < _msg->geometry().polyline().point_size(); ++i)
- {
- elem->AddElement("point")->Set(
- msgs::Convert(_msg->geometry().polyline().point(i)));
- }
- }
- else if (_msg->geometry().type() == msgs::Geometry::MESH)
- {
- sdf::ElementPtr elem = geomElem->AddElement("mesh");
- elem->GetElement("uri")->Set(_msg->geometry().mesh().filename());
-
- if (_msg->geometry().mesh().has_submesh())
- {
- elem->GetElement("submesh")->GetElement("name")->Set(
- _msg->geometry().mesh().submesh());
- }
-
- if (_msg->geometry().mesh().has_center_submesh())
- {
- elem->GetElement("submesh")->GetElement("center")->Set(
- _msg->geometry().mesh().center_submesh());
- }
- }
- }
-
- if (_msg->has_pose())
- {
- sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("pose");
- math::Pose p(msgs::Convert(_msg->pose().position()),
- msgs::Convert(_msg->pose().orientation()));
-
- elem->Set(p);
- }
-
- if (_msg->has_material())
- {
- if (_msg->material().has_script())
- {
- sdf::ElementPtr elem =
- this->dataPtr->sdf->GetElement("material")->GetElement("script");
- elem->GetElement("name")->Set(_msg->material().script().name());
- for (int i = 0; i < _msg->material().script().uri_size(); ++i)
- {
- sdf::ElementPtr uriElem = elem->AddElement("uri");
- uriElem->Set(_msg->material().script().uri(i));
- }
- }
-
- if (_msg->material().has_ambient())
- {
- sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("material");
- elem->GetElement("ambient")->Set(
- msgs::Convert(_msg->material().ambient()));
- }
-
- if (_msg->material().has_diffuse())
- {
- sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("material");
- elem->GetElement("diffuse")->Set(
- msgs::Convert(_msg->material().diffuse()));
- }
-
- if (_msg->material().has_specular())
- {
- sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("material");
- elem->GetElement("specular")->Set(
- msgs::Convert(_msg->material().specular()));
- }
-
- if (_msg->material().has_emissive())
- {
- sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("material");
- elem->GetElement("emissive")->Set(
- msgs::Convert(_msg->material().emissive()));
- }
-
- if (_msg->material().has_lighting())
- {
- sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("material");
- elem->GetElement("lighting")->Set(_msg->material().lighting());
- }
- }
-
- if (_msg->has_cast_shadows())
- this->dataPtr->sdf->GetElement("cast_shadows")->Set(_msg->cast_shadows());
-
- if (_msg->has_laser_retro())
- this->dataPtr->sdf->GetElement("laser_retro")->Set(_msg->laser_retro());
-
- if (_msg->has_plugin())
- {
- sdf::ElementPtr elem = this->dataPtr->sdf->GetElement("plugin");
- if (_msg->plugin().has_name())
- elem->GetAttribute("name")->Set(_msg->plugin().name());
- if (_msg->plugin().has_filename())
- elem->GetAttribute("filename")->Set(_msg->plugin().filename());
- if (_msg->plugin().has_innerxml())
- {
- TiXmlDocument innerXML;
- innerXML.Parse(_msg->plugin().innerxml().c_str());
- sdf::copyChildren(elem, innerXML.RootElement());
- }
- }
-
+ this->dataPtr->sdf = msgs::VisualToSDF(*_msg.get());
this->Load();
this->UpdateFromMsg(_msg);
}
@@ -443,7 +308,6 @@ void Visual::Load(sdf::ElementPtr _sdf)
{
this->dataPtr->sdf->Copy(_sdf);
this->Load();
- this->dataPtr->scene->AddVisual(shared_from_this());
}
//////////////////////////////////////////////////
@@ -460,23 +324,23 @@ void Visual::Load()
// Read the desired position and rotation of the mesh
pose = this->dataPtr->sdf->Get<math::Pose>("pose");
- std::string meshName = this->GetMeshName();
- std::string subMeshName = this->GetSubMeshName();
+ std::string mesh = this->GetMeshName();
+ std::string subMesh = this->GetSubMeshName();
bool centerSubMesh = this->GetCenterSubMesh();
- if (!meshName.empty())
+ if (!mesh.empty())
{
try
{
// Create the visual
stream << "VISUAL_" << this->dataPtr->sceneNode->getName();
- obj = this->AttachMesh(meshName, subMeshName, centerSubMesh,
+ obj = this->AttachMesh(mesh, subMesh, centerSubMesh,
stream.str());
}
catch(Ogre::Exception &e)
{
gzerr << "Ogre Error:" << e.getFullDescription() << "\n";
- gzerr << "Unable to create a mesh from " << meshName << "\n";
+ gzerr << "Unable to create a mesh from " << mesh << "\n";
return;
}
}
@@ -505,6 +369,7 @@ void Visual::Load()
{
sdf::ElementPtr geomElem = this->dataPtr->sdf->GetElement("geometry");
+ bool hasGeom = true;
if (geomElem->HasElement("box"))
{
this->dataPtr->scale =
@@ -532,16 +397,33 @@ void Visual::Load()
this->dataPtr->scale =
geomElem->GetElement("mesh")->Get<math::Vector3>("scale");
}
- }
+ else
+ {
+ hasGeom = false;
+ }
- this->dataPtr->sceneNode->setScale(this->dataPtr->scale.x,
- this->dataPtr->scale.y, this->dataPtr->scale.z);
+ if (hasGeom)
+ {
+ // geom values give the absolute size so compute a scale that will
+ // be mulitiply by the current scale to get to the geom size.
+ math::Vector3 derivedScale = Conversions::Convert(
+ this->dataPtr->sceneNode->_getDerivedScale());
+ math::Vector3 toScale = this->dataPtr->scale / derivedScale;
+ this->dataPtr->sceneNode->scale(toScale.x, toScale.y, toScale.z);
+ }
+ }
// Set the material of the mesh
if (this->dataPtr->sdf->HasElement("material"))
{
sdf::ElementPtr matElem =
this->dataPtr->sdf->GetElement("material");
+
+ // clone the material sdf to preserve the new values to be set
+ // as updating the material name via SetMaterial can affect the
+ // ambient/diffuse/specular/emissive color sdf elements.
+ sdf::ElementPtr matElemClone = matElem->Clone();
+
if (matElem->HasElement("script"))
{
sdf::ElementPtr scriptElem = matElem->GetElement("script");
@@ -562,24 +444,41 @@ void Visual::Load()
this->SetMaterial(matName);
}
+ if (matElemClone->HasElement("ambient"))
+ this->SetAmbient(matElemClone->Get<common::Color>("ambient"));
+ if (matElemClone->HasElement("diffuse"))
+ this->SetDiffuse(matElemClone->Get<common::Color>("diffuse"));
+ if (matElemClone->HasElement("specular"))
+ this->SetSpecular(matElemClone->Get<common::Color>("specular"));
+ if (matElemClone->HasElement("emissive"))
+ this->SetEmissive(matElemClone->Get<common::Color>("emissive"));
+
if (matElem->HasElement("lighting"))
{
this->SetLighting(matElem->Get<bool>("lighting"));
}
+ }
- if (matElem->HasElement("ambient"))
- this->SetAmbient(matElem->Get<common::Color>("ambient"));
- if (matElem->HasElement("diffuse"))
- this->SetDiffuse(matElem->Get<common::Color>("diffuse"));
- if (matElem->HasElement("specular"))
- this->SetSpecular(matElem->Get<common::Color>("specular"));
- if (matElem->HasElement("emissive"))
- this->SetEmissive(matElem->Get<common::Color>("emissive"));
+ if (this->dataPtr->sdf->HasElement("transparency"))
+ {
+ this->SetTransparency(this->dataPtr->sdf->Get<float>("transparency"));
}
// Allow the mesh to cast shadows
this->SetCastShadows(this->dataPtr->sdf->Get<bool>("cast_shadows"));
this->LoadPlugins();
+ this->dataPtr->scene->AddVisual(shared_from_this());
+
+ // Set meta information
+ if (this->dataPtr->sdf->HasElement("meta"))
+ {
+ if (this->dataPtr->sdf->GetElement("meta")->HasElement("layer"))
+ {
+ this->dataPtr->layer =
+ this->dataPtr->sdf->GetElement("meta")->Get<int32_t>("layer");
+ rendering::Events::newLayer(this->dataPtr->layer);
+ }
+ }
}
//////////////////////////////////////////////////
@@ -671,7 +570,8 @@ void Visual::DetachVisual(const std::string &_name)
{
VisualPtr childVis = (*iter);
this->dataPtr->children.erase(iter);
- this->dataPtr->sceneNode->removeChild(childVis->GetSceneNode());
+ if (this->dataPtr->sceneNode)
+ this->dataPtr->sceneNode->removeChild(childVis->GetSceneNode());
childVis->GetParent().reset();
break;
}
@@ -689,6 +589,26 @@ void Visual::AttachObject(Ogre::MovableObject *_obj)
if (!this->HasAttachedObject(_obj->getName()))
{
+ // update to use unique materials
+ Ogre::Entity *entity = dynamic_cast<Ogre::Entity *>(_obj);
+ if (entity)
+ {
+ for (unsigned j = 0; j < entity->getNumSubEntities(); ++j)
+ {
+ Ogre::SubEntity *subEntity = entity->getSubEntity(j);
+ Ogre::MaterialPtr material = subEntity->getMaterial();
+ if (!material.isNull() &&
+ material->getName().find("_MATERIAL_") == std::string::npos)
+ {
+ std::string newMaterialName;
+ newMaterialName = this->dataPtr->sceneNode->getName() +
+ "_MATERIAL_" + material->getName();
+ material = material->clone(newMaterialName);
+ subEntity->setMaterial(material);
+ }
+ }
+ }
+
this->dataPtr->sceneNode->attachObject(_obj);
if (this->dataPtr->useRTShader && this->dataPtr->scene->GetInitialized() &&
_obj->getName().find("__COLLISION_VISUAL__") == std::string::npos)
@@ -726,7 +646,11 @@ unsigned int Visual::GetAttachedObjectCount() const
//////////////////////////////////////////////////
void Visual::DetachObjects()
{
- this->dataPtr->sceneNode->detachAllObjects();
+ if (this->dataPtr->sceneNode)
+ this->dataPtr->sceneNode->detachAllObjects();
+ this->dataPtr->meshName = "";
+ this->dataPtr->subMeshName = "";
+ this->dataPtr->myMaterialName = "";
}
//////////////////////////////////////////////////
@@ -772,6 +696,9 @@ Ogre::MovableObject *Visual::AttachMesh(const std::string &_meshName,
if (_meshName.empty())
return NULL;
+ this->dataPtr->meshName = _meshName;
+ this->dataPtr->subMeshName = _subMesh;
+
Ogre::MovableObject *obj;
std::string objName = _objName;
std::string meshName = _meshName;
@@ -782,8 +709,17 @@ Ogre::MovableObject *Visual::AttachMesh(const std::string &_meshName,
this->InsertMesh(_meshName, _subMesh, _centerSubmesh);
- obj = (Ogre::MovableObject*)
- (this->dataPtr->sceneNode->getCreator()->createEntity(objName, meshName));
+ if (this->dataPtr->sceneNode->getCreator()->hasEntity(objName))
+ {
+ obj = (Ogre::MovableObject*)
+ (this->dataPtr->sceneNode->getCreator()->getEntity(objName));
+ }
+ else
+ {
+ obj = (Ogre::MovableObject*)
+ (this->dataPtr->sceneNode->getCreator()->createEntity(objName,
+ meshName));
+ }
this->AttachObject(obj);
return obj;
@@ -798,7 +734,7 @@ void Visual::SetScale(const math::Vector3 &_scale)
this->dataPtr->scale = _scale;
// update geom size based on scale.
- this->UpdateGeomSize(_scale);
+ this->UpdateGeomSize(this->dataPtr->scale);
this->dataPtr->sceneNode->setScale(
Conversions::Convert(this->dataPtr->scale));
@@ -813,20 +749,28 @@ void Visual::UpdateGeomSize(const math::Vector3 &_scale)
(*iter)->UpdateGeomSize(_scale);
}
- sdf::ElementPtr geomElem = this->dataPtr->sdf->GetElement("geometry");
+ math::Vector3 derivedScale = math::Vector3::One;
+ VisualPtr parentVis = this->GetParent();
+ if (parentVis)
+ {
+ derivedScale = Conversions::Convert(
+ parentVis->GetSceneNode()->_getDerivedScale());
+ }
+ sdf::ElementPtr geomElem = this->dataPtr->sdf->GetElement("geometry");
if (geomElem->HasElement("box"))
{
geomElem->GetElement("box")->GetElement("size")->Set(_scale);
}
else if (geomElem->HasElement("sphere"))
{
- geomElem->GetElement("sphere")->GetElement("radius")->Set(_scale.x/2.0);
+ geomElem->GetElement("sphere")->GetElement("radius")->Set(
+ _scale.x*0.5);
}
else if (geomElem->HasElement("cylinder"))
{
geomElem->GetElement("cylinder")->GetElement("radius")
- ->Set(_scale.x/2.0);
+ ->Set(_scale.x*0.5);
geomElem->GetElement("cylinder")->GetElement("length")->Set(_scale.z);
}
else if (geomElem->HasElement("mesh"))
@@ -849,9 +793,14 @@ void Visual::SetLighting(bool _lighting)
if (this->dataPtr->useRTShader)
{
- // Detach from RTShaderSystem otherwise setting lighting here will have
- // no effect if shaders are used.
- RTShaderSystem::Instance()->DetachEntity(this);
+ if (this->dataPtr->lighting)
+ RTShaderSystem::Instance()->AttachEntity(this);
+ else
+ {
+ // Detach from RTShaderSystem otherwise setting lighting here will have
+ // no effect if shaders are used.
+ RTShaderSystem::Instance()->DetachEntity(this);
+ }
}
try
@@ -910,8 +859,16 @@ void Visual::SetLighting(bool _lighting)
{
(*iter)->SetLighting(this->dataPtr->lighting);
}
+
+ this->dataPtr->sdf->GetElement("material")
+ ->GetElement("lighting")->Set(this->dataPtr->lighting);
}
+//////////////////////////////////////////////////
+bool Visual::GetLighting() const
+{
+ return this->dataPtr->lighting;
+}
//////////////////////////////////////////////////
void Visual::SetMaterial(const std::string &_materialName, bool _unique)
@@ -919,6 +876,13 @@ void Visual::SetMaterial(const std::string &_materialName, bool _unique)
if (_materialName.empty() || _materialName == "__default__")
return;
+ common::Color matAmbient;
+ common::Color matDiffuse;
+ common::Color matSpecular;
+ common::Color matEmissive;
+ bool matColor = rendering::Material::GetMaterialAsColor(
+ _materialName, matAmbient, matDiffuse, matSpecular, matEmissive);
+
if (_unique)
{
// Create a custom material name
@@ -926,7 +890,11 @@ void Visual::SetMaterial(const std::string &_materialName, bool _unique)
newMaterialName = this->dataPtr->sceneNode->getName() + "_MATERIAL_" +
_materialName;
- if (this->GetMaterialName() == newMaterialName)
+ if (this->GetMaterialName() == newMaterialName &&
+ matAmbient == this->GetAmbient() &&
+ matDiffuse == this->GetDiffuse() &&
+ matSpecular == this->GetSpecular() &&
+ matEmissive == this->GetEmissive())
return;
this->dataPtr->myMaterialName = newMaterialName;
@@ -980,16 +948,19 @@ void Visual::SetMaterial(const std::string &_materialName, bool _unique)
try
{
- for (int i = 0; i < this->dataPtr->sceneNode->numAttachedObjects(); i++)
+ for (unsigned int i = 0;
+ i < this->dataPtr->sceneNode->numAttachedObjects(); ++i)
{
Ogre::MovableObject *obj = this->dataPtr->sceneNode->getAttachedObject(i);
-
- if (dynamic_cast<Ogre::Entity*>(obj))
- ((Ogre::Entity*)obj)->setMaterialName(this->dataPtr->myMaterialName);
- else if (dynamic_cast<Ogre::SimpleRenderable*>(obj))
+ Ogre::Entity *entity = dynamic_cast<Ogre::Entity *>(obj);
+ if (entity)
+ entity->setMaterialName(this->dataPtr->myMaterialName);
+ else
{
- ((Ogre::SimpleRenderable*)obj)->setMaterial(
- this->dataPtr->myMaterialName);
+ Ogre::SimpleRenderable *simpleRenderable =
+ dynamic_cast<Ogre::SimpleRenderable *>(obj);
+ if (simpleRenderable)
+ simpleRenderable->setMaterial(this->dataPtr->myMaterialName);
}
}
@@ -1020,8 +991,17 @@ void Visual::SetMaterial(const std::string &_materialName, bool _unique)
<< ". Object will appear white.\n";
}
+ // check if material has color components, if so, set them.
+ if (matColor)
+ {
+ this->SetAmbient(matAmbient);
+ this->SetDiffuse(matDiffuse);
+ this->SetSpecular(matSpecular);
+ this->SetEmissive(matEmissive);
+ }
+
// Re-apply the transparency filter for the last known transparency value
- this->SetTransparencyInnerLoop();
+ this->SetTransparencyInnerLoop(this->dataPtr->sceneNode);
// Apply material to all child visuals
for (std::vector<VisualPtr>::iterator iter = this->dataPtr->children.begin();
@@ -1036,6 +1016,9 @@ void Visual::SetMaterial(const std::string &_materialName, bool _unique)
{
RTShaderSystem::Instance()->UpdateShaders();
}
+
+ this->dataPtr->sdf->GetElement("material")->GetElement("script")
+ ->GetElement("name")->Set(_materialName);
}
/////////////////////////////////////////////////
@@ -1095,11 +1078,16 @@ void Visual::SetAmbient(const common::Color &_color)
for (unsigned int i = 0; i < this->dataPtr->children.size(); ++i)
{
- this->dataPtr->children[i]->SetSpecular(_color);
+ this->dataPtr->children[i]->SetAmbient(_color);
}
+
+ this->dataPtr->ambient = _color;
+
+ this->dataPtr->sdf->GetElement("material")
+ ->GetElement("ambient")->Set(_color);
}
-/// Set the diffuse color of the visual
+/////////////////////////////////////////////////
void Visual::SetDiffuse(const common::Color &_color)
{
if (!this->dataPtr->lighting)
@@ -1145,7 +1133,9 @@ void Visual::SetDiffuse(const common::Color &_color)
for (passCount = 0; passCount < technique->getNumPasses(); passCount++)
{
pass = technique->getPass(passCount);
- pass->setDiffuse(Conversions::Convert(_color));
+ dc = Conversions::Convert(_color);
+ pass->setDiffuse(dc);
+ this->dataPtr->transparency = 1.0f - dc.a;
}
}
}
@@ -1155,9 +1145,14 @@ void Visual::SetDiffuse(const common::Color &_color)
{
this->dataPtr->children[i]->SetDiffuse(_color);
}
+
+ this->dataPtr->diffuse = _color;
+
+ this->dataPtr->sdf->GetElement("material")
+ ->GetElement("diffuse")->Set(_color);
}
-/// Set the specular color of the visual
+/////////////////////////////////////////////////
void Visual::SetSpecular(const common::Color &_color)
{
if (!this->dataPtr->lighting)
@@ -1211,58 +1206,87 @@ void Visual::SetSpecular(const common::Color &_color)
{
this->dataPtr->children[i]->SetSpecular(_color);
}
+
+ this->dataPtr->specular = _color;
+
+ this->dataPtr->sdf->GetElement("material")
+ ->GetElement("specular")->Set(_color);
}
-/////////////////////////////////////////////////
-void Visual::AttachAxes()
+//////////////////////////////////////////////////
+void Visual::SetEmissive(const common::Color &_color)
{
- std::ostringstream nodeName;
+ for (unsigned int i = 0; i < this->dataPtr->sceneNode->numAttachedObjects();
+ i++)
+ {
+ Ogre::Entity *entity = NULL;
+ Ogre::MovableObject *obj = this->dataPtr->sceneNode->getAttachedObject(i);
+
+ entity = dynamic_cast<Ogre::Entity*>(obj);
- nodeName << this->dataPtr->sceneNode->getName() << "_AXES_NODE";
+ if (!entity)
+ continue;
- if (!this->dataPtr->sceneNode->getCreator()->hasEntity("axis_cylinder"))
- this->InsertMesh(common::MeshManager::Instance()->GetMesh("axis_cylinder"));
+ // For each ogre::entity
+ for (unsigned int j = 0; j < entity->getNumSubEntities(); j++)
+ {
+ Ogre::SubEntity *subEntity = entity->getSubEntity(j);
+ Ogre::MaterialPtr material = subEntity->getMaterial();
- Ogre::SceneNode *node = this->dataPtr->sceneNode->createChildSceneNode(
- nodeName.str());
- Ogre::SceneNode *x, *y, *z;
+ unsigned int techniqueCount, passCount;
+ Ogre::Technique *technique;
+ Ogre::Pass *pass;
+ Ogre::ColourValue dc;
- x = node->createChildSceneNode(nodeName.str() + "_axisX");
- x->setInheritScale(true);
- x->translate(.25, 0, 0);
- x->yaw(Ogre::Radian(M_PI/2.0));
+ for (techniqueCount = 0; techniqueCount < material->getNumTechniques();
+ techniqueCount++)
+ {
+ technique = material->getTechnique(techniqueCount);
- y = node->createChildSceneNode(nodeName.str() + "_axisY");
- y->setInheritScale(true);
- y->translate(0, .25, 0);
- y->pitch(Ogre::Radian(M_PI/2.0));
+ for (passCount = 0; passCount < technique->getNumPasses();
+ passCount++)
+ {
+ pass = technique->getPass(passCount);
+ pass->setSelfIllumination(Conversions::Convert(_color));
+ }
+ }
+ }
+ }
- z = node->createChildSceneNode(nodeName.str() + "_axisZ");
- z->translate(0, 0, .25);
- z->setInheritScale(true);
+ for (unsigned int i = 0; i < this->dataPtr->children.size(); ++i)
+ {
+ this->dataPtr->children[i]->SetEmissive(_color);
+ }
- Ogre::MovableObject *xobj, *yobj, *zobj;
+ this->dataPtr->emissive = _color;
- xobj = (Ogre::MovableObject*)(node->getCreator()->createEntity(
- nodeName.str()+"X_AXIS", "axis_cylinder"));
- xobj->setCastShadows(false);
- ((Ogre::Entity*)xobj)->setMaterialName("Gazebo/Red");
+ this->dataPtr->sdf->GetElement("material")
+ ->GetElement("emissive")->Set(_color);
+}
- yobj = (Ogre::MovableObject*)(node->getCreator()->createEntity(
- nodeName.str()+"Y_AXIS", "axis_cylinder"));
- yobj->setCastShadows(false);
- ((Ogre::Entity*)yobj)->setMaterialName("Gazebo/Green");
+/////////////////////////////////////////////////
+common::Color Visual::GetAmbient() const
+{
+ return this->dataPtr->ambient;
+}
- zobj = (Ogre::MovableObject*)(node->getCreator()->createEntity(
- nodeName.str()+"Z_AXIS", "axis_cylinder"));
- zobj->setCastShadows(false);
- ((Ogre::Entity*)zobj)->setMaterialName("Gazebo/Blue");
+/////////////////////////////////////////////////
+common::Color Visual::GetDiffuse() const
+{
+ return this->dataPtr->diffuse;
+}
- x->attachObject(xobj);
- y->attachObject(yobj);
- z->attachObject(zobj);
+/////////////////////////////////////////////////
+common::Color Visual::GetSpecular() const
+{
+ return this->dataPtr->specular;
}
+/////////////////////////////////////////////////
+common::Color Visual::GetEmissive() const
+{
+ return this->dataPtr->emissive;
+}
//////////////////////////////////////////////////
void Visual::SetWireframe(bool _show)
@@ -1316,13 +1340,13 @@ void Visual::SetWireframe(bool _show)
}
//////////////////////////////////////////////////
-void Visual::SetTransparencyInnerLoop()
+void Visual::SetTransparencyInnerLoop(Ogre::SceneNode *_sceneNode)
{
- for (unsigned int i = 0; i < this->dataPtr->sceneNode->numAttachedObjects();
+ for (unsigned int i = 0; i < _sceneNode->numAttachedObjects();
i++)
{
Ogre::Entity *entity = NULL;
- Ogre::MovableObject *obj = this->dataPtr->sceneNode->getAttachedObject(i);
+ Ogre::MovableObject *obj = _sceneNode->getAttachedObject(i);
entity = dynamic_cast<Ogre::Entity*>(obj);
@@ -1338,19 +1362,20 @@ void Visual::SetTransparencyInnerLoop()
Ogre::SubEntity *subEntity = entity->getSubEntity(j);
Ogre::MaterialPtr material = subEntity->getMaterial();
- unsigned int techniqueCount, passCount;
+ unsigned int techniqueCount, passCount, unitStateCount;
Ogre::Technique *technique;
Ogre::Pass *pass;
Ogre::ColourValue dc;
for (techniqueCount = 0; techniqueCount < material->getNumTechniques();
- techniqueCount++)
+ ++techniqueCount)
{
technique = material->getTechnique(techniqueCount);
- for (passCount = 0; passCount < technique->getNumPasses(); passCount++)
+ for (passCount = 0; passCount < technique->getNumPasses(); ++passCount)
{
pass = technique->getPass(passCount);
+
// Need to fix transparency
if (!pass->isProgrammable() &&
pass->getPolygonMode() == Ogre::PM_SOLID)
@@ -1370,8 +1395,20 @@ void Visual::SetTransparencyInnerLoop()
}
dc = pass->getDiffuse();
- dc.a =(1.0f - this->dataPtr->transparency);
+ dc.a = (1.0f - this->dataPtr->transparency);
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);
+ }
}
}
}
@@ -1381,22 +1418,39 @@ void Visual::SetTransparencyInnerLoop()
//////////////////////////////////////////////////
void Visual::SetTransparency(float _trans)
{
- if (math::equal(_trans, this->dataPtr->transparency))
+ if (math::equal(this->dataPtr->transparency, _trans))
return;
this->dataPtr->transparency = std::min(
std::max(_trans, static_cast<float>(0.0)), static_cast<float>(1.0));
- std::vector<VisualPtr>::iterator iter;
- for (iter = this->dataPtr->children.begin();
- iter != this->dataPtr->children.end(); ++iter)
+
+ for (auto child : this->dataPtr->children)
{
- (*iter)->SetTransparency(_trans);
+ // Don't change some visualizations when link changes
+ if (!(this->GetType() == VT_LINK &&
+ (child->GetType() == VT_GUI ||
+ child->GetType() == VT_PHYSICS ||
+ child->GetType() == VT_SENSOR)))
+ {
+ child->SetTransparency(_trans);
+ }
}
- this->SetTransparencyInnerLoop();
+ this->SetTransparencyInnerLoop(this->dataPtr->sceneNode);
+
+ // For child nodes' scene nodes
+ for (unsigned int i = 0; i < this->dataPtr->sceneNode->numChildren(); ++i)
+ {
+ Ogre::SceneNode *childSceneNode = dynamic_cast<Ogre::SceneNode*>(
+ this->dataPtr->sceneNode->getChild(i));
+
+ this->SetTransparencyInnerLoop(childSceneNode);
+ }
if (this->dataPtr->useRTShader && this->dataPtr->scene->GetInitialized())
RTShaderSystem::Instance()->UpdateShaders();
+
+ this->dataPtr->sdf->GetElement("transparency")->Set(_trans);
}
//////////////////////////////////////////////////
@@ -1421,6 +1475,17 @@ void Visual::SetHighlighted(bool _highlighted)
{
this->dataPtr->boundingBox->SetVisible(false);
}
+
+ // If this is a link, highlight frame visual
+ if (this->GetType() == VT_LINK)
+ {
+ VisualPtr linkFrameVis;
+ for (auto child : this->dataPtr->children)
+ {
+ if (child->GetName().find("LINK_FRAME_VISUAL__") != std::string::npos)
+ child->SetHighlighted(_highlighted);
+ }
+ }
}
//////////////////////////////////////////////////
@@ -1434,92 +1499,56 @@ bool Visual::GetHighlighted() const
}
//////////////////////////////////////////////////
-void Visual::SetEmissive(const common::Color &_color)
+float Visual::GetTransparency()
{
- for (unsigned int i = 0; i < this->dataPtr->sceneNode->numAttachedObjects();
- i++)
+ return this->dataPtr->transparency;
+}
+
+//////////////////////////////////////////////////
+void Visual::SetCastShadows(bool _shadows)
+{
+ for (int i = 0; i < this->dataPtr->sceneNode->numAttachedObjects(); i++)
{
- Ogre::Entity *entity = NULL;
Ogre::MovableObject *obj = this->dataPtr->sceneNode->getAttachedObject(i);
+ obj->setCastShadows(_shadows);
+ }
- entity = dynamic_cast<Ogre::Entity*>(obj);
+ if (this->IsStatic() && this->dataPtr->staticGeom)
+ this->dataPtr->staticGeom->setCastShadows(_shadows);
- if (!entity)
- continue;
+ this->dataPtr->castShadows = _shadows;
+ this->dataPtr->sdf->GetElement("cast_shadows")->Set(_shadows);
+}
- // For each ogre::entity
- for (unsigned int j = 0; j < entity->getNumSubEntities(); j++)
- {
- Ogre::SubEntity *subEntity = entity->getSubEntity(j);
- Ogre::MaterialPtr material = subEntity->getMaterial();
-
- unsigned int techniqueCount, passCount;
- Ogre::Technique *technique;
- Ogre::Pass *pass;
- Ogre::ColourValue dc;
-
- for (techniqueCount = 0; techniqueCount < material->getNumTechniques();
- techniqueCount++)
- {
- technique = material->getTechnique(techniqueCount);
-
- for (passCount = 0; passCount < technique->getNumPasses();
- passCount++)
- {
- pass = technique->getPass(passCount);
- pass->setSelfIllumination(Conversions::Convert(_color));
- }
- }
- }
- }
-
- for (unsigned int i = 0; i < this->dataPtr->children.size(); ++i)
- {
- this->dataPtr->children[i]->SetEmissive(_color);
- }
-}
+//////////////////////////////////////////////////
+bool Visual::GetCastShadows() const
+{
+ return this->dataPtr->castShadows;
+}
//////////////////////////////////////////////////
-float Visual::GetTransparency()
-{
- return this->dataPtr->transparency;
-}
-
-//////////////////////////////////////////////////
-void Visual::SetCastShadows(bool shadows)
+void Visual::SetVisible(bool _visible, bool _cascade)
{
- for (int i = 0; i < this->dataPtr->sceneNode->numAttachedObjects(); i++)
+ this->dataPtr->sceneNode->setVisible(_visible, _cascade);
+ if (_cascade)
{
- Ogre::MovableObject *obj = this->dataPtr->sceneNode->getAttachedObject(i);
- obj->setCastShadows(shadows);
+ for (auto child: this->dataPtr->children)
+ child->SetVisible(_visible);
}
- if (this->IsStatic() && this->dataPtr->staticGeom)
- this->dataPtr->staticGeom->setCastShadows(shadows);
-}
-
-//////////////////////////////////////////////////
-void Visual::SetVisible(bool _visible, bool _cascade)
-{
- this->dataPtr->sceneNode->setVisible(_visible, _cascade);
this->dataPtr->visible = _visible;
}
//////////////////////////////////////////////////
uint32_t Visual::GetVisibilityFlags()
{
- if (this->dataPtr->sceneNode->numAttachedObjects() > 0)
- {
- return this->dataPtr->sceneNode->getAttachedObject(0)->getVisibilityFlags();
- }
-
- return GZ_VISIBILITY_ALL;
+ return this->dataPtr->visibilityFlags;
}
//////////////////////////////////////////////////
void Visual::ToggleVisible()
{
- this->SetVisible(!this->GetVisible());
+ this->SetVisible(!this->GetVisible(), true);
}
//////////////////////////////////////////////////
@@ -1540,6 +1569,8 @@ void Visual::SetPosition(const math::Vector3 &_pos)
}*/
GZ_ASSERT(this->dataPtr->sceneNode, "Visual SceneNode is NULL");
this->dataPtr->sceneNode->setPosition(_pos.x, _pos.y, _pos.z);
+
+ this->dataPtr->sdf->GetElement("pose")->Set(this->GetPose());
}
//////////////////////////////////////////////////
@@ -1548,6 +1579,8 @@ void Visual::SetRotation(const math::Quaternion &_rot)
GZ_ASSERT(this->dataPtr->sceneNode, "Visual SceneNode is NULL");
this->dataPtr->sceneNode->setOrientation(
Ogre::Quaternion(_rot.w, _rot.x, _rot.y, _rot.z));
+
+ this->dataPtr->sdf->GetElement("pose")->Set(this->GetPose());
}
//////////////////////////////////////////////////
@@ -1605,16 +1638,19 @@ math::Pose Visual::GetWorldPose() const
Ogre::Vector3 vpos;
Ogre::Quaternion vquatern;
- vpos = this->dataPtr->sceneNode->_getDerivedPosition();
- pose.pos.x = vpos.x;
- pose.pos.y = vpos.y;
- pose.pos.z = vpos.z;
+ if (this->dataPtr->sceneNode)
+ {
+ vpos = this->dataPtr->sceneNode->_getDerivedPosition();
+ pose.pos.x = vpos.x;
+ pose.pos.y = vpos.y;
+ pose.pos.z = vpos.z;
- vquatern = this->dataPtr->sceneNode->_getDerivedOrientation();
- pose.rot.w = vquatern.w;
- pose.rot.x = vquatern.x;
- pose.rot.y = vquatern.y;
- pose.rot.z = vquatern.z;
+ vquatern = this->dataPtr->sceneNode->_getDerivedOrientation();
+ pose.rot.w = vquatern.w;
+ pose.rot.x = vquatern.x;
+ pose.rot.y = vquatern.y;
+ pose.rot.z = vquatern.z;
+ }
return pose;
}
@@ -1732,7 +1768,7 @@ void Visual::SetRibbonTrail(bool _value, const common::Color &_initialColor,
DynamicLines *Visual::CreateDynamicLine(RenderOpType _type)
{
this->dataPtr->preRenderConnection = event::Events::ConnectPreRender(
- boost::bind(&Visual::Update, shared_from_this()));
+ boost::bind(&Visual::Update, this));
DynamicLines *line = new DynamicLines(_type);
this->dataPtr->lines.push_back(line);
@@ -1801,7 +1837,8 @@ void Visual::GetBoundsHelper(Ogre::SceneNode *node, math::Box &box) const
{
std::string str = Ogre::any_cast<std::string>(any);
if (str.substr(0, 3) == "rot" || str.substr(0, 5) == "trans"
- || str.substr(0, 5) == "scale")
+ || str.substr(0, 5) == "scale" ||
+ str.find("_APPLY_WRENCH_") != std::string::npos)
continue;
}
@@ -1929,11 +1966,11 @@ void Visual::InsertMesh(const common::Mesh *_mesh, const std::string &_subMesh,
if (node->GetParent())
ogreSkeleton->getBone(node->GetParent()->GetName())->addChild(bone);
- math::Matrix4 trans = node->GetTransform();
- math::Vector3 pos = trans.GetTranslation();
- math::Quaternion q = trans.GetRotation();
- bone->setPosition(Ogre::Vector3(pos.x, pos.y, pos.z));
- bone->setOrientation(Ogre::Quaternion(q.w, q.x, q.y, q.z));
+ ignition::math::Matrix4d trans = node->Transform();
+ ignition::math::Vector3d pos = trans.Translation();
+ ignition::math::Quaterniond q = trans.Rotation();
+ bone->setPosition(Ogre::Vector3(pos.X(), pos.Y(), pos.Z()));
+ bone->setOrientation(Ogre::Quaternion(q.W(), q.X(), q.Y(), q.Z()));
bone->setInheritOrientation(true);
bone->setManuallyControlled(true);
bone->setInitialState();
@@ -1962,7 +1999,7 @@ void Visual::InsertMesh(const common::Mesh *_mesh, const std::string &_subMesh,
// Recenter the vertices if requested.
if (_centerSubmesh)
- subMesh.Center();
+ subMesh.Center(ignition::math::Vector3d::Zero);
ogreSubMesh = ogreMesh->createSubMesh();
ogreSubMesh->useSharedVertices = false;
@@ -2061,21 +2098,21 @@ void Visual::InsertMesh(const common::Mesh *_mesh, const std::string &_subMesh,
// Add all the vertices
for (j = 0; j < subMesh.GetVertexCount(); j++)
{
- *vertices++ = subMesh.GetVertex(j).x;
- *vertices++ = subMesh.GetVertex(j).y;
- *vertices++ = subMesh.GetVertex(j).z;
+ *vertices++ = subMesh.Vertex(j).X();
+ *vertices++ = subMesh.Vertex(j).Y();
+ *vertices++ = subMesh.Vertex(j).Z();
if (subMesh.GetNormalCount() > 0)
{
- *vertices++ = subMesh.GetNormal(j).x;
- *vertices++ = subMesh.GetNormal(j).y;
- *vertices++ = subMesh.GetNormal(j).z;
+ *vertices++ = subMesh.Normal(j).X();
+ *vertices++ = subMesh.Normal(j).Y();
+ *vertices++ = subMesh.Normal(j).Z();
}
if (subMesh.GetTexCoordCount() > 0)
{
- *vertices++ = subMesh.GetTexCoord(j).x;
- *vertices++ = subMesh.GetTexCoord(j).y;
+ *vertices++ = subMesh.TexCoord(j).X();
+ *vertices++ = subMesh.TexCoord(j).Y();
}
}
@@ -2100,13 +2137,13 @@ void Visual::InsertMesh(const common::Mesh *_mesh, const std::string &_subMesh,
iBuf->unlock();
}
- math::Vector3 max = _mesh->GetMax();
- math::Vector3 min = _mesh->GetMin();
+ ignition::math::Vector3d max = _mesh->Max();
+ ignition::math::Vector3d min = _mesh->Min();
if (_mesh->HasSkeleton())
{
- min = math::Vector3(-1, -1, -1);
- max = math::Vector3(1, 1, 1);
+ min = ignition::math::Vector3d(-1, -1, -1);
+ max = ignition::math::Vector3d(1, 1, 1);
}
if (!max.IsFinite())
@@ -2116,8 +2153,8 @@ void Visual::InsertMesh(const common::Mesh *_mesh, const std::string &_subMesh,
gzthrow("Min bounding box is not finite[" << min << "]\n");
ogreMesh->_setBounds(Ogre::AxisAlignedBox(
- Ogre::Vector3(min.x, min.y, min.z),
- Ogre::Vector3(max.x, max.y, max.z)),
+ Ogre::Vector3(min.X(), min.Y(), min.Z()),
+ Ogre::Vector3(max.X(), max.Y(), max.Z())),
false);
// this line makes clear the mesh is loaded (avoids memory leaks)
@@ -2137,14 +2174,143 @@ void Visual::UpdateFromMsg(const boost::shared_ptr< msgs::Visual const> &_msg)
this->MakeStatic();
*/
+ // Set meta information
+ if (_msg->has_meta())
+ {
+ if (_msg->meta().has_layer())
+ {
+ this->dataPtr->layer = _msg->meta().layer();
+ rendering::Events::newLayer(this->dataPtr->layer);
+ }
+ }
+
if (_msg->has_pose())
- this->SetPose(msgs::Convert(_msg->pose()));
+ this->SetPose(msgs::ConvertIgn(_msg->pose()));
if (_msg->has_visible())
this->SetVisible(_msg->visible());
- if (_msg->has_transparency())
- this->SetTransparency(_msg->transparency());
+ if (_msg->has_scale())
+ this->SetScale(msgs::ConvertIgn(_msg->scale()));
+
+ if (_msg->has_geometry() && _msg->geometry().has_type())
+ {
+ std::string newGeometryType =
+ msgs::ConvertGeometryType(_msg->geometry().type());
+
+ std::string geometryType = this->GetGeometryType();
+ std::string geometryName = this->GetMeshName();
+
+ std::string newGeometryName = geometryName;
+ if (_msg->geometry().has_mesh() && _msg->geometry().mesh().has_filename())
+ newGeometryName = _msg->geometry().mesh().filename();
+
+ if (newGeometryType != geometryType ||
+ (newGeometryType == "mesh" && newGeometryName != geometryName))
+ {
+ std::string origMaterial = this->dataPtr->myMaterialName;
+ float origTransparency = this->dataPtr->transparency;
+
+ sdf::ElementPtr geomElem = this->dataPtr->sdf->GetElement("geometry");
+ geomElem->ClearElements();
+
+ this->DetachObjects();
+
+ if (newGeometryType == "box" || newGeometryType == "cylinder" ||
+ newGeometryType == "sphere" || newGeometryType == "plane")
+ {
+ this->AttachMesh("unit_" + newGeometryType);
+ sdf::ElementPtr shapeElem = geomElem->AddElement(newGeometryType);
+ if (newGeometryType == "sphere" || newGeometryType == "cylinder")
+ shapeElem->GetElement("radius")->Set(0.5);
+ }
+ else if (newGeometryType == "mesh")
+ {
+ std::string filename = _msg->geometry().mesh().filename();
+ std::string meshName = common::find_file(filename);
+ std::string submeshName;
+ bool centerSubmesh = false;
+
+ if (meshName.empty())
+ {
+ meshName = "unit_box";
+ gzerr << "No mesh found, setting mesh to a unit box" << std::endl;
+ }
+ else
+ {
+ if (_msg->geometry().mesh().has_submesh())
+ submeshName= _msg->geometry().mesh().submesh();
+ if (_msg->geometry().mesh().has_center_submesh())
+ centerSubmesh= _msg->geometry().mesh().center_submesh();
+ }
+
+ this->AttachMesh(meshName, submeshName, centerSubmesh);
+
+ sdf::ElementPtr meshElem = geomElem->AddElement(newGeometryType);
+ if (!filename.empty())
+ meshElem->GetElement("uri")->Set(filename);
+ if (!submeshName.empty())
+ {
+ sdf::ElementPtr submeshElem = meshElem->GetElement("submesh");
+ submeshElem->GetElement("name")->Set(submeshName);
+ submeshElem->GetElement("center")->Set(centerSubmesh);
+ }
+ }
+ this->SetTransparency(origTransparency);
+ this->SetMaterial(origMaterial);
+ }
+
+ math::Vector3 geomScale(1, 1, 1);
+
+ if (_msg->geometry().type() == msgs::Geometry::BOX)
+ {
+ geomScale = msgs::ConvertIgn(_msg->geometry().box().size());
+ }
+ else if (_msg->geometry().type() == msgs::Geometry::CYLINDER)
+ {
+ geomScale.x = _msg->geometry().cylinder().radius() * 2.0;
+ geomScale.y = _msg->geometry().cylinder().radius() * 2.0;
+ geomScale.z = _msg->geometry().cylinder().length();
+ }
+ else if (_msg->geometry().type() == msgs::Geometry::SPHERE)
+ {
+ geomScale.x = geomScale.y = geomScale.z
+ = _msg->geometry().sphere().radius() * 2.0;
+ }
+ else if (_msg->geometry().type() == msgs::Geometry::PLANE)
+ {
+ if (_msg->geometry().plane().has_size())
+ {
+ geomScale.x = _msg->geometry().plane().size().x();
+ geomScale.y = _msg->geometry().plane().size().y();
+ }
+ }
+ else if (_msg->geometry().type() == msgs::Geometry::IMAGE)
+ {
+ geomScale.x = geomScale.y = geomScale.z
+ = _msg->geometry().image().scale();
+ }
+ else if (_msg->geometry().type() == msgs::Geometry::HEIGHTMAP)
+ {
+ geomScale = msgs::ConvertIgn(_msg->geometry().heightmap().size());
+ }
+ else if (_msg->geometry().type() == msgs::Geometry::MESH)
+ {
+ if (_msg->geometry().mesh().has_scale())
+ {
+ geomScale = msgs::ConvertIgn(_msg->geometry().mesh().scale());
+ }
+ }
+ else if (_msg->geometry().type() == msgs::Geometry::EMPTY ||
+ _msg->geometry().type() == msgs::Geometry::POLYLINE)
+ {
+ // do nothing for now - keep unit scale.
+ }
+ else
+ gzerr << "Unknown geometry type[" << _msg->geometry().type() << "]\n";
+
+ this->SetScale(geomScale);
+ }
if (_msg->has_material())
{
@@ -2160,7 +2326,11 @@ void Visual::UpdateFromMsg(const boost::shared_ptr< msgs::Visual const> &_msg)
RenderEngine::Instance()->AddResourcePath(
_msg->material().script().uri(i));
}
- this->SetMaterial(_msg->material().script().name());
+ if (_msg->material().script().has_name() &&
+ !_msg->material().script().name().empty())
+ {
+ this->SetMaterial(_msg->material().script().name());
+ }
}
if (_msg->material().has_ambient())
@@ -2175,6 +2345,7 @@ void Visual::UpdateFromMsg(const boost::shared_ptr< msgs::Visual const> &_msg)
if (_msg->material().has_emissive())
this->SetEmissive(msgs::Convert(_msg->material().emissive()));
+
if (_msg->material().has_shader_type())
{
if (_msg->material().shader_type() == msgs::Material::VERTEX)
@@ -2195,67 +2366,19 @@ void Visual::UpdateFromMsg(const boost::shared_ptr< msgs::Visual const> &_msg)
{
this->SetShaderType("normal_map_tangent_space");
}
+ else
+ {
+ gzerr << "Unrecognized shader type" << std::endl;
+ }
if (_msg->material().has_normal_map())
this->SetNormalMap(_msg->material().normal_map());
}
}
- if (_msg->has_scale())
- this->SetScale(msgs::Convert(_msg->scale()));
-
- // TODO: Make sure this isn't necessary
- if (_msg->has_geometry() && _msg->geometry().has_type())
+ if (_msg->has_transparency())
{
- math::Vector3 geomScale(1, 1, 1);
-
- if (_msg->geometry().type() == msgs::Geometry::BOX)
- {
- geomScale = msgs::Convert(_msg->geometry().box().size());
- }
- else if (_msg->geometry().type() == msgs::Geometry::CYLINDER)
- {
- geomScale.x = _msg->geometry().cylinder().radius() * 2.0;
- geomScale.y = _msg->geometry().cylinder().radius() * 2.0;
- geomScale.z = _msg->geometry().cylinder().length();
- }
- else if (_msg->geometry().type() == msgs::Geometry::SPHERE)
- {
- geomScale.x = geomScale.y = geomScale.z
- = _msg->geometry().sphere().radius() * 2.0;
- }
- else if (_msg->geometry().type() == msgs::Geometry::PLANE)
- {
- geomScale.x = geomScale.y = 1.0;
- if (_msg->geometry().plane().has_size())
- {
- geomScale.x = _msg->geometry().plane().size().x();
- geomScale.y = _msg->geometry().plane().size().y();
- }
- geomScale.z = 1.0;
- }
- else if (_msg->geometry().type() == msgs::Geometry::IMAGE)
- {
- geomScale.x = geomScale.y = geomScale.z
- = _msg->geometry().image().scale();
- }
- else if (_msg->geometry().type() == msgs::Geometry::HEIGHTMAP)
- geomScale = msgs::Convert(_msg->geometry().heightmap().size());
- else if (_msg->geometry().type() == msgs::Geometry::MESH)
- {
- if (_msg->geometry().mesh().has_scale())
- geomScale = msgs::Convert(_msg->geometry().mesh().scale());
- else
- geomScale.x = geomScale.y = geomScale.z = 1.0;
- }
- else if (_msg->geometry().type() == msgs::Geometry::EMPTY)
- geomScale.x = geomScale.y = geomScale.z = 1.0;
- else if (_msg->geometry().type() == msgs::Geometry::POLYLINE)
- geomScale.x = geomScale.y = geomScale.z = 1.0;
- else
- gzerr << "Unknown geometry type[" << _msg->geometry().type() << "]\n";
-
- this->SetScale(geomScale);
+ this->SetTransparency(_msg->transparency());
}
/*if (msg->points.size() > 0)
@@ -2277,13 +2400,50 @@ VisualPtr Visual::GetParent() const
VisualPtr Visual::GetRootVisual()
{
VisualPtr p = shared_from_this();
- while (p->GetParent() && p->GetParent()->GetName() != "__world_node__")
+ while (p->GetParent() &&
+ p->GetParent() != this->dataPtr->scene->GetWorldVisual())
+ {
p = p->GetParent();
+ }
return p;
}
//////////////////////////////////////////////////
+VisualPtr Visual::GetNthAncestor(unsigned int _n)
+{
+ // Get visual's depth
+ unsigned int depth = this->GetDepth();
+
+ // Must be deeper than ancestor
+ if (depth < _n)
+ return NULL;
+
+ // Get ancestor
+ VisualPtr p = shared_from_this();
+ while (p->GetParent() && depth != _n)
+ {
+ p = p->GetParent();
+ --depth;
+ }
+
+ return p;
+}
+
+//////////////////////////////////////////////////
+unsigned int Visual::GetDepth() const
+{
+ boost::shared_ptr<Visual const> p = shared_from_this();
+ unsigned int depth = 0;
+ while (p->GetParent())
+ {
+ p = p->GetParent();
+ ++depth;
+ }
+ return depth;
+}
+
+//////////////////////////////////////////////////
bool Visual::IsPlane() const
{
if (this->dataPtr->sdf->HasElement("geometry"))
@@ -2305,8 +2465,39 @@ bool Visual::IsPlane() const
}
//////////////////////////////////////////////////
+std::string Visual::GetGeometryType() const
+{
+ if (this->dataPtr->sdf->HasElement("geometry"))
+ {
+ sdf::ElementPtr geomElem = this->dataPtr->sdf->GetElement("geometry");
+ if (geomElem->HasElement("box"))
+ return "box";
+ else if (geomElem->HasElement("sphere"))
+ return "sphere";
+ else if (geomElem->HasElement("cylinder"))
+ return "cylinder";
+ else if (geomElem->HasElement("plane"))
+ return "plane";
+ else if (geomElem->HasElement("image"))
+ return "image";
+ else if (geomElem->HasElement("polyline"))
+ return "polyline";
+ else if (geomElem->HasElement("mesh"))
+ return "mesh";
+ else if (geomElem->HasElement("heightmap"))
+ return "heightmap";
+ }
+ return "";
+}
+
+//////////////////////////////////////////////////
std::string Visual::GetMeshName() const
{
+ if (!this->dataPtr->meshName.empty())
+ {
+ return this->dataPtr->meshName;
+ }
+
if (this->dataPtr->sdf->HasElement("geometry"))
{
sdf::ElementPtr geomElem = this->dataPtr->sdf->GetElement("geometry");
@@ -2325,21 +2516,27 @@ std::string Visual::GetMeshName() const
if (!meshManager->IsValidFilename(polyLineName))
{
- std::vector<math::Vector2d> vertices;
- sdf::ElementPtr pointElem =
- geomElem->GetElement("polyline")->GetElement("point");
+ sdf::ElementPtr polylineElem = geomElem->GetElement("polyline");
- while (pointElem)
+ std::vector<std::vector<ignition::math::Vector2d> > polylines;
+ while (polylineElem)
{
- math::Vector2d point = pointElem->Get<math::Vector2d>();
- vertices.push_back(point);
- pointElem = pointElem->GetNextElement("point");
+ std::vector<ignition::math::Vector2d> vertices;
+ sdf::ElementPtr pointElem = polylineElem->GetElement("point");
+ while (pointElem)
+ {
+ ignition::math::Vector2d point =
+ pointElem->Get<ignition::math::Vector2d>();
+ vertices.push_back(point);
+ pointElem = pointElem->GetNextElement("point");
+ }
+ polylineElem = polylineElem->GetNextElement("polyline");
+ polylines.push_back(vertices);
}
- meshManager->CreateExtrudedPolyline(polyLineName, vertices,
- geomElem->GetElement("polyline")->Get<double>("height"),
- math::Vector2d(1, 1));
- }
+ meshManager->CreateExtrudedPolyline(polyLineName, polylines,
+ geomElem->GetElement("polyline")->Get<double>("height"));
+ }
return polyLineName;
}
else if (geomElem->HasElement("mesh") || geomElem->HasElement("heightmap"))
@@ -2369,6 +2566,11 @@ std::string Visual::GetMeshName() const
//////////////////////////////////////////////////
std::string Visual::GetSubMeshName() const
{
+ if (!this->dataPtr->subMeshName.empty())
+ {
+ return this->dataPtr->subMeshName;
+ }
+
std::string result;
if (this->dataPtr->sdf->HasElement("geometry"))
@@ -2450,8 +2652,7 @@ void Visual::MoveToPositions(const std::vector<math::Pose> &_pts,
if (!this->dataPtr->preRenderConnection)
{
this->dataPtr->preRenderConnection =
- event::Events::ConnectPreRender(boost::bind(&Visual::Update,
- shared_from_this()));
+ event::Events::ConnectPreRender(boost::bind(&Visual::Update, this));
}
}
@@ -2490,8 +2691,7 @@ void Visual::MoveToPosition(const math::Pose &_pose, double _time)
this->dataPtr->prevAnimTime = common::Time::GetWallTime();
this->dataPtr->preRenderConnection =
- event::Events::ConnectPreRender(boost::bind(&Visual::Update,
- shared_from_this()));
+ event::Events::ConnectPreRender(boost::bind(&Visual::Update, this));
}
//////////////////////////////////////////////////
@@ -2569,6 +2769,8 @@ void Visual::SetVisibilityFlags(uint32_t _flags)
for (int j = 0; j < sn->numAttachedObjects(); ++j)
sn->getAttachedObject(j)->setVisibilityFlags(_flags);
}
+
+ this->dataPtr->visibilityFlags = _flags;
}
//////////////////////////////////////////////////
@@ -2577,11 +2779,9 @@ void Visual::ShowJoints(bool _show)
if (this->GetName().find("JOINT_VISUAL__") != std::string::npos)
this->SetVisible(_show);
- std::vector<VisualPtr>::iterator iter;
- for (iter = this->dataPtr->children.begin();
- iter != this->dataPtr->children.end(); ++iter)
+ for (auto &child : this->dataPtr->children)
{
- (*iter)->ShowJoints(_show);
+ child->ShowJoints(_show);
}
}
@@ -2591,11 +2791,33 @@ void Visual::ShowCOM(bool _show)
if (this->GetName().find("COM_VISUAL__") != std::string::npos)
this->SetVisible(_show);
- std::vector<VisualPtr>::iterator iter;
- for (iter = this->dataPtr->children.begin();
- iter != this->dataPtr->children.end(); ++iter)
+ for (auto &child : this->dataPtr->children)
+ {
+ child->ShowCOM(_show);
+ }
+}
+
+//////////////////////////////////////////////////
+void Visual::ShowInertia(bool _show)
+{
+ if (this->GetName().find("INERTIA_VISUAL__") != std::string::npos)
+ this->SetVisible(_show);
+
+ for (auto &child : this->dataPtr->children)
+ {
+ child->ShowInertia(_show);
+ }
+}
+
+//////////////////////////////////////////////////
+void Visual::ShowLinkFrame(bool _show)
+{
+ if (this->GetName().find("LINK_FRAME_VISUAL__") != std::string::npos)
+ this->SetVisible(_show);
+
+ for (auto &child : this->dataPtr->children)
{
- (*iter)->ShowCOM(_show);
+ child->ShowLinkFrame(_show);
}
}
@@ -2708,5 +2930,119 @@ uint32_t Visual::GetId() const
//////////////////////////////////////////////////
void Visual::SetId(uint32_t _id)
{
+ if (this->dataPtr->id == _id)
+ return;
+
+ // set new id and also let the scene know that the id has changed.
+ this->dataPtr->scene->SetVisualId(shared_from_this(), _id);
this->dataPtr->id = _id;
}
+
+//////////////////////////////////////////////////
+sdf::ElementPtr Visual::GetSDF() const
+{
+ return this->dataPtr->sdf;
+}
+
+//////////////////////////////////////////////////
+Visual::VisualType Visual::GetType() const
+{
+ return this->dataPtr->type;
+}
+
+//////////////////////////////////////////////////
+void Visual::SetType(const Visual::VisualType _type)
+{
+ this->dataPtr->type = _type;
+}
+
+//////////////////////////////////////////////////
+void Visual::ToggleLayer(const int32_t _layer)
+{
+ // Visuals with negative layers are always visible
+ if (this->dataPtr->layer < 0)
+ return;
+
+ if (this->dataPtr->layer == _layer)
+ {
+ this->ToggleVisible();
+ }
+}
+
+//////////////////////////////////////////////////
+Visual::VisualType Visual::ConvertVisualType(const msgs::Visual::Type &_type)
+{
+ Visual::VisualType visualType = Visual::VT_ENTITY;
+
+ switch (_type)
+ {
+ case msgs::Visual::ENTITY:
+ visualType = Visual::VT_ENTITY;
+ break;
+ case msgs::Visual::MODEL:
+ visualType = Visual::VT_MODEL;
+ break;
+ case msgs::Visual::LINK:
+ visualType = Visual::VT_LINK;
+ break;
+ case msgs::Visual::VISUAL:
+ visualType = Visual::VT_VISUAL;
+ break;
+ case msgs::Visual::COLLISION:
+ visualType = Visual::VT_COLLISION;
+ break;
+ case msgs::Visual::SENSOR:
+ visualType = Visual::VT_SENSOR;
+ break;
+ case msgs::Visual::GUI:
+ visualType = Visual::VT_GUI;
+ break;
+ case msgs::Visual::PHYSICS:
+ visualType = Visual::VT_PHYSICS;
+ break;
+ default:
+ gzerr << "Cannot convert visual type. Defaults to 'VT_ENTITY'"
+ << std::endl;
+ break;
+ }
+ return visualType;
+}
+
+//////////////////////////////////////////////////
+msgs::Visual::Type Visual::ConvertVisualType(const Visual::VisualType &_type)
+{
+ msgs::Visual::Type visualType = msgs::Visual::ENTITY;
+
+ switch (_type)
+ {
+ case Visual::VT_ENTITY:
+ visualType = msgs::Visual::ENTITY;
+ break;
+ case Visual::VT_MODEL:
+ visualType = msgs::Visual::MODEL;
+ break;
+ case Visual::VT_LINK:
+ visualType = msgs::Visual::LINK;
+ break;
+ case Visual::VT_VISUAL:
+ visualType = msgs::Visual::VISUAL;
+ break;
+ case Visual::VT_COLLISION:
+ visualType = msgs::Visual::COLLISION;
+ break;
+ case Visual::VT_SENSOR:
+ visualType = msgs::Visual::SENSOR;
+ break;
+ case Visual::VT_GUI:
+ visualType = msgs::Visual::GUI;
+ break;
+ case Visual::VT_PHYSICS:
+ visualType = msgs::Visual::PHYSICS;
+ break;
+ default:
+ gzerr << "Cannot convert visual type. Defaults to 'msgs::Visual::ENTITY'"
+ << std::endl;
+ break;
+ }
+ return visualType;
+}
diff --git a/gazebo/rendering/Visual.hh b/gazebo/rendering/Visual.hh
index ec7aa17..1107034 100644
--- a/gazebo/rendering/Visual.hh
+++ b/gazebo/rendering/Visual.hh
@@ -14,13 +14,9 @@
* limitations under the License.
*
*/
-/* Desc: Ogre Visual Class
- * Author: Nate Koenig
- * Date: 14 Dec 2007
- */
-#ifndef _VISUAL_HH_
-#define _VISUAL_HH_
+#ifndef _GAZEBO_VISUAL_HH_
+#define _GAZEBO_VISUAL_HH_
#include <boost/enable_shared_from_this.hpp>
#include <string>
@@ -59,8 +55,30 @@ namespace gazebo
/// \class Visual Visual.hh rendering/rendering.hh
/// \brief A renderable object
- class GAZEBO_VISIBLE Visual : public boost::enable_shared_from_this<Visual>
+ class GZ_RENDERING_VISIBLE Visual :
+ public boost::enable_shared_from_this<Visual>
{
+ /// \brief Type of visual
+ public: enum VisualType
+ {
+ /// \brief Entity visual
+ VT_ENTITY,
+ /// \brief Model visual
+ VT_MODEL,
+ /// \brief Link visual
+ VT_LINK,
+ /// \brief Visual visual
+ VT_VISUAL,
+ /// \brief Collision visual
+ VT_COLLISION,
+ /// \brief Sensor visual
+ VT_SENSOR,
+ /// \brief GUI visual
+ VT_GUI,
+ /// \brief Physics data visual
+ VT_PHYSICS
+ };
+
/// \brief Constructor
/// \param[in] _name Name of the visual.
/// \param[in] _parent Parent of the visual.
@@ -84,7 +102,7 @@ namespace gazebo
public: void Init();
/// \brief Helper for the destructor
- public: void Fini();
+ public: virtual void Fini();
/// \brief Clone the visual with a new name.
/// \param[in] _name Name of the cloned Visual.
@@ -106,6 +124,14 @@ namespace gazebo
/// \brief Update the visual.
public: void Update();
+ /// \brief Get the visual SDF. Note that visuals are abstract. This SDF
+ /// could be associated with a visual that represents a model, a link,
+ /// a visual (inside a link), or a visualization object
+ /// (e.g. LaserVisual). Therefore this SDF may store more fields than
+ /// actually used.
+ /// \return SDF of the visual.
+ public: sdf::ElementPtr GetSDF() const;
+
/// \brief Set the name of the visual
/// \param[in] _name Name of the visual
public: void SetName(const std::string &_name);
@@ -171,6 +197,10 @@ namespace gazebo
/// \return The scaling factor.
public: math::Vector3 GetScale();
+ /// \brief Get whether or not lighting is enabled.
+ /// \return True if lighting is enabled.
+ public: bool GetLighting() const;
+
/// \brief Set whether or not to enable or disable lighting.
/// \param[in] _lighting True to enable lighting.
public: void SetLighting(bool _lighting);
@@ -195,8 +225,21 @@ namespace gazebo
/// \param[in] _color Specular color.
public: void SetSpecular(const common::Color &_color);
- /// \brief Attach visualization axes
- public: void AttachAxes();
+ /// \brief Get the ambient color of the visual.
+ /// \return Ambient color.
+ public: common::Color GetAmbient() const;
+
+ /// \brief Get the diffuse color of the visual.
+ /// \return Diffuse color.
+ public: common::Color GetDiffuse() const;
+
+ /// \brief Get the specular color of the visual.
+ /// \return Specular color.
+ public: common::Color GetSpecular() const;
+
+ /// \brief Get the emissive color of the visual.
+ /// \return Emissive color.
+ public: common::Color GetEmissive() const;
/// \brief Enable or disable wireframe for this visual.
/// \param[in] _show True to enable wireframe for this visual.
@@ -204,7 +247,8 @@ namespace gazebo
/// \brief Set the transparency of a single visual without calling
/// UpdateShaders.
- private: void SetTransparencyInnerLoop();
+ /// \param[in] _sceneNode The target scene node.
+ private: void SetTransparencyInnerLoop(Ogre::SceneNode *_sceneNode);
/// \brief Set the transparency.
/// \param[in] _trans The transparency, between 0 and 1 where 0 is no
@@ -218,7 +262,7 @@ namespace gazebo
/// \brief Set the visual to be visually highlighted. This is most
/// often used when an object is selected by a user via the GUI.
/// \param[in] _highlighted True to enable the highlighting.
- public: void SetHighlighted(bool _highlighted);
+ public: virtual void SetHighlighted(bool _highlighted);
/// \brief Get whether or not the visual is visually highlighted. This is
/// most often means that an object is selected by a user via the GUI.
@@ -229,6 +273,10 @@ namespace gazebo
/// \param[in] _color The emissive color.
public: virtual void SetEmissive(const common::Color &_color);
+ /// \brief Get whether the visual casts shadows.
+ /// \return True if the visual casts shadows.
+ public: bool GetCastShadows() const;
+
/// \brief Set whether the visual should cast shadows.
/// \param[in] _shadows True to enable shadows.
public: void SetCastShadows(bool _shadows);
@@ -376,6 +424,19 @@ namespace gazebo
/// visual.
public: VisualPtr GetRootVisual();
+ /// \brief Get the nth ancestor counting from the world visual.
+ /// GetNthAncestor(0) returns the world visual. GetNthAncestor(1) returns
+ /// the RootVisual. GetNthAncestor(2) returns the ancestor which is a
+ /// child of the root visual and so on.
+ /// \param[in] _n Depth of the ancestor.
+ /// \return The nth ancestor counting from the world.
+ public: VisualPtr GetNthAncestor(unsigned int _n);
+
+ /// \brief Get the depth of this visual, where 0 is the depth of the
+ /// world visual.
+ /// \return This visual's depth.
+ public: unsigned int GetDepth() const;
+
/// \brief Get the shader type.
/// \return String of the shader type: "vertex", "pixel",
/// "normal_map_object_space", "normal_map_tangent_space".
@@ -441,6 +502,14 @@ namespace gazebo
/// \param[in] _show True to show center of mass visualizations.
public: void ShowCOM(bool _show);
+ /// \brief Display inertia visuals.
+ /// \param[in] _show True to show inertia visualizations.
+ public: void ShowInertia(bool _show);
+
+ /// \brief Display link frame visuals.
+ /// \param[in] _show True to show link frame visualizations.
+ public: void ShowLinkFrame(bool _show);
+
/// \brief Set animation skeleton pose.
/// \param[in] _pose Skelton message
public: void SetSkeletonPose(const msgs::PoseAnimation &_pose);
@@ -463,6 +532,10 @@ namespace gazebo
/// \brief Set the id associated with this visual
public: void SetId(uint32_t _id);
+ /// \brief Get the geometry type.
+ /// \return Type of geometry in string.
+ public: std::string GetGeometryType() const;
+
/// \brief The name of the mesh set in the visual's SDF.
/// \return Name of the mesh.
public: std::string GetMeshName() const;
@@ -475,6 +548,31 @@ namespace gazebo
/// \brief Clear parents.
public: void ClearParent();
+ /// \brief Toggle layer visibility. If the visual is
+ /// on the specified layer its visibility will be toggled.
+ /// \param[in] _layer Index of the layer to toggle.
+ public: void ToggleLayer(const int32_t _layer);
+
+ /// \brief Get type of visual.
+ /// \return Visual type.
+ public: Visual::VisualType GetType() const;
+
+ /// \brief Set type of visual.
+ /// \param[in] _type Visual type.
+ public: void SetType(const Visual::VisualType _type);
+
+ /// \brief Convert from msgs::Visual::Type to VisualType.
+ /// \param[in] _type A msgs::Visual::Type enum.
+ /// \return VisualType enum.
+ public: static Visual::VisualType ConvertVisualType(
+ const msgs::Visual::Type &_type);
+
+ /// \brief Convert from msgs::Visual::Type to VisualType.
+ /// \param[in] _type VisualType enum.
+ /// \return A msgs::Visual::Type enum.
+ public: static msgs::Visual::Type ConvertVisualType(
+ const Visual::VisualType &_type);
+
/// \internal
/// \brief Constructor used by inherited classes
/// \param[in] _dataPtr Pointer to private data.
diff --git a/gazebo/rendering/VisualPrivate.hh b/gazebo/rendering/VisualPrivate.hh
index 4b65474..242d0ea 100644
--- a/gazebo/rendering/VisualPrivate.hh
+++ b/gazebo/rendering/VisualPrivate.hh
@@ -33,6 +33,7 @@
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Vector2d.hh"
+#include "gazebo/rendering/Visual.hh"
#include "gazebo/rendering/RenderTypes.hh"
#include "gazebo/common/CommonTypes.hh"
@@ -55,6 +56,31 @@ namespace gazebo
/// \brief Private data for the Visual class
class VisualPrivate
{
+ /// \brief Constructor
+ public: VisualPrivate()
+ : sceneNode(NULL),
+ transparency(0),
+ castShadows(true),
+ isStatic(false),
+ staticGeom(NULL),
+ visible(true),
+ ribbonTrail(NULL),
+ skeleton(NULL),
+ animState(NULL),
+ useRTShader(true),
+ initialized(false),
+ boundingBox(NULL),
+ id(0),
+ lighting(true),
+ visibilityFlags(GZ_VISIBILITY_ALL),
+ type(Visual::VT_ENTITY),
+ layer(0)
+ {
+ }
+
+ /// \brief Default destructor
+ public: virtual ~VisualPrivate() = default;
+
/// \brief Pointer to the visual's scene.
public: ScenePtr scene;
@@ -76,6 +102,9 @@ namespace gazebo
/// \brief Transparency value.
public: float transparency;
+ /// \brief True if visual casts shadows.
+ public: bool castShadows;
+
/// \brief True if the visual is static, which allows Ogre to improve
/// performance.
public: bool isStatic;
@@ -139,6 +168,34 @@ namespace gazebo
/// \brief A list of visual plugins.
public: std::vector<VisualPluginPtr> plugins;
+
+ /// \brief The visual's mesh name.
+ public: std::string meshName;
+
+ /// \brief The visual's submesh name.
+ public: std::string subMeshName;
+
+ /// \brief Ambient color of the visual.
+ public: common::Color ambient;
+
+ /// \brief Diffuse color of the visual.
+ public: common::Color diffuse;
+
+ /// \brief Specular color of the visual.
+ public: common::Color specular;
+
+ /// \brief Emissive color of the visual.
+ public: common::Color emissive;
+
+ /// \brief Visibility flags of the visual.
+ public: uint32_t visibilityFlags;
+
+ /// \brief type
+ public: Visual::VisualType type;
+
+ /// \brief Index of the layer to which this visual belongs. Layers
+ /// act similar to layers in photoshop.
+ public: int32_t layer;
};
/// \}
}
diff --git a/gazebo/rendering/Visual_TEST.cc b/gazebo/rendering/Visual_TEST.cc
index 3dbc21d..6eeb5fa 100644
--- a/gazebo/rendering/Visual_TEST.cc
+++ b/gazebo/rendering/Visual_TEST.cc
@@ -20,7 +20,7 @@
#include "gazebo/rendering/RenderingIface.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/Visual.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
@@ -29,6 +29,94 @@ class Visual_TEST : public ServerFixture
};
/////////////////////////////////////////////////
+std::string GetVisualSDFString(const std::string &_name,
+ const std::string &_geomType = "box",
+ gazebo::math::Pose _pose = gazebo::math::Pose::Zero,
+ double _transparency = 0, bool _castShadows = true,
+ const std::string &_material = "Gazebo/Grey",
+ bool _lighting = true,
+ const common::Color &_ambient = common::Color::White,
+ const common::Color &_diffuse = common::Color::White,
+ const common::Color &_specular = common::Color::Black,
+ const common::Color &_emissive = common::Color::Black)
+{
+ std::stringstream visualString;
+ visualString
+ << "<sdf version='1.5'>"
+ << " <visual name='" << _name << "'>"
+ << " <pose>" << _pose << "</pose>"
+ << " <geometry>";
+ if (_geomType == "box")
+ {
+ visualString
+ << " <box>"
+ << " <size>1.0 1.0 1.0</size>"
+ << " </box>";
+ }
+ else if (_geomType == "sphere")
+ {
+ visualString
+ << " <sphere>"
+ << " <radius>0.5</radius>"
+ << " </sphere>";
+ }
+ else if (_geomType == "cylinder")
+ {
+ visualString
+ << " <cylinder>"
+ << " <radius>0.5</radius>"
+ << " <length>1.0</length>"
+ << " </cylinder>";
+ }
+ visualString
+ << " </geometry>"
+ << " <material>"
+ << " <script>"
+ << " <uri>file://media/materials/scripts/gazebo.material</uri>"
+ << " <name>" << _material << "</name>"
+ << " </script>"
+ << " <lighting>" << _lighting << "</lighting>"
+ << " <ambient>" << _ambient << "</ambient>"
+ << " <diffuse>" << _diffuse << "</diffuse>"
+ << " <specular>" << _specular << "</specular>"
+ << " <emissive>" << _emissive << "</emissive>"
+ << " </material>"
+ << " <transparency>" << _transparency << "</transparency>"
+ << " <cast_shadows>" << _castShadows << "</cast_shadows>"
+ << " </visual>"
+ << "</sdf>";
+ return visualString.str();
+}
+
+/////////////////////////////////////////////////
+void CreateColorMaterial(const std::string &_materialName,
+ const common::Color &_ambient, const common::Color &_diffuse,
+ const common::Color &_specular, const common::Color &_emissive)
+{
+ // test setup - create a material for testing
+ Ogre::MaterialPtr ogreMaterial =
+ Ogre::MaterialManager::getSingleton().create(_materialName, "General");
+ for (unsigned int i = 0; i < ogreMaterial->getNumTechniques(); ++i)
+ {
+ Ogre::Technique *technique = ogreMaterial->getTechnique(i);
+ for (unsigned int j = 0; j < technique->getNumPasses(); ++j)
+ {
+ Ogre::Pass *pass = technique->getPass(j);
+ pass->setAmbient(rendering::Conversions::Convert(_ambient));
+ pass->setDiffuse(rendering::Conversions::Convert(_diffuse));
+ pass->setSpecular(rendering::Conversions::Convert(_specular));
+ pass->setSelfIllumination(rendering::Conversions::Convert(_emissive));
+ EXPECT_EQ(rendering::Conversions::Convert(pass->getAmbient()), _ambient);
+ EXPECT_EQ(rendering::Conversions::Convert(pass->getDiffuse()), _diffuse);
+ EXPECT_EQ(rendering::Conversions::Convert(pass->getSpecular()),
+ _specular);
+ EXPECT_EQ(rendering::Conversions::Convert(pass->getSelfIllumination()),
+ _emissive);
+ }
+ }
+}
+
+/////////////////////////////////////////////////
TEST_F(Visual_TEST, BoundingBox)
{
Load("worlds/empty.world");
@@ -90,6 +178,559 @@ TEST_F(Visual_TEST, BoundingBox)
}
/////////////////////////////////////////////////
+TEST_F(Visual_TEST, GetGeometryType)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ // box geom
+ sdf::ElementPtr boxSDF(new sdf::Element);
+ sdf::initFile("visual.sdf", boxSDF);
+ sdf::readString(GetVisualSDFString("visual_box", "box"), boxSDF);
+ gazebo::rendering::VisualPtr boxVis(
+ new gazebo::rendering::Visual("box_visual", scene));
+ boxVis->Load(boxSDF);
+ EXPECT_EQ(boxVis->GetGeometryType(), "box");
+
+ // sphere geom
+ sdf::ElementPtr sphereSDF(new sdf::Element);
+ sdf::initFile("visual.sdf", sphereSDF);
+ sdf::readString(GetVisualSDFString("visual_sphere", "sphere"), sphereSDF);
+ gazebo::rendering::VisualPtr sphereVis(
+ new gazebo::rendering::Visual("sphere_visual", scene));
+ sphereVis->Load(sphereSDF);
+ EXPECT_EQ(sphereVis->GetGeometryType(), "sphere");
+
+ // cylinder geom
+ sdf::ElementPtr cylinderSDF(new sdf::Element);
+ sdf::initFile("visual.sdf", cylinderSDF);
+ sdf::readString(GetVisualSDFString("visual_cylinder", "cylinder"),
+ cylinderSDF);
+ gazebo::rendering::VisualPtr cylinderVis(
+ new gazebo::rendering::Visual("cylinder_visual", scene));
+ cylinderVis->Load(cylinderSDF);
+ EXPECT_EQ(cylinderVis->GetGeometryType(), "cylinder");
+}
+
+/////////////////////////////////////////////////
+TEST_F(Visual_TEST, CastShadows)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ // load a model that casts shadows by default
+ sdf::ElementPtr boxSDF(new sdf::Element);
+ sdf::initFile("visual.sdf", boxSDF);
+ sdf::readString(GetVisualSDFString("visual_box_has_shadows", "box",
+ gazebo::math::Pose::Zero, 0, true), boxSDF);
+ gazebo::rendering::VisualPtr boxVis(
+ new gazebo::rendering::Visual("box_visual", scene));
+ boxVis->Load(boxSDF);
+ EXPECT_TRUE(boxVis->GetCastShadows());
+
+ // load another model that does not cast shadows by default
+ sdf::ElementPtr boxSDF2(new sdf::Element);
+ sdf::initFile("visual.sdf", boxSDF2);
+ sdf::readString(GetVisualSDFString("visual_box_has_no_shadows", "box",
+ gazebo::math::Pose::Zero, 0, false), boxSDF2);
+ gazebo::rendering::VisualPtr boxVis2(
+ new gazebo::rendering::Visual("box_visual2", scene));
+ boxVis2->Load(boxSDF2);
+ EXPECT_FALSE(boxVis2->GetCastShadows());
+
+ // test changing cast shadow property
+ boxVis->SetCastShadows(false);
+ EXPECT_FALSE(boxVis->GetCastShadows());
+ boxVis2->SetCastShadows(true);
+ EXPECT_TRUE(boxVis2->GetCastShadows());
+}
+
+/////////////////////////////////////////////////
+TEST_F(Visual_TEST, Transparency)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ sdf::ElementPtr sphereSDF(new sdf::Element);
+ sdf::initFile("visual.sdf", sphereSDF);
+ sdf::readString(GetVisualSDFString("visual_sphere_no_transparency", "sphere",
+ gazebo::math::Pose::Zero, 0), sphereSDF);
+ gazebo::rendering::VisualPtr sphereVis(
+ new gazebo::rendering::Visual("sphere_visual", scene));
+ sphereVis->Load(sphereSDF);
+ EXPECT_DOUBLE_EQ(sphereVis->GetTransparency(), 0);
+
+ sdf::ElementPtr sphereSDF2(new sdf::Element);
+ sdf::initFile("visual.sdf", sphereSDF2);
+ sdf::readString(GetVisualSDFString("visual_sphere_semi_transparent", "sphere",
+ gazebo::math::Pose::Zero, 0.5), sphereSDF2);
+ gazebo::rendering::VisualPtr sphereVis2(
+ new gazebo::rendering::Visual("sphere_visual2", scene));
+ sphereVis2->Load(sphereSDF2);
+ EXPECT_DOUBLE_EQ(sphereVis2->GetTransparency(), 0.5f);
+
+ // test changing transparency property
+ sphereVis->SetTransparency(0.3f);
+ EXPECT_DOUBLE_EQ(sphereVis->GetTransparency(), 0.3f);
+ sphereVis2->SetTransparency(1.0f);
+ EXPECT_DOUBLE_EQ(sphereVis2->GetTransparency(), 1.0f);
+}
+
+/////////////////////////////////////////////////
+TEST_F(Visual_TEST, Material)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ sdf::ElementPtr boxSDF(new sdf::Element);
+ sdf::initFile("visual.sdf", boxSDF);
+ sdf::readString(GetVisualSDFString("visual_box_red", "box",
+ gazebo::math::Pose::Zero, 0, true, "Gazebo/Red"), boxSDF);
+ gazebo::rendering::VisualPtr boxVis(
+ new gazebo::rendering::Visual("box_visual", scene));
+ boxVis->Load(boxSDF);
+ // visual generates a new material with unique name so the name is slightly
+ // longer with a prefix of the visual name
+ EXPECT_TRUE(
+ boxVis->GetMaterialName().find("Gazebo/Red") != std::string::npos);
+
+ sdf::ElementPtr boxSDF2(new sdf::Element);
+ sdf::initFile("visual.sdf", boxSDF2);
+ sdf::readString(GetVisualSDFString("visual_box_white", "box",
+ gazebo::math::Pose::Zero, 0, true, "Gazebo/White", true), boxSDF2);
+ gazebo::rendering::VisualPtr boxVis2(
+ new gazebo::rendering::Visual("box_visual2", scene));
+ boxVis2->Load(boxSDF2);
+ EXPECT_TRUE(
+ boxVis2->GetMaterialName().find("Gazebo/White") != std::string::npos);
+
+ // test changing to use non-unique materials
+ boxVis->SetMaterial("Gazebo/Yellow", false);
+ EXPECT_EQ(boxVis->GetMaterialName(), "Gazebo/Yellow");
+ boxVis2->SetMaterial("Gazebo/OrangeTransparent", false);
+ EXPECT_EQ(boxVis2->GetMaterialName(), "Gazebo/OrangeTransparent");
+}
+
+/////////////////////////////////////////////////
+TEST_F(Visual_TEST, Lighting)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ sdf::ElementPtr cylinderSDF(new sdf::Element);
+ sdf::initFile("visual.sdf", cylinderSDF);
+ sdf::readString(GetVisualSDFString("visual_cylinder_lighting", "cylinder",
+ gazebo::math::Pose::Zero, 0, true, "Gazebo/Grey", true), cylinderSDF);
+ gazebo::rendering::VisualPtr cylinderVis(
+ new gazebo::rendering::Visual("cylinder_visual", scene));
+ cylinderVis->Load(cylinderSDF);
+ EXPECT_TRUE(cylinderVis->GetLighting());
+
+ sdf::ElementPtr cylinderSDF2(new sdf::Element);
+ sdf::initFile("visual.sdf", cylinderSDF2);
+ sdf::readString(GetVisualSDFString("visual_cylinder_no_lighting", "cylinder",
+ gazebo::math::Pose::Zero, 0, true, "Gazebo/Grey", false), cylinderSDF2);
+ gazebo::rendering::VisualPtr cylinderVis2(
+ new gazebo::rendering::Visual("cylinder_visual2", scene));
+ cylinderVis2->Load(cylinderSDF2);
+ EXPECT_FALSE(cylinderVis2->GetLighting());
+
+ // test changing lighting property
+ cylinderVis->SetLighting(false);
+ EXPECT_FALSE(cylinderVis->GetLighting());
+ cylinderVis2->SetLighting(true);
+ EXPECT_TRUE(cylinderVis2->GetLighting());
+}
+
+/////////////////////////////////////////////////
+TEST_F(Visual_TEST, Color)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ sdf::ElementPtr cylinderSDF(new sdf::Element);
+ sdf::initFile("visual.sdf", cylinderSDF);
+ sdf::readString(GetVisualSDFString("visual_cylinder_black", "cylinder",
+ gazebo::math::Pose::Zero, 0, true, "Gazebo/Grey", true,
+ common::Color::Black, common::Color::Black, common::Color::Black,
+ common::Color::Black), cylinderSDF);
+ gazebo::rendering::VisualPtr cylinderVis(
+ new gazebo::rendering::Visual("cylinder_visual", scene));
+ cylinderVis->Load(cylinderSDF);
+ EXPECT_EQ(cylinderVis->GetAmbient(), common::Color::Black);
+ EXPECT_EQ(cylinderVis->GetDiffuse(), common::Color::Black);
+ EXPECT_EQ(cylinderVis->GetSpecular(), common::Color::Black);
+ EXPECT_EQ(cylinderVis->GetEmissive(), common::Color::Black);
+
+ sdf::ElementPtr cylinderSDF2(new sdf::Element);
+ sdf::initFile("visual.sdf", cylinderSDF2);
+ sdf::readString(GetVisualSDFString("visual_cylinder_color", "cylinder",
+ gazebo::math::Pose::Zero, 0, true, "Gazebo/Grey", true,
+ common::Color::Green, common::Color::Blue, common::Color::Red,
+ common::Color::Yellow), cylinderSDF2);
+ gazebo::rendering::VisualPtr cylinderVis2(
+ new gazebo::rendering::Visual("cylinder_visual2", scene));
+ cylinderVis2->Load(cylinderSDF2);
+ EXPECT_EQ(cylinderVis2->GetAmbient(), common::Color::Green);
+ EXPECT_EQ(cylinderVis2->GetDiffuse(), common::Color::Blue);
+ EXPECT_EQ(cylinderVis2->GetSpecular(), common::Color::Red);
+ EXPECT_EQ(cylinderVis2->GetEmissive(), common::Color::Yellow);
+
+ // test changing ambient/diffuse/specular colors
+ {
+ common::Color color(0.1, 0.2, 0.3, 0.4);
+ cylinderVis->SetAmbient(color);
+ EXPECT_EQ(cylinderVis->GetAmbient(), color);
+ }
+ {
+ common::Color color(1.0, 1.0, 1.0, 1.0);
+ cylinderVis->SetDiffuse(color);
+ EXPECT_EQ(cylinderVis->GetDiffuse(), color);
+ }
+ {
+ common::Color color(0.5, 0.6, 0.4, 0.0);
+ cylinderVis->SetSpecular(color);
+ EXPECT_EQ(cylinderVis->GetSpecular(), color);
+ }
+ {
+ common::Color color(0.9, 0.8, 0.7, 0.6);
+ cylinderVis->SetEmissive(color);
+ EXPECT_EQ(cylinderVis->GetEmissive(), color);
+ }
+
+ {
+ common::Color color(0.0, 0.0, 0.0, 0.0);
+ cylinderVis2->SetAmbient(color);
+ EXPECT_EQ(cylinderVis2->GetAmbient(), color);
+ }
+ {
+ common::Color color(1.0, 1.0, 1.0, 1.0);
+ cylinderVis2->SetDiffuse(color);
+ EXPECT_EQ(cylinderVis2->GetDiffuse(), color);
+ }
+ // test with color values that are out of range but should still work,
+ // rendering engine should clamp the values internally
+ {
+ common::Color color(5.0, 5.0, 5.5, 5.1);
+ cylinderVis2->SetSpecular(color);
+ EXPECT_EQ(cylinderVis2->GetSpecular(), color);
+ }
+ {
+ common::Color color(-5.0, -5.0, -5.5, -5.1);
+ cylinderVis2->SetEmissive(color);
+ EXPECT_EQ(cylinderVis2->GetEmissive(), color);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(Visual_TEST, ColorMaterial)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ std::string materialName = "Test/Grey";
+ CreateColorMaterial(materialName, common::Color(0.3, 0.3, 0.3, 1.0),
+ common::Color(0.7, 0.7, 0.7, 1.0), common::Color(0.01, 0.01, 0.01, 1.0),
+ common::Color::Black);
+
+ // test with a visual that only has a material name and no color components.
+ std::string visualName = "boxMaterialColor";
+ math::Pose visualPose = math::Pose::Zero;
+ std::stringstream visualString;
+ visualString
+ << "<sdf version='" << SDF_VERSION << "'>"
+ << " <visual name='" << visualName << "'>"
+ << " <pose>" << visualPose << "</pose>"
+ << " <geometry>"
+ << " <box>"
+ << " <size>1.0 1.0 1.0</size>"
+ << " </box>"
+ << " </geometry>"
+ << " <material>"
+ << " <script>"
+ << " <uri>file://media/materials/scripts/gazebo.material</uri>"
+ << " <name>" << materialName << "</name>"
+ << " </script>"
+ << " </material>"
+ << " </visual>"
+ << "</sdf>";
+
+ sdf::ElementPtr boxSDF(new sdf::Element);
+ sdf::initFile("visual.sdf", boxSDF);
+ sdf::readString(visualString.str(), boxSDF);
+ gazebo::rendering::VisualPtr boxVis(
+ new gazebo::rendering::Visual("box_visual", scene));
+ boxVis->Load(boxSDF);
+
+ EXPECT_TRUE(
+ boxVis->GetMaterialName().find(materialName) != std::string::npos);
+
+ // Verify the visual color components are the same as the ones specified in
+ // the material script
+ EXPECT_EQ(boxVis->GetAmbient(), common::Color(0.3, 0.3, 0.3, 1.0));
+ EXPECT_EQ(boxVis->GetDiffuse(), common::Color(0.7, 0.7, 0.7, 1.0));
+ EXPECT_EQ(boxVis->GetSpecular(), common::Color(0.01, 0.01, 0.01, 1.0));
+ EXPECT_EQ(boxVis->GetEmissive(), common::Color::Black);
+
+ // test changing diffuse colors and verify color again.
+ common::Color redColor(1.0, 0.0, 0.0, 1.0);
+ boxVis->SetDiffuse(redColor);
+ EXPECT_EQ(boxVis->GetAmbient(), common::Color(0.3, 0.3, 0.3, 1.0));
+ EXPECT_EQ(boxVis->GetDiffuse(), redColor);
+ EXPECT_EQ(boxVis->GetSpecular(), common::Color(0.01, 0.01, 0.01, 1.0));
+ EXPECT_EQ(boxVis->GetEmissive(), common::Color::Black);
+
+ // test setting a different material name
+ std::string greenMaterialName = "Test/Green";
+ CreateColorMaterial(greenMaterialName, common::Color(0.0, 1.0, 0.0, 1.0),
+ common::Color(0.0, 1.0, 0.0, 1.0), common::Color(0.1, 0.1, 0.1, 1.0),
+ common::Color::Black);
+ boxVis->SetMaterial(greenMaterialName);
+ EXPECT_TRUE(
+ boxVis->GetMaterialName().find(greenMaterialName) != std::string::npos);
+
+ // Verify the visual color components are the same as the ones in the new
+ // material script
+ EXPECT_EQ(boxVis->GetAmbient(), common::Color(0.0, 1.0, 0.0, 1.0));
+ EXPECT_EQ(boxVis->GetDiffuse(), common::Color(0.0, 1.0, 0.0, 1.0));
+ EXPECT_EQ(boxVis->GetSpecular(), common::Color(0.1, 0.1, 0.1, 1.0));
+ EXPECT_EQ(boxVis->GetEmissive(), common::Color::Black);
+
+ // test setting back to original material color
+ boxVis->SetMaterial(materialName);
+ EXPECT_TRUE(
+ boxVis->GetMaterialName().find(materialName) != std::string::npos);
+
+ // Verify the visual color components are the same as the ones in the
+ // original material script
+ EXPECT_EQ(boxVis->GetAmbient(), common::Color(0.3, 0.3, 0.3, 1.0));
+ EXPECT_EQ(boxVis->GetDiffuse(), common::Color(0.7, 0.7, 0.7, 1.0));
+ EXPECT_EQ(boxVis->GetSpecular(), common::Color(0.01, 0.01, 0.01, 1.0));
+ EXPECT_EQ(boxVis->GetEmissive(), common::Color::Black);
+
+ // test with a semi-transparent color material
+ std::string redTransparentMaterialName = "Test/RedTransparent";
+ CreateColorMaterial(redTransparentMaterialName,
+ common::Color(1.0, 0.0, 0.0, 0.2), common::Color(1.0, 0.0, 0.0, 0.4),
+ common::Color(0.1, 0.1, 0.1, 0.6), common::Color(1.0, 0.0, 0.0, 0.8));
+ boxVis->SetMaterial(redTransparentMaterialName);
+ EXPECT_TRUE(boxVis->GetMaterialName().find(redTransparentMaterialName)
+ != std::string::npos);
+
+ // Verify the visual color components are the same as the ones in the new
+ // material script
+ EXPECT_EQ(boxVis->GetAmbient(), common::Color(1.0, 0.0, 0.0, 0.2));
+ EXPECT_EQ(boxVis->GetDiffuse(), common::Color(1.0, 0.0, 0.0, 0.4));
+ EXPECT_EQ(boxVis->GetSpecular(), common::Color(0.1, 0.1, 0.1, 0.6));
+ EXPECT_EQ(boxVis->GetEmissive(), common::Color(1.0, 0.0, 0.0, 0.8));
+
+ // update transparency and verify diffuse alpha value has changed
+ boxVis->SetTransparency(0.5f);
+ EXPECT_DOUBLE_EQ(boxVis->GetTransparency(), 0.5f);
+ EXPECT_EQ(boxVis->GetAmbient(), common::Color(1.0, 0.0, 0.0, 0.2));
+ EXPECT_EQ(boxVis->GetDiffuse(), common::Color(1.0, 0.0, 0.0, 0.5));
+ EXPECT_EQ(boxVis->GetSpecular(), common::Color(0.1, 0.1, 0.1, 0.6));
+ EXPECT_EQ(boxVis->GetEmissive(), common::Color(1.0, 0.0, 0.0, 0.8));
+}
+
+/////////////////////////////////////////////////
+TEST_F(Visual_TEST, UpdateMeshFromMsg)
+{
+ Load("worlds/empty.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ sdf::ElementPtr meshUpdateSDF(new sdf::Element);
+ sdf::initFile("visual.sdf", meshUpdateSDF);
+ sdf::readString(GetVisualSDFString("visual_mesh_update"), meshUpdateSDF);
+ gazebo::rendering::VisualPtr meshUpdateVis(
+ new gazebo::rendering::Visual("visual_mesh_update_visual", scene));
+ meshUpdateVis->Load(meshUpdateSDF);
+
+ EXPECT_EQ(meshUpdateVis->GetMeshName(), "unit_box");
+ EXPECT_EQ(meshUpdateVis->GetSubMeshName(), "");
+
+ msgs::VisualPtr visualMsg(new msgs::Visual);
+ msgs::Geometry *geomMsg = visualMsg->mutable_geometry();
+ geomMsg->set_type(msgs::Geometry::MESH);
+ msgs::MeshGeom *meshMsg = geomMsg->mutable_mesh();
+ std::string meshFile = "polaris_ranger_ev/meshes/polaris.dae";
+ meshMsg->set_filename("model://" + meshFile);
+ meshMsg->set_submesh("Steering_Wheel");
+ meshUpdateVis->UpdateFromMsg(visualMsg);
+
+ // verify new mesh and submesh names
+ EXPECT_TRUE(meshUpdateVis->GetMeshName().find(meshFile) != std::string::npos);
+ EXPECT_EQ(meshUpdateVis->GetSubMeshName(), "Steering_Wheel");
+
+ // verify updated sdf
+ sdf::ElementPtr visualUpdateSDF = meshUpdateVis->GetSDF();
+ sdf::ElementPtr geomSDF = visualUpdateSDF->GetElement("geometry");
+ sdf::ElementPtr meshSDF = geomSDF->GetElement("mesh");
+ EXPECT_EQ(meshSDF->Get<std::string>("uri"), "model://" + meshFile);
+ sdf::ElementPtr submeshSDF = meshSDF->GetElement("submesh");
+ EXPECT_EQ(submeshSDF->Get<std::string>("name"), "Steering_Wheel");
+}
+
+/////////////////////////////////////////////////
+TEST_F(Visual_TEST, GetAncestors)
+{
+ Load("worlds/blank.world");
+
+ gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
+ ASSERT_TRUE(scene != NULL);
+
+ // Get world visual
+ gazebo::rendering::VisualPtr world = scene->GetWorldVisual();
+ ASSERT_TRUE(world != NULL);
+
+ // Create a visual as child of the world visual
+ gazebo::rendering::VisualPtr vis1;
+ vis1.reset(new gazebo::rendering::Visual("vis1", scene->GetWorldVisual()));
+ vis1->Load();
+
+ // Create a visual as child of vis1
+ gazebo::rendering::VisualPtr vis2;
+ vis2.reset(new gazebo::rendering::Visual("vis2", vis1));
+ vis2->Load();
+
+ // Create a visual as child of vis2
+ gazebo::rendering::VisualPtr vis3_1;
+ vis3_1.reset(new gazebo::rendering::Visual("vis3_1", vis2));
+ vis3_1->Load();
+
+ // Create one more visual as child of vis2
+ gazebo::rendering::VisualPtr vis3_2;
+ vis3_2.reset(new gazebo::rendering::Visual("vis3_2", vis2));
+ vis3_2->Load();
+
+ // Create a visual as child of vis3_1
+ gazebo::rendering::VisualPtr vis4;
+ vis4.reset(new gazebo::rendering::Visual("vis4", vis3_1));
+ vis4->Load();
+
+ // Check depths
+ EXPECT_EQ(world->GetDepth(), 0u);
+ EXPECT_EQ(vis1->GetDepth(), 1u);
+ EXPECT_EQ(vis2->GetDepth(), 2u);
+ EXPECT_EQ(vis3_1->GetDepth(), 3u);
+ EXPECT_EQ(vis3_2->GetDepth(), 3u);
+ EXPECT_EQ(vis4->GetDepth(), 4u);
+
+ // Check parents
+ EXPECT_TRUE(world->GetParent() == NULL);
+ EXPECT_EQ(vis1->GetParent(), world);
+ EXPECT_EQ(vis2->GetParent(), vis1);
+ EXPECT_EQ(vis3_1->GetParent(), vis2);
+ EXPECT_EQ(vis3_2->GetParent(), vis2);
+ EXPECT_EQ(vis4->GetParent(), vis3_1);
+
+ // Check that world is its own root
+ EXPECT_EQ(world->GetRootVisual(), world);
+ // Check that vis1 is the root for all others
+ EXPECT_EQ(vis1->GetRootVisual(), vis1);
+ EXPECT_EQ(vis2->GetRootVisual(), vis1);
+ EXPECT_EQ(vis3_1->GetRootVisual(), vis1);
+ EXPECT_EQ(vis3_2->GetRootVisual(), vis1);
+ EXPECT_EQ(vis4->GetRootVisual(), vis1);
+
+ // Check that world is 0th ancestor for all of them
+ EXPECT_EQ(world->GetNthAncestor(0), world);
+ EXPECT_EQ(vis1->GetNthAncestor(0), world);
+ EXPECT_EQ(vis2->GetNthAncestor(0), world);
+ EXPECT_EQ(vis3_1->GetNthAncestor(0), world);
+ EXPECT_EQ(vis3_2->GetNthAncestor(0), world);
+ EXPECT_EQ(vis4->GetNthAncestor(0), world);
+
+ // Check that the 1st ancestor is the root visual
+ EXPECT_TRUE(world->GetNthAncestor(1) == NULL);
+ EXPECT_EQ(vis1->GetNthAncestor(1), vis1->GetRootVisual());
+ EXPECT_EQ(vis2->GetNthAncestor(1), vis2->GetRootVisual());
+ EXPECT_EQ(vis3_1->GetNthAncestor(1), vis3_1->GetRootVisual());
+ EXPECT_EQ(vis3_2->GetNthAncestor(1), vis3_2->GetRootVisual());
+ EXPECT_EQ(vis4->GetNthAncestor(1), vis4->GetRootVisual());
+
+ // Check 2nd ancestor
+ EXPECT_TRUE(world->GetNthAncestor(2) == NULL);
+ EXPECT_TRUE(vis1->GetNthAncestor(2) == NULL);
+ EXPECT_EQ(vis2->GetNthAncestor(2), vis2);
+ EXPECT_EQ(vis3_1->GetNthAncestor(2), vis2);
+ EXPECT_EQ(vis3_2->GetNthAncestor(2), vis2);
+ EXPECT_EQ(vis4->GetNthAncestor(2), vis2);
+
+ // Check 3rd ancestor
+ EXPECT_TRUE(world->GetNthAncestor(3) == NULL);
+ EXPECT_TRUE(vis1->GetNthAncestor(3) == NULL);
+ EXPECT_TRUE(vis2->GetNthAncestor(3) == NULL);
+ EXPECT_EQ(vis3_1->GetNthAncestor(3), vis3_1);
+ EXPECT_EQ(vis3_2->GetNthAncestor(3), vis3_2);
+ EXPECT_EQ(vis4->GetNthAncestor(3), vis3_1);
+
+ // Check 4th ancestor
+ EXPECT_TRUE(world->GetNthAncestor(4) == NULL);
+ EXPECT_TRUE(vis1->GetNthAncestor(4) == NULL);
+ EXPECT_TRUE(vis2->GetNthAncestor(4) == NULL);
+ EXPECT_TRUE(vis3_1->GetNthAncestor(4) == NULL);
+ EXPECT_TRUE(vis3_2->GetNthAncestor(4) == NULL);
+ EXPECT_EQ(vis4->GetNthAncestor(4), vis4);
+}
+
+/////////////////////////////////////////////////
+TEST_F(Visual_TEST, ConvertVisualType)
+{
+ // convert from msgs::Visual::Type to Visual::VisualType
+ EXPECT_EQ(msgs::Visual::ENTITY,
+ rendering::Visual::ConvertVisualType(rendering::Visual::VT_ENTITY));
+ EXPECT_EQ(msgs::Visual::MODEL,
+ rendering::Visual::ConvertVisualType(rendering::Visual::VT_MODEL));
+ EXPECT_EQ(msgs::Visual::LINK,
+ rendering::Visual::ConvertVisualType(rendering::Visual::VT_LINK));
+ EXPECT_EQ(msgs::Visual::VISUAL,
+ rendering::Visual::ConvertVisualType(rendering::Visual::VT_VISUAL));
+ EXPECT_EQ(msgs::Visual::COLLISION,
+ rendering::Visual::ConvertVisualType(rendering::Visual::VT_COLLISION));
+ EXPECT_EQ(msgs::Visual::SENSOR,
+ rendering::Visual::ConvertVisualType(rendering::Visual::VT_SENSOR));
+ EXPECT_EQ(msgs::Visual::GUI,
+ rendering::Visual::ConvertVisualType(rendering::Visual::VT_GUI));
+ EXPECT_EQ(msgs::Visual::PHYSICS,
+ rendering::Visual::ConvertVisualType(rendering::Visual::VT_PHYSICS));
+
+ // convert from Visual::VisualType to msgs::Visual::Type
+ EXPECT_EQ(rendering::Visual::VT_ENTITY,
+ rendering::Visual::ConvertVisualType(msgs::Visual::ENTITY));
+ EXPECT_EQ(rendering::Visual::VT_MODEL,
+ rendering::Visual::ConvertVisualType(msgs::Visual::MODEL));
+ EXPECT_EQ(rendering::Visual::VT_LINK,
+ rendering::Visual::ConvertVisualType(msgs::Visual::LINK));
+ EXPECT_EQ(rendering::Visual::VT_VISUAL,
+ rendering::Visual::ConvertVisualType(msgs::Visual::VISUAL));
+ EXPECT_EQ(rendering::Visual::VT_COLLISION,
+ rendering::Visual::ConvertVisualType(msgs::Visual::COLLISION));
+ EXPECT_EQ(rendering::Visual::VT_SENSOR,
+ rendering::Visual::ConvertVisualType(msgs::Visual::SENSOR));
+ EXPECT_EQ(rendering::Visual::VT_GUI,
+ rendering::Visual::ConvertVisualType(msgs::Visual::GUI));
+ EXPECT_EQ(rendering::Visual::VT_PHYSICS,
+ rendering::Visual::ConvertVisualType(msgs::Visual::PHYSICS));
+}
+
+/////////////////////////////////////////////////
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
diff --git a/gazebo/rendering/WindowManager.cc b/gazebo/rendering/WindowManager.cc
index cfe5fff..a930e79 100644
--- a/gazebo/rendering/WindowManager.cc
+++ b/gazebo/rendering/WindowManager.cc
@@ -67,7 +67,9 @@ void WindowManager::Fini()
//////////////////////////////////////////////////
void WindowManager::SetCamera(int _windowId, CameraPtr _camera)
{
- this->windows[_windowId]->removeAllViewports();
+ if (static_cast<unsigned int>(_windowId) < this->windows.size() &&
+ this->windows[_windowId])
+ this->windows[_windowId]->removeAllViewports();
_camera->SetRenderTarget(this->windows[_windowId]);
}
@@ -80,13 +82,14 @@ int WindowManager::CreateWindow(const std::string &_ogreHandle,
Ogre::NameValuePairList params;
Ogre::RenderWindow *window = NULL;
-#ifdef Q_OS_MAC
+ // Mac and Windows *must* use externalWindow handle.
+#if defined(Q_OS_MAC) || defined(_MSC_VER)
params["externalWindowHandle"] = _ogreHandle;
#else
params["parentWindowHandle"] = _ogreHandle;
#endif
- params["externalGLControl"] = true;
params["FSAA"] = "4";
+ params["stereoMode"] = "Frame Sequential";
// Set the macAPI for Ogre based on the Qt implementation
#ifdef QT_MAC_USE_COCOA
@@ -96,6 +99,10 @@ int WindowManager::CreateWindow(const std::string &_ogreHandle,
params["macAPI"] = "carbon";
#endif
+ // Hide window if dimensions are less than or equal to one.
+ if (_width <= 1 && _height <=1)
+ params["border"] = "none";
+
std::ostringstream stream;
stream << "OgreWindow(" << windowCounter++ << ")";
@@ -122,9 +129,12 @@ int WindowManager::CreateWindow(const std::string &_ogreHandle,
if (window)
{
window->setActive(true);
- // window->setVisible(true);
+ window->setVisible(true);
window->setAutoUpdated(false);
+ // Windows needs to reposition the render window to 0,0.
+ window->reposition(0, 0);
+
this->windows.push_back(window);
}
diff --git a/gazebo/rendering/WindowManager.hh b/gazebo/rendering/WindowManager.hh
index f06a93f..eb397cb 100644
--- a/gazebo/rendering/WindowManager.hh
+++ b/gazebo/rendering/WindowManager.hh
@@ -17,6 +17,12 @@
#ifndef _WINDOWMANAGER_HH_
#define _WINDOWMANAGER_HH_
+#ifdef _WIN32
+ // Oh, yeah, CreateWindow is taken, too.
+ #include <windows.h>
+ #undef CreateWindow
+#endif
+
#include <string>
#include <vector>
@@ -37,7 +43,7 @@ namespace gazebo
/// \class WindowManager WindowManager.hh rendering/rendering.hh
/// \brief Class to mangage render windows.
- class GAZEBO_VISIBLE WindowManager
+ class GZ_RENDERING_VISIBLE WindowManager
{
/// \brief Constructor
public: WindowManager();
diff --git a/gazebo/rendering/WireBox.cc b/gazebo/rendering/WireBox.cc
index 2d9d885..09df717 100644
--- a/gazebo/rendering/WireBox.cc
+++ b/gazebo/rendering/WireBox.cc
@@ -122,4 +122,3 @@ math::Box WireBox::GetBox() const
{
return this->dataPtr->box;
}
-
diff --git a/gazebo/rendering/WireBox.hh b/gazebo/rendering/WireBox.hh
index 1e3e50d..c2d19ff 100644
--- a/gazebo/rendering/WireBox.hh
+++ b/gazebo/rendering/WireBox.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class WireBox WireBox.hh rendering/rendering.hh
/// \brief Draws a wireframe box.
- class GAZEBO_VISIBLE WireBox
+ class GZ_RENDERING_VISIBLE WireBox
{
/// \brief Constructor
/// \param[in] _box Dimension of the box to draw.
diff --git a/gazebo/rendering/WrenchVisual.cc b/gazebo/rendering/WrenchVisual.cc
index 00619e3..3c0f254 100644
--- a/gazebo/rendering/WrenchVisual.cc
+++ b/gazebo/rendering/WrenchVisual.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/common/MeshManager.hh"
#include "gazebo/transport/transport.hh"
@@ -35,6 +41,8 @@ WrenchVisual::WrenchVisual(const std::string &_name, VisualPtr _vis,
WrenchVisualPrivate *dPtr =
reinterpret_cast<WrenchVisualPrivate *>(this->dataPtr);
+ dPtr->type = VT_PHYSICS;
+
dPtr->enabled = true;
dPtr->receivedMsg = false;
@@ -120,8 +128,8 @@ WrenchVisual::~WrenchVisual()
void WrenchVisual::Load(ConstJointPtr &_msg)
{
Visual::Load();
- this->SetPosition(msgs::Convert(_msg->pose().position()));
- this->SetRotation(msgs::Convert(_msg->pose().orientation()));
+ this->SetPosition(msgs::ConvertIgn(_msg->pose().position()));
+ this->SetRotation(msgs::ConvertIgn(_msg->pose().orientation()));
}
/////////////////////////////////////////////////
@@ -152,9 +160,10 @@ void WrenchVisual::Update()
exp(-dPtr->wrenchMsg->wrench().torque().z() / magScale)) - offset;
magScale = 50000;
- math::Vector3 force = msgs::Convert(dPtr->wrenchMsg->wrench().force());
+ ignition::math::Vector3d force =
+ msgs::ConvertIgn(dPtr->wrenchMsg->wrench().force());
double forceScale = (2.0 * vRange) / (1 +
- exp(force.GetSquaredLength() / magScale)) - offset;
+ exp(force.SquaredLength() / magScale)) - offset;
dPtr->forceLine->SetPoint(1, force*forceScale);
dPtr->forceLine->Update();
diff --git a/gazebo/rendering/WrenchVisual.hh b/gazebo/rendering/WrenchVisual.hh
index c59255c..00818f4 100644
--- a/gazebo/rendering/WrenchVisual.hh
+++ b/gazebo/rendering/WrenchVisual.hh
@@ -33,7 +33,7 @@ namespace gazebo
/// \class WrenchVisual WrenchVisual.hh rendering/rendering.hh
/// \brief Visualization for sonar data.
- class GAZEBO_VISIBLE WrenchVisual : public Visual
+ class GZ_RENDERING_VISIBLE WrenchVisual : public Visual
{
/// \brief Constructor.
/// \param[in] _name Name of the visual.
diff --git a/gazebo/rendering/deferred_shading/AmbientLight.hh b/gazebo/rendering/deferred_shading/AmbientLight.hh
index c1e1522..2a0129b 100644
--- a/gazebo/rendering/deferred_shading/AmbientLight.hh
+++ b/gazebo/rendering/deferred_shading/AmbientLight.hh
@@ -35,7 +35,7 @@ namespace gazebo
// XXX Could make this a singleton/make it private to the
// DeferredShadingSystem e.g.
template<class techniquePolicy>
- class GAZEBO_VISIBLE AmbientLight
+ class GZ_RENDERING_DEFERRED_VISIBLE AmbientLight
: public Ogre::SimpleRenderable, public techniquePolicy
{
/// \brief Constructor
diff --git a/gazebo/rendering/deferred_shading/CMakeLists.txt b/gazebo/rendering/deferred_shading/CMakeLists.txt
index 4e687ac..c1f5907 100644
--- a/gazebo/rendering/deferred_shading/CMakeLists.txt
+++ b/gazebo/rendering/deferred_shading/CMakeLists.txt
@@ -33,6 +33,8 @@ link_directories(
${ogre_library_dirs}
)
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_RENDERING_DEFERRED")
+
gz_add_library(gazebo_rendering_deferred ${sources})
target_link_libraries(gazebo_rendering_deferred
diff --git a/gazebo/rendering/deferred_shading/DeferredLight.hh b/gazebo/rendering/deferred_shading/DeferredLight.hh
index 1f24bf9..56408aa 100644
--- a/gazebo/rendering/deferred_shading/DeferredLight.hh
+++ b/gazebo/rendering/deferred_shading/DeferredLight.hh
@@ -31,7 +31,8 @@ namespace gazebo
/// Deferred light geometry. Each instance matches a normal light.
/// Should not be created by the user.
/// XXX support other types of light other than point lights.
- class GAZEBO_VISIBLE DeferredLight: public Ogre::SimpleRenderable
+ class GZ_RENDERING_DEFERRED_VISIBLE DeferredLight :
+ public Ogre::SimpleRenderable
{
/// \brief Constructor
public: DeferredLight(MaterialGenerator *_gen, Ogre::Light *_parentLight,
diff --git a/gazebo/rendering/deferred_shading/DeferredLightCP.hh b/gazebo/rendering/deferred_shading/DeferredLightCP.hh
index 9b98a7f..210f390 100644
--- a/gazebo/rendering/deferred_shading/DeferredLightCP.hh
+++ b/gazebo/rendering/deferred_shading/DeferredLightCP.hh
@@ -39,7 +39,7 @@ namespace gazebo
/// calls of the spheres (point lights), cones (spotlights) and quads
/// (directional lights) after the GBuffer has been constructed
template<typename techniquePolicy>
- class GAZEBO_VISIBLE DeferredLightRenderOperation
+ class GZ_RENDERING_DEFERRED_VISIBLE DeferredLightRenderOperation
: public Ogre::CompositorInstance::RenderSystemOperation,
public techniquePolicy
{
@@ -233,7 +233,7 @@ namespace gazebo
/// \brief The custom composition pass that is used for rendering the light
/// geometry. This class needs to be registered with the CompositorManager
template<typename techniquePolicy>
- class GAZEBO_VISIBLE DeferredLightCompositionPass
+ class GZ_RENDERING_DEFERRED_VISIBLE DeferredLightCompositionPass
: public Ogre::CustomCompositionPass, public techniquePolicy
{
/// @copydoc CustomCompositionPass::createOperation
diff --git a/gazebo/rendering/deferred_shading/GBufferMaterialGenerator.cc b/gazebo/rendering/deferred_shading/GBufferMaterialGenerator.cc
index bab34f6..a526fe9 100644
--- a/gazebo/rendering/deferred_shading/GBufferMaterialGenerator.cc
+++ b/gazebo/rendering/deferred_shading/GBufferMaterialGenerator.cc
@@ -15,10 +15,13 @@
*
*/
+#include <OgrePrerequisites.h>
+
#include <OgreMaterialManager.h>
#include <OgreGpuProgramManager.h>
#include <OgreStringConverter.h>
#include <OgreHighLevelGpuProgramManager.h>
+#include <OgreHighLevelGpuProgram.h>
#include <OgreTechnique.h>
#include "gazebo/rendering/deferred_shading/GBufferMaterialGenerator.hh"
@@ -155,6 +158,7 @@ Ogre::GpuProgramPtr GBufferMaterialGeneratorImpl::GenerateVertexShader(
return Ogre::GpuProgramPtr(ptrProgram);
}
+
/////////////////////////////////////////////////
Ogre::GpuProgramPtr GBufferMaterialGeneratorImpl::GenerateFragmentShader(
MaterialGenerator::Perm _permutation)
diff --git a/gazebo/rendering/deferred_shading/GBufferMaterialGenerator.hh b/gazebo/rendering/deferred_shading/GBufferMaterialGenerator.hh
index d4d96a1..147a576 100644
--- a/gazebo/rendering/deferred_shading/GBufferMaterialGenerator.hh
+++ b/gazebo/rendering/deferred_shading/GBufferMaterialGenerator.hh
@@ -30,7 +30,8 @@ namespace gazebo
/// there. in order to support more, either expand this class or make sure
/// that objects that will not get treated correctly will not have
/// materials generated for them.
- class GAZEBO_VISIBLE GBufferMaterialGenerator : public MaterialGenerator
+ class GZ_RENDERING_DEFERRED_VISIBLE GBufferMaterialGenerator :
+ public MaterialGenerator
{
/// \brief The types of g buffers
public: enum GBufferType
diff --git a/gazebo/rendering/deferred_shading/GBufferSchemeHandler.hh b/gazebo/rendering/deferred_shading/GBufferSchemeHandler.hh
index 6c5cd2e..936ee36 100644
--- a/gazebo/rendering/deferred_shading/GBufferSchemeHandler.hh
+++ b/gazebo/rendering/deferred_shading/GBufferSchemeHandler.hh
@@ -35,7 +35,7 @@ namespace gazebo
/// @note This does not support all the possible rendering techniques
/// out there. in order to support more, either expand this class or
/// specify the techniques in the materials.
- class GAZEBO_VISIBLE GBufferSchemeHandler :
+ class GZ_RENDERING_DEFERRED_VISIBLE GBufferSchemeHandler :
public Ogre::MaterialManager::Listener
{
public: GBufferSchemeHandler(GBufferMaterialGenerator::GBufferType _type)
diff --git a/gazebo/rendering/deferred_shading/GeomUtils.cc b/gazebo/rendering/deferred_shading/GeomUtils.cc
index d4ab0a3..0a9c4a1 100644
--- a/gazebo/rendering/deferred_shading/GeomUtils.cc
+++ b/gazebo/rendering/deferred_shading/GeomUtils.cc
@@ -16,6 +16,7 @@
*/
#include <OgreMeshManager.h>
+#include <OgreMesh.h>
#include <OgreSubMesh.h>
#include <OgreHardwareBufferManager.h>
diff --git a/gazebo/rendering/deferred_shading/GeomUtils.hh b/gazebo/rendering/deferred_shading/GeomUtils.hh
index ee0c8ce..278ccd4 100644
--- a/gazebo/rendering/deferred_shading/GeomUtils.hh
+++ b/gazebo/rendering/deferred_shading/GeomUtils.hh
@@ -25,7 +25,7 @@ namespace gazebo
{
namespace rendering
{
- class GAZEBO_VISIBLE GeomUtils
+ class GZ_RENDERING_DEFERRED_VISIBLE GeomUtils
{
/// \brief Create a sphere Mesh with a given name, radius, number of
/// rings and number of segments
diff --git a/gazebo/rendering/deferred_shading/LightMaterialGenerator.hh b/gazebo/rendering/deferred_shading/LightMaterialGenerator.hh
index 74f97b5..9d48e4f 100644
--- a/gazebo/rendering/deferred_shading/LightMaterialGenerator.hh
+++ b/gazebo/rendering/deferred_shading/LightMaterialGenerator.hh
@@ -42,7 +42,7 @@ namespace gazebo
class LightMaterialGeneratorGLSL;
template<typename techniquePolicy>
- class GAZEBO_VISIBLE LightMaterialGenerator :
+ class GZ_RENDERING_DEFERRED_VISIBLE LightMaterialGenerator :
public MaterialGenerator, techniquePolicy
{
/// Permutation of light materials
@@ -74,7 +74,7 @@ namespace gazebo
};
template<typename techniquePolicy>
- class GAZEBO_VISIBLE LightMaterialGeneratorGLSL :
+ class GZ_RENDERING_DEFERRED_VISIBLE LightMaterialGeneratorGLSL :
public MaterialGenerator::Impl, public techniquePolicy
{
public: typedef MaterialGenerator::Perm Perm;
diff --git a/gazebo/rendering/deferred_shading/ListenerFactoryLogic.hh b/gazebo/rendering/deferred_shading/ListenerFactoryLogic.hh
index 0410b7d..9f1a519 100644
--- a/gazebo/rendering/deferred_shading/ListenerFactoryLogic.hh
+++ b/gazebo/rendering/deferred_shading/ListenerFactoryLogic.hh
@@ -28,7 +28,8 @@ namespace gazebo
{
/// The simple types of compositor logics will all do the same thing -
/// Attach a listener to the created compositor
- class GAZEBO_VISIBLE ListenerFactoryLogic : public Ogre::CompositorLogic
+ class GZ_RENDERING_DEFERRED_VISIBLE ListenerFactoryLogic :
+ public Ogre::CompositorLogic
{
//// @copydoc CompositorLogic::compositorInstanceCreated
public: virtual void compositorInstanceCreated(
diff --git a/gazebo/rendering/deferred_shading/MaterialGenerator.hh b/gazebo/rendering/deferred_shading/MaterialGenerator.hh
index 4b04522..bf606fd 100644
--- a/gazebo/rendering/deferred_shading/MaterialGenerator.hh
+++ b/gazebo/rendering/deferred_shading/MaterialGenerator.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// all these out is a tedious job. Of course it is possible to always use
/// the material with all features, but that might result in large, slow
/// shader programs. This class provides an efficient solution to that.
- class GAZEBO_VISIBLE MaterialGenerator
+ class GZ_RENDERING_DEFERRED_VISIBLE MaterialGenerator
{
/// Bitfield used to signify a material permutations
public: typedef uint32_t Perm;
@@ -56,7 +56,7 @@ namespace gazebo
/// fragment shader and vertex shader). These methods are only called
/// once for every permutation, after which the result is stored and
/// re-used.
- public: class Impl
+ public: class GZ_RENDERING_DEFERRED_VISIBLE Impl
{
public: virtual ~Impl();
public: virtual Ogre::GpuProgramPtr GenerateVertexShader(
diff --git a/gazebo/rendering/deferred_shading/MergeCP.hh b/gazebo/rendering/deferred_shading/MergeCP.hh
index 0357828..da30044 100644
--- a/gazebo/rendering/deferred_shading/MergeCP.hh
+++ b/gazebo/rendering/deferred_shading/MergeCP.hh
@@ -30,7 +30,7 @@ namespace gazebo
// composition pass. This is the class that will send the actual render
// calls of the spheres (point lights), cones (spotlights) and quads
// (directional lights) after the GBuffer has been constructed
- class GAZEBO_VISIBLE MergeRenderOperation :
+ class GZ_RENDERING_DEFERRED_VISIBLE MergeRenderOperation :
public Ogre::CompositorInstance::RenderSystemOperation
{
public: MergeRenderOperation(Ogre::CompositorInstance *_instance,
@@ -45,7 +45,7 @@ namespace gazebo
/// The custom composition pass that is used for rendering the light
/// geometry. This class needs to be registered with the CompositorManager
- class GAZEBO_VISIBLE MergeCompositionPass :
+ class GZ_RENDERING_DEFERRED_VISIBLE MergeCompositionPass :
public Ogre::CustomCompositionPass
{
/// @copydoc CustomCompositionPass::createOperation
diff --git a/gazebo/rendering/deferred_shading/MergeMaterialGenerator.cc b/gazebo/rendering/deferred_shading/MergeMaterialGenerator.cc
index dc35eda..f20151d 100644
--- a/gazebo/rendering/deferred_shading/MergeMaterialGenerator.cc
+++ b/gazebo/rendering/deferred_shading/MergeMaterialGenerator.cc
@@ -19,6 +19,7 @@
#include <OgreGpuProgramManager.h>
#include <OgreStringConverter.h>
#include <OgreHighLevelGpuProgramManager.h>
+#include <OgreHighLevelGpuProgram.h>
#include <OgreTechnique.h>
#include <OgreTextureUnitState.h>
diff --git a/gazebo/rendering/deferred_shading/MergeMaterialGenerator.hh b/gazebo/rendering/deferred_shading/MergeMaterialGenerator.hh
index 2fafc6d..004d306 100644
--- a/gazebo/rendering/deferred_shading/MergeMaterialGenerator.hh
+++ b/gazebo/rendering/deferred_shading/MergeMaterialGenerator.hh
@@ -35,7 +35,8 @@ namespace gazebo
/// there. In order to support more, either expand this class or make sure
/// that objects that will not get treated correctly will not have materials
/// generated for them.
- class GAZEBO_VISIBLE MergeMaterialGenerator : public MaterialGenerator
+ class GZ_RENDERING_DEFERRED_VISIBLE MergeMaterialGenerator :
+ public MaterialGenerator
{
/// The relevant options for materials
public: enum MaterialPermutations
diff --git a/gazebo/rendering/deferred_shading/MergeSchemeHandler.hh b/gazebo/rendering/deferred_shading/MergeSchemeHandler.hh
index b86c22b..d1b4c63 100644
--- a/gazebo/rendering/deferred_shading/MergeSchemeHandler.hh
+++ b/gazebo/rendering/deferred_shading/MergeSchemeHandler.hh
@@ -30,7 +30,7 @@ namespace gazebo
{
/// Class for handling materials who did not specify techniques for the
/// merging step of deferred/inferred lighting.
- class GAZEBO_VISIBLE MergeSchemeHandler :
+ class GZ_RENDERING_DEFERRED_VISIBLE MergeSchemeHandler :
public Ogre::MaterialManager::Listener
{
public: MergeSchemeHandler(bool _useDSF) : useDSF(_useDSF)
diff --git a/gazebo/rendering/deferred_shading/NullSchemeHandler.hh b/gazebo/rendering/deferred_shading/NullSchemeHandler.hh
index 6242587..678d21b 100644
--- a/gazebo/rendering/deferred_shading/NullSchemeHandler.hh
+++ b/gazebo/rendering/deferred_shading/NullSchemeHandler.hh
@@ -25,7 +25,7 @@ namespace gazebo
namespace rendering
{
/// \brief Class for skipping materials which do not have the scheme defined
- class GAZEBO_VISIBLE NullSchemeHandler :
+ class GZ_RENDERING_DEFERRED_VISIBLE NullSchemeHandler :
public Ogre::MaterialManager::Listener
{
/** @copydoc MaterialManager::Listener::handleSchemeNotFound */
diff --git a/gazebo/rendering/deferred_shading/SSAOLogic.hh b/gazebo/rendering/deferred_shading/SSAOLogic.hh
index ab60126..21ff3e2 100644
--- a/gazebo/rendering/deferred_shading/SSAOLogic.hh
+++ b/gazebo/rendering/deferred_shading/SSAOLogic.hh
@@ -28,7 +28,7 @@ namespace gazebo
{
namespace rendering
{
- class GAZEBO_VISIBLE SSAOLogic : public ListenerFactoryLogic
+ class GZ_RENDERING_DEFERRED_VISIBLE SSAOLogic : public ListenerFactoryLogic
{
/// @copydoc ListenerFactoryLogic::createListener
protected: virtual Ogre::CompositorInstance::Listener *createListener(
diff --git a/gazebo/rendering/deferred_shading/TechniqueDefinitions.hh b/gazebo/rendering/deferred_shading/TechniqueDefinitions.hh
index c88243e..86a9ffa 100644
--- a/gazebo/rendering/deferred_shading/TechniqueDefinitions.hh
+++ b/gazebo/rendering/deferred_shading/TechniqueDefinitions.hh
@@ -24,7 +24,7 @@ namespace gazebo
{
namespace rendering
{
- class GAZEBO_VISIBLE NullTechnique
+ class GZ_RENDERING_DEFERRED_VISIBLE NullTechnique
{
protected: std::string GetMaterialPrefix() const
{return "NullTechnique";}
@@ -33,7 +33,7 @@ namespace gazebo
protected: bool UseMaterialProperties() const
{return true;}
};
- class GAZEBO_VISIBLE DeferredShading
+ class GZ_RENDERING_DEFERRED_VISIBLE DeferredShading
{
protected: std::string GetMaterialPrefix() const
{return "DeferredShading";}
@@ -42,7 +42,7 @@ namespace gazebo
protected: bool UseMaterialProperties() const
{return true;}
};
- class GAZEBO_VISIBLE DeferredLighting
+ class GZ_RENDERING_DEFERRED_VISIBLE DeferredLighting
{
protected: std::string GetMaterialPrefix() const
{return "DeferredLighting";}
@@ -51,7 +51,7 @@ namespace gazebo
protected: bool UseMaterialProperties() const
{return false;}
};
- class GAZEBO_VISIBLE InferredLighting
+ class GZ_RENDERING_DEFERRED_VISIBLE InferredLighting
{
protected: std::string GetMaterialPrefix() const
{return "InferredLighting";}
diff --git a/gazebo/rendering/ogre_gazebo.h b/gazebo/rendering/ogre_gazebo.h
index f81fbb5..f81637c 100644
--- a/gazebo/rendering/ogre_gazebo.h
+++ b/gazebo/rendering/ogre_gazebo.h
@@ -24,6 +24,7 @@
#pragma clang diagnostic ignored "-W#warnings"
#include <OGRE/Ogre.h>
+#include <OGRE/OgreBillboard.h>
#include <OGRE/OgreImageCodec.h>
#include <OGRE/OgreMovableObject.h>
#include <OGRE/OgreRenderable.h>
diff --git a/gazebo/rendering/selection_buffer/CMakeLists.txt b/gazebo/rendering/selection_buffer/CMakeLists.txt
index 8ce541c..1e644d0 100644
--- a/gazebo/rendering/selection_buffer/CMakeLists.txt
+++ b/gazebo/rendering/selection_buffer/CMakeLists.txt
@@ -12,6 +12,8 @@ set (headers
SelectionRenderListener.hh
)
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_SELECTION_BUFF")
+
gz_add_library(gazebo_selection_buffer ${sources})
target_link_libraries(gazebo_selection_buffer
diff --git a/gazebo/rendering/selection_buffer/MaterialSwitcher.cc b/gazebo/rendering/selection_buffer/MaterialSwitcher.cc
index a67b602..0faf806 100644
--- a/gazebo/rendering/selection_buffer/MaterialSwitcher.cc
+++ b/gazebo/rendering/selection_buffer/MaterialSwitcher.cc
@@ -68,7 +68,9 @@ Ogre::Technique *MaterialSwitcher::handleSchemeNotFound(
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
// OGRE 1.9 changes the shared pointer definition
- #if (OGRE_VERSION < ((1 << 16) | (9 << 8) | 0))
+ // But the 1.9 RC, which we're using on Windows, doesn't have the
+ // staticCast change. It will be in the final release.
+ #if (OGRE_VERSION < ((1 << 16) | (9 << 8) | 0)) || defined(_WIN32)
// Make sure we keep the same depth properties so that
// certain overlay objects can be picked by the mouse.
Ogre::Technique *newTechnique =
diff --git a/gazebo/rendering/selection_buffer/MaterialSwitcher.hh b/gazebo/rendering/selection_buffer/MaterialSwitcher.hh
index fee981e..3310911 100644
--- a/gazebo/rendering/selection_buffer/MaterialSwitcher.hh
+++ b/gazebo/rendering/selection_buffer/MaterialSwitcher.hh
@@ -37,7 +37,7 @@ namespace gazebo
};*/
class SelectionBuffer;
- class GAZEBO_VISIBLE MaterialSwitcher :
+ class GZ_SELECTION_BUFF_VISIBLE MaterialSwitcher :
public Ogre::MaterialManager::Listener
{
/// \brief Constructor
diff --git a/gazebo/rendering/selection_buffer/SelectionBuffer.cc b/gazebo/rendering/selection_buffer/SelectionBuffer.cc
index 4caaca8..7939a0e 100644
--- a/gazebo/rendering/selection_buffer/SelectionBuffer.cc
+++ b/gazebo/rendering/selection_buffer/SelectionBuffer.cc
@@ -27,7 +27,7 @@ using namespace rendering;
/////////////////////////////////////////////////
SelectionBuffer::SelectionBuffer(const std::string &_cameraName,
Ogre::SceneManager *_mgr, Ogre::RenderTarget *_renderTarget)
-: sceneMgr(_mgr), renderTarget(_renderTarget),
+: sceneMgr(_mgr), renderTarget(_renderTarget), renderTexture(0),
buffer(0), pixelBox(0)
{
this->camera = this->sceneMgr->getCamera(_cameraName);
@@ -197,8 +197,10 @@ void SelectionBuffer::CreateRTTOverlays()
panel->setPosition(10, 10);
panel->setDimensions(400, 280);
panel->setMaterialName("SelectionDebugMaterial");
+#if OGRE_VERSION_MAJOR == 1 && OGRE_VERSION_MINOR <= 9
this->selectionDebugOverlay->add2D(panel);
this->selectionDebugOverlay->hide();
+#endif
}
else
{
@@ -209,6 +211,7 @@ void SelectionBuffer::CreateRTTOverlays()
}
/////////////////////////////////////////////////
+#if OGRE_VERSION_MAJOR == 1 && OGRE_VERSION_MINOR <= 9
void SelectionBuffer::ShowOverlay(bool _show)
{
if (_show)
@@ -216,3 +219,9 @@ void SelectionBuffer::ShowOverlay(bool _show)
else
this->selectionDebugOverlay->hide();
}
+#else
+void SelectionBuffer::ShowOverlay(bool /*_show*/)
+{
+ gzerr << "Selection debug overlay disabled for Ogre > 1.9\n";
+}
+#endif
diff --git a/gazebo/rendering/selection_buffer/SelectionBuffer.hh b/gazebo/rendering/selection_buffer/SelectionBuffer.hh
index ce3d7d6..172435b 100644
--- a/gazebo/rendering/selection_buffer/SelectionBuffer.hh
+++ b/gazebo/rendering/selection_buffer/SelectionBuffer.hh
@@ -38,7 +38,7 @@ namespace gazebo
class SelectionRenderListener;
class Scene;
- class GAZEBO_VISIBLE SelectionBuffer
+ class GZ_SELECTION_BUFF_VISIBLE SelectionBuffer
{
/// \brief Constructor
/// \param[in] _camera Name of the camera to generate a selection
diff --git a/gazebo/rendering/selection_buffer/SelectionRenderListener.hh b/gazebo/rendering/selection_buffer/SelectionRenderListener.hh
index c531a7d..8923fdd 100644
--- a/gazebo/rendering/selection_buffer/SelectionRenderListener.hh
+++ b/gazebo/rendering/selection_buffer/SelectionRenderListener.hh
@@ -29,7 +29,7 @@ namespace gazebo
// the compositor MaterialManager.Listener should NOT be running all the
// time - rather only when we're specifically rendering the target that
// needs it
- class GAZEBO_VISIBLE SelectionRenderListener :
+ class GZ_SELECTION_BUFF_VISIBLE SelectionRenderListener :
public Ogre::RenderTargetListener
{
/// \brief Constructor
diff --git a/gazebo/rendering/skyx/CMakeLists.txt b/gazebo/rendering/skyx/CMakeLists.txt
index 7f3433b..78be35a 100644
--- a/gazebo/rendering/skyx/CMakeLists.txt
+++ b/gazebo/rendering/skyx/CMakeLists.txt
@@ -55,6 +55,8 @@ set (VCLOUDS_SOURCE_FILES
# Add include directory
include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSKYX_LIB")
+
gz_add_library(gazebo_skyx ${SKYX_SOURCE_FILES} ${VCLOUDS_SOURCE_FILES})
target_link_libraries(gazebo_skyx ${OGRE_LIBRARIES})
gz_install_library(gazebo_skyx)
diff --git a/gazebo/rendering/skyx/include/Prerequisites.h b/gazebo/rendering/skyx/include/Prerequisites.h
index ab355fb..99d5548 100644
--- a/gazebo/rendering/skyx/include/Prerequisites.h
+++ b/gazebo/rendering/skyx/include/Prerequisites.h
@@ -36,19 +36,25 @@ http://www.gnu.org/copyleft/lesser.txt.
/// Include external headers
#include <OGRE/Ogre.h>
+#include <OGRE/OgreFrameListener.h>
+#include <OGRE/OgreBillboard.h>
/// Define the dll export qualifier if compiling for Windows
-#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32
- #ifdef SKYX_LIB
- #define DllExport __declspec (dllexport)
- #else
- #define DllExport __declspec (dllimport)
- #endif
+#if defined BUILDING_STATIC_LIBS
+ #define DllExport
#else
- #if __GNUC__ >= 4
- #define DllExport __attribute__ ((visibility ("default")))
+ #if OGRE_PLATFORM == OGRE_PLATFORM_WIN32
+ #ifdef SKYX_LIB
+ #define DllExport __declspec (dllexport)
+ #else
+ #define DllExport __declspec (dllimport)
+ #endif
#else
- #define DllExport
+ #if __GNUC__ >= 4
+ #define DllExport __attribute__ ((visibility ("default")))
+ #else
+ #define DllExport
+ #endif
#endif
#endif
diff --git a/gazebo/sensors/AltimeterSensor.cc b/gazebo/sensors/AltimeterSensor.cc
new file mode 100644
index 0000000..7dda153
--- /dev/null
+++ b/gazebo/sensors/AltimeterSensor.cc
@@ -0,0 +1,214 @@
+/*
+ * 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.
+ *
+*/
+#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 "gazebo/common/common.hh"
+#include "gazebo/physics/physics.hh"
+#include "gazebo/transport/transport.hh"
+
+#include "gazebo/sensors/SensorFactory.hh"
+#include "gazebo/sensors/Noise.hh"
+#include "gazebo/sensors/AltimeterSensorPrivate.hh"
+#include "gazebo/sensors/AltimeterSensor.hh"
+
+using namespace gazebo;
+using namespace sensors;
+
+GZ_REGISTER_STATIC_SENSOR("altimeter", AltimeterSensor)
+
+/////////////////////////////////////////////////
+AltimeterSensor::AltimeterSensor()
+: Sensor(sensors::OTHER), dataPtr(new AltimeterSensorPrivate)
+{
+}
+
+/////////////////////////////////////////////////
+AltimeterSensor::~AltimeterSensor()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+void AltimeterSensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf)
+{
+ Sensor::Load(_worldName, _sdf);
+}
+
+/////////////////////////////////////////////////
+std::string AltimeterSensor::GetTopic() const
+{
+ std::string topicName = "~/" + this->parentName + '/' + this->GetName();
+ if (this->sdf->HasElement("topic"))
+ topicName += '/' + this->sdf->Get<std::string>("topic");
+ boost::replace_all(topicName, "::", "/");
+
+ return topicName;
+}
+
+/////////////////////////////////////////////////
+void AltimeterSensor::Load(const std::string &_worldName)
+{
+ Sensor::Load(_worldName);
+
+ physics::EntityPtr parentEntity = this->world->GetEntity(this->parentName);
+ this->dataPtr->parentLink =
+ boost::dynamic_pointer_cast<physics::Link>(parentEntity);
+
+ this->dataPtr->altPub =
+ this->node->Advertise<msgs::Altimeter>(this->GetTopic(), 50);
+
+ // Parse sdf noise parameters
+ sdf::ElementPtr altElem = this->sdf->GetElement("altimeter");
+
+ if (!altElem)
+ {
+ gzerr << "Missing <altimeter> element in sensor["
+ << this->GetName() << "]\n";
+ }
+ else
+ {
+ // Load altimeter vertical position noise
+ if (altElem->HasElement("vertical_position") &&
+ altElem->GetElement("vertical_position")->HasElement("noise"))
+ {
+ sdf::ElementPtr vertPosElem = altElem->GetElement("vertical_position");
+ this->noises[ALTIMETER_POSITION_NOISE_METERS] =
+ NoiseFactory::NewNoiseModel(vertPosElem->GetElement("noise"));
+ }
+
+ // Load altimeter vertical velocity noise
+ if (altElem->HasElement("vertical_velocity") &&
+ altElem->GetElement("vertical_velocity")->HasElement("noise"))
+ {
+ sdf::ElementPtr vertVelElem = altElem->GetElement("vertical_velocity");
+ this->noises[ALTIMETER_VELOCITY_NOISE_METERS_PER_S] =
+ NoiseFactory::NewNoiseModel(vertVelElem->GetElement("noise"));
+ }
+
+ // Initialise reference altitude
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+ this->dataPtr->altMsg.set_vertical_reference((this->pose +
+ this->dataPtr->parentLink->GetWorldPose().Ign()).Pos().Z());
+ }
+}
+
+/////////////////////////////////////////////////
+void AltimeterSensor::Fini()
+{
+ Sensor::Fini();
+ this->dataPtr->parentLink.reset();
+}
+
+//////////////////////////////////////////////////
+void AltimeterSensor::Init()
+{
+ Sensor::Init();
+}
+
+//////////////////////////////////////////////////
+bool AltimeterSensor::UpdateImpl(bool /*_force*/)
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+
+ // Get latest pose information
+ if (this->dataPtr->parentLink)
+ {
+ ignition::math::Pose3d parentPose =
+ this->dataPtr->parentLink->GetWorldPose().Ign();
+
+ // Get pose in gazebo reference frame
+ ignition::math::Pose3d altPose = this->pose + parentPose;
+
+ ignition::math::Vector3d altVel =
+ this->dataPtr->parentLink->GetWorldLinearVel(this->pose.Pos()).Ign();
+
+ // Apply noise to the position and velocity
+ if (this->noises.find(ALTIMETER_POSITION_NOISE_METERS) !=
+ this->noises.end())
+ {
+ this->dataPtr->altMsg.set_vertical_position(
+ this->noises[ALTIMETER_POSITION_NOISE_METERS]->Apply(
+ altPose.Pos().Z() - this->dataPtr->altMsg.vertical_reference()));
+ }
+ else
+ {
+ this->dataPtr->altMsg.set_vertical_position(
+ altPose.Pos().Z() - this->dataPtr->altMsg.vertical_reference());
+ }
+
+ if (this->noises.find(ALTIMETER_VELOCITY_NOISE_METERS_PER_S) !=
+ this->noises.end())
+ {
+ this->dataPtr->altMsg.set_vertical_velocity(
+ this->noises[ALTIMETER_VELOCITY_NOISE_METERS_PER_S]->Apply(
+ altVel.Z()));
+ }
+ else
+ {
+ this->dataPtr->altMsg.set_vertical_velocity(altVel.Z());
+ }
+ }
+
+ // Save the time of the measurement
+ msgs::Set(this->dataPtr->altMsg.mutable_time(), this->world->GetSimTime());
+
+ // Publish the message if needed
+ if (this->dataPtr->altPub)
+ this->dataPtr->altPub->Publish(this->dataPtr->altMsg);
+
+ return true;
+}
+
+//////////////////////////////////////////////////
+double AltimeterSensor::Altitude() const
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+ return this->dataPtr->altMsg.vertical_position();
+}
+
+//////////////////////////////////////////////////
+double AltimeterSensor::VerticalVelocity() const
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+ return this->dataPtr->altMsg.vertical_velocity();
+}
+
+//////////////////////////////////////////////////
+double AltimeterSensor::ReferenceAltitude() const
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+ return this->dataPtr->altMsg.vertical_reference();
+}
+
+//////////////////////////////////////////////////
+void AltimeterSensor::SetReferenceAltitude(const double _refAlt)
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+
+ // Recompute the last altitude with the new vertical reference height
+ this->dataPtr->altMsg.set_vertical_position(
+ this->dataPtr->altMsg.vertical_position() -
+ (_refAlt - this->dataPtr->altMsg.vertical_reference()));
+
+ // Save the new reference height
+ this->dataPtr->altMsg.set_vertical_reference(_refAlt);
+}
diff --git a/gazebo/sensors/AltimeterSensor.hh b/gazebo/sensors/AltimeterSensor.hh
new file mode 100644
index 0000000..b796a1e
--- /dev/null
+++ b/gazebo/sensors/AltimeterSensor.hh
@@ -0,0 +1,93 @@
+/*
+ * 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_ALTIMETER_SENSOR_HH_
+#define _GAZEBO_ALTIMETER_SENSOR_HH_
+
+#include <string>
+
+#include <sdf/sdf.hh>
+
+#include "gazebo/sensors/Sensor.hh"
+#include "gazebo/common/CommonTypes.hh"
+#include "gazebo/physics/PhysicsTypes.hh"
+#include "gazebo/sensors/SensorTypes.hh"
+#include "gazebo/transport/TransportTypes.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace sensors
+ {
+ // Forward declare private data class
+ class AltimeterSensorPrivate;
+
+ /// \addtogroup gazebo_sensors
+ /// \{
+
+ /// \brief AltimeterSensor to provide vertical position and velocity
+ class GAZEBO_VISIBLE AltimeterSensor: public Sensor
+ {
+ /// \brief Constructor.
+ public: AltimeterSensor();
+
+ /// \brief Destructor.
+ public: virtual ~AltimeterSensor();
+
+ // Documentation inherited
+ public: virtual void Load(const std::string &_worldName,
+ sdf::ElementPtr _sdf);
+
+ // Documentation inherited
+ public: virtual void Load(const std::string &_worldName);
+
+ // Documentation inherited
+ public: virtual void Init();
+
+ // Documentation inherited
+ public: virtual std::string GetTopic() const;
+
+ // Documentation inherited
+ protected: virtual bool UpdateImpl(bool _force);
+
+ // Documentation inherited
+ public: virtual void Fini();
+
+ /// \brief Accessor for current vertical position
+ /// \return Current vertical position
+ public: double Altitude() const;
+
+ /// \brief Accessor for current vertical velocity
+ /// \return Current vertical velocity
+ public: double VerticalVelocity() const;
+
+ /// \brief Accessor for the reference altitude
+ /// \return Current reference altitude
+ public: double ReferenceAltitude() const;
+
+ /// \brief Accessor for current vertical velocity
+ /// \param[in] _refAlt reference altitude
+ public: void SetReferenceAltitude(const double _refAlt);
+
+ /// \internal
+ /// \brief Private data pointer
+ private: AltimeterSensorPrivate *dataPtr;
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/sensors/AltimeterSensorPrivate.hh b/gazebo/sensors/AltimeterSensorPrivate.hh
new file mode 100644
index 0000000..7962515
--- /dev/null
+++ b/gazebo/sensors/AltimeterSensorPrivate.hh
@@ -0,0 +1,49 @@
+/*
+ * 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_ALTIMETER_SENSOR_PRIVATE_HH_
+#define _GAZEBO_ALTIMETER_SENSOR_PRIVATE_HH_
+
+#include <mutex>
+#include <string>
+
+#include "gazebo/transport/TransportTypes.hh"
+#include "gazebo/physics/PhysicsTypes.hh"
+#include "gazebo/msgs/msgs.hh"
+
+namespace gazebo
+{
+ namespace sensors
+ {
+ /// \internal
+ /// \brief AltimeterSensor private data
+ class AltimeterSensorPrivate
+ {
+ /// \brief Mutex to protect reads and writes.
+ public: mutable std::mutex mutex;
+
+ /// \brief Altimeter data publisher.
+ public: transport::PublisherPtr altPub;
+
+ /// \brief Parent link of this sensor.
+ public: physics::LinkPtr parentLink;
+
+ /// \brief Stores most recent altimeter sensor data.
+ public: msgs::Altimeter altMsg;
+ };
+ }
+}
+#endif
diff --git a/gazebo/sensors/AltimeterSensor_TEST.cc b/gazebo/sensors/AltimeterSensor_TEST.cc
new file mode 100644
index 0000000..311e772
--- /dev/null
+++ b/gazebo/sensors/AltimeterSensor_TEST.cc
@@ -0,0 +1,310 @@
+/*
+ * 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 <ignition/math/Pose3.hh>
+
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+#include "gazebo/sensors/AltimeterSensor.hh"
+
+#define TOL 1e-4
+
+using namespace gazebo;
+
+/// \brief Test class for the altimeter sensor
+class AltimeterSensor_TEST : public ServerFixture,
+ public testing::WithParamInterface<const char*>
+{
+ /// \brief Check that a simple altimeter works correctly
+ /// The default should be inserted with altitude = 0 and velocity = 0
+ /// \param[in] _physicsEngine The type of physics engine to use.
+ public: void BasicAltimeterSensorCheck(const std::string &_physicsEngine);
+
+ /// \brief Check that an altimeter at a non-zero altitude works.
+ /// \param[in] _physicsEngine The type of physics engine to use.
+ public: void NonzeroAltimeterSensorCheck(const std::string &_physicsEngine);
+
+ /// \brief Check that a falling altimeter has the correct velocity.
+ /// \param[in] _physicsEngine The type of physics engine to use.
+ public: void LinearAltimeterSensorCheck(const std::string &_physicsEngine);
+
+ /// \brief Check that a rotating altimeter has the correct velocity.
+ /// \param[in] _physicsEngine The type of physics engine to use.
+ public: void AngularAltimeterSensorCheck(const std::string &_physicsEngine);
+
+ /// \brief Check that a rotating and falling altimeter has the correct
+ /// velocity.
+ /// \param[in] _physicsEngine The type of physics engine to use.
+ public: void LinearAngularAltimeterSensorCheck(
+ const std::string &_physicsEngine);
+};
+
+// An altitude sensor
+static std::string altSensorString =
+"<sdf version='1.5'>"
+" <sensor name='altimeter' type='altimeter'>"
+" <always_on>1</always_on>"
+" <update_rate>10.0</update_rate>"
+" <altimeter>"
+" </altimeter>"
+" </sensor>"
+"</sdf>";
+
+/////////////////////////////////////////////////
+void AltimeterSensor_TEST::BasicAltimeterSensorCheck(
+ const std::string &_physicsEngine)
+{
+ Load("worlds/empty.world", false, _physicsEngine);
+ sensors::SensorManager *mgr = sensors::SensorManager::Instance();
+
+ physics::WorldPtr world = physics::get_world("default");
+
+ sdf::ElementPtr sdf(new sdf::Element);
+ sdf::initFile("sensor.sdf", sdf);
+ sdf::readString(altSensorString, sdf);
+
+ // Create the altimeter sensor
+ std::string sensorName = mgr->CreateSensor(sdf, "default",
+ "ground_plane::link", 0);
+
+ // Make sure the returned sensor name is correct
+ EXPECT_EQ(sensorName,
+ std::string("default::ground_plane::link::altimeter"));
+
+ // Update the sensor manager so that it can process new sensors.
+ mgr->Update();
+
+ // Get a pointer to the altimeter sensor
+ sensors::AltimeterSensorPtr sensor =
+ boost::dynamic_pointer_cast<sensors::AltimeterSensor>
+ (mgr->GetSensor(sensorName));
+
+ // Make sure the above dynamic cast worked.
+ EXPECT_TRUE(sensor != NULL);
+
+ // By default the altitude of the sensor should be zero
+ EXPECT_DOUBLE_EQ(sensor->Altitude(), 0.0);
+ EXPECT_DOUBLE_EQ(sensor->VerticalVelocity(), 0.0);
+}
+
+/////////////////////////////////////////////////
+// Check linear velocity is correct
+void AltimeterSensor_TEST::LinearAltimeterSensorCheck(
+ const std::string &_physicsEngine)
+{
+ Load("worlds/empty.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);
+
+ // Spawn an altimeter
+ std::string modelName = "altModel";
+ std::string altSensorName = "altSensor";
+ ignition::math::Pose3d modelPose(0, 0, 10, 0, 0, 0);
+ std::string topic = "~/" + altSensorName + "_" + _physicsEngine;
+ SpawnUnitAltimeterSensor(modelName, altSensorName,
+ "box", topic, modelPose.Pos(), modelPose.Rot().Euler());
+
+ // Get the altimeter
+ sensors::SensorPtr sensor = sensors::get_sensor("altSensor");
+ sensors::AltimeterSensorPtr altSensor =
+ boost::dynamic_pointer_cast<sensors::AltimeterSensor>(sensor);
+ ASSERT_TRUE(altSensor != NULL);
+
+ sensors::SensorManager::Instance()->Init();
+ altSensor->SetActive(true);
+
+ int steps = 10;
+ world->Step(steps);
+ altSensor->Update(true);
+
+ // The altimeter should have a velocity of v = g * dt
+ EXPECT_DOUBLE_EQ(altSensor->VerticalVelocity(),
+ physics->GetGravity().z * (physics->GetMaxStepSize()*steps));
+}
+
+/////////////////////////////////////////////////
+// Check rotational velocity is correct
+void AltimeterSensor_TEST::AngularAltimeterSensorCheck(
+ const std::string &_physicsEngine)
+{
+ Load("worlds/test_altimeter_rotation.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);
+
+ physics::ModelPtr model = world->GetModel("model");
+ ASSERT_TRUE(model != NULL);
+
+ physics::JointPtr joint = model->GetJoint("joint");
+ ASSERT_TRUE(joint != NULL);
+
+ sensors::SensorPtr sensor = sensors::get_sensor("altimeter");
+ sensors::AltimeterSensorPtr altSensor =
+ boost::dynamic_pointer_cast<sensors::AltimeterSensor>(sensor);
+
+ ASSERT_TRUE(altSensor != NULL);
+
+ sensors::SensorManager::Instance()->Init();
+ altSensor->SetActive(true);
+
+ int steps = 1;
+ world->Step(steps);
+ altSensor->Update(true);
+
+ // Get the link's angular velocity
+ ignition::math::Vector3d avel =
+ model->GetLink("link")->GetRelativeAngularVel().Ign();
+
+ // Expect the altimeter's velocity to equal the angular velocity at the
+ // end of the rod.
+ EXPECT_NEAR(altSensor->VerticalVelocity(), avel.Sum() * 10, 1e-3);
+}
+
+/////////////////////////////////////////////////
+// Check angular and linear velocity is correct
+void AltimeterSensor_TEST::LinearAngularAltimeterSensorCheck(
+ const std::string &_physicsEngine)
+{
+ Load("worlds/test_altimeter_linear_angular.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);
+
+ physics::ModelPtr model = world->GetModel("model");
+ ASSERT_TRUE(model != NULL);
+
+ physics::JointPtr joint = model->GetJoint("joint");
+ ASSERT_TRUE(joint != NULL);
+
+ sensors::SensorPtr sensor = sensors::get_sensor("altimeter");
+ sensors::AltimeterSensorPtr altSensor =
+ boost::dynamic_pointer_cast<sensors::AltimeterSensor>(sensor);
+
+ ASSERT_TRUE(altSensor != NULL);
+
+ sensors::SensorManager::Instance()->Init();
+ altSensor->SetActive(true);
+
+ int steps = 10;
+ world->Step(steps);
+ altSensor->Update(true);
+
+ // Angular velocity of the rod
+ ignition::math::Vector3d avel =
+ model->GetLink("link")->GetRelativeAngularVel().Ign();
+
+ // Linear velocity of the rod at the location that is attached to
+ // the prismatic joint.
+ ignition::math::Vector3d lvel =
+ model->GetLink("link")->GetWorldLinearVel(
+ ignition::math::Vector3d(0, -5, 0)).Ign();
+
+ // Expect the altimeter's velocity to equal the angular velocity at the
+ // end of the rod + the rod's linear velocity.
+ EXPECT_NEAR(altSensor->VerticalVelocity(),
+ avel.Sum() * 10 + lvel.Z(), 1e-4);
+}
+
+/////////////////////////////////////////////////
+// If inserted at X=0,Y=0,Z=10m
+void AltimeterSensor_TEST::NonzeroAltimeterSensorCheck(
+ const std::string &_physicsEngine)
+{
+ Load("worlds/empty.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);
+
+ // Spawn an altimeter sensor at a height of 10m
+ std::string modelName = "altModel";
+ std::string altSensorName = "altSensor";
+ ignition::math::Pose3d modelPose(0, 0, 10, 0, 0, 0);
+ std::string topic = "~/" + altSensorName + "_" + _physicsEngine;
+ SpawnUnitAltimeterSensor(modelName, altSensorName,
+ "box", topic, modelPose.Pos(), modelPose.Rot().Euler());
+
+ sensors::SensorPtr sensor = sensors::get_sensor(altSensorName);
+ sensors::AltimeterSensorPtr altSensor =
+ boost::dynamic_pointer_cast<sensors::AltimeterSensor>(sensor);
+
+ ASSERT_TRUE(altSensor != NULL);
+
+ sensors::SensorManager::Instance()->Init();
+ altSensor->SetActive(true);
+
+ // Check for match
+ EXPECT_DOUBLE_EQ(altSensor->ReferenceAltitude(), 10.0);
+ EXPECT_DOUBLE_EQ(altSensor->Altitude(), 0.0);
+ EXPECT_DOUBLE_EQ(altSensor->VerticalVelocity(), 0.0);
+}
+
+/////////////////////////////////////////////////
+TEST_P(AltimeterSensor_TEST, BasicAltimeterSensorCheck)
+{
+ BasicAltimeterSensorCheck(GetParam());
+}
+
+/////////////////////////////////////////////////
+TEST_P(AltimeterSensor_TEST, LinearAltimeterSensorCheck)
+{
+ LinearAltimeterSensorCheck(GetParam());
+}
+
+/////////////////////////////////////////////////
+TEST_P(AltimeterSensor_TEST, AngularAltimeterSensorCheck)
+{
+ AngularAltimeterSensorCheck(GetParam());
+}
+
+/////////////////////////////////////////////////
+TEST_P(AltimeterSensor_TEST, LinearAngularAltimeterSensorCheck)
+{
+ LinearAngularAltimeterSensorCheck(GetParam());
+}
+
+/////////////////////////////////////////////////
+TEST_P(AltimeterSensor_TEST, NonzeroAltimeterSensorCheck)
+{
+ NonzeroAltimeterSensorCheck(GetParam());
+}
+
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, AltimeterSensor_TEST,
+ PHYSICS_ENGINE_VALUES);
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/sensors/CMakeLists.txt b/gazebo/sensors/CMakeLists.txt
index 40b0f8f..fb72ac3 100644
--- a/gazebo/sensors/CMakeLists.txt
+++ b/gazebo/sensors/CMakeLists.txt
@@ -1,6 +1,11 @@
include (${gazebo_cmake_dir}/GazeboUtils.cmake)
+if (WIN32)
+ include_directories(${libdl_include_dir})
+endif()
+
set (sources
+ AltimeterSensor.cc
CameraSensor.cc
ContactSensor.cc
DepthCameraSensor.cc
@@ -9,6 +14,7 @@ set (sources
GpsSensor.cc
GpuRaySensor.cc
ImuSensor.cc
+ MagnetometerSensor.cc
MultiCameraSensor.cc
Noise.cc
RaySensor.cc
@@ -25,6 +31,7 @@ set (sources
)
set (headers
+ AltimeterSensor.hh
CameraSensor.hh
ContactSensor.hh
DepthCameraSensor.hh
@@ -33,6 +40,7 @@ set (headers
GpsSensor.hh
GpuRaySensor.hh
ImuSensor.hh
+ MagnetometerSensor.hh
MultiCameraSensor.hh
Noise.hh
RaySensor.hh
@@ -50,9 +58,11 @@ set (headers
)
set (gtest_sources
+ AltimeterSensor_TEST.cc
ForceTorqueSensor_TEST.cc
GpsSensor_TEST.cc
ImuSensor_TEST.cc
+ MagnetometerSensor_TEST.cc
Noise_TEST.cc
RaySensor_TEST.cc
Sensor_TEST.cc
@@ -78,7 +88,7 @@ endforeach()
configure_file (${CMAKE_CURRENT_SOURCE_DIR}/sensors.hh.in ${CMAKE_CURRENT_BINARY_DIR}/sensors.hh)
-add_library(gazebo_sensors SHARED ${sources})
+gz_add_library(gazebo_sensors ${sources})
target_link_libraries(gazebo_sensors
gazebo_common
diff --git a/gazebo/sensors/CameraSensor.cc b/gazebo/sensors/CameraSensor.cc
index 651af1e..8c76bbb 100644
--- a/gazebo/sensors/CameraSensor.cc
+++ b/gazebo/sensors/CameraSensor.cc
@@ -14,10 +14,11 @@
* limitations under the License.
*
*/
-/* Desc: A camera sensor using OpenGL
- * Author: Nate Koenig
- * Date: 15 July 2003
- */
+#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 "gazebo/common/Events.hh"
#include "gazebo/common/Exception.hh"
@@ -131,20 +132,19 @@ void CameraSensor::Init()
this->camera->Init();
this->camera->CreateRenderTexture(this->GetName() + "_RttTex");
- math::Pose cameraPose = this->pose;
+ ignition::math::Pose3d cameraPose = this->pose;
if (cameraSdf->HasElement("pose"))
- cameraPose = cameraSdf->Get<math::Pose>("pose") + cameraPose;
+ cameraPose = cameraSdf->Get<ignition::math::Pose3d>("pose") + cameraPose;
this->camera->SetWorldPose(cameraPose);
this->camera->AttachToVisual(this->parentId, true);
if (cameraSdf->HasElement("noise"))
{
- NoisePtr noise =
- NoiseFactory::NewNoiseModel(cameraSdf->GetElement("noise"),
+ this->noises[CAMERA_NOISE] =
+ NoiseFactory::NewNoiseModel(cameraSdf->GetElement("noise"),
this->GetType());
- this->noises.push_back(noise);
- noise->SetCamera(this->camera);
+ this->noises[CAMERA_NOISE]->SetCamera(this->camera);
}
}
else
diff --git a/gazebo/sensors/CameraSensor.hh b/gazebo/sensors/CameraSensor.hh
index 9ea7aac..68b2f3a 100644
--- a/gazebo/sensors/CameraSensor.hh
+++ b/gazebo/sensors/CameraSensor.hh
@@ -14,13 +14,8 @@
* limitations under the License.
*
*/
-/* Desc: A persepective X11 OpenGL Camera Sensor
- * Author: Nate Koenig
- * Date: 15 July 2003
- */
-
-#ifndef _CAMERASENSOR_HH_
-#define _CAMERASENSOR_HH_
+#ifndef _GAZEBO_CAMERASENSOR_HH_
+#define _GAZEBO_CAMERASENSOR_HH_
#include <string>
diff --git a/gazebo/sensors/ContactSensor.cc b/gazebo/sensors/ContactSensor.cc
index 71594d3..35eee05 100644
--- a/gazebo/sensors/ContactSensor.cc
+++ b/gazebo/sensors/ContactSensor.cc
@@ -19,6 +19,12 @@
* Date: 09 Sept. 2008
*/
+#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 "gazebo/common/Exception.hh"
@@ -92,6 +98,7 @@ void ContactSensor::Load(const std::string &_worldName)
std::string entityName =
this->world->GetEntity(this->parentName)->GetScopedName();
+ std::string filterName = entityName + "::" + this->GetName();
// Get all the collision elements
while (collisionElem)
@@ -112,7 +119,7 @@ void ContactSensor::Load(const std::string &_worldName)
// this sensor
physics::ContactManager *mgr =
this->world->GetPhysicsEngine()->GetContactManager();
- std::string topic = mgr->CreateFilter(entityName, this->collisions);
+ std::string topic = mgr->CreateFilter(filterName, this->collisions);
if (!this->contactSub)
{
this->contactSub = this->node->Subscribe(topic,
@@ -207,10 +214,11 @@ void ContactSensor::Fini()
{
std::string entityName =
this->world->GetEntity(this->parentName)->GetScopedName();
+ std::string filterName = entityName + "::" + this->GetName();
physics::ContactManager *mgr =
this->world->GetPhysicsEngine()->GetContactManager();
- mgr->RemoveFilter(entityName);
+ mgr->RemoveFilter(filterName);
}
this->contactSub.reset();
diff --git a/gazebo/sensors/ContactSensor.hh b/gazebo/sensors/ContactSensor.hh
index d7dcb46..55e55a7 100644
--- a/gazebo/sensors/ContactSensor.hh
+++ b/gazebo/sensors/ContactSensor.hh
@@ -14,13 +14,8 @@
* limitations under the License.
*
*/
-/* Desc: Contact sensor
- * Author: Nate Koenig
- * Date: 09 Sept. 2008
-*/
-
-#ifndef _CONTACTSENSOR_HH_
-#define _CONTACTSENSOR_HH_
+#ifndef _GAZEBO_CONTACTSENSOR_HH_
+#define _GAZEBO_CONTACTSENSOR_HH_
#include <vector>
#include <map>
@@ -29,7 +24,6 @@
#include "gazebo/msgs/msgs.hh"
-#include "gazebo/math/Angle.hh"
#include "gazebo/sensors/Sensor.hh"
#include "gazebo/physics/Contact.hh"
#include "gazebo/util/system.hh"
diff --git a/gazebo/sensors/DepthCameraSensor.cc b/gazebo/sensors/DepthCameraSensor.cc
index 3311889..996d0e7 100644
--- a/gazebo/sensors/DepthCameraSensor.cc
+++ b/gazebo/sensors/DepthCameraSensor.cc
@@ -14,10 +14,11 @@
* limitations under the License.
*
*/
-/* Desc: A camera sensor using OpenGL
- * Author: Nate Koenig
- * Date: 15 July 2003
- */
+#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>
diff --git a/gazebo/sensors/DepthCameraSensor.hh b/gazebo/sensors/DepthCameraSensor.hh
index ef277dd..db74e7d 100644
--- a/gazebo/sensors/DepthCameraSensor.hh
+++ b/gazebo/sensors/DepthCameraSensor.hh
@@ -14,13 +14,8 @@
* limitations under the License.
*
*/
-/* Desc: A persepective X11 OpenGL Camera Sensor
- * Author: Nate Koenig
- * Date: 15 July 2003
- */
-
-#ifndef _DEPTHCAMERASENSOR_HH_
-#define _DEPTHCAMERASENSOR_HH_
+#ifndef _GAZEBO_DEPTHCAMERASENSOR_HH_
+#define _GAZEBO_DEPTHCAMERASENSOR_HH_
#include <string>
diff --git a/gazebo/sensors/ForceTorqueSensor.cc b/gazebo/sensors/ForceTorqueSensor.cc
index 2552486..065a404 100644
--- a/gazebo/sensors/ForceTorqueSensor.cc
+++ b/gazebo/sensors/ForceTorqueSensor.cc
@@ -14,6 +14,11 @@
* 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 "gazebo/physics/World.hh"
#include "gazebo/physics/PhysicsEngine.hh"
@@ -100,10 +105,9 @@ void ForceTorqueSensor::Load(const std::string &_worldName,
// is expressed in joint orientation
GZ_ASSERT(this->parentJoint,
"parentJoint should be defined by single argument Load()");
- math::Quaternion rotationChildSensor =
- (this->pose + this->parentJoint->GetInitialAnchorPose()).rot;
- this->rotationSensorChild =
- rotationChildSensor.GetInverse().GetAsMatrix3();
+ ignition::math::Quaterniond rotationChildSensor =
+ (this->pose + this->parentJoint->GetInitialAnchorPose().Ign()).Rot();
+ this->rotationSensorChild = rotationChildSensor.Inverse();
// Handle measure direction
bool defaultDirectionIsParentToChild = false;
@@ -178,13 +182,25 @@ physics::JointPtr ForceTorqueSensor::GetJoint() const
//////////////////////////////////////////////////
math::Vector3 ForceTorqueSensor::GetForce() const
{
- return msgs::Convert(this->wrenchMsg.wrench().force());
+ return this->Force();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d ForceTorqueSensor::Force() const
+{
+ return msgs::ConvertIgn(this->wrenchMsg.wrench().force());
}
//////////////////////////////////////////////////
math::Vector3 ForceTorqueSensor::GetTorque() const
{
- return msgs::Convert(this->wrenchMsg.wrench().torque());
+ return this->Torque();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d ForceTorqueSensor::Torque() const
+{
+ return msgs::ConvertIgn(this->wrenchMsg.wrench().torque());
}
//////////////////////////////////////////////////
@@ -198,33 +214,33 @@ bool ForceTorqueSensor::UpdateImpl(bool /*_force*/)
physics::JointWrench wrench = this->parentJoint->GetForceTorque(0u);
// Get the force and torque in the appropriate frame.
- math::Vector3 measuredForce;
- math::Vector3 measuredTorque;
+ ignition::math::Vector3d measuredForce;
+ ignition::math::Vector3d measuredTorque;
if (this->measureFrame == PARENT_LINK)
{
if (this->parentToChild)
{
- measuredForce = wrench.body1Force;
- measuredTorque = wrench.body1Torque;
+ measuredForce = wrench.body1Force.Ign();
+ measuredTorque = wrench.body1Torque.Ign();
}
else
{
- measuredForce = -1*wrench.body1Force;
- measuredTorque = -1*wrench.body1Torque;
+ measuredForce = -1*wrench.body1Force.Ign();
+ measuredTorque = -1*wrench.body1Torque.Ign();
}
}
else if (this->measureFrame == CHILD_LINK)
{
if (!this->parentToChild)
{
- measuredForce = wrench.body2Force;
- measuredTorque = wrench.body2Torque;
+ measuredForce = wrench.body2Force.Ign();
+ measuredTorque = wrench.body2Torque.Ign();
}
else
{
- measuredForce = -1*wrench.body2Force;
- measuredTorque = -1*wrench.body2Torque;
+ measuredForce = -1*wrench.body2Force.Ign();
+ measuredTorque = -1*wrench.body2Torque.Ign();
}
}
else
@@ -233,13 +249,13 @@ bool ForceTorqueSensor::UpdateImpl(bool /*_force*/)
"measureFrame must be PARENT_LINK, CHILD_LINK or SENSOR");
if (!this->parentToChild)
{
- measuredForce = rotationSensorChild*wrench.body2Force;
- measuredTorque = rotationSensorChild*wrench.body2Torque;
+ measuredForce = rotationSensorChild*wrench.body2Force.Ign();
+ measuredTorque = rotationSensorChild*wrench.body2Torque.Ign();
}
else
{
- measuredForce = rotationSensorChild*(-1*wrench.body2Force);
- measuredTorque = rotationSensorChild*(-1*wrench.body2Torque);
+ measuredForce = rotationSensorChild*(-1*wrench.body2Force.Ign());
+ measuredTorque = rotationSensorChild*(-1*wrench.body2Torque.Ign());
}
}
diff --git a/gazebo/sensors/ForceTorqueSensor.hh b/gazebo/sensors/ForceTorqueSensor.hh
index 6ce7477..cec6c9b 100644
--- a/gazebo/sensors/ForceTorqueSensor.hh
+++ b/gazebo/sensors/ForceTorqueSensor.hh
@@ -14,9 +14,8 @@
* limitations under the License.
*
*/
-
-#ifndef _FORCETORQUESENSOR_HH_
-#define _FORCETORQUESENSOR_HH_
+#ifndef _GAZEBO_FORCETORQUESENSOR_HH_
+#define _GAZEBO_FORCETORQUESENSOR_HH_
#include <string>
@@ -56,12 +55,24 @@ namespace gazebo
public: virtual std::string GetTopic() const;
/// \brief Get the current joint torque.
- /// \return The latested measured torque.
- public: math::Vector3 GetTorque() const;
+ /// \return The latest measured torque.
+ /// \deprecated See Torque() function that returns an
+ /// ignition::math::Vector3d object.
+ public: math::Vector3 GetTorque() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the current joint torque.
+ /// \return The latest measured torque.
+ public: ignition::math::Vector3d Torque() const;
+
+ /// \brief Get the current joint force.
+ /// \return The latested measured force.
+ /// \deprecated See Force() function that returns an
+ /// ignition::math::Vector3d object.
+ public: math::Vector3 GetForce() const GAZEBO_DEPRECATED(6.0);
/// \brief Get the current joint force.
/// \return The latested measured force.
- public: math::Vector3 GetForce() const;
+ public: ignition::math::Vector3d Force() const;
/// \brief Get Parent Joint
/// \return Pointer to the joint containing this sensor
@@ -122,7 +133,7 @@ namespace gazebo
/// \brief Rotation matrix than transforms a vector expressed in child
/// orientation in a vector expressed in joint orientation.
/// Necessary is the measure is specified in joint frame.
- private: math::Matrix3 rotationSensorChild;
+ private: ignition::math::Matrix3d rotationSensorChild;
};
/// \}
}
diff --git a/gazebo/sensors/ForceTorqueSensor_TEST.cc b/gazebo/sensors/ForceTorqueSensor_TEST.cc
index e09e18b..e1c51ff 100644
--- a/gazebo/sensors/ForceTorqueSensor_TEST.cc
+++ b/gazebo/sensors/ForceTorqueSensor_TEST.cc
@@ -16,8 +16,8 @@
*/
#include <gtest/gtest.h>
-#include "test/ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
@@ -70,8 +70,8 @@ void ForceTorqueSensor_TEST::ForceTorqueTest(const std::string &_physicsEngine)
// Make sure the above dynamic cast worked.
EXPECT_TRUE(sensor != NULL);
- EXPECT_EQ(sensor->GetTorque(), math::Vector3(0, 0, 0));
- EXPECT_EQ(sensor->GetForce(), math::Vector3(0, 0, 0));
+ EXPECT_EQ(sensor->Torque(), ignition::math::Vector3d(0, 0, 0));
+ EXPECT_EQ(sensor->Force(), ignition::math::Vector3d(0, 0, 0));
EXPECT_TRUE(sensor->IsActive());
}
diff --git a/gazebo/sensors/GaussianNoiseModel.cc b/gazebo/sensors/GaussianNoiseModel.cc
index 73c32aa..3099dbb 100644
--- a/gazebo/sensors/GaussianNoiseModel.cc
+++ b/gazebo/sensors/GaussianNoiseModel.cc
@@ -14,13 +14,17 @@
* 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/math/special_functions/round.hpp>
+#include <ignition/math/Helpers.hh>
+#include <ignition/math/Rand.hh>
#include "gazebo/common/Assert.hh"
#include "gazebo/common/Console.hh"
-#include "gazebo/math/Helpers.hh"
-#include "gazebo/math/Rand.hh"
#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/Camera.hh"
#include "gazebo/sensors/GaussianNoiseModel.hh"
@@ -49,9 +53,9 @@ namespace gazebo
// Sample three values within the range [0,1.0] and set them for use in
// the fragment shader, which will interpret them as offsets from (0,0)
// to use when computing pseudo-random values.
- Ogre::Vector3 offsets(math::Rand::GetDblUniform(0.0, 1.0),
- math::Rand::GetDblUniform(0.0, 1.0),
- math::Rand::GetDblUniform(0.0, 1.0));
+ Ogre::Vector3 offsets(ignition::math::Rand::DblUniform(0.0, 1.0),
+ ignition::math::Rand::DblUniform(0.0, 1.0),
+ ignition::math::Rand::DblUniform(0.0, 1.0));
// These calls are setting parameters that are declared in two places:
// 1. media/materials/scripts/gazebo.material, in
// fragment_program Gazebo/GaussianCameraNoiseFS
@@ -110,12 +114,14 @@ void GaussianNoiseModel::Load(sdf::ElementPtr _sdf)
biasMean = _sdf->Get<double>("bias_mean");
if (_sdf->HasElement("bias_stddev"))
biasStdDev = _sdf->Get<double>("bias_stddev");
- this->bias = math::Rand::GetDblNormal(biasMean, biasStdDev);
+ this->bias = ignition::math::Rand::DblNormal(biasMean, biasStdDev);
// With equal probability, we pick a negative bias (by convention,
// rateBiasMean should be positive, though it would work fine if
// negative).
- if (math::Rand::GetDblUniform() < 0.5)
+ if (ignition::math::Rand::DblUniform() < 0.5)
this->bias = -this->bias;
+
+ /// \todo Remove this, and use Noise::Print. See ImuSensor for an example
gzlog << "applying Gaussian noise model with mean " << this->mean
<< ", stddev " << this->stdDev
<< ", bias " << this->bias << std::endl;
@@ -127,7 +133,7 @@ void GaussianNoiseModel::Load(sdf::ElementPtr _sdf)
{
gzerr << "Noise precision cannot be less than 0" << std::endl;
}
- else if (!math::equal(this->precision, 0.0, 1e-6))
+ else if (!ignition::math::equal(this->precision, 0.0, 1e-6))
{
this->quantized = true;
}
@@ -144,14 +150,14 @@ void GaussianNoiseModel::Fini()
double GaussianNoiseModel::ApplyImpl(double _in)
{
// Add independent (uncorrelated) Gaussian noise to each input value.
- double whiteNoise = math::Rand::GetDblNormal(this->mean, this->stdDev);
+ double whiteNoise = ignition::math::Rand::DblNormal(this->mean, this->stdDev);
double output = _in + this->bias + whiteNoise;
if (this->quantized)
{
// Apply this->precision
- if (!math::equal(this->precision, 0.0, 1e-6))
+ if (!ignition::math::equal(this->precision, 0.0, 1e-6))
{
- output = boost::math::round(output / this->precision) * this->precision;
+ output = std::round(output / this->precision) * this->precision;
}
}
return output;
@@ -176,6 +182,16 @@ double GaussianNoiseModel::GetBias() const
}
//////////////////////////////////////////////////
+void GaussianNoiseModel::Print(std::ostream &_out) const
+{
+ _out << "Gaussian noise, mean[" << this->mean << "], "
+ << "stdDev[" << this->stdDev << "] "
+ << "bias[" << this->bias << "] "
+ << "precision[" << this->precision << "] "
+ << "quantized[" << this->quantized << "]";
+}
+
+//////////////////////////////////////////////////
ImageGaussianNoiseModel::ImageGaussianNoiseModel()
: GaussianNoiseModel()
{
@@ -218,3 +234,13 @@ void ImageGaussianNoiseModel::Fini()
this->gaussianNoiseCompositorListener.get());
}
}
+
+//////////////////////////////////////////////////
+void ImageGaussianNoiseModel::Print(std::ostream &_out) const
+{
+ _out << "Image Gaussian noise, mean[" << this->mean << "], "
+ << "stdDev[" << this->stdDev << "] "
+ << "bias[" << this->bias << "] "
+ << "precision[" << this->precision << "] "
+ << "quantized[" << this->quantized << "]";
+}
diff --git a/gazebo/sensors/GaussianNoiseModel.hh b/gazebo/sensors/GaussianNoiseModel.hh
index 16bf54e..888ceeb 100644
--- a/gazebo/sensors/GaussianNoiseModel.hh
+++ b/gazebo/sensors/GaussianNoiseModel.hh
@@ -69,6 +69,9 @@ namespace gazebo
/// \return Bias on output.
public: double GetBias() const;
+ /// Documentation inherited
+ public: virtual void Print(std::ostream &_out) const;
+
/// \brief If type starts with GAUSSIAN, the mean of the distribution
/// from which we sample when adding noise.
protected: double mean;
@@ -107,6 +110,9 @@ namespace gazebo
// Documentation inherited.
public: virtual void SetCamera(rendering::CameraPtr _camera);
+ /// Documentation inherited
+ public: virtual void Print(std::ostream &_out) const;
+
/// \brief Gaussian noise compositor.
public: Ogre::CompositorInstance *gaussianNoiseInstance;
diff --git a/gazebo/sensors/GpsSensor.cc b/gazebo/sensors/GpsSensor.cc
index fdedaf6..d9b3336 100644
--- a/gazebo/sensors/GpsSensor.cc
+++ b/gazebo/sensors/GpsSensor.cc
@@ -15,10 +15,15 @@
*
*/
+#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 "gazebo/sensors/SensorFactory.hh"
#include "gazebo/common/common.hh"
-#include "gazebo/math/gzmath.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/sensors/Noise.hh"
#include "gazebo/transport/transport.hh"
@@ -39,10 +44,6 @@ GpsSensor::GpsSensor()
/////////////////////////////////////////////////
GpsSensor::~GpsSensor()
{
- this->horizontalPositionNoise.reset();
- this->verticalPositionNoise.reset();
- this->horizontalVelocityNoise.reset();
- this->verticalVelocityNoise.reset();
}
/////////////////////////////////////////////////
@@ -74,19 +75,29 @@ void GpsSensor::Load(const std::string &_worldName)
// Load position noise parameters
{
sdf::ElementPtr posElem = gpsElem->GetElement("position_sensing");
- this->horizontalPositionNoise = NoiseFactory::NewNoiseModel(
- posElem->GetElement("horizontal")->GetElement("noise"));
- this->verticalPositionNoise = NoiseFactory::NewNoiseModel(
- posElem->GetElement("vertical")->GetElement("noise"));
+ this->noises[GPS_POSITION_LATITUDE_NOISE_METERS] =
+ NoiseFactory::NewNoiseModel(
+ posElem->GetElement("horizontal")->GetElement("noise"));
+ this->noises[GPS_POSITION_LONGITUDE_NOISE_METERS] =
+ NoiseFactory::NewNoiseModel(
+ posElem->GetElement("horizontal")->GetElement("noise"));
+ this->noises[GPS_POSITION_ALTITUDE_NOISE_METERS] =
+ NoiseFactory::NewNoiseModel(
+ posElem->GetElement("vertical")->GetElement("noise"));
}
// Load velocity noise parameters
{
sdf::ElementPtr velElem = gpsElem->GetElement("velocity_sensing");
- this->horizontalVelocityNoise = NoiseFactory::NewNoiseModel(
- velElem->GetElement("horizontal")->GetElement("noise"));
- this->verticalVelocityNoise = NoiseFactory::NewNoiseModel(
- velElem->GetElement("vertical")->GetElement("noise"));
+ this->noises[GPS_VELOCITY_LATITUDE_NOISE_METERS] =
+ NoiseFactory::NewNoiseModel(
+ velElem->GetElement("horizontal")->GetElement("noise"));
+ this->noises[GPS_VELOCITY_LONGITUDE_NOISE_METERS] =
+ NoiseFactory::NewNoiseModel(
+ velElem->GetElement("horizontal")->GetElement("noise"));
+ this->noises[GPS_VELOCITY_ALTITUDE_NOISE_METERS] =
+ NoiseFactory::NewNoiseModel(
+ velElem->GetElement("vertical")->GetElement("noise"));
}
}
@@ -115,37 +126,51 @@ bool GpsSensor::UpdateImpl(bool /*_force*/)
// Measure position and apply noise
{
// Get postion in Cartesian gazebo frame
- math::Pose gpsPose = this->pose + this->parentLink->GetWorldPose();
+ ignition::math::Pose3d gpsPose = this->pose +
+ this->parentLink->GetWorldPose().Ign();
// Apply position noise before converting to global frame
- gpsPose.pos.x = this->horizontalPositionNoise->Apply(gpsPose.pos.x);
- gpsPose.pos.y = this->horizontalPositionNoise->Apply(gpsPose.pos.y);
- gpsPose.pos.z = this->verticalPositionNoise->Apply(gpsPose.pos.z);
+ gpsPose.Pos().X(
+ this->noises[GPS_POSITION_LATITUDE_NOISE_METERS]->Apply(
+ gpsPose.Pos().X()));
+ gpsPose.Pos().Y(
+ this->noises[GPS_POSITION_LONGITUDE_NOISE_METERS]->Apply(
+ gpsPose.Pos().Y()));
+ gpsPose.Pos().Z(
+ this->noises[GPS_POSITION_ALTITUDE_NOISE_METERS]->Apply(
+ gpsPose.Pos().Z()));
// Convert to global frames
- math::Vector3 spherical = this->sphericalCoordinates->
- SphericalFromLocal(gpsPose.pos);
- this->lastGpsMsg.set_latitude_deg(spherical.x);
- this->lastGpsMsg.set_longitude_deg(spherical.y);
- this->lastGpsMsg.set_altitude(spherical.z);
+ ignition::math::Vector3d spherical = this->sphericalCoordinates->
+ SphericalFromLocal(gpsPose.Pos());
+ this->lastGpsMsg.set_latitude_deg(spherical.X());
+ this->lastGpsMsg.set_longitude_deg(spherical.Y());
+ this->lastGpsMsg.set_altitude(spherical.Z());
}
// Measure velocity and apply noise
{
- math::Vector3 gpsVelocity =
- this->parentLink->GetWorldLinearVel(this->pose.pos);
+ ignition::math::Vector3d gpsVelocity =
+ this->parentLink->GetWorldLinearVel(this->pose.Pos()).Ign();
// Convert to global frame
- gpsVelocity = this->sphericalCoordinates->GlobalFromLocal(gpsVelocity);
+ gpsVelocity =
+ this->sphericalCoordinates->GlobalFromLocal(gpsVelocity);
// Apply noise after converting to global frame
- gpsVelocity.x = this->horizontalVelocityNoise->Apply(gpsVelocity.x);
- gpsVelocity.y = this->horizontalVelocityNoise->Apply(gpsVelocity.y);
- gpsVelocity.z = this->verticalVelocityNoise->Apply(gpsVelocity.z);
-
- this->lastGpsMsg.set_velocity_east(gpsVelocity.x);
- this->lastGpsMsg.set_velocity_north(gpsVelocity.y);
- this->lastGpsMsg.set_velocity_up(gpsVelocity.z);
+ gpsVelocity.X(
+ this->noises[GPS_VELOCITY_LATITUDE_NOISE_METERS]->Apply(
+ gpsVelocity.X()));
+ gpsVelocity.Y(
+ this->noises[GPS_VELOCITY_LONGITUDE_NOISE_METERS]->Apply(
+ gpsVelocity.Y()));
+ gpsVelocity.Z(
+ this->noises[GPS_VELOCITY_ALTITUDE_NOISE_METERS]->Apply(
+ gpsVelocity.Z()));
+
+ this->lastGpsMsg.set_velocity_east(gpsVelocity.X());
+ this->lastGpsMsg.set_velocity_north(gpsVelocity.Y());
+ this->lastGpsMsg.set_velocity_up(gpsVelocity.Z());
}
}
this->lastMeasurementTime = this->world->GetSimTime();
@@ -160,16 +185,28 @@ bool GpsSensor::UpdateImpl(bool /*_force*/)
//////////////////////////////////////////////////
math::Angle GpsSensor::GetLongitude() const
{
- math::Angle angle;
- angle.SetFromDegree(this->lastGpsMsg.longitude_deg());
+ return this->Longitude();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle GpsSensor::Longitude() const
+{
+ ignition::math::Angle angle;
+ angle.Degree(this->lastGpsMsg.longitude_deg());
return angle;
}
//////////////////////////////////////////////////
math::Angle GpsSensor::GetLatitude() const
{
- math::Angle angle;
- angle.SetFromDegree(this->lastGpsMsg.latitude_deg());
+ return this->Latitude();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle GpsSensor::Latitude() const
+{
+ ignition::math::Angle angle;
+ angle.Degree(this->lastGpsMsg.latitude_deg());
return angle;
}
diff --git a/gazebo/sensors/GpsSensor.hh b/gazebo/sensors/GpsSensor.hh
index 56fe25c..7700caf 100644
--- a/gazebo/sensors/GpsSensor.hh
+++ b/gazebo/sensors/GpsSensor.hh
@@ -14,9 +14,8 @@
* limitations under the License.
*
*/
-
-#ifndef _GPSSENSOR_HH_
-#define _GPSSENSOR_HH_
+#ifndef _GAZEBO_GPSSENSOR_HH_
+#define _GAZEBO_GPSSENSOR_HH_
#include <string>
@@ -64,28 +63,28 @@ namespace gazebo
/// \brief Accessor for current longitude angle
/// \return Current longitude angle.
- public: math::Angle GetLongitude() const;
+ /// \deprecated See Longitude() function that return an
+ /// ignition::math::Angle object.
+ public: math::Angle GetLongitude() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Accessor for current longitude angle
+ /// \return Current longitude angle.
+ public: ignition::math::Angle Longitude() const;
/// \brief Accessor for current latitude angle
/// \return Current latitude angle.
- public: math::Angle GetLatitude() const;
+ /// \deprecated See Latitude() function that return an
+ /// ignition::math::Angle object.
+ public: math::Angle GetLatitude() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Accessor for current latitude angle
+ /// \return Current latitude angle.
+ public: ignition::math::Angle Latitude() const;
/// \brief Accessor for current altitude
/// \return Current altitude above sea level.
public: double GetAltitude() const;
- /// \brief Noise attributes for horizontal position measurement.
- private: NoisePtr horizontalPositionNoise;
-
- /// \brief Noise attributes for vertical position measurement.
- private: NoisePtr verticalPositionNoise;
-
- /// \brief Noise attributes for horizontal velocity measurement.
- private: NoisePtr horizontalVelocityNoise;
-
- /// \brief Noise attributes for vertical velocity measurement.
- private: NoisePtr verticalVelocityNoise;
-
/// \brief GPS data publisher.
private: transport::PublisherPtr gpsPub;
diff --git a/gazebo/sensors/GpsSensor_TEST.cc b/gazebo/sensors/GpsSensor_TEST.cc
index c6a0bf4..d874f60 100644
--- a/gazebo/sensors/GpsSensor_TEST.cc
+++ b/gazebo/sensors/GpsSensor_TEST.cc
@@ -16,7 +16,7 @@
*/
#include <gtest/gtest.h>
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class GpsSensor_TEST : public ServerFixture
@@ -102,8 +102,8 @@ TEST_F(GpsSensor_TEST, CreateGps)
// Make sure the above dynamic cast worked.
EXPECT_TRUE(sensor != NULL);
- EXPECT_DOUBLE_EQ(sensor->GetLatitude().Radian(), 0.0);
- EXPECT_DOUBLE_EQ(sensor->GetLongitude().Radian(), 0.0);
+ EXPECT_DOUBLE_EQ(sensor->Latitude().Radian(), 0.0);
+ EXPECT_DOUBLE_EQ(sensor->Longitude().Radian(), 0.0);
EXPECT_DOUBLE_EQ(sensor->GetAltitude(), 0.0);
EXPECT_TRUE(sensor->IsActive());
diff --git a/gazebo/sensors/GpuRaySensor.cc b/gazebo/sensors/GpuRaySensor.cc
index 6cc16c1..c67ba99 100644
--- a/gazebo/sensors/GpuRaySensor.cc
+++ b/gazebo/sensors/GpuRaySensor.cc
@@ -14,10 +14,11 @@
* limitations under the License.
*
*/
-/* Desc: Ray proximity sensor
- * Author: Mihai Emanuel Dolha
- * Date: 29 March 2012
-*/
+#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 "gazebo/physics/World.hh"
#include "gazebo/physics/Entity.hh"
@@ -26,8 +27,6 @@
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Events.hh"
-#include "gazebo/math/Rand.hh"
-
#include "gazebo/transport/transport.hh"
#include "gazebo/rendering/Scene.hh"
@@ -106,9 +105,9 @@ void GpuRaySensor::Load(const std::string &_worldName)
// Handle noise model settings.
if (rayElem->HasElement("noise"))
{
- this->noises.push_back(
+ this->noises[GPU_RAY_NOISE] =
NoiseFactory::NewNoiseModel(rayElem->GetElement("noise"),
- this->GetType()));
+ this->GetType());
}
this->parentEntity = this->world->GetEntity(this->parentName);
@@ -160,16 +159,15 @@ void GpuRaySensor::Init()
this->laserCam->SetNearClip(this->GetRangeMin());
this->laserCam->SetFarClip(this->GetRangeMax());
- this->laserCam->SetHorzFOV(
- (this->GetAngleMax() - this->GetAngleMin()).Radian());
- this->laserCam->SetVertFOV((this->GetVerticalAngleMax()
- - this->GetVerticalAngleMin()).Radian());
+ this->laserCam->SetHorzFOV((this->AngleMax() - this->AngleMin()).Radian());
+ this->laserCam->SetVertFOV(
+ (this->VerticalAngleMax() - this->VerticalAngleMin()).Radian());
this->laserCam->SetHorzHalfAngle(
- (this->GetAngleMax() + this->GetAngleMin()).Radian() / 2.0);
+ (this->AngleMax() + this->AngleMin()).Radian() / 2.0);
- this->laserCam->SetVertHalfAngle((this->GetVerticalAngleMax()
- + this->GetVerticalAngleMin()).Radian() / 2.0);
+ this->laserCam->SetVertHalfAngle((this->VerticalAngleMax()
+ + this->VerticalAngleMin()).Radian() / 2.0);
if (this->GetHorzFOV() > 2 * M_PI)
this->laserCam->SetHorzFOV(2*M_PI);
@@ -379,6 +377,12 @@ double GpuRaySensor::GetRangeCountRatio() const
//////////////////////////////////////////////////
math::Angle GpuRaySensor::GetAngleMin() const
{
+ return this->AngleMin();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle GpuRaySensor::AngleMin() const
+{
return this->horzElem->Get<double>("min_angle");
}
@@ -391,6 +395,12 @@ void GpuRaySensor::SetAngleMin(double _angle)
//////////////////////////////////////////////////
math::Angle GpuRaySensor::GetAngleMax() const
{
+ return this->AngleMax();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle GpuRaySensor::AngleMax() const
+{
return this->horzElem->Get<double>("max_angle");
}
@@ -415,7 +425,7 @@ double GpuRaySensor::GetRangeMax() const
/////////////////////////////////////////////////
double GpuRaySensor::GetAngleResolution() const
{
- return (this->GetAngleMax() - this->GetAngleMin()).Radian() /
+ return (this->AngleMax() - this->AngleMin()).Radian() /
(this->GetRangeCount()-1);
}
@@ -466,10 +476,16 @@ int GpuRaySensor::GetVerticalRangeCount() const
//////////////////////////////////////////////////
math::Angle GpuRaySensor::GetVerticalAngleMin() const
{
+ return this->VerticalAngleMin();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle GpuRaySensor::VerticalAngleMin() const
+{
if (this->scanElem->HasElement("vertical"))
return this->vertElem->Get<double>("min_angle");
else
- return math::Angle(0);
+ return ignition::math::Angle(0);
}
//////////////////////////////////////////////////
@@ -482,16 +498,22 @@ void GpuRaySensor::SetVerticalAngleMin(double _angle)
//////////////////////////////////////////////////
math::Angle GpuRaySensor::GetVerticalAngleMax() const
{
+ return this->VerticalAngleMax();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle GpuRaySensor::VerticalAngleMax() const
+{
if (this->scanElem->HasElement("vertical"))
return this->vertElem->Get<double>("max_angle");
else
- return math::Angle(0);
+ return ignition::math::Angle(0);
}
//////////////////////////////////////////////////
double GpuRaySensor::GetVerticalAngleResolution() const
{
- return (this->GetVerticalAngleMax() - this->GetVerticalAngleMin()).Radian() /
+ return (this->VerticalAngleMax() - this->VerticalAngleMin()).Radian() /
(this->GetVerticalRangeCount()-1);
}
@@ -570,14 +592,14 @@ bool GpuRaySensor::UpdateImpl(bool /*_force*/)
// Store the latest laser scans into laserMsg
msgs::Set(scan->mutable_world_pose(),
- this->pose + this->parentEntity->GetWorldPose());
- scan->set_angle_min(this->GetAngleMin().Radian());
- scan->set_angle_max(this->GetAngleMax().Radian());
+ this->pose + this->parentEntity->GetWorldPose().Ign());
+ scan->set_angle_min(this->AngleMin().Radian());
+ scan->set_angle_max(this->AngleMax().Radian());
scan->set_angle_step(this->GetAngleResolution());
scan->set_count(this->GetRayCount());
- scan->set_vertical_angle_min(this->GetVerticalAngleMin().Radian());
- scan->set_vertical_angle_max(this->GetVerticalAngleMax().Radian());
+ scan->set_vertical_angle_min(this->VerticalAngleMin().Radian());
+ scan->set_vertical_angle_max(this->VerticalAngleMax().Radian());
scan->set_vertical_angle_step(this->GetVerticalAngleResolution());
scan->set_vertical_count(this->GetVerticalRayCount());
@@ -594,13 +616,23 @@ bool GpuRaySensor::UpdateImpl(bool /*_force*/)
int index = j * this->GetRayCount() + i;
double range = this->laserCam->GetLaserData()[index * 3];
- if (!this->noises.empty())
+ // Mask ranges outside of min/max to +/- inf, as per REP 117
+ if (range >= this->GetRangeMax())
+ {
+ range = GZ_DBL_INF;
+ }
+ else if (range <= this->GetRangeMin())
+ {
+ range = -GZ_DBL_INF;
+ }
+ else if (this->noises.find(GPU_RAY_NOISE) != this->noises.end())
{
- range = this->noises[0]->Apply(range);
- range = math::clamp(range, this->GetRangeMin(), this->GetRangeMax());
+ range = this->noises[GPU_RAY_NOISE]->Apply(range);
+ range = ignition::math::clamp(range,
+ this->GetRangeMin(), this->GetRangeMax());
}
- range = math::isnan(range) ? this->GetRangeMax() : range;
+ range = ignition::math::isnan(range) ? this->GetRangeMax() : range;
if (add)
{
diff --git a/gazebo/sensors/GpuRaySensor.hh b/gazebo/sensors/GpuRaySensor.hh
index 1cd04b8..ab8c3ad 100644
--- a/gazebo/sensors/GpuRaySensor.hh
+++ b/gazebo/sensors/GpuRaySensor.hh
@@ -26,6 +26,9 @@
#include <string>
#include <boost/thread/mutex.hpp>
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Pose3.hh>
+
#include "gazebo/math/Angle.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/transport/TransportTypes.hh"
@@ -42,6 +45,7 @@ namespace gazebo
/// \class GpuRaySensor GpuRaySensor.hh sensors/sensors.hh
/// \addtogroup gazebo_sensors
/// \{
+
/// \brief GPU based laser sensor
///
/// This sensor cast rays into the world, tests for intersections, and
@@ -84,7 +88,13 @@ namespace gazebo
/// \brief Get the minimum angle
/// \return The minimum angle
- public: math::Angle GetAngleMin() const;
+ /// \deprecated See AngleMin() function that returns an
+ /// ignition::math::Angle object.
+ public: math::Angle GetAngleMin() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the minimum angle
+ /// \return The minimum angle
+ public: ignition::math::Angle AngleMin() const;
/// \brief Set the scan minimum angle
/// \param[in] _angle The minimum angle
@@ -92,7 +102,13 @@ namespace gazebo
/// \brief Get the maximum angle
/// \return the maximum angle
- public: math::Angle GetAngleMax() const;
+ /// \deprecated See AngleMax() function that returns an
+ /// ignition::math::Angle object.
+ public: math::Angle GetAngleMax() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the maximum angle
+ /// \return the maximum angle
+ public: ignition::math::Angle AngleMax() const;
/// \brief Set the scan maximum angle
/// \param[in] _angle The maximum angle
@@ -136,7 +152,13 @@ namespace gazebo
/// \brief Get the vertical scan bottom angle
/// \return The minimum angle of the scan block
- public: math::Angle GetVerticalAngleMin() const;
+ /// \deprecated See VerticalAngleMin() function that returns an
+ /// ignition::math::Angle object.
+ public: math::Angle GetVerticalAngleMin() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the vertical scan bottom angle
+ /// \return The minimum angle of the scan block
+ public: ignition::math::Angle VerticalAngleMin() const;
/// \brief Set the vertical scan bottom angle
/// \param[in] _angle The minimum angle of the scan block
@@ -144,7 +166,13 @@ namespace gazebo
/// \brief Get the vertical scan line top angle
/// \return The Maximum angle of the scan block
- public: math::Angle GetVerticalAngleMax() const;
+ /// \deprecated See VerticalAngleMax() function that returns an
+ /// ignition::math::Angle object.
+ public: math::Angle GetVerticalAngleMax() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the vertical scan line top angle
+ /// \return The Maximum angle of the scan block
+ public: ignition::math::Angle VerticalAngleMax() const;
/// \brief Set the vertical scan line top angle
/// \param[in] _angle The Maximum angle of the scan block
@@ -162,7 +190,7 @@ namespace gazebo
/// problem by using SetActive(false) <your accessor loop>
/// SetActive(true).
/// \param[in] _index Index of specific ray
- /// \return Returns DBL_MAX for no detection.
+ /// \return Returns RangeMax for no detection.
public: double GetRange(int _index);
/// \brief Get all the ranges
diff --git a/gazebo/sensors/GpuRaySensor_TEST.cc b/gazebo/sensors/GpuRaySensor_TEST.cc
index 48718fc..41052c2 100644
--- a/gazebo/sensors/GpuRaySensor_TEST.cc
+++ b/gazebo/sensors/GpuRaySensor_TEST.cc
@@ -16,8 +16,8 @@
*/
#include <gtest/gtest.h>
-#include "gazebo/math/Angle.hh"
-#include "test/ServerFixture.hh"
+#include <ignition/math/Angle.hh>
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class GPURaySensor_TEST : public ServerFixture
@@ -52,10 +52,10 @@ TEST_F(GPURaySensor_TEST, CreateLaser)
// Make sure the above dynamic cast worked.
EXPECT_TRUE(sensor != NULL);
- double angleRes = (sensor->GetAngleMax() - sensor->GetAngleMin()).Radian() /
+ double angleRes = (sensor->AngleMax() - sensor->AngleMin()).Radian() /
sensor->GetRayCount();
- EXPECT_EQ(sensor->GetAngleMin(), math::Angle(-1.396263));
- EXPECT_EQ(sensor->GetAngleMax(), math::Angle(1.396263));
+ EXPECT_EQ(sensor->AngleMin(), ignition::math::Angle(-1.396263));
+ EXPECT_EQ(sensor->AngleMax(), ignition::math::Angle(1.396263));
EXPECT_NEAR(sensor->GetRangeMin(), 0.08, 1e-6);
EXPECT_NEAR(sensor->GetRangeMax(), 10.0, 1e-6);
EXPECT_NEAR(sensor->GetAngleResolution(), angleRes, 1e-3);
@@ -65,8 +65,8 @@ TEST_F(GPURaySensor_TEST, CreateLaser)
EXPECT_EQ(sensor->GetVerticalRayCount(), 1);
EXPECT_EQ(sensor->GetVerticalRangeCount(), 1);
- EXPECT_EQ(sensor->GetVerticalAngleMin(), 0.0);
- EXPECT_EQ(sensor->GetVerticalAngleMax(), 0.0);
+ EXPECT_EQ(sensor->VerticalAngleMin(), 0.0);
+ EXPECT_EQ(sensor->VerticalAngleMax(), 0.0);
EXPECT_TRUE(sensor->IsActive());
EXPECT_TRUE(sensor->IsHorizontal());
@@ -98,8 +98,8 @@ TEST_F(GPURaySensor_TEST, CreateLaser)
// Check that all the range values
for (unsigned int i = 0; i < ranges.size(); ++i)
{
- EXPECT_NEAR(ranges[i], sensor->GetRangeMax(), 1e-6);
- EXPECT_NEAR(sensor->GetRange(i), ranges[i], 1e-6);
+ EXPECT_DOUBLE_EQ(ranges[i], GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(sensor->GetRange(i), ranges[i]);
EXPECT_NEAR(sensor->GetRetro(i), 0, 1e-6);
EXPECT_EQ(sensor->GetFiducial(i), -1);
}
diff --git a/gazebo/sensors/ImuSensor.cc b/gazebo/sensors/ImuSensor.cc
index 1da46a4..45f86e9 100644
--- a/gazebo/sensors/ImuSensor.cc
+++ b/gazebo/sensors/ImuSensor.cc
@@ -14,11 +14,13 @@
* limitations under the License.
*
*/
-/* Desc: IMU sensor
- * Author: Matt Thompson
- * Date: 6 September 2008
-*/
+#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 <ignition/math/Rand.hh>
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Publisher.hh"
@@ -31,6 +33,7 @@
#include "gazebo/physics/World.hh"
#include "gazebo/physics/PhysicsEngine.hh"
+#include "gazebo/sensors/Noise.hh"
#include "gazebo/sensors/SensorFactory.hh"
#include "gazebo/sensors/ImuSensor.hh"
@@ -59,6 +62,7 @@ void ImuSensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf)
{
Sensor::Load(_worldName, _sdf);
+ // CASE 1 : Topic is specified in the sensor itself (should be deprecated!)
if (this->sdf->HasElement("imu") &&
this->sdf->GetElement("imu")->HasElement("topic") &&
this->sdf->GetElement("imu")->Get<std::string>("topic")
@@ -67,6 +71,7 @@ void ImuSensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf)
this->pub = this->node->Advertise<msgs::IMU>(
this->sdf->GetElement("imu")->Get<std::string>("topic"), 500);
}
+ // CASE 2 : Topic is specified in parent sensor definition
else
{
std::string topicName = "~/";
@@ -76,66 +81,203 @@ void ImuSensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf)
this->pub = this->node->Advertise<msgs::IMU>(topicName, 500);
}
- // Handle noise model settings.
- this->noiseActive = false;
+ // Get the imu element pointer
sdf::ElementPtr imuElem = this->sdf->GetElement("imu");
+
+ // CASE 1 : Noise is defined within the sensor
+ // Deprecated in Gazebo 6.0
if (imuElem->HasElement("noise"))
{
+ gzwarn << "Deprecation: IMU noise SDF value have changed. Please refer to"
+ << " http://sdformat.org/spec?ver=1.5&elem=sensor#sensor_imu\n";
+
sdf::ElementPtr noiseElem = imuElem->GetElement("noise");
std::string type = noiseElem->Get<std::string>("type");
if (type == "gaussian")
{
- this->noiseActive = true;
- this->noiseType = GAUSSIAN;
- this->rateNoiseMean = 0.0;
- this->rateNoiseStdDev = 0.0;
- this->rateBias = 0.0;
- this->accelNoiseMean = 0.0;
- this->accelNoiseStdDev = 0.0;
- this->accelBias = 0.0;
if (noiseElem->HasElement("rate"))
{
sdf::ElementPtr rateElem = noiseElem->GetElement("rate");
- this->rateNoiseMean = rateElem->Get<double>("mean");
- this->rateNoiseStdDev = rateElem->Get<double>("stddev");
- double rateBiasMean = rateElem->Get<double>("bias_mean");
- double rateBiasStddev = rateElem->Get<double>("bias_stddev");
- // Sample the bias that we'll use later
- this->rateBias = math::Rand::GetDblNormal(rateBiasMean, rateBiasStddev);
- // With equal probability, we pick a negative bias (by convention,
- // rateBiasMean should be positive, though it would work fine if
- // negative).
- if (math::Rand::GetDblUniform() < 0.5)
- this->rateBias = -this->rateBias;
- gzlog << "applying Gaussian noise model to rate with mean " <<
- this->rateNoiseMean << " and stddev " << this->rateNoiseStdDev <<
- ", bias " << this->rateBias << std::endl;
+
+ // Rename rate -> noise to enforce forward compatibility
+ rateElem->SetName("noise");
+ rateElem->AddAttribute("type", "string", "gaussian", true);
+
+ // Create the noise streams
+ this->noises[IMU_ANGVEL_X_NOISE_RADIANS_PER_S] =
+ NoiseFactory::NewNoiseModel(rateElem);
+ this->noises[IMU_ANGVEL_Y_NOISE_RADIANS_PER_S] =
+ NoiseFactory::NewNoiseModel(rateElem);
+ this->noises[IMU_ANGVEL_Z_NOISE_RADIANS_PER_S] =
+ NoiseFactory::NewNoiseModel(rateElem);
+
+ // Rename noise -> rate to enforce forward compatibility
+ rateElem->SetName("rate");
+
+ std::ostringstream out;
+ out << "Applying angular velocity noise to IMU["
+ << this->GetName() << "].\n";
+
+ out << " X: ";
+ this->noises[IMU_ANGVEL_X_NOISE_RADIANS_PER_S]->Print(out);
+ out << std::endl;
+
+ out << " Y: ";
+ this->noises[IMU_ANGVEL_Y_NOISE_RADIANS_PER_S]->Print(out);
+ out << std::endl;
+
+ out << " Z: ";
+ this->noises[IMU_ANGVEL_Z_NOISE_RADIANS_PER_S]->Print(out);
+ out << std::endl;
+
+ gzlog << out.str();
}
+
if (noiseElem->HasElement("accel"))
{
sdf::ElementPtr accelElem = noiseElem->GetElement("accel");
- this->accelNoiseMean = accelElem->Get<double>("mean");
- this->accelNoiseStdDev = accelElem->Get<double>("stddev");
- double accelBiasMean = accelElem->Get<double>("bias_mean");
- double accelBiasStddev = accelElem->Get<double>("bias_stddev");
- // Sample the bias that we'll use later
- this->accelBias = math::Rand::GetDblNormal(accelBiasMean,
- accelBiasStddev);
- // With equal probability, we pick a negative bias (by convention,
- // accelBiasMean should be positive, though it would work fine if
- // negative).
- if (math::Rand::GetDblUniform() < 0.5)
- this->accelBias = -this->accelBias;
- gzlog << "applying Gaussian noise model to accel with mean " <<
- this->accelNoiseMean << " and stddev " << this->accelNoiseStdDev <<
- ", bias " << this->accelBias << std::endl;
+
+ // Rename accel -> noise to enforce forward compatibility
+ accelElem->SetName("noise");
+ accelElem->AddAttribute("type", "string", "gaussian", true);
+
+ // Create the noise streams
+ this->noises[IMU_LINACC_X_NOISE_METERS_PER_S_SQR] =
+ NoiseFactory::NewNoiseModel(accelElem);
+ this->noises[IMU_LINACC_Y_NOISE_METERS_PER_S_SQR] =
+ NoiseFactory::NewNoiseModel(accelElem);
+ this->noises[IMU_LINACC_Z_NOISE_METERS_PER_S_SQR] =
+ NoiseFactory::NewNoiseModel(accelElem);
+
+ // Rename noise -> accel to enforce forward compatibility
+ accelElem->SetName("accel");
+
+ std::ostringstream out;
+ out << "Applying linear acceleration noise to IMU["
+ << this->GetName() << "].\n";
+
+ out << " X: ";
+ this->noises[IMU_LINACC_X_NOISE_METERS_PER_S_SQR]->Print(out);
+ out << std::endl;
+
+ out << " Y: ";
+ this->noises[IMU_LINACC_Y_NOISE_METERS_PER_S_SQR]->Print(out);
+ out << std::endl;
+
+ out << " Z: ";
+ this->noises[IMU_LINACC_Z_NOISE_METERS_PER_S_SQR]->Print(out);
+ out << std::endl;
+
+ gzlog << out.str();
}
}
else
+ {
gzwarn << "ignoring unknown noise model type \"" << type << "\"" <<
std::endl;
+ }
+ }
+ // CASE 2: noise specified using newer generic SDF noise models
+ else
+ {
+ // If an angular velocity noise models have been specified, create them
+ if (imuElem->HasElement("angular_velocity"))
+ {
+ std::ostringstream out;
+
+ out << "Applying angular velocity noise to IMU["
+ << this->GetName() << "].\n";
+
+ sdf::ElementPtr angularElem = imuElem->GetElement("angular_velocity");
+
+ if (angularElem->HasElement("x") &&
+ angularElem->GetElement("x")->HasElement("noise"))
+ {
+ this->noises[IMU_ANGVEL_X_NOISE_RADIANS_PER_S] =
+ NoiseFactory::NewNoiseModel(
+ angularElem->GetElement("x")->GetElement("noise"));
+
+ out << " X: ";
+ this->noises[IMU_ANGVEL_X_NOISE_RADIANS_PER_S]->Print(out);
+ out << std::endl;
+ }
+
+ if (angularElem->HasElement("y") &&
+ angularElem->GetElement("y")->HasElement("noise"))
+ {
+ this->noises[IMU_ANGVEL_Y_NOISE_RADIANS_PER_S] =
+ NoiseFactory::NewNoiseModel(
+ angularElem->GetElement("y")->GetElement("noise"));
+
+ out << " Y: ";
+ this->noises[IMU_ANGVEL_Y_NOISE_RADIANS_PER_S]->Print(out);
+ out << std::endl;
+ }
+
+ if (angularElem->HasElement("z") &&
+ angularElem->GetElement("z")->HasElement("noise"))
+ {
+ this->noises[IMU_ANGVEL_Z_NOISE_RADIANS_PER_S] =
+ NoiseFactory::NewNoiseModel(
+ angularElem->GetElement("z")->GetElement("noise"));
+
+ out << " Z: ";
+ this->noises[IMU_ANGVEL_Z_NOISE_RADIANS_PER_S]->Print(out);
+ out << std::endl;
+ }
+
+ gzlog << out.str();
+ }
+
+ // If linear acceleration noise models have been specified, create them
+ if (imuElem->HasElement("linear_acceleration"))
+ {
+ std::ostringstream out;
+ out << "Applying linear acceleration noise to IMU["
+ << this->GetName() << "].\n";
+
+ sdf::ElementPtr linearElem = imuElem->GetElement("linear_acceleration");
+ if (linearElem->HasElement("x") &&
+ linearElem->GetElement("x")->HasElement("noise"))
+ {
+ this->noises[IMU_LINACC_X_NOISE_METERS_PER_S_SQR] =
+ NoiseFactory::NewNoiseModel(
+ linearElem->GetElement("x")->GetElement("noise"));
+
+ out << " X: ";
+ this->noises[IMU_LINACC_X_NOISE_METERS_PER_S_SQR]->Print(out);
+ out << std::endl;
+ }
+
+ if (linearElem->HasElement("y") &&
+ linearElem->GetElement("y")->HasElement("noise"))
+ {
+ this->noises[IMU_LINACC_Y_NOISE_METERS_PER_S_SQR] =
+ NoiseFactory::NewNoiseModel(
+ linearElem->GetElement("y")->GetElement("noise"));
+
+ out << " Y: ";
+ this->noises[IMU_LINACC_Y_NOISE_METERS_PER_S_SQR]->Print(out);
+ out << std::endl;
+ }
+
+ if (linearElem->HasElement("z") &&
+ linearElem->GetElement("z")->HasElement("noise"))
+ {
+ this->noises[IMU_LINACC_Z_NOISE_METERS_PER_S_SQR] =
+ NoiseFactory::NewNoiseModel(
+ linearElem->GetElement("z")->GetElement("noise"));
+
+ out << " Z: ";
+ this->noises[IMU_LINACC_Z_NOISE_METERS_PER_S_SQR]->Print(out);
+ out << std::endl;
+ }
+
+ gzlog << out.str();
+ }
}
+ // Start publishing measurements on the topic.
this->parentEntity->SetPublishData(true);
std::string topic = "~/" + this->parentEntity->GetScopedName();
@@ -156,9 +298,9 @@ void ImuSensor::Load(const std::string &_worldName)
gzthrow("IMU has invalid parent[" + this->parentName +
"]. Must be a link\n");
}
- this->referencePose = this->pose + this->parentEntity->GetWorldPose();
- this->lastLinearVel = this->referencePose.rot.RotateVector(
- this->parentEntity->GetWorldLinearVel());
+ this->referencePose = this->pose + this->parentEntity->GetWorldPose().Ign();
+ this->lastLinearVel = this->referencePose.Rot().RotateVector(
+ this->parentEntity->GetWorldLinearVel().Ign());
}
//////////////////////////////////////////////////
@@ -194,28 +336,51 @@ void ImuSensor::OnLinkData(ConstLinkDataPtr &_msg)
//////////////////////////////////////////////////
math::Vector3 ImuSensor::GetAngularVelocity() const
{
+ return this->AngularVelocity();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d ImuSensor::AngularVelocity(const bool _noiseFree) const
+{
boost::mutex::scoped_lock lock(this->mutex);
- return msgs::Convert(this->imuMsg.angular_velocity());
+ if (_noiseFree)
+ return this->angularVel;
+ return msgs::ConvertIgn(this->imuMsg.angular_velocity());
}
//////////////////////////////////////////////////
math::Vector3 ImuSensor::GetLinearAcceleration() const
{
+ return this->LinearAcceleration();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d ImuSensor::LinearAcceleration(
+ const bool _noiseFree) const
+{
boost::mutex::scoped_lock lock(this->mutex);
- return msgs::Convert(this->imuMsg.linear_acceleration());
+ if (_noiseFree)
+ return this->linearAcc;
+ return msgs::ConvertIgn(this->imuMsg.linear_acceleration());
}
//////////////////////////////////////////////////
math::Quaternion ImuSensor::GetOrientation() const
{
+ return this->Orientation();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Quaterniond ImuSensor::Orientation() const
+{
boost::mutex::scoped_lock lock(this->mutex);
- return msgs::Convert(this->imuMsg.orientation());
+ return msgs::ConvertIgn(this->imuMsg.orientation());
}
//////////////////////////////////////////////////
void ImuSensor::SetReferencePose()
{
- this->referencePose = this->pose + this->parentEntity->GetWorldPose();
+ this->referencePose = this->pose + this->parentEntity->GetWorldPose().Ign();
}
//////////////////////////////////////////////////
@@ -243,88 +408,91 @@ bool ImuSensor::UpdateImpl(bool /*_force*/)
double dt = (timestamp - this->lastMeasurementTime).Double();
+ this->lastMeasurementTime = timestamp;
+
if (dt > 0.0)
{
boost::mutex::scoped_lock lock(this->mutex);
this->imuMsg.set_entity_name(this->parentName);
- this->gravity = this->world->GetPhysicsEngine()->GetGravity();
+ this->gravity = this->world->GetPhysicsEngine()->GetGravity().Ign();
msgs::Set(this->imuMsg.mutable_stamp(), timestamp);
- math::Pose parentEntityPose = this->parentEntity->GetWorldPose();
- math::Pose imuPose = this->pose + parentEntityPose;
+ ignition::math::Pose3d parentEntityPose =
+ this->parentEntity->GetWorldPose().Ign();
+ ignition::math::Pose3d imuPose = this->pose + parentEntityPose;
- // Set the IMU angular velocity
- math::Vector3 imuWorldAngularVel
- = msgs::Convert(msg.angular_velocity());
+ // Get the angular velocity
+ ignition::math::Vector3d imuWorldAngularVel = msgs::ConvertIgn(
+ msg.angular_velocity());
- msgs::Set(this->imuMsg.mutable_angular_velocity(),
- imuPose.rot.GetInverse().RotateVector(
- imuWorldAngularVel));
+ // Set the IMU angular velocity
+ this->angularVel = imuPose.Rot().Inverse().RotateVector(
+ imuWorldAngularVel);
+ msgs::Set(this->imuMsg.mutable_angular_velocity(), this->angularVel);
// Compute and set the IMU linear acceleration
- math::Vector3 imuWorldLinearVel
- = msgs::Convert(msg.linear_velocity());
+ ignition::math::Vector3d imuWorldLinearVel
+ = msgs::ConvertIgn(msg.linear_velocity());
// Get the correct vel for imu's that are at an offset from parent link
imuWorldLinearVel +=
- imuWorldAngularVel.Cross(parentEntityPose.pos - imuPose.pos);
- this->linearAcc = imuPose.rot.GetInverse().RotateVector(
+ imuWorldAngularVel.Cross(parentEntityPose.Pos() - imuPose.Pos());
+ this->linearAcc = imuPose.Rot().Inverse().RotateVector(
(imuWorldLinearVel - this->lastLinearVel) / dt);
// Add contribution from gravity
- this->linearAcc -= imuPose.rot.GetInverse().RotateVector(this->gravity);
+ this->linearAcc -= imuPose.Rot().Inverse().RotateVector(this->gravity);
msgs::Set(this->imuMsg.mutable_linear_acceleration(), this->linearAcc);
// Set the IMU orientation
msgs::Set(this->imuMsg.mutable_orientation(),
- (imuPose - this->referencePose).rot);
+ (imuPose - this->referencePose).Rot());
this->lastLinearVel = imuWorldLinearVel;
- this->lastMeasurementTime = timestamp;
-
- if (this->noiseActive)
+ // Apply noise models
+ for (auto const &keyNoise : this->noises)
{
- switch (this->noiseType)
+ switch (keyNoise.first)
{
- case GAUSSIAN:
- // Add Gaussian noise + fixed bias to each rate
+ case IMU_ANGVEL_X_NOISE_RADIANS_PER_S:
this->imuMsg.mutable_angular_velocity()->set_x(
- this->imuMsg.angular_velocity().x() + this->rateBias +
- math::Rand::GetDblNormal(this->rateNoiseMean,
- this->rateNoiseStdDev));
+ keyNoise.second->Apply(this->imuMsg.angular_velocity().x()));
+ break;
+ case IMU_ANGVEL_Y_NOISE_RADIANS_PER_S:
this->imuMsg.mutable_angular_velocity()->set_y(
- this->imuMsg.angular_velocity().y() + this->rateBias +
- math::Rand::GetDblNormal(this->rateNoiseMean,
- this->rateNoiseStdDev));
+ keyNoise.second->Apply(this->imuMsg.angular_velocity().y()));
+ break;
+ case IMU_ANGVEL_Z_NOISE_RADIANS_PER_S:
this->imuMsg.mutable_angular_velocity()->set_z(
- this->imuMsg.angular_velocity().z() + this->rateBias +
- math::Rand::GetDblNormal(this->rateNoiseMean,
- this->rateNoiseStdDev));
-
- // Add Gaussian noise + fixed bias to each acceleration
+ keyNoise.second->Apply(this->imuMsg.angular_velocity().z()));
+ break;
+ case IMU_LINACC_X_NOISE_METERS_PER_S_SQR:
this->imuMsg.mutable_linear_acceleration()->set_x(
- this->imuMsg.linear_acceleration().x() + this->accelBias +
- math::Rand::GetDblNormal(this->accelNoiseMean,
- this->accelNoiseStdDev));
+ keyNoise.second->Apply(this->imuMsg.linear_acceleration().x()));
+ break;
+ case IMU_LINACC_Y_NOISE_METERS_PER_S_SQR:
this->imuMsg.mutable_linear_acceleration()->set_y(
- this->imuMsg.linear_acceleration().y() + this->accelBias +
- math::Rand::GetDblNormal(this->accelNoiseMean,
- this->accelNoiseStdDev));
+ keyNoise.second->Apply(this->imuMsg.linear_acceleration().y()));
+ break;
+ case IMU_LINACC_Z_NOISE_METERS_PER_S_SQR:
this->imuMsg.mutable_linear_acceleration()->set_z(
- this->imuMsg.linear_acceleration().z() + this->accelBias +
- math::Rand::GetDblNormal(this->accelNoiseMean,
- this->accelNoiseStdDev));
-
- // TODO: add noise to orientation
+ keyNoise.second->Apply(this->imuMsg.linear_acceleration().z()));
break;
default:
- GZ_ASSERT(false, "Invalid noise model type");
+ std::ostringstream out;
+ out << "Removing unrecognized noise model: ";
+ keyNoise.second->Print(out);
+ out << std::endl;
+ gzwarn << out.str() << std::endl;
+ this->noises.erase(keyNoise.first);
+ break;
}
}
+ // Publish the message
if (this->pub)
this->pub->Publish(this->imuMsg);
}
diff --git a/gazebo/sensors/ImuSensor.hh b/gazebo/sensors/ImuSensor.hh
index b60050d..e23c3b0 100644
--- a/gazebo/sensors/ImuSensor.hh
+++ b/gazebo/sensors/ImuSensor.hh
@@ -14,12 +14,14 @@
* limitations under the License.
*
*/
-
-#ifndef _IMUSENSOR_HH_
-#define _IMUSENSOR_HH_
+#ifndef _GAZEBO_IMUSENSOR_HH_
+#define _GAZEBO_IMUSENSOR_HH_
#include <vector>
#include <string>
+#include <ignition/math/Pose3.hh>
+#include <ignition/math/Quaternion.hh>
+#include <ignition/math/Vector3.hh>
#include "gazebo/physics/PhysicsTypes.hh"
#include "gazebo/sensors/Sensor.hh"
@@ -63,16 +65,42 @@ namespace gazebo
/// \brief Returns the angular velocity.
/// \return Angular velocity.
- public: math::Vector3 GetAngularVelocity() const;
+ /// \deprecated See AngularVelocity() function that returns an
+ /// ignition::math::Vector3d object.
+ public: math::Vector3 GetAngularVelocity() const GAZEBO_DEPRECATED(6.0);
/// \brief Returns the imu linear acceleration
/// \return Linear acceleration.
- public: math::Vector3 GetLinearAcceleration() const;
+ /// \deprecated See LinearVelocity() function that returns an
+ /// ignition::math::Vector3d object.
+ public: math::Vector3 GetLinearAcceleration() const
+ GAZEBO_DEPRECATED(6.0);
/// \brief get orientation of the IMU relative to the reference pose
/// \return returns the orientation quaternion of the IMU relative to
/// the imu reference pose.
- public: math::Quaternion GetOrientation() const;
+ /// \deprecated See Orientation() function that returns an
+ /// ignition::math::Quaterniond object.
+ public: math::Quaternion GetOrientation() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Returns the angular velocity.
+ /// \param[in] _noiseFree True if the returned measurement should
+ /// not use noise.
+ /// \return Angular velocity.
+ public: ignition::math::Vector3d AngularVelocity(
+ const bool _noiseFree = false) const;
+
+ /// \brief Returns the imu linear acceleration
+ /// \param[in] _noiseFree True if the returned measurement should
+ /// not use noise.
+ /// \return Linear acceleration.
+ public: ignition::math::Vector3d LinearAcceleration(
+ const bool _noiseFree = false) const;
+
+ /// \brief get orientation of the IMU relative to the reference pose
+ /// \return returns the orientation quaternion of the IMU relative to
+ /// the imu reference pose.
+ public: ignition::math::Quaterniond Orientation() const;
/// \brief Sets the current pose as the IMU reference pose
public: void SetReferencePose();
@@ -85,16 +113,16 @@ namespace gazebo
private: void OnLinkData(ConstLinkDataPtr &_msg);
/// \brief Imu reference pose
- private: math::Pose referencePose;
+ private: ignition::math::Pose3d referencePose;
/// \brief Save previous imu linear velocity for computing acceleration.
- private: math::Vector3 lastLinearVel;
+ private: ignition::math::Vector3d lastLinearVel;
- /// \brief Imu linear acceleration
- private: math::Vector3 linearAcc;
+ /// \brief Noise free linear acceleration
+ private: ignition::math::Vector3d linearAcc;
/// \brief store gravity vector to be added to the imu output.
- private: math::Vector3 gravity;
+ private: ignition::math::Vector3d gravity;
/// \brief Imu data publisher
private: transport::PublisherPtr pub;
@@ -120,41 +148,8 @@ namespace gazebo
/// \brief True if new link data is received
private: bool dataDirty;
- /// \brief Which noise type we support
- private: enum NoiseModelType
- {
- NONE,
- GAUSSIAN
- };
-
- /// \brief If true, apply the noise model specified by other noise
- /// parameters
- private: bool noiseActive;
-
- /// \brief Which type of noise we're applying
- private: enum NoiseModelType noiseType;
-
- /// \brief If noiseType==GAUSSIAN, the mean of the distibution
- /// from which we sample when adding noise to accelerations
- private: double accelNoiseMean;
-
- /// \brief If accelNoiseType==GAUSSIAN, the standard devation of the
- /// distibution from which we sample when adding noise to accelerations
- private: double accelNoiseStdDev;
-
- /// \brief If noiseType==GAUSSIAN, the bias we'll add to acceleratations
- private: double accelBias;
-
- /// \brief If noiseType==GAUSSIAN, the mean of the distibution
- /// from which we sample when adding noise to rates
- private: double rateNoiseMean;
-
- /// \brief If noiseType==GAUSSIAN, the standard devation of the
- /// distibution from which we sample when adding noise to rates
- private: double rateNoiseStdDev;
-
- /// \brief If noiseType==GAUSSIAN, the bias we'll add to rates
- private: double rateBias;
+ /// \brief Noise free angular velocity.
+ private: ignition::math::Vector3d angularVel;
};
/// \}
}
diff --git a/gazebo/sensors/ImuSensor_TEST.cc b/gazebo/sensors/ImuSensor_TEST.cc
index 5453714..b62b462 100644
--- a/gazebo/sensors/ImuSensor_TEST.cc
+++ b/gazebo/sensors/ImuSensor_TEST.cc
@@ -18,8 +18,8 @@
#include <sys/time.h>
#include <gtest/gtest.h>
-#include "test/ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#include "gazebo/sensors/ImuSensor.hh"
#define TOL 1e-4
@@ -70,9 +70,9 @@ void ImuSensor_TEST::BasicImuSensorCheck(const std::string &_physicsEngine)
// Make sure the above dynamic cast worked.
EXPECT_TRUE(sensor != NULL);
- EXPECT_EQ(sensor->GetAngularVelocity(), math::Vector3::Zero);
- EXPECT_EQ(sensor->GetLinearAcceleration(), math::Vector3::Zero);
- EXPECT_EQ(sensor->GetOrientation(), math::Quaternion(0, 0, 0, 0));
+ EXPECT_EQ(sensor->AngularVelocity(), ignition::math::Vector3d::Zero);
+ EXPECT_EQ(sensor->LinearAcceleration(), ignition::math::Vector3d::Zero);
+ EXPECT_EQ(sensor->Orientation(), ignition::math::Quaterniond(0, 0, 0, 0));
}
/////////////////////////////////////////////////
@@ -88,6 +88,12 @@ void ImuSensor_TEST::LinearAccelerationTest(const std::string &_physicsEngine)
ASSERT_TRUE(physics != NULL);
EXPECT_EQ(physics->GetType(), _physicsEngine);
+ if (_physicsEngine == "simbody")
+ {
+ // default accuracy flunks this test, increase accuracy setting
+ physics->SetParam("accuracy", 0.0001);
+ }
+
double z = 3;
double gravityZ = physics->GetGravity().z;
double stepSize = physics->GetMaxStepSize();
@@ -110,19 +116,19 @@ void ImuSensor_TEST::LinearAccelerationTest(const std::string &_physicsEngine)
sensors::SensorManager::Instance()->Init();
imuSensor->SetActive(true);
- EXPECT_EQ(imuSensor->GetAngularVelocity(), math::Vector3::Zero);
- EXPECT_EQ(imuSensor->GetLinearAcceleration(), math::Vector3::Zero);
- EXPECT_EQ(imuSensor->GetOrientation(), math::Quaternion(0, 0, 0, 0));
+ EXPECT_EQ(imuSensor->AngularVelocity(), ignition::math::Vector3d::Zero);
+ EXPECT_EQ(imuSensor->LinearAcceleration(), ignition::math::Vector3d::Zero);
+ EXPECT_EQ(imuSensor->Orientation(), ignition::math::Quaterniond(0, 0, 0, 0));
// step world and verify imu's linear acceleration is zero on free fall
world->Step(200);
- EXPECT_NEAR(imuSensor->GetLinearAcceleration().x, 0, TOL);
- EXPECT_NEAR(imuSensor->GetLinearAcceleration().y, 0, TOL);
- EXPECT_NEAR(imuSensor->GetLinearAcceleration().z, 0, TOL);
+ EXPECT_NEAR(imuSensor->LinearAcceleration().X(), 0, TOL);
+ EXPECT_NEAR(imuSensor->LinearAcceleration().Y(), 0, TOL);
+ EXPECT_NEAR(imuSensor->LinearAcceleration().Z(), 0, TOL);
world->Step(1);
- EXPECT_NEAR(imuSensor->GetLinearAcceleration().x, 0, TOL);
- EXPECT_NEAR(imuSensor->GetLinearAcceleration().y, 0, TOL);
- EXPECT_NEAR(imuSensor->GetLinearAcceleration().z, 0, TOL);
+ EXPECT_NEAR(imuSensor->LinearAcceleration().X(), 0, TOL);
+ EXPECT_NEAR(imuSensor->LinearAcceleration().Y(), 0, TOL);
+ EXPECT_NEAR(imuSensor->LinearAcceleration().Z(), 0, TOL);
// Predict time of contact with ground plane.
double tHit = sqrt((z-0.5) / (-gravityZ));
@@ -133,9 +139,9 @@ void ImuSensor_TEST::LinearAccelerationTest(const std::string &_physicsEngine)
EXPECT_GT(steps, 0);
world->Step(steps);
- EXPECT_NEAR(imuSensor->GetLinearAcceleration().x, 0, TOL);
- EXPECT_NEAR(imuSensor->GetLinearAcceleration().y, 0, TOL);
- EXPECT_NEAR(imuSensor->GetLinearAcceleration().z, -gravityZ, 0.4);
+ EXPECT_NEAR(imuSensor->LinearAcceleration().X(), 0, TOL);
+ EXPECT_NEAR(imuSensor->LinearAcceleration().Y(), 0, TOL);
+ EXPECT_NEAR(imuSensor->LinearAcceleration().Z(), -gravityZ, 0.4);
}
/////////////////////////////////////////////////
diff --git a/gazebo/sensors/MagnetometerSensor.cc b/gazebo/sensors/MagnetometerSensor.cc
new file mode 100644
index 0000000..c1d310d
--- /dev/null
+++ b/gazebo/sensors/MagnetometerSensor.cc
@@ -0,0 +1,176 @@
+/*
+ * 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.
+ *
+*/
+
+#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 <ignition/math/Pose3.hh>
+
+#include "gazebo/transport/Node.hh"
+#include "gazebo/physics/PhysicsEngine.hh"
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/Link.hh"
+#include "gazebo/sensors/Noise.hh"
+#include "gazebo/sensors/SensorFactory.hh"
+#include "gazebo/sensors/MagnetometerSensorPrivate.hh"
+#include "gazebo/sensors/MagnetometerSensor.hh"
+
+using namespace gazebo;
+using namespace sensors;
+
+GZ_REGISTER_STATIC_SENSOR("magnetometer", MagnetometerSensor)
+
+/////////////////////////////////////////////////
+MagnetometerSensor::MagnetometerSensor()
+ : Sensor(sensors::OTHER), dataPtr(new MagnetometerSensorPrivate)
+{
+}
+
+/////////////////////////////////////////////////
+MagnetometerSensor::~MagnetometerSensor()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+void MagnetometerSensor::Load(const std::string &_worldName,
+ sdf::ElementPtr _sdf)
+{
+ Sensor::Load(_worldName, _sdf);
+}
+
+/////////////////////////////////////////////////
+std::string MagnetometerSensor::GetTopic() const
+{
+ std::string topicName = "~/" + this->parentName + '/' + this->GetName();
+ if (this->sdf->HasElement("topic"))
+ topicName += '/' + this->sdf->Get<std::string>("topic");
+ boost::replace_all(topicName, "::", "/");
+
+ return topicName;
+}
+
+/////////////////////////////////////////////////
+void MagnetometerSensor::Load(const std::string &_worldName)
+{
+ Sensor::Load(_worldName);
+
+ physics::EntityPtr parentEntity = this->world->GetEntity(this->parentName);
+ this->dataPtr->parentLink =
+ boost::dynamic_pointer_cast<physics::Link>(parentEntity);
+
+ this->dataPtr->magPub = this->node->Advertise<msgs::Magnetometer>(
+ this->GetTopic(), 50);
+
+ // Parse sdf noise parameters
+ sdf::ElementPtr magElem = this->sdf->GetElement("magnetometer");
+
+ if (!magElem)
+ {
+ gzerr << "Missing <magnetometer> element in sensor["
+ << this->GetName() << "]\n";
+ }
+ else
+ // Load magnetic field noise parameters
+ {
+ if (magElem->HasElement("x") &&
+ magElem->GetElement("x")->HasElement("noise"))
+ {
+ this->noises[MAGNETOMETER_X_NOISE_TESLA] = NoiseFactory::NewNoiseModel(
+ magElem->GetElement("x")->GetElement("noise"));
+ }
+
+ if (magElem->HasElement("y") &&
+ magElem->GetElement("y")->HasElement("noise"))
+ {
+ this->noises[MAGNETOMETER_Y_NOISE_TESLA] = NoiseFactory::NewNoiseModel(
+ magElem->GetElement("y")->GetElement("noise"));
+ }
+
+ if (magElem->HasElement("z") &&
+ magElem->GetElement("z")->HasElement("noise"))
+ {
+ this->noises[MAGNETOMETER_Z_NOISE_TESLA] = NoiseFactory::NewNoiseModel(
+ magElem->GetElement("z")->GetElement("noise"));
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void MagnetometerSensor::Fini()
+{
+ Sensor::Fini();
+ this->dataPtr->parentLink.reset();
+}
+
+//////////////////////////////////////////////////
+void MagnetometerSensor::Init()
+{
+ Sensor::Init();
+}
+
+//////////////////////////////////////////////////
+bool MagnetometerSensor::UpdateImpl(bool /*_force*/)
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+
+ // Get latest pose information
+ if (this->dataPtr->parentLink)
+ {
+ // Get pose in gazebo reference frame
+ ignition::math::Pose3d magPose =
+ this->pose + this->dataPtr->parentLink->GetWorldPose().Ign();
+
+ // Get the reference magnetic field
+ ignition::math::Vector3d field =
+ this->world->GetPhysicsEngine()->MagneticField();
+
+ // Rotate the magnetic field into the body frame
+ field = magPose.Rot().Inverse().RotateVector(field);
+
+ // Apply magnetometer noise after converting to body frame
+ if (this->noises.find(MAGNETOMETER_X_NOISE_TESLA) != this->noises.end())
+ field.X(this->noises[MAGNETOMETER_X_NOISE_TESLA]->Apply(field.X()));
+ if (this->noises.find(MAGNETOMETER_Y_NOISE_TESLA) != this->noises.end())
+ field.Y(this->noises[MAGNETOMETER_Y_NOISE_TESLA]->Apply(field.Y()));
+ if (this->noises.find(MAGNETOMETER_Z_NOISE_TESLA) != this->noises.end())
+ field.Z(this->noises[MAGNETOMETER_Z_NOISE_TESLA]->Apply(field.Z()));
+
+ // Set the body-frame magnetic field strength
+ msgs::Set(this->dataPtr->magMsg.mutable_field_tesla(), field);
+ }
+
+ // Save the time of the measurement
+ msgs::Set(this->dataPtr->magMsg.mutable_time(), this->world->GetSimTime());
+
+ // Publish the message if needed
+ if (this->dataPtr->magPub)
+ this->dataPtr->magPub->Publish(this->dataPtr->magMsg);
+
+ return true;
+}
+
+//////////////////////////////////////////////////
+ignition::math::Vector3d MagnetometerSensor::MagneticField() const
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+ return msgs::ConvertIgn(this->dataPtr->magMsg.field_tesla());
+}
diff --git a/gazebo/sensors/MagnetometerSensor.hh b/gazebo/sensors/MagnetometerSensor.hh
new file mode 100644
index 0000000..04e9b2c
--- /dev/null
+++ b/gazebo/sensors/MagnetometerSensor.hh
@@ -0,0 +1,79 @@
+/*
+ * 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_MAGNETOMETER_SENSOR_HH_
+#define _GAZEBO_MAGNETOMETER_SENSOR_HH_
+
+#include <string>
+
+#include <sdf/sdf.hh>
+#include <ignition/math/Vector3.hh>
+
+#include "gazebo/sensors/Sensor.hh"
+#include "gazebo/common/CommonTypes.hh"
+#include "gazebo/sensors/SensorTypes.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace sensors
+ {
+ // Forward declare private data
+ class MagnetometerSensorPrivate;
+
+ /// \addtogroup gazebo_sensors
+ /// \{
+
+ /// \brief MagnetometerSensor to provide magnetic field measurement.
+ class GAZEBO_VISIBLE MagnetometerSensor: public Sensor
+ {
+ /// \brief Constructor.
+ public: MagnetometerSensor();
+
+ /// \brief Destructor.
+ public: virtual ~MagnetometerSensor();
+
+ // Documentation inherited
+ public: virtual void Load(const std::string & _worldName,
+ sdf::ElementPtr _sdf);
+
+ // Documentation inherited
+ public: virtual void Load(const std::string & _worldName);
+
+ // Documentation inherited
+ public: virtual void Init();
+
+ // Documentation inherited
+ public: virtual std::string GetTopic() const;
+
+ // Documentation inherited
+ protected: virtual bool UpdateImpl(bool _force);
+
+ // Documentation inherited
+ public: virtual void Fini();
+
+ /// \brief Accessor for current magnetic field in Tesla
+ /// \return Current magnetic field
+ public: ignition::math::Vector3d MagneticField() const;
+
+ /// \brief Private data pointer.
+ private: MagnetometerSensorPrivate *dataPtr;
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/sensors/MagnetometerSensorPrivate.hh b/gazebo/sensors/MagnetometerSensorPrivate.hh
new file mode 100644
index 0000000..9db845a
--- /dev/null
+++ b/gazebo/sensors/MagnetometerSensorPrivate.hh
@@ -0,0 +1,49 @@
+/*
+ * 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_MAGNETOMETER_SENSOR_PRIVATE_HH_
+#define _GAZEBO_MAGNETOMETER_SENSOR_PRIVATE_HH_
+
+#include <mutex>
+
+#include "gazebo/transport/TransportTypes.hh"
+#include "gazebo/physics/PhysicsTypes.hh"
+#include "gazebo/msgs/msgs.hh"
+
+namespace gazebo
+{
+ namespace sensors
+ {
+ /// \internal
+ /// \brief MagnetometerSensor private data.
+ class MagnetometerSensorPrivate
+ {
+ /// \brief Mutex to protect reads and writes.
+ public: mutable std::mutex mutex;
+
+ /// \brief Magnetometer data publisher.
+ public: transport::PublisherPtr magPub;
+
+ /// \brief Parent link of this sensor.
+ public: physics::LinkPtr parentLink;
+
+ /// \brief Stores most recent magnetometer sensor data.
+ public: msgs::Magnetometer magMsg;
+ };
+ }
+}
+#endif
diff --git a/gazebo/sensors/MagnetometerSensor_TEST.cc b/gazebo/sensors/MagnetometerSensor_TEST.cc
new file mode 100644
index 0000000..1ecf828
--- /dev/null
+++ b/gazebo/sensors/MagnetometerSensor_TEST.cc
@@ -0,0 +1,150 @@
+/*
+ * 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 <ignition/math/Vector3.hh>
+
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+#include "gazebo/sensors/MagnetometerSensor.hh"
+
+#define TOL 1e-4
+
+using namespace gazebo;
+
+class MagnetometerSensor_TEST : public ServerFixture,
+ public testing::WithParamInterface<const char*>
+{
+ /// \brief Check that a model at (0,0,0,0,0,0) has mag field equal to global
+ /// \param[in] _physicsEngine The type of physics engine to use.
+ public: void BasicMagnetometerSensorCheck(const std::string &_physicsEngine);
+
+ /// \brief Rotate sensor and check magnetic field.
+ /// \param[in] _physicsEngine The type of physics engine to use.
+ public: void RotateMagnetometerSensorCheck(const std::string &_physicsEngine);
+};
+
+// A noise-free magnetic field strength sensor
+static std::string magSensorString =
+"<sdf version='1.5'>"
+" <sensor name='magnetometer' type='magnetometer'>"
+" <always_on>1</always_on>"
+" <update_rate>10.0</update_rate>"
+" <magnetometer>"
+" </magnetometer>"
+" </sensor>"
+"</sdf>";
+
+/////////////////////////////////////////////////
+void MagnetometerSensor_TEST::BasicMagnetometerSensorCheck(
+ const std::string &_physicsEngine)
+{
+ Load("worlds/empty.world", false, _physicsEngine);
+ sensors::SensorManager *mgr = sensors::SensorManager::Instance();
+ ASSERT_TRUE(mgr != NULL);
+
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ sdf::ElementPtr sdf(new sdf::Element);
+ sdf::initFile("sensor.sdf", sdf);
+ sdf::readString(magSensorString, sdf);
+
+ // Create the magnetometer sensor
+ std::string sensorName = mgr->CreateSensor(sdf, "default",
+ "ground_plane::link", 0);
+
+ // Make sure the returned sensor name is correct
+ EXPECT_EQ(sensorName,
+ std::string("default::ground_plane::link::magnetometer"));
+
+ // Update the sensor manager so that it can process new sensors.
+ mgr->Update();
+
+ // Get a pointer to the magnetometer sensor
+ sensors::MagnetometerSensorPtr sensor =
+ boost::dynamic_pointer_cast<sensors::MagnetometerSensor>
+ (mgr->GetSensor(sensorName));
+
+ // Make sure the above dynamic cast worked.
+ EXPECT_TRUE(sensor != NULL);
+
+ // At pose [0,0,0,0,0,0] the body frame magnetic field should be default
+ EXPECT_EQ(sensor->MagneticField(),
+ world->GetPhysicsEngine()->MagneticField());
+}
+
+/////////////////////////////////////////////////
+void MagnetometerSensor_TEST::RotateMagnetometerSensorCheck(
+ const std::string &_physicsEngine)
+{
+ Load("worlds/empty.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);
+
+ // Spawn a magnetometer sensor with a PI/2 aniclockwise rotation about Z axis
+ std::string modelName = "magModel";
+ std::string magSensorName = "magSensor";
+ ignition::math::Pose3d modelPose(0, 0, 0, 0, 0, IGN_PI_2);
+ std::string topic = "~/" + magSensorName + "_" + _physicsEngine;
+ SpawnUnitMagnetometerSensor(modelName, magSensorName,
+ "box", topic, modelPose.Pos(), modelPose.Rot().Euler());
+
+ sensors::SensorPtr sensor = sensors::get_sensor(magSensorName);
+ sensors::MagnetometerSensorPtr magSensor =
+ boost::dynamic_pointer_cast<sensors::MagnetometerSensor>(sensor);
+
+ ASSERT_TRUE(magSensor != NULL);
+
+ sensors::SensorManager::Instance()->Init();
+ magSensor->SetActive(true);
+
+ world->Step(10);
+
+ // Determine the magnetic field in the body frame
+ ignition::math::Vector3d field = modelPose.Rot().Inverse().RotateVector(
+ world->GetPhysicsEngine()->MagneticField());
+
+ // Check for match
+ EXPECT_EQ(magSensor->MagneticField(), field);
+}
+
+/////////////////////////////////////////////////
+TEST_P(MagnetometerSensor_TEST, BasicMagnetometerSensorCheck)
+{
+ BasicMagnetometerSensorCheck(GetParam());
+}
+
+/////////////////////////////////////////////////
+TEST_P(MagnetometerSensor_TEST, RotateMagnetometerSensorCheck)
+{
+ RotateMagnetometerSensorCheck(GetParam());
+}
+
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, MagnetometerSensor_TEST,
+ PHYSICS_ENGINE_VALUES);
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/sensors/MultiCameraSensor.cc b/gazebo/sensors/MultiCameraSensor.cc
index 64065db..be17bea 100644
--- a/gazebo/sensors/MultiCameraSensor.cc
+++ b/gazebo/sensors/MultiCameraSensor.cc
@@ -14,6 +14,13 @@
* 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 <ignition/math/Pose3.hh>
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Image.hh"
@@ -110,6 +117,9 @@ void MultiCameraSensor::Init()
}
}
+ // Each camera has its own noise pointer
+ int noiseIndex = 0;
+
// Create and initialize all the cameras
sdf::ElementPtr cameraSdf = this->sdf->GetElement("camera");
while (cameraSdf)
@@ -134,26 +144,27 @@ void MultiCameraSensor::Init()
camera->Init();
camera->CreateRenderTexture(camera->GetName() + "_RttTex");
- math::Pose cameraPose = this->pose;
+ ignition::math::Pose3d cameraPose = this->pose;
if (cameraSdf->HasElement("pose"))
- cameraPose = cameraSdf->Get<math::Pose>("pose") + cameraPose;
+ cameraPose = cameraSdf->Get<ignition::math::Pose3d>("pose") + cameraPose;
camera->SetWorldPose(cameraPose);
camera->AttachToVisual(this->parentId, true);
- // Handle noise model settings.
if (cameraSdf->HasElement("noise"))
{
- NoisePtr noise =
- NoiseFactory::NewNoiseModel(cameraSdf->GetElement("noise"),
- this->GetType());
- this->noises.push_back(noise);
- noise->SetCamera(camera);
+ // Create a noise model and attach the camera
+ this->noises[noiseIndex] = NoiseFactory::NewNoiseModel(
+ cameraSdf->GetElement("noise"), this->GetType());
+ this->noises[noiseIndex]->SetCamera(camera);
}
else
{
- this->noises.push_back(NoisePtr(new Noise(Noise::NONE)));
+ this->noises[noiseIndex] = NoisePtr(new Noise(Noise::NONE));
}
+ // Increment the noise index -- one for each camera in the setup
+ noiseIndex++;
+
{
boost::mutex::scoped_lock lock(this->cameraMutex);
this->cameras.push_back(camera);
diff --git a/gazebo/sensors/MultiCameraSensor.hh b/gazebo/sensors/MultiCameraSensor.hh
index 99d9f8f..92ccc2d 100644
--- a/gazebo/sensors/MultiCameraSensor.hh
+++ b/gazebo/sensors/MultiCameraSensor.hh
@@ -14,9 +14,8 @@
* limitations under the License.
*
*/
-
-#ifndef _MULTICAMERASENSOR_HH_
-#define _MULTICAMERASENSOR_HH_
+#ifndef _GAZEBO_MULTICAMERASENSOR_HH_
+#define _GAZEBO_MULTICAMERASENSOR_HH_
#include <string>
#include <vector>
diff --git a/gazebo/sensors/Noise.cc b/gazebo/sensors/Noise.cc
index f9b1f79..db0b767 100644
--- a/gazebo/sensors/Noise.cc
+++ b/gazebo/sensors/Noise.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/common/Assert.hh"
#include "gazebo/common/Console.hh"
@@ -140,3 +146,11 @@ void Noise::Fini()
{
this->customNoiseCallback = NULL;
}
+
+//////////////////////////////////////////////////
+void Noise::Print(std::ostream &_out) const
+{
+ _out << "Noise with type[" << this->type << "] "
+ << "does not have an overloaded Print function. "
+ << "No more information is available.";
+}
diff --git a/gazebo/sensors/Noise.hh b/gazebo/sensors/Noise.hh
index ed104c8..793f423 100644
--- a/gazebo/sensors/Noise.hh
+++ b/gazebo/sensors/Noise.hh
@@ -107,6 +107,10 @@ namespace gazebo
/// \param[in] _camera Camera associated to an image sensor
public: virtual void SetCamera(rendering::CameraPtr _camera);
+ /// \brief Output information about the noise model.
+ /// \param[in] _out Output stream
+ public: virtual void Print(std::ostream &_out) const;
+
/// \brief Which type of noise we're applying
private: NoiseType type;
diff --git a/gazebo/sensors/Noise_TEST.cc b/gazebo/sensors/Noise_TEST.cc
index baed14a..11de1bc 100644
--- a/gazebo/sensors/Noise_TEST.cc
+++ b/gazebo/sensors/Noise_TEST.cc
@@ -22,7 +22,8 @@
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
-#include "gazebo/math/Rand.hh"
+#include <ignition/math/Rand.hh>
+
#include "gazebo/sensors/Noise.hh"
#include "gazebo/sensors/GaussianNoiseModel.hh"
#include "test/util.hh"
@@ -110,7 +111,7 @@ void NoNoise(sensors::NoisePtr _noise, unsigned int _count)
// Expect no change in input value
for (unsigned int i = 0; i < _count; ++i)
{
- double x = math::Rand::GetDblUniform(-1e6, 1e6);
+ double x = ignition::math::Rand::DblUniform(-1e6, 1e6);
EXPECT_NEAR(x, _noise->Apply(x), 1e-6);
}
}
diff --git a/gazebo/sensors/RFIDSensor.cc b/gazebo/sensors/RFIDSensor.cc
index 3716fb4..343c24b 100644
--- a/gazebo/sensors/RFIDSensor.cc
+++ b/gazebo/sensors/RFIDSensor.cc
@@ -19,6 +19,12 @@
* Date: 6th December 2011
*/
+#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 "gazebo/physics/World.hh"
#include "gazebo/physics/Entity.hh"
@@ -131,7 +137,7 @@ bool RFIDSensor::UpdateImpl(bool /*_force*/)
if (this->scanPub)
{
msgs::Pose msg;
- msgs::Set(&msg, this->entity->GetWorldPose());
+ msgs::Set(&msg, this->entity->GetWorldPose().Ign());
this->scanPub->Publish(msg);
}
@@ -146,7 +152,7 @@ void RFIDSensor::EvaluateTags()
// iterate through the tags contained given rfid tag manager
for (ci = this->tags.begin(); ci != this->tags.end(); ++ci)
{
- math::Pose pos = (*ci)->GetTagPose();
+ ignition::math::Pose3d pos = (*ci)->TagPose();
// std::cout << "link: " << tagModelPtr->GetName() << std::endl;
// std::cout << "link pos: x" << pos.pos.x
// << " y:" << pos.pos.y
@@ -158,13 +164,19 @@ void RFIDSensor::EvaluateTags()
//////////////////////////////////////////////////
bool RFIDSensor::CheckTagRange(const math::Pose &_pose)
{
+ return this->CheckTagRange(_pose.Ign());
+}
+
+//////////////////////////////////////////////////
+bool RFIDSensor::CheckTagRange(const ignition::math::Pose3d &_pose)
+{
// copy sensor vector pos into a temp var
- math::Vector3 v;
- v = _pose.pos - this->entity->GetWorldPose().pos;
+ ignition::math::Vector3d v;
+ v = _pose.Pos() - this->entity->GetWorldPose().Ign().Pos();
// std::cout << v.GetLength() << std::endl;
- if (v.GetLength() <= 5.0)
+ if (v.Length() <= 5.0)
{
// std::cout << "detected " << v.GetLength() << std::endl;
return true;
@@ -179,18 +191,3 @@ void RFIDSensor::AddTag(RFIDTag *_tag)
{
this->tags.push_back(_tag);
}
-
-//////////////////////////////////////////////////
-/*bool RFIDSensor::CheckRayIntersection(const math::Pose &_pose)
-{
- math::Vector3 d;
- //calculate direction, by adding 2 vectors?
- d = _pose.pos + entity->GetWorldPose().pos;
-
- Ogre::Ray ray(rendering::Conversions::Convert(entity->GetWorldPose().pos),rendering::Conversions::Convert(d));
- query->setRay(ray);
- Ogre::RaySceneQueryResult &result = query->execute();
- return false;
- return false;
-}*/
-
diff --git a/gazebo/sensors/RFIDSensor.hh b/gazebo/sensors/RFIDSensor.hh
index cd81989..1d322be 100644
--- a/gazebo/sensors/RFIDSensor.hh
+++ b/gazebo/sensors/RFIDSensor.hh
@@ -14,13 +14,8 @@
* limitations under the License.
*
*/
-/* Desc: Gazebo RFID Sensor
- * Author: Jonas Mellin & Zakiruz Zaman
- * Date: 6th December 2011
- */
-
-#ifndef _RFIDSENSOR_HH_
-#define _RFIDSENSOR_HH_
+#ifndef _GAZEBO_RFIDSENSOR_HH_
+#define _GAZEBO_RFIDSENSOR_HH_
#include <vector>
#include <string>
@@ -81,22 +76,19 @@ namespace gazebo
/// \brief Check the range for one RFID tag.
/// \param[in] _pose Pose of a tag.
/// \return Checks if tag is in range
- private: bool CheckTagRange(const math::Pose &_pose);
+ /// \deprecated See CheckTagRange() that accepts an
+ /// ignition::math::Pose3d object.
+ private: bool CheckTagRange(const math::Pose &_pose)
+ GAZEBO_DEPRECATED(6.0);
- /// \brief Checks if ray intersects RFID sensor.
- /// \param[in] _pose Pose to compare against.
- /// \return True if intersects, false if not.
- // private: bool CheckRayIntersection(const math::Pose &_pose);
+ /// \brief Check the range for one RFID tag.
+ /// \param[in] _pose Pose of a tag.
+ /// \return Checks if tag is in range
+ private: bool CheckTagRange(const ignition::math::Pose3d &_pose);
/// \brief Parent entity
private: physics::EntityPtr entity;
- /// \brief Unused
- // private: physics::CollisionPtr laserCollision;
-
- /// \brief Unused
- // private: physics::RayShapePtr laserShape;
-
/// \brief Publisher for RFID pose messages.
private: transport::PublisherPtr scanPub;
diff --git a/gazebo/sensors/RFIDTag.cc b/gazebo/sensors/RFIDTag.cc
index 70f370d..19e708a 100644
--- a/gazebo/sensors/RFIDTag.cc
+++ b/gazebo/sensors/RFIDTag.cc
@@ -14,10 +14,11 @@
* limitations under the License.
*
*/
-/* Desc: RFID Tag
- * Author: Jonas Mellin & Zakiruz Zaman
- * Date: 6th December 2011
- */
+#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 "gazebo/common/Exception.hh"
@@ -96,7 +97,7 @@ bool RFIDTag::UpdateImpl(bool /*_force*/)
if (this->scanPub)
{
msgs::Pose msg;
- msgs::Set(&msg, entity->GetWorldPose());
+ msgs::Set(&msg, entity->GetWorldPose().Ign());
// msg.set_position(link->GetWorldPose().pos);
// msg.set_orientation(link->GetWorldPose().rot);
@@ -123,3 +124,15 @@ bool RFIDTag::UpdateImpl(bool /*_force*/)
return true;
}
+
+/////////////////////////////////////////////////
+math::Pose RFIDTag::GetTagPose() const
+{
+ return this->TagPose();
+}
+
+/////////////////////////////////////////////////
+ignition::math::Pose3d RFIDTag::TagPose() const
+{
+ return entity->GetWorldPose().Ign();
+}
diff --git a/gazebo/sensors/RFIDTag.hh b/gazebo/sensors/RFIDTag.hh
index 82cce07..dfbdf42 100644
--- a/gazebo/sensors/RFIDTag.hh
+++ b/gazebo/sensors/RFIDTag.hh
@@ -14,20 +14,17 @@
* limitations under the License.
*
*/
-/* Desc: RFID Tag
- * Author: Jonas Mellin & Zakiruz Zaman
- * Date: 6th December 2011
- */
-
-#ifndef _RFIDTAG_HH_
-#define _RFIDTAG_HH_
+#ifndef _GAZEBO_RFIDTAG_HH_
+#define _GAZEBO_RFIDTAG_HH_
#include <vector>
#include <string>
+#include <ignition/math/Pose3.hh>
+#include "gazebo/common/CommonTypes.hh"
#include "gazebo/transport/TransportTypes.hh"
#include "gazebo/sensors/Sensor.hh"
-#include "gazebo/math/gzmath.hh"
+#include "gazebo/math/Pose.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/util/system.hh"
@@ -66,8 +63,13 @@ namespace gazebo
/// \brief Returns pose of tag in world coordinate.
/// \return Pose of object.
- public: math::Pose GetTagPose() const
- {return entity->GetWorldPose();}
+ /// \deprecated See TagPose() function that returns an
+ /// ignition::math::Pose3d object.
+ public: math::Pose GetTagPose() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Returns pose of tag in world coordinate.
+ /// \return Pose of object.
+ public: ignition::math::Pose3d TagPose() const;
/// \brief Pointer the entity that has the RFID tag.
private: physics::EntityPtr entity;
diff --git a/gazebo/sensors/RaySensor.cc b/gazebo/sensors/RaySensor.cc
index 1e5a587..fef761e 100644
--- a/gazebo/sensors/RaySensor.cc
+++ b/gazebo/sensors/RaySensor.cc
@@ -14,10 +14,11 @@
* limitations under the License.
*
*/
-/* Desc: Ray proximity sensor
- * Author: Carle Cote
- * Date: 23 february 2004
-*/
+#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 "gazebo/physics/World.hh"
#include "gazebo/physics/MultiRayShape.hh"
@@ -104,9 +105,9 @@ void RaySensor::Load(const std::string &_worldName)
sdf::ElementPtr rayElem = this->sdf->GetElement("ray");
if (rayElem->HasElement("noise"))
{
- this->noises.push_back(
+ this->noises[RAY_NOISE] =
NoiseFactory::NewNoiseModel(rayElem->GetElement("noise"),
- this->GetType()));
+ this->GetType());
}
this->parentEntity = this->world->GetEntity(this->parentName);
@@ -145,8 +146,14 @@ void RaySensor::Fini()
//////////////////////////////////////////////////
math::Angle RaySensor::GetAngleMin() const
{
+ return this->AngleMin();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle RaySensor::AngleMin() const
+{
if (this->laserShape)
- return this->laserShape->GetMinAngle();
+ return this->laserShape->GetMinAngle().Ign();
else
return -1;
}
@@ -154,8 +161,14 @@ math::Angle RaySensor::GetAngleMin() const
//////////////////////////////////////////////////
math::Angle RaySensor::GetAngleMax() const
{
+ return this->AngleMax();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle RaySensor::AngleMax() const
+{
if (this->laserShape)
- return this->laserShape->GetMaxAngle();
+ return ignition::math::Angle(this->laserShape->GetMaxAngle().Radian());
else
return -1;
}
@@ -181,7 +194,7 @@ double RaySensor::GetRangeMax() const
//////////////////////////////////////////////////
double RaySensor::GetAngleResolution() const
{
- return (this->GetAngleMax() - this->GetAngleMin()).Radian() /
+ return (this->AngleMax() - this->AngleMin()).Radian() /
(this->GetRangeCount()-1);
}
@@ -237,8 +250,17 @@ int RaySensor::GetVerticalRangeCount() const
//////////////////////////////////////////////////
math::Angle RaySensor::GetVerticalAngleMin() const
{
+ return this->VerticalAngleMin();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle RaySensor::VerticalAngleMin() const
+{
if (this->laserShape)
- return this->laserShape->GetVerticalMinAngle();
+ {
+ return ignition::math::Angle(
+ this->laserShape->GetVerticalMinAngle().Radian());
+ }
else
return -1;
}
@@ -246,8 +268,17 @@ math::Angle RaySensor::GetVerticalAngleMin() const
//////////////////////////////////////////////////
math::Angle RaySensor::GetVerticalAngleMax() const
{
+ return math::Angle(this->VerticalAngleMax().Radian());
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle RaySensor::VerticalAngleMax() const
+{
if (this->laserShape)
- return this->laserShape->GetVerticalMaxAngle();
+ {
+ return ignition::math::Angle(
+ this->laserShape->GetVerticalMaxAngle().Radian());
+ }
else
return -1;
}
@@ -255,7 +286,7 @@ math::Angle RaySensor::GetVerticalAngleMax() const
//////////////////////////////////////////////////
double RaySensor::GetVerticalAngleResolution() const
{
- return (this->GetVerticalAngleMax() - this->GetVerticalAngleMin()).Radian() /
+ return (this->VerticalAngleMax() - this->VerticalAngleMin()).Radian() /
(this->GetVerticalRangeCount()-1);
}
@@ -349,14 +380,14 @@ bool RaySensor::UpdateImpl(bool /*_force*/)
// Store the latest laser scans into laserMsg
msgs::Set(scan->mutable_world_pose(),
- this->pose + this->parentEntity->GetWorldPose());
- scan->set_angle_min(this->GetAngleMin().Radian());
- scan->set_angle_max(this->GetAngleMax().Radian());
+ this->pose + this->parentEntity->GetWorldPose().Ign());
+ scan->set_angle_min(this->AngleMin().Radian());
+ scan->set_angle_max(this->AngleMax().Radian());
scan->set_angle_step(this->GetAngleResolution());
scan->set_count(this->GetRangeCount());
- scan->set_vertical_angle_min(this->GetVerticalAngleMin().Radian());
- scan->set_vertical_angle_max(this->GetVerticalAngleMax().Radian());
+ scan->set_vertical_angle_min(this->VerticalAngleMin().Radian());
+ scan->set_vertical_angle_max(this->VerticalAngleMax().Radian());
scan->set_vertical_angle_step(this->GetVerticalAngleResolution());
scan->set_vertical_count(this->GetVerticalRangeCount());
@@ -463,11 +494,21 @@ bool RaySensor::UpdateImpl(bool /*_force*/)
intensity = this->laserShape->GetRetro(j * this->GetRayCount() + i);
}
- if (!this->noises.empty())
+ // Mask ranges outside of min/max to +/- inf, as per REP 117
+ if (range >= this->GetRangeMax())
+ {
+ range = GZ_DBL_INF;
+ }
+ else if (range <= this->GetRangeMin())
+ {
+ range = -GZ_DBL_INF;
+ }
+ else if (this->noises.find(RAY_NOISE) != this->noises.end())
{
// currently supports only one noise model per laser sensor
- range = this->noises[0]->Apply(range);
- range = math::clamp(range, this->GetRangeMin(), this->GetRangeMax());
+ range = this->noises[RAY_NOISE]->Apply(range);
+ range = ignition::math::clamp(range,
+ this->GetRangeMin(), this->GetRangeMax());
}
scan->add_ranges(range);
diff --git a/gazebo/sensors/RaySensor.hh b/gazebo/sensors/RaySensor.hh
index 487717c..8fa508c 100644
--- a/gazebo/sensors/RaySensor.hh
+++ b/gazebo/sensors/RaySensor.hh
@@ -14,12 +14,15 @@
* limitations under the License.
*
*/
-#ifndef _RAYSENSOR_HH_
-#define _RAYSENSOR_HH_
+#ifndef _GAZEBO_RAYSENSOR_HH_
+#define _GAZEBO_RAYSENSOR_HH_
#include <vector>
#include <string>
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Pose3.hh>
+
#include "gazebo/math/Angle.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/transport/TransportTypes.hh"
@@ -70,11 +73,23 @@ namespace gazebo
/// \brief Get the minimum angle
/// \return The minimum angle object
- public: math::Angle GetAngleMin() const;
+ /// \deprecated See AngleMin() function that returns an
+ /// ignition::math::Angle object.
+ public: math::Angle GetAngleMin() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the minimum angle
+ /// \return The minimum angle object
+ public: ignition::math::Angle AngleMin() const;
+
+ /// \brief Get the maximum angle
+ /// \return the maximum angle object
+ /// \deprecated See AngleMax() function that returns an
+ /// ignition::math::Angle object.
+ public: math::Angle GetAngleMax() const GAZEBO_DEPRECATED(6.0);
/// \brief Get the maximum angle
/// \return the maximum angle object
- public: math::Angle GetAngleMax() const;
+ public: ignition::math::Angle AngleMax() const;
/// \brief Get the angle in radians between each range
/// \return Resolution of the angle
@@ -110,11 +125,23 @@ namespace gazebo
/// \brief Get the vertical scan bottom angle
/// \return The minimum angle of the scan block
- public: math::Angle GetVerticalAngleMin() const;
+ /// \deprecated See VerticalAngleMin() function that returns an
+ /// ignition::math::Angle object.
+ public: math::Angle GetVerticalAngleMin() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the vertical scan bottom angle
+ /// \return The minimum angle of the scan block
+ public: ignition::math::Angle VerticalAngleMin() const;
+
+ /// \brief Get the vertical scan line top angle
+ /// \return The Maximum angle of the scan block
+ /// \deprecated See VerticalAngleMax() function that returns an
+ /// ignition::math::Angle object.
+ public: math::Angle GetVerticalAngleMax() const GAZEBO_DEPRECATED(6.0);
/// \brief Get the vertical scan line top angle
/// \return The Maximum angle of the scan block
- public: math::Angle GetVerticalAngleMax() const;
+ public: ignition::math::Angle VerticalAngleMax() const;
/// \brief Get the vertical angle in radians between each range
/// \return Resolution of the angle
@@ -128,7 +155,7 @@ namespace gazebo
/// problem by using SetActive(false) <your accessor loop>
/// SetActive(true).
/// \param[in] _index Index of specific ray
- /// \return Returns DBL_MAX for no detection.
+ /// \return Returns RangeMax for no detection.
public: double GetRange(unsigned int _index);
/// \brief Get all the ranges
diff --git a/gazebo/sensors/RaySensor_TEST.cc b/gazebo/sensors/RaySensor_TEST.cc
index 7573a5d..7c4bf45 100644
--- a/gazebo/sensors/RaySensor_TEST.cc
+++ b/gazebo/sensors/RaySensor_TEST.cc
@@ -18,7 +18,7 @@
#include <gtest/gtest.h>
#include <sdf/sdf.hh>
#include "gazebo/math/Angle.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class RaySensor_TEST : public ServerFixture
@@ -107,10 +107,10 @@ TEST_F(RaySensor_TEST, CreateLaser)
// Make sure the above dynamic cast worked.
EXPECT_TRUE(sensor != NULL);
- double angleRes = (sensor->GetAngleMax() - sensor->GetAngleMin()).Radian() /
+ double angleRes = (sensor->AngleMax() - sensor->AngleMin()).Radian() /
sensor->GetRayCount();
- EXPECT_EQ(sensor->GetAngleMin(), math::Angle(-2.2689));
- EXPECT_EQ(sensor->GetAngleMax(), math::Angle(2.2689));
+ EXPECT_EQ(sensor->AngleMin(), ignition::math::Angle(-2.2689));
+ EXPECT_EQ(sensor->AngleMax(), ignition::math::Angle(2.2689));
EXPECT_NEAR(sensor->GetRangeMin(), 0.08, 1e-6);
EXPECT_NEAR(sensor->GetRangeMax(), 10.0, 1e-6);
EXPECT_NEAR(sensor->GetAngleResolution(), angleRes, 1e-3);
@@ -120,8 +120,8 @@ TEST_F(RaySensor_TEST, CreateLaser)
EXPECT_EQ(sensor->GetVerticalRayCount(), 1);
EXPECT_EQ(sensor->GetVerticalRangeCount(), 1);
- EXPECT_EQ(sensor->GetVerticalAngleMin(), 0);
- EXPECT_EQ(sensor->GetVerticalAngleMax(), 0);
+ EXPECT_EQ(sensor->VerticalAngleMin(), 0);
+ EXPECT_EQ(sensor->VerticalAngleMax(), 0);
EXPECT_TRUE(sensor->IsActive());
@@ -136,8 +136,8 @@ TEST_F(RaySensor_TEST, CreateLaser)
// Check that all the range values
for (unsigned int i = 0; i < ranges.size(); ++i)
{
- EXPECT_NEAR(ranges[i], sensor->GetRangeMax(), 1e-6);
- EXPECT_NEAR(sensor->GetRange(i), ranges[i], 1e-6);
+ EXPECT_DOUBLE_EQ(ranges[i], GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(sensor->GetRange(i), ranges[i]);
EXPECT_NEAR(sensor->GetRetro(i), 0, 1e-6);
EXPECT_EQ(sensor->GetFiducial(i), -1);
}
@@ -191,8 +191,8 @@ TEST_F(RaySensor_TEST, LaserScanResolution)
// Check that all the range values
for (unsigned int i = 0; i < ranges.size(); ++i)
{
- EXPECT_NEAR(ranges[i], sensor->GetRangeMax(), 1e-6);
- EXPECT_NEAR(sensor->GetRange(i), ranges[i], 1e-6);
+ EXPECT_DOUBLE_EQ(ranges[i], GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(sensor->GetRange(i), ranges[i]);
EXPECT_NEAR(sensor->GetRetro(i), 0, 1e-6);
EXPECT_EQ(sensor->GetFiducial(i), -1);
}
diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc
index b3ba44a..758f67d 100644
--- a/gazebo/sensors/Sensor.cc
+++ b/gazebo/sensors/Sensor.cc
@@ -14,10 +14,11 @@
* limitations under the License.
*
*/
-/* Desc: Base class for all sensors
- * Author: Nathan Koenig
- * Date: 25 May 2007
- */
+#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 <sdf/sdf.hh>
@@ -80,9 +81,7 @@ Sensor::~Sensor()
this->sdf->Reset();
this->sdf.reset();
this->connections.clear();
-
- for (unsigned int i = 0; i < this->noises.size(); ++i)
- this->noises[i].reset();
+ this->noises.clear();
}
//////////////////////////////////////////////////
@@ -97,7 +96,7 @@ void Sensor::Load(const std::string &_worldName)
{
if (this->sdf->HasElement("pose"))
{
- this->pose = this->sdf->Get<math::Pose>("pose");
+ this->pose = this->sdf->Get<ignition::math::Pose3d>("pose");
}
if (this->sdf->Get<bool>("always_on"))
@@ -230,8 +229,8 @@ void Sensor::Update(bool _force)
//////////////////////////////////////////////////
void Sensor::Fini()
{
- for (unsigned int i= 0; i < this->noises.size(); ++i)
- this->noises[i]->Fini();
+ for (auto &it : this->noises)
+ it.second->Fini();
this->active = false;
this->plugins.clear();
@@ -289,6 +288,12 @@ bool Sensor::IsActive()
//////////////////////////////////////////////////
math::Pose Sensor::GetPose() const
{
+ return this->Pose();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Pose3d Sensor::Pose() const
+{
return this->pose;
}
@@ -352,7 +357,7 @@ void Sensor::FillMsg(msgs::Sensor &_msg)
_msg.set_type(this->GetType());
_msg.set_parent(this->GetParentName());
_msg.set_parent_id(this->GetParentId());
- msgs::Set(_msg.mutable_pose(), this->GetPose());
+ msgs::Set(_msg.mutable_pose(), this->Pose());
_msg.set_visualize(this->GetVisualize());
_msg.set_topic(this->GetTopic());
@@ -394,12 +399,71 @@ SensorCategory Sensor::GetCategory() const
//////////////////////////////////////////////////
NoisePtr Sensor::GetNoise(unsigned int _index) const
{
- if (_index >= this->noises.size())
+ // By default, there is no noise
+ SensorNoiseType noiseType = NO_NOISE;
+
+ // Camera mapping
+ if (this->GetType().compare("camera") == 0)
+ {
+ noiseType = CAMERA_NOISE;
+ }
+ // GpuRay mapping
+ else if (this->GetType().compare("gpu_ray") == 0)
+ {
+ noiseType = GPU_RAY_NOISE;
+ }
+ // RaySensor mapping
+ else if (this->GetType().compare("ray") == 0)
+ {
+ noiseType = RAY_NOISE;
+ }
+ // GpsSensor mapping
+ else if (this->GetType().compare("gps") == 0)
+ {
+ switch (_index)
+ {
+ case 0:
+ noiseType = GPS_POSITION_LATITUDE_NOISE_METERS;
+ break;
+ case 1:
+ noiseType = GPS_POSITION_LONGITUDE_NOISE_METERS;
+ break;
+ case 2:
+ noiseType = GPS_POSITION_ALTITUDE_NOISE_METERS;
+ break;
+ case 3:
+ noiseType = GPS_VELOCITY_LATITUDE_NOISE_METERS;
+ break;
+ case 4:
+ noiseType = GPS_VELOCITY_LONGITUDE_NOISE_METERS;
+ break;
+ case 5:
+ noiseType = GPS_VELOCITY_ALTITUDE_NOISE_METERS;
+ break;
+ default:
+ noiseType = NO_NOISE;
+ break;
+ }
+ }
+ // Special case: unlimited number of multi-camera noise streams
+ else if (this->GetType().compare("multicamera") == 0)
+ {
+ if (this->noises.find(_index) != this->noises.end())
+ return this->noises.at(_index);
+ }
+
+ return this->GetNoise(noiseType);
+}
+
+//////////////////////////////////////////////////
+NoisePtr Sensor::GetNoise(const SensorNoiseType _type) const
+{
+ if (this->noises.find(_type) == this->noises.end())
{
- gzerr << "Get noise index out of range" << std::endl;
+ gzerr << "Get noise index not valid" << std::endl;
return NoisePtr();
}
- return this->noises[_index];
+ return this->noises.at(_type);
}
//////////////////////////////////////////////////
diff --git a/gazebo/sensors/Sensor.hh b/gazebo/sensors/Sensor.hh
index 29ef2bd..40d4588 100644
--- a/gazebo/sensors/Sensor.hh
+++ b/gazebo/sensors/Sensor.hh
@@ -14,15 +14,17 @@
* limitations under the License.
*
*/
-#ifndef _SENSOR_HH_
-#define _SENSOR_HH_
+#ifndef _GAZEBO_SENSOR_HH_
+#define _GAZEBO_SENSOR_HH_
#include <boost/enable_shared_from_this.hpp>
#include <boost/thread/mutex.hpp>
#include <vector>
+#include <map>
#include <string>
#include <sdf/sdf.hh>
+#include <ignition/math/Pose3.hh>
#include "gazebo/physics/PhysicsTypes.hh"
#include "gazebo/rendering/RenderTypes.hh"
@@ -129,7 +131,13 @@ namespace gazebo
/// \brief Get the current pose.
/// \return Current pose of the sensor.
- public: virtual math::Pose GetPose() const;
+ /// \deprecated See Pose() function that returns an
+ /// ignition::math::Pose3d object.
+ public: virtual math::Pose GetPose() const GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the current pose.
+ /// \return Current pose of the sensor.
+ public: virtual ignition::math::Pose3d Pose() const;
/// \brief Set whether the sensor is active or not.
/// \param[in] _value True if active, false if not.
@@ -200,10 +208,19 @@ namespace gazebo
public: uint32_t GetParentId() const;
/// \brief Get the sensor's noise model.
+ /// Depracted in favour of GetNoise(const SensorNoiseType _type)
+ /// which explicitly specifies the noise stream
/// \param[in] _index Index of the noise model. For most sensors this
/// will be 0. For a multi camera sensor the index can be >=0.
/// \return The sensor's noise model.
- public: NoisePtr GetNoise(unsigned int _index = 0) const;
+ public: NoisePtr GetNoise(unsigned int _index = 0) const
+ GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Get the sensor's noise model for a specified noise type.
+ /// \param[in] _type Index of the noise type. Refer to
+ /// SensorNoiseType enumeration for possible indices
+ /// \return The sensor's noise model for the given noise type
+ public: NoisePtr GetNoise(const SensorNoiseType _type) const;
/// \brief Return true if the sensor needs to be updated.
/// \return True when sensor should be updated.
@@ -220,7 +237,7 @@ namespace gazebo
protected: sdf::ElementPtr sdf;
/// \brief Pose of the sensor.
- protected: math::Pose pose;
+ protected: ignition::math::Pose3d pose;
/// \brief All event connections.
protected: std::vector<event::ConnectionPtr> connections;
@@ -258,7 +275,12 @@ namespace gazebo
protected: common::Time lastMeasurementTime;
/// \brief Noise added to sensor data
- protected: std::vector<NoisePtr> noises;
+ /// The key maps to a SensorNoiseType, and is kept as an int value
+ /// for backward compatibilty with Gazebo 5&6.
+ /// \todo: Change to std::map<SensorNoiseType, NoisePtr> in Gazebo7.
+ /// Adding the word GAZEBO_DEPRECATED here so that a grep will find
+ /// the above note.
+ protected: std::map<int, NoisePtr> noises;
/// \brief Mutex to protect resetting lastUpdateTime.
private: boost::mutex mutexLastUpdateTime;
diff --git a/gazebo/sensors/SensorFactory.cc b/gazebo/sensors/SensorFactory.cc
index 34ba77d..ba160b4 100644
--- a/gazebo/sensors/SensorFactory.cc
+++ b/gazebo/sensors/SensorFactory.cc
@@ -20,10 +20,17 @@
* Date: 18 May 2003
*/
+#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 "gazebo/sensors/SensorManager.hh"
#include "gazebo/sensors/SensorFactory.hh"
#include "gazebo/sensors/Sensor.hh"
+void RegisterAltimeterSensor();
void RegisterCameraSensor();
void RegisterContactSensor();
void RegisterDepthCameraSensor();
@@ -31,6 +38,7 @@ void RegisterMultiCameraSensor();
void RegisterGpsSensor();
void RegisterGpuRaySensor();
void RegisterImuSensor();
+void RegisterMagnetometerSensor();
void RegisterRaySensor();
void RegisterRFIDSensor();
void RegisterRFIDTag();
@@ -47,6 +55,7 @@ std::map<std::string, SensorFactoryFn> SensorFactory::sensorMap;
/////////////////////////////////////////////////
void SensorFactory::RegisterAll()
{
+ RegisterAltimeterSensor();
RegisterCameraSensor();
RegisterContactSensor();
RegisterDepthCameraSensor();
@@ -54,6 +63,7 @@ void SensorFactory::RegisterAll()
RegisterGpsSensor();
RegisterGpuRaySensor();
RegisterImuSensor();
+ RegisterMagnetometerSensor();
RegisterRaySensor();
RegisterRFIDSensor();
RegisterRFIDTag();
diff --git a/gazebo/sensors/SensorManager.cc b/gazebo/sensors/SensorManager.cc
index c6dc95b..6f64beb 100644
--- a/gazebo/sensors/SensorManager.cc
+++ b/gazebo/sensors/SensorManager.cc
@@ -14,11 +14,12 @@
* limitations under the License.
*
*/
-/*
- * Desc: Class to manager all sensors
- * Author: Nate Koenig
- * Date: 18 Dec 2009
- */
+#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 "gazebo/common/Assert.hh"
#include "gazebo/common/Time.hh"
diff --git a/gazebo/sensors/SensorManager.hh b/gazebo/sensors/SensorManager.hh
index 606b0d6..097fe29 100644
--- a/gazebo/sensors/SensorManager.hh
+++ b/gazebo/sensors/SensorManager.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _SENSORMANAGER_HH_
-#define _SENSORMANAGER_HH_
+#ifndef _GAZEBO_SENSORMANAGER_HH_
+#define _GAZEBO_SENSORMANAGER_HH_
#include <boost/thread.hpp>
#include <string>
diff --git a/gazebo/sensors/SensorManager_TEST.cc b/gazebo/sensors/SensorManager_TEST.cc
index 404df68..b850154 100644
--- a/gazebo/sensors/SensorManager_TEST.cc
+++ b/gazebo/sensors/SensorManager_TEST.cc
@@ -18,7 +18,7 @@
#include <gtest/gtest.h>
#include "gazebo/physics/PhysicsIface.hh"
#include "gazebo/common/Time.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class SensorManager_TEST : public ServerFixture
diff --git a/gazebo/sensors/SensorTypes.hh b/gazebo/sensors/SensorTypes.hh
index 9b30689..dd83564 100644
--- a/gazebo/sensors/SensorTypes.hh
+++ b/gazebo/sensors/SensorTypes.hh
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#ifndef _SENSORTYPES_HH_
-#define _SENSORTYPES_HH_
+#ifndef _GAZEBO_SENSORTYPES_HH_
+#define _GAZEBO_SENSORTYPES_HH_
#include <vector>
#include <boost/shared_ptr.hpp>
@@ -30,9 +30,11 @@ namespace gazebo
/// \brief Sensors namespace
namespace sensors
{
+ class AltimeterSensor;
class Sensor;
class RaySensor;
class CameraSensor;
+ class MagnetometerSensor;
class MultiCameraSensor;
class DepthCameraSensor;
class ContactSensor;
@@ -50,6 +52,10 @@ namespace gazebo
class WirelessTransmitter;
class WirelessReceiver;
+ /// \def AltimeterSensorPtr
+ /// \brief Shared pointer to AltimeterSensor
+ typedef boost::shared_ptr<AltimeterSensor> AltimeterSensorPtr;
+
/// \def SensorPtr
/// \brief Shared pointer to Sensor
typedef boost::shared_ptr<Sensor> SensorPtr;
@@ -62,6 +68,10 @@ namespace gazebo
/// \brief Shared pointer to CameraSensor
typedef boost::shared_ptr<CameraSensor> CameraSensorPtr;
+ /// \def MagnetometerSensorPtr
+ /// \brief Shared pointer to MagnetometerSensor
+ typedef boost::shared_ptr<MagnetometerSensor> MagnetometerSensorPtr;
+
/// \def MultiCameraSensorPtr
/// \brief Shared pointer to MultiCameraSensor
typedef boost::shared_ptr<MultiCameraSensor> MultiCameraSensorPtr;
@@ -126,6 +136,10 @@ namespace gazebo
/// \brief Shared pointer to WirelessReceiver
typedef boost::shared_ptr<WirelessReceiver> WirelessReceiverPtr;
+ /// \def AltimeterSensor_V
+ /// \brief Vector of AltimeterSensor shared pointers
+ typedef std::vector<AltimeterSensor> AltimeterSensor_V;
+
/// \def Sensor_V
/// \brief Vector of Sensor shared pointers
typedef std::vector<SensorPtr> Sensor_V;
@@ -178,7 +192,94 @@ namespace gazebo
/// \brief Vector of WirelessReceiver
typedef std::vector<WirelessReceiver> WirelessReceiver_V;
-
+ /// \def SensorNoiseType
+ /// \brief Eumeration of all sensor noise types
+ enum SensorNoiseType
+ {
+ /// \brief Noise streams for the Camera sensor
+ /// \sa CameraSensor
+ NO_NOISE = 0,
+
+ /// \brief Noise streams for the Camera sensor
+ /// \sa CameraSensor
+ CAMERA_NOISE = 1,
+
+ /// \brief Noise streams for the GPU ray sensor
+ /// \sa GpuRaySensor
+ GPU_RAY_NOISE = 2,
+
+ /// \brief GPS position latitude noise streams
+ /// \sa GpsSensor
+ GPS_POSITION_LATITUDE_NOISE_METERS = 3,
+
+ /// \brief GPS position longitude noise streams
+ /// \sa GpsSensor
+ GPS_POSITION_LONGITUDE_NOISE_METERS = 4,
+
+ /// \brief GPS position altitude noise streams
+ /// \sa GpsSensor
+ GPS_POSITION_ALTITUDE_NOISE_METERS = 5,
+
+ /// \brief GPS velocity latitude noise streams
+ /// \sa GpsSensor
+ GPS_VELOCITY_LATITUDE_NOISE_METERS = 6,
+
+ /// \brief GPS velocity longitude noise streams
+ /// \sa GpsSensor
+ GPS_VELOCITY_LONGITUDE_NOISE_METERS = 7,
+
+ /// \brief GPS velocity altitude noise streams
+ /// \sa GpsSensor
+ GPS_VELOCITY_ALTITUDE_NOISE_METERS = 8,
+
+ /// \brief Noise streams for the ray sensor
+ /// \sa RaySensor
+ RAY_NOISE = 9,
+
+ /// \brief Magnetometer body-frame X axis noise in Tesla
+ /// \sa MagnetometerSensor
+ MAGNETOMETER_X_NOISE_TESLA = 10,
+
+ /// \brief Magnetometer body-frame Y axis noise in Tesla
+ /// \sa MagnetometerSensor
+ MAGNETOMETER_Y_NOISE_TESLA = 11,
+
+ /// \brief Magnetometer body-frame Z axis noise in Tesla
+ /// \sa MagnetometerSensor
+ MAGNETOMETER_Z_NOISE_TESLA = 12,
+
+ /// \brief Vertical noise stream for the altimeter sensor
+ /// \sa AltimeterSensor
+ ALTIMETER_POSITION_NOISE_METERS = 13,
+
+ /// \brief Velocity noise streams for the altimeter sensor
+ /// \sa AltimeterSensor
+ ALTIMETER_VELOCITY_NOISE_METERS_PER_S = 14,
+
+ /// \brief IMU angular velocity X noise stream
+ /// \sa ImuSensor
+ IMU_ANGVEL_X_NOISE_RADIANS_PER_S = 15,
+
+ /// \brief IMU angular velocity Y noise stream
+ /// \sa ImuSensor
+ IMU_ANGVEL_Y_NOISE_RADIANS_PER_S = 16,
+
+ /// \brief IMU angular velocity Z noise stream
+ /// \sa ImuSensor
+ IMU_ANGVEL_Z_NOISE_RADIANS_PER_S = 17,
+
+ /// \brief IMU linear acceleration X noise stream
+ /// \sa ImuSensor
+ IMU_LINACC_X_NOISE_METERS_PER_S_SQR = 18,
+
+ /// \brief IMU linear acceleration Y noise stream
+ /// \sa ImuSensor
+ IMU_LINACC_Y_NOISE_METERS_PER_S_SQR = 19,
+
+ /// \brief IMU linear acceleration Z noise stream
+ /// \sa ImuSensor
+ IMU_LINACC_Z_NOISE_METERS_PER_S_SQR = 20
+ };
/// \}
}
}
diff --git a/gazebo/sensors/Sensor_TEST.cc b/gazebo/sensors/Sensor_TEST.cc
index 888fe0a..d8488ec 100644
--- a/gazebo/sensors/Sensor_TEST.cc
+++ b/gazebo/sensors/Sensor_TEST.cc
@@ -18,14 +18,15 @@
#include <gtest/gtest.h>
#include "gazebo/physics/PhysicsIface.hh"
#include "gazebo/common/Time.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class Sensor_TEST : public ServerFixture
{
};
-boost::condition_variable g_countCondition;
+boost::condition_variable g_hokuyoCountCondition;
+boost::condition_variable g_imuCountCondition;
// global variable and callback for tracking hokuyo sensor messages
unsigned int g_hokuyoMsgCount;
@@ -33,7 +34,16 @@ void ReceiveHokuyoMsg(ConstLaserScanStampedPtr &/*_msg*/)
{
g_hokuyoMsgCount++;
if (g_hokuyoMsgCount >= 20)
- g_countCondition.notify_one();
+ g_hokuyoCountCondition.notify_one();
+}
+
+// global variable and callback for tracking imu sensor messages
+unsigned int g_imuMsgCount;
+void ReceiveImuMsg(ConstLaserScanStampedPtr &/*_msg*/)
+{
+ g_imuMsgCount++;
+ if (g_imuMsgCount >= 20)
+ g_imuCountCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -47,7 +57,7 @@ TEST_F(Sensor_TEST, UpdateAfterReset)
ASSERT_TRUE(world != NULL);
unsigned int i;
- double updateRate, now, then;
+ double updateRateHokuyo, updateRateImu, now, then;
// get the sensor manager
sensors::SensorManager *mgr = sensors::SensorManager::Instance();
@@ -58,50 +68,74 @@ TEST_F(Sensor_TEST, UpdateAfterReset)
sensor = mgr->GetSensor("default::hokuyo::link::laser");
ASSERT_TRUE(sensor != NULL);
+ sensors::SensorPtr imuSensor;
+ imuSensor = mgr->GetSensor("default::box_model::box_link::box_imu_sensor");
+ ASSERT_TRUE(imuSensor != NULL);
+
// set update rate to 30 Hz
- updateRate = 30.0;
- sensor->SetUpdateRate(updateRate);
+ updateRateHokuyo = 30.0;
+ updateRateImu = 1000.0;
+ sensor->SetUpdateRate(updateRateHokuyo);
+ imuSensor->SetUpdateRate(updateRateImu);
gzdbg << sensor->GetScopedName() << " loaded with update rate of "
- << sensor->GetUpdateRate() << " Hz\n";
+ << sensor->GetUpdateRate() << " Hz"
+ << std::endl;
+ gzdbg << imuSensor->GetScopedName() << " loaded with update rate of "
+ << imuSensor->GetUpdateRate() << " Hz"
+ << std::endl;
g_hokuyoMsgCount = 0;
+ g_imuMsgCount = 0;
- // Subscribe to hokuyo laser scan messages
+ // Subscribe to sensor messages
transport::NodePtr node = transport::NodePtr(new transport::Node());
node->Init();
- transport::SubscriberPtr sceneSub = node->Subscribe(
+ transport::SubscriberPtr laserSub = node->Subscribe(
"~/hokuyo/link/laser/scan", &ReceiveHokuyoMsg);
+ transport::SubscriberPtr imuSub = node->Subscribe(
+ "~/box_model/box_link/box_imu_sensor/imu", &ReceiveImuMsg);
// Wait for messages to arrive
{
boost::mutex countMutex;
boost::mutex::scoped_lock lock(countMutex);
- g_countCondition.wait(lock);
+ g_hokuyoCountCondition.wait(lock);
+ g_imuCountCondition.wait(lock);
}
unsigned int hokuyoMsgCount = g_hokuyoMsgCount;
+ unsigned int imuMsgCount = g_imuMsgCount;
now = world->GetSimTime().Double();
- gzdbg << "counted " << hokuyoMsgCount << " messages in "
+ gzdbg << "counted " << hokuyoMsgCount << " hokuyo messages in "
+ << now << " seconds\n";
+ gzdbg << "counted " << imuMsgCount << " imu messages in "
<< now << " seconds\n";
// Expect at least 50% of specified update rate
EXPECT_GT(static_cast<double>(hokuyoMsgCount),
- updateRate*now * 0.5);
+ updateRateHokuyo*now * 0.5);
+ EXPECT_GT(static_cast<double>(imuMsgCount),
+ updateRateImu*now * 0.5);
// Wait another 1.5 seconds
for (i = 0; i < 15; ++i)
common::Time::MSleep(100);
hokuyoMsgCount = g_hokuyoMsgCount;
+ imuMsgCount = g_imuMsgCount;
now = world->GetSimTime().Double();
- gzdbg << "counted " << hokuyoMsgCount << " messages in "
+ gzdbg << "counted " << hokuyoMsgCount << " hokuyo messages in "
+ << now << " seconds\n";
+ gzdbg << "counted " << imuMsgCount << " imu messages in "
<< now << " seconds\n";
// Expect at least 50% of specified update rate
EXPECT_GT(static_cast<double>(hokuyoMsgCount),
- updateRate*now * 0.5);
+ updateRateHokuyo*now * 0.5);
+ EXPECT_GT(static_cast<double>(imuMsgCount),
+ updateRateImu*now * 0.5);
// Send reset world message
transport::PublisherPtr worldControlPub =
@@ -120,35 +154,47 @@ TEST_F(Sensor_TEST, UpdateAfterReset)
// Count messages again for 2 second
g_hokuyoMsgCount = 0;
+ g_imuMsgCount = 0;
for (i = 0; i < 20; ++i)
{
common::Time::MSleep(100);
}
hokuyoMsgCount = g_hokuyoMsgCount;
+ imuMsgCount = g_imuMsgCount;
now = world->GetSimTime().Double() - now;
- gzdbg << "counted " << hokuyoMsgCount << " messages in "
- << now << " seconds. Expected[" << updateRate * now * 0.5 << "]\n";
+ gzdbg << "counted " << hokuyoMsgCount << " hokuyo messages in "
+ << now << " seconds\n";
+ gzdbg << "counted " << imuMsgCount << " imu messages in "
+ << now << " seconds\n";
// Expect at least 50% of specified update rate
// Note: this is where the failure documented in issue #236 occurs
EXPECT_GT(static_cast<double>(hokuyoMsgCount),
- updateRate*now * 0.5);
+ updateRateHokuyo*now * 0.5);
+ EXPECT_GT(static_cast<double>(imuMsgCount),
+ updateRateImu*now * 0.5);
// Count messages again for 2 more seconds
then = now;
g_hokuyoMsgCount = 0;
+ g_imuMsgCount = 0;
for (i = 0; i < 20; ++i)
{
common::Time::MSleep(100);
}
hokuyoMsgCount = g_hokuyoMsgCount;
+ imuMsgCount = g_imuMsgCount;
now = world->GetSimTime().Double();
- gzdbg << "counted " << hokuyoMsgCount << " messages in "
- << now - then << " seconds\n";
+ gzdbg << "counted " << hokuyoMsgCount << " hokuyo messages in "
+ << now << " seconds\n";
+ gzdbg << "counted " << imuMsgCount << " imu messages in "
+ << now << " seconds\n";
// Expect at least 50% of specified update rate
EXPECT_GT(static_cast<double>(hokuyoMsgCount),
- updateRate*(now-then) * 0.5);
+ updateRateHokuyo*(now-then) * 0.5);
+ EXPECT_GT(static_cast<double>(imuMsgCount),
+ updateRateImu*(now-then) * 0.5);
}
/////////////////////////////////////////////////
diff --git a/gazebo/sensors/SensorsIface.cc b/gazebo/sensors/SensorsIface.cc
index 470d31f..1e6f854 100644
--- a/gazebo/sensors/SensorsIface.cc
+++ b/gazebo/sensors/SensorsIface.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/common/Console.hh"
#include "gazebo/transport/TransportIface.hh"
diff --git a/gazebo/sensors/SonarSensor.cc b/gazebo/sensors/SonarSensor.cc
index 079633b..55bc911 100644
--- a/gazebo/sensors/SonarSensor.cc
+++ b/gazebo/sensors/SonarSensor.cc
@@ -15,6 +15,14 @@
*
*/
+#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 <ignition/math/Vector3.hh>
+
#include "gazebo/physics/World.hh"
#include "gazebo/physics/SurfaceParams.hh"
#include "gazebo/physics/MeshShape.hh"
@@ -28,9 +36,6 @@
#include "gazebo/transport/Publisher.hh"
#include "gazebo/msgs/msgs.hh"
-#include "gazebo/math/Vector3.hh"
-#include "gazebo/math/Rand.hh"
-
#include "gazebo/sensors/SensorFactory.hh"
#include "gazebo/sensors/SonarSensor.hh"
@@ -139,14 +144,14 @@ void SonarSensor::Load(const std::string &_worldName)
// Use a scaled cone mesh for the sonar collision shape.
this->sonarShape->SetMesh("unit_cone");
- this->sonarShape->SetScale(math::Vector3(this->radius*2.0,
+ this->sonarShape->SetScale(ignition::math::Vector3d(this->radius*2.0,
this->radius*2.0, range));
// Position the collision shape properly. Without this, the shape will be
// centered at the start of the sonar.
- math::Vector3 offset(0, 0, range * 0.5);
- offset = this->pose.rot.RotateVector(offset);
- this->sonarMidPose.Set(this->pose.pos - offset, this->pose.rot);
+ ignition::math::Vector3d offset(0, 0, range * 0.5);
+ offset = this->pose.Rot().RotateVector(offset);
+ this->sonarMidPose.Set(this->pose.Pos() - offset, this->pose.Rot());
this->sonarCollision->SetRelativePose(this->sonarMidPose);
this->sonarCollision->SetInitialRelativePose(this->sonarMidPose);
@@ -247,8 +252,9 @@ bool SonarSensor::UpdateImpl(bool /*_force*/)
this->lastMeasurementTime = this->world->GetSimTime();
msgs::Set(this->sonarMsg.mutable_time(), this->lastMeasurementTime);
- math::Pose referencePose = this->pose + this->parentEntity->GetWorldPose();
- math::Vector3 pos;
+ ignition::math::Pose3d referencePose =
+ this->pose + this->parentEntity->GetWorldPose().Ign();
+ ignition::math::Vector3d pos;
if (!this->incomingContacts.empty())
this->sonarMsg.mutable_sonar()->set_range(this->rangeMax);
@@ -266,9 +272,9 @@ bool SonarSensor::UpdateImpl(bool /*_force*/)
for (int j = 0; j < (*iter)->contact(i).position_size(); ++j)
{
- pos = msgs::Convert((*iter)->contact(i).position(j));
- math::Vector3 relPos = pos - referencePose.pos;
- double len = pos.Distance(referencePose.pos);
+ pos = msgs::ConvertIgn((*iter)->contact(i).position(j));
+ ignition::math::Vector3d relPos = pos - referencePose.Pos();
+ double len = pos.Distance(referencePose.Pos());
// Debug output:
// std::cout << " SP[" << this->pose.pos << "] P[" << pos
diff --git a/gazebo/sensors/SonarSensor.hh b/gazebo/sensors/SonarSensor.hh
index d1c709c..e631d2d 100644
--- a/gazebo/sensors/SonarSensor.hh
+++ b/gazebo/sensors/SonarSensor.hh
@@ -14,15 +14,13 @@
* limitations under the License.
*
*/
-
-#ifndef _SONARSENSOR_HH_
-#define _SONARSENSOR_HH_
+#ifndef _GAZEBO_SONARSENSOR_HH_
+#define _GAZEBO_SONARSENSOR_HH_
#include <string>
#include <list>
+#include <ignition/math/Pose3.hh>
-#include "gazebo/math/Angle.hh"
-#include "gazebo/math/Pose.hh"
#include "gazebo/transport/TransportTypes.hh"
#include "gazebo/sensors/Sensor.hh"
#include "gazebo/util/system.hh"
@@ -131,7 +129,7 @@ namespace gazebo
private: ContactMsgs_L incomingContacts;
/// \brief Pose of the sonar shape's midpoint.
- private: math::Pose sonarMidPose;
+ private: ignition::math::Pose3d sonarMidPose;
/// \brief Minimum range
private: double rangeMin;
diff --git a/gazebo/sensors/SonarSensor_TEST.cc b/gazebo/sensors/SonarSensor_TEST.cc
index 57884da..147f050 100644
--- a/gazebo/sensors/SonarSensor_TEST.cc
+++ b/gazebo/sensors/SonarSensor_TEST.cc
@@ -16,8 +16,8 @@
*/
#include <gtest/gtest.h>
-#include "test/ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
diff --git a/gazebo/sensors/WirelessReceiver.cc b/gazebo/sensors/WirelessReceiver.cc
index 9e965be..0814cf6 100644
--- a/gazebo/sensors/WirelessReceiver.cc
+++ b/gazebo/sensors/WirelessReceiver.cc
@@ -14,8 +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 <ignition/math/Pose3.hh>
-#include "gazebo/math/Rand.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/sensors/SensorFactory.hh"
#include "gazebo/sensors/SensorManager.hh"
@@ -99,9 +105,9 @@ bool WirelessReceiver::UpdateImpl(bool /*_force*/)
double txFreq;
this->referencePose =
- this->pose + this->parentEntity.lock()->GetWorldPose();
+ this->pose + this->parentEntity.lock()->GetWorldPose().Ign();
- math::Pose myPos = this->referencePose;
+ ignition::math::Pose3d myPos = this->referencePose;
Sensor_V sensors = SensorManager::Instance()->GetSensors();
for (Sensor_V::iterator it = sensors.begin(); it != sensors.end(); ++it)
{
@@ -111,7 +117,7 @@ bool WirelessReceiver::UpdateImpl(bool /*_force*/)
boost::static_pointer_cast<WirelessTransmitter>(*it);
txFreq = transmitter->GetFreq();
- rxPower = transmitter->GetSignalStrength(myPos, this->GetGain());
+ rxPower = transmitter->SignalStrength(myPos, this->GetGain());
// Discard if the frequency received is out of our frequency range,
// or if the received signal strengh is lower than the sensivity
diff --git a/gazebo/sensors/WirelessReceiver_TEST.cc b/gazebo/sensors/WirelessReceiver_TEST.cc
index fd9c526..cdfd76f 100644
--- a/gazebo/sensors/WirelessReceiver_TEST.cc
+++ b/gazebo/sensors/WirelessReceiver_TEST.cc
@@ -18,7 +18,7 @@
#include <gtest/gtest.h>
#include <boost/algorithm/string/find.hpp>
#include <boost/regex.hpp>
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class WirelessReceiver_TEST : public ServerFixture
diff --git a/gazebo/sensors/WirelessTransceiver.cc b/gazebo/sensors/WirelessTransceiver.cc
index 128cff3..f929ae5 100644
--- a/gazebo/sensors/WirelessTransceiver.cc
+++ b/gazebo/sensors/WirelessTransceiver.cc
@@ -14,9 +14,13 @@
* 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 <sstream>
-#include "gazebo/math/Rand.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/sensors/SensorFactory.hh"
#include "gazebo/sensors/SensorManager.hh"
@@ -62,7 +66,8 @@ void WirelessTransceiver::Load(const std::string &_worldName)
GZ_ASSERT(this->parentEntity.lock() != NULL, "parentEntity is NULL");
- this->referencePose = this->pose + this->parentEntity.lock()->GetWorldPose();
+ this->referencePose = this->pose +
+ this->parentEntity.lock()->GetWorldPose().Ign();
if (!this->sdf->HasElement("transceiver"))
{
diff --git a/gazebo/sensors/WirelessTransceiver.hh b/gazebo/sensors/WirelessTransceiver.hh
index 93081b6..24d67ef 100644
--- a/gazebo/sensors/WirelessTransceiver.hh
+++ b/gazebo/sensors/WirelessTransceiver.hh
@@ -14,11 +14,12 @@
* limitations under the License.
*
*/
-
-#ifndef _WIRELESS_TRANSCEIVER_HH_
-#define _WIRELESS_TRANSCEIVER_HH_
+#ifndef _GAZEBO_WIRELESS_TRANSCEIVER_HH_
+#define _GAZEBO_WIRELESS_TRANSCEIVER_HH_
#include <string>
+#include <ignition/math/Pose3.hh>
+
#include "gazebo/physics/PhysicsTypes.hh"
#include "gazebo/sensors/Sensor.hh"
#include "gazebo/transport/TransportTypes.hh"
@@ -74,7 +75,7 @@ namespace gazebo
protected: boost::weak_ptr<physics::Link> parentEntity;
/// \brief Sensor reference pose
- protected: math::Pose referencePose;
+ protected: ignition::math::Pose3d referencePose;
};
/// \}
}
diff --git a/gazebo/sensors/WirelessTransmitter.cc b/gazebo/sensors/WirelessTransmitter.cc
index 1093351..fdf0332 100644
--- a/gazebo/sensors/WirelessTransmitter.cc
+++ b/gazebo/sensors/WirelessTransmitter.cc
@@ -14,8 +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 <ignition/math/Rand.hh>
-#include "gazebo/math/Rand.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/sensors/SensorFactory.hh"
@@ -90,13 +96,13 @@ void WirelessTransmitter::Init()
bool WirelessTransmitter::UpdateImpl(bool /*_force*/)
{
this->referencePose =
- this->pose + this->parentEntity.lock()->GetWorldPose();
+ this->pose + this->parentEntity.lock()->GetWorldPose().Ign();
if (this->visualize)
{
msgs::PropagationGrid msg;
- math::Pose pos;
- math::Pose worldPose;
+ ignition::math::Pose3d pos;
+ ignition::math::Pose3d worldPose;
double strength;
msgs::PropagationParticle *p;
@@ -110,11 +116,12 @@ bool WirelessTransmitter::UpdateImpl(bool /*_force*/)
worldPose = pos + this->referencePose;
- if (this->referencePose.pos.Distance(worldPose.pos) <= this->MaxRadius)
+ if (this->referencePose.Pos().Distance(worldPose.Pos()) <=
+ this->MaxRadius)
{
// For the propagation model assume the receiver antenna has the same
// gain as the transmitter
- strength = this->GetSignalStrength(worldPose, this->GetGain());
+ strength = this->SignalStrength(worldPose, this->GetGain());
// Add a new particle to the grid
p = msg.add_particle();
@@ -143,19 +150,28 @@ double WirelessTransmitter::GetFreq() const
}
/////////////////////////////////////////////////
-double WirelessTransmitter::GetSignalStrength(const math::Pose &_receiver,
- const double rxGain)
+double WirelessTransmitter::GetSignalStrength(
+ const math::Pose &_receiver,
+ const double _rxGain)
+{
+ return this->SignalStrength(_receiver.Ign(), _rxGain);
+}
+
+/////////////////////////////////////////////////
+double WirelessTransmitter::SignalStrength(
+ const ignition::math::Pose3d &_receiver,
+ const double _rxGain)
{
std::string entityName;
double dist;
- math::Vector3 end = _receiver.pos;
- math::Vector3 start = this->referencePose.pos;
+ ignition::math::Vector3d end = _receiver.Pos();
+ ignition::math::Vector3d start = this->referencePose.Pos();
// Avoid computing the intersection of coincident points
// This prevents an assertion in bullet (issue #849)
if (start == end)
{
- end.z += 0.00001;
+ end.Z() += 0.00001;
}
// Acquire the mutex for avoiding race condition with the physics engine
@@ -176,12 +192,12 @@ double WirelessTransmitter::GetSignalStrength(const math::Pose &_receiver,
}
double distance = std::max(1.0,
- this->referencePose.pos.Distance(_receiver.pos));
- double x = abs(math::Rand::GetDblNormal(0.0, ModelStdDesv));
+ this->referencePose.Pos().Distance(_receiver.Pos()));
+ double x = std::abs(ignition::math::Rand::DblNormal(0.0, ModelStdDesv));
double wavelength = common::SpeedOfLight / (this->GetFreq() * 1000000);
// Hata-Okumara propagation model
- double rxPower = this->GetPower() + this->GetGain() + rxGain - x +
+ double rxPower = this->GetPower() + this->GetGain() + _rxGain - x +
20 * log10(wavelength) - 20 * log10(4 * M_PI) - 10 * n * log10(distance);
return rxPower;
diff --git a/gazebo/sensors/WirelessTransmitter.hh b/gazebo/sensors/WirelessTransmitter.hh
index f365ae5..4a1c164 100644
--- a/gazebo/sensors/WirelessTransmitter.hh
+++ b/gazebo/sensors/WirelessTransmitter.hh
@@ -14,9 +14,8 @@
* limitations under the License.
*
*/
-
-#ifndef _WIRELESS_TRANSMITTER_HH_
-#define _WIRELESS_TRANSMITTER_HH_
+#ifndef _GAZEBO_WIRELESS_TRANSMITTER_HH_
+#define _GAZEBO_WIRELESS_TRANSMITTER_HH_
#include <string>
#include "gazebo/physics/physics.hh"
@@ -71,9 +70,20 @@ namespace gazebo
public: double GetFreq() const;
/// \brief Returns the signal strength in a given world's point (dBm).
+ /// \param[in] _receiver Pose of the receiver
+ /// \param[in] _rxGain Receiver gain value
/// \return Signal strength in a world's point (dBm).
+ /// \deprecated See SignalStrength() function that accepts an
+ /// ignition::math::Pose3d object.
public: double GetSignalStrength(const math::Pose &_receiver,
- const double rxGain);
+ const double _rxGain) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Returns the signal strength in a given world's point (dBm).
+ /// \param[in] _receiver Pose of the receiver
+ /// \param[in] _rxGain Receiver gain value
+ /// \return Signal strength in a world's point (dBm).
+ public: double SignalStrength(const ignition::math::Pose3d &_receiver,
+ const double _rxGain);
/// \brief Size of the grid used for visualization.
private: static const double Step;
diff --git a/gazebo/sensors/WirelessTransmitter_TEST.cc b/gazebo/sensors/WirelessTransmitter_TEST.cc
index 2728edb..62b91c2 100644
--- a/gazebo/sensors/WirelessTransmitter_TEST.cc
+++ b/gazebo/sensors/WirelessTransmitter_TEST.cc
@@ -16,7 +16,7 @@
*/
#include <gtest/gtest.h>
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class WirelessTransmitter_TEST : public ServerFixture
@@ -63,11 +63,13 @@ WirelessTransmitter_TEST::WirelessTransmitter_TEST()
double freq = 2442.0;
double power = 14.5;
double gain = 2.6;
- math::Pose txPose(math::Vector3(0.0, 0.0, 0.055), math::Quaternion(0, 0, 0));
+ ignition::math::Pose3d txPose(
+ ignition::math::Vector3d(0.0, 0.0, 0.055),
+ ignition::math::Quaterniond(0, 0, 0));
// Spawn a wireless transmitter with sensor visualization
- SpawnWirelessTransmitterSensor(txModelName, txSensorName, txPose.pos,
- txPose.rot.GetAsEuler(), txEssid, freq, power, gain);
+ SpawnWirelessTransmitterSensor(txModelName, txSensorName, txPose.Pos(),
+ txPose.Rot().Euler(), txEssid, freq, power, gain);
this->tx = boost::static_pointer_cast<sensors::WirelessTransmitter>(
sensors::SensorManager::Instance()->GetSensor(txSensorName));
@@ -139,15 +141,18 @@ void WirelessTransmitter_TEST::TestSignalStrength()
{
int samples = 100;
double signStrengthAvg = 0.0;
- math::Pose txPose(math::Vector3(3.0, 3.0, 0.055), math::Quaternion(0, 0, 0));
- math::Pose txPoseOccluded(math::Vector3(-3.0, -3.0, 0.055),
- math::Quaternion(0, 0, 0));
+ ignition::math::Pose3d txPose(
+ ignition::math::Vector3d(3.0, 3.0, 0.055),
+ ignition::math::Quaterniond(0, 0, 0));
+ ignition::math::Pose3d txPoseOccluded(
+ ignition::math::Vector3d(-3.0, -3.0, 0.055),
+ ignition::math::Quaterniond(0, 0, 0));
// Take some samples and get the average signal strength
for (int i = 0; i < samples; ++i)
{
this->tx->Update(true);
- signStrengthAvg += tx->GetSignalStrength(txPose, tx->GetGain());
+ signStrengthAvg += this->tx->SignalStrength(txPose, tx->GetGain());
}
signStrengthAvg /= samples;
@@ -199,11 +204,13 @@ void WirelessTransmitter_TEST::TestUpdateImplNoVisual()
double freq = 2442.0;
double power = 14.5;
double gain = 2.6;
- math::Pose txPose(math::Vector3(3.0, 3.0, 0.055), math::Quaternion(0, 0, 0));
+ ignition::math::Pose3d txPose(
+ ignition::math::Vector3d(3.0, 3.0, 0.055),
+ ignition::math::Quaterniond(0, 0, 0));
// Spawn a wireless transmitter without sensor visualization
SpawnWirelessTransmitterSensor(txModelName + "NoVisual",
- txNoVisualSensorName, txPose.pos, txPose.rot.GetAsEuler(),
+ txNoVisualSensorName, txPose.Pos(), txPose.Rot().Euler(),
txEssid + "NoVisual", freq, power, gain, false);
txNoVisual = boost::static_pointer_cast<sensors::WirelessTransmitter>(
diff --git a/gazebo/test/CMakeLists.txt b/gazebo/test/CMakeLists.txt
new file mode 100644
index 0000000..0f71e8b
--- /dev/null
+++ b/gazebo/test/CMakeLists.txt
@@ -0,0 +1,10 @@
+# Build ServerFixture library
+add_library(gazebo_test_fixture STATIC ServerFixture.cc ServerFixtureRecord.cc)
+target_link_libraries(gazebo_test_fixture libgazebo)
+
+gz_install_library(gazebo_test_fixture)
+gz_install_includes("test"
+ helper_physics_generator.hh
+ ServerFixture.hh
+ ${PROJECT_BINARY_DIR}/test_config.h
+)
diff --git a/gazebo/test/ServerFixture.cc b/gazebo/test/ServerFixture.cc
new file mode 100644
index 0000000..7855978
--- /dev/null
+++ b/gazebo/test/ServerFixture.cc
@@ -0,0 +1,1528 @@
+/*
+ * Copyright (C) 2012-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.
+ *
+*/
+#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 <stdio.h>
+#include <string>
+#include <cmath>
+
+#include "gazebo/gazebo.hh"
+#include "ServerFixture.hh"
+
+using namespace gazebo;
+
+/////////////////////////////////////////////////
+std::string gazebo::custom_exec(std::string _cmd)
+{
+ _cmd += " 2>/dev/null";
+
+#ifdef _WIN32
+ FILE *pipe = _popen(_cmd.c_str(), "r");
+#else
+ FILE *pipe = popen(_cmd.c_str(), "r");
+#endif
+
+ if (!pipe)
+ return "ERROR";
+
+ char buffer[128];
+ std::string result = "";
+
+ while (!feof(pipe))
+ {
+ if (fgets(buffer, 128, pipe) != NULL)
+ result += buffer;
+ }
+
+#ifdef _WIN32
+ _pclose(pipe);
+#else
+ pclose(pipe);
+#endif
+
+ return result;
+}
+
+/////////////////////////////////////////////////
+ServerFixture::ServerFixture()
+{
+ this->server = NULL;
+ this->serverRunning = false;
+ this->paused = false;
+ this->percentRealTime = 0;
+ this->gotImage = 0;
+ this->imgData = NULL;
+ this->serverThread = NULL;
+ this->uniqueCounter = 0;
+
+ gzLogInit("test-", "test.log");
+ gazebo::common::Console::SetQuiet(false);
+ common::SystemPaths::Instance()->AddGazeboPaths(
+ TEST_INTEGRATION_PATH);
+
+ // Add local search paths
+ boost::filesystem::path path;
+
+ path = PROJECT_SOURCE_PATH;
+ gazebo::common::SystemPaths::Instance()->AddGazeboPaths(path.string());
+
+ path = PROJECT_SOURCE_PATH;
+ path /= "gazebo";
+ gazebo::common::SystemPaths::Instance()->AddGazeboPaths(path.string());
+
+ path = PROJECT_BINARY_PATH;
+ path /= "plugins";
+ gazebo::common::SystemPaths::Instance()->AddPluginPaths(path.string());
+
+ path = PROJECT_BINARY_PATH;
+ path /= "test";
+ path /= "plugins";
+ gazebo::common::SystemPaths::Instance()->AddPluginPaths(path.string());
+
+ path = TEST_PATH;
+ gazebo::common::SystemPaths::Instance()->AddGazeboPaths(path.string());
+}
+
+/////////////////////////////////////////////////
+ServerFixture::~ServerFixture()
+{
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::TearDown()
+{
+ this->Unload();
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::Unload()
+{
+ gzdbg << "ServerFixture::Unload" << std::endl;
+ this->serverRunning = false;
+ if (this->node)
+ this->node->Fini();
+
+ if (this->server)
+ {
+ this->server->Stop();
+
+ if (this->serverThread)
+ {
+ this->serverThread->join();
+ }
+ }
+
+ delete this->serverThread;
+ this->serverThread = NULL;
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::Load(const std::string &_worldFilename)
+{
+ this->Load(_worldFilename, false);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::Load(const std::string &_worldFilename, bool _paused)
+{
+ std::string s("");
+ this->Load(_worldFilename, _paused, s);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::Load(const std::string &_worldFilename,
+ bool _paused, const std::string &_physics,
+ const std::vector<std::string> &_systemPlugins)
+{
+ delete this->server;
+ this->server = NULL;
+
+ // Create, load, and run the server in its own thread
+ this->serverThread = new boost::thread(
+ boost::bind(&ServerFixture::RunServer, this, _worldFilename,
+ _paused, _physics, _systemPlugins));
+
+ // Wait for the server to come up
+ // Use a 60 second timeout.
+ int waitCount = 0, maxWaitCount = 6000;
+ while ((!this->server || !this->server->GetInitialized()) &&
+ ++waitCount < maxWaitCount)
+ common::Time::MSleep(100);
+ gzdbg << "ServerFixture load in "
+ << static_cast<double>(waitCount)/10.0
+ << " seconds, timeout after "
+ << static_cast<double>(maxWaitCount)/10.0
+ << " seconds\n";
+
+ if (waitCount >= maxWaitCount)
+ this->launchTimeoutFailure(
+ "while waiting for Load() function", waitCount);
+
+ this->node = transport::NodePtr(new transport::Node());
+ ASSERT_NO_THROW(this->node->Init());
+ this->poseSub = this->node->Subscribe("~/pose/local/info",
+ &ServerFixture::OnPose, this, true);
+ this->statsSub = this->node->Subscribe("~/world_stats",
+ &ServerFixture::OnStats, this);
+
+ this->factoryPub =
+ this->node->Advertise<msgs::Factory>("~/factory");
+
+ this->requestPub =
+ this->node->Advertise<msgs::Request>("~/request");
+
+ // Wait for the world to reach the correct pause state.
+ // This might not work properly with multiple worlds.
+ // Use a 30 second timeout.
+ waitCount = 0;
+ maxWaitCount = 3000;
+ while ((!physics::get_world() ||
+ physics::get_world()->IsPaused() != _paused) &&
+ ++waitCount < maxWaitCount)
+ common::Time::MSleep(100);
+ ASSERT_LT(waitCount, maxWaitCount);
+
+ this->factoryPub->WaitForConnection();
+ this->requestPub->WaitForConnection();
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::RunServer(const std::string &_worldFilename)
+{
+ this->RunServer(_worldFilename, false, "");
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::RunServer(const std::string &_worldFilename, bool _paused,
+ const std::string &_physics,
+ const std::vector<std::string> &_systemPlugins)
+{
+ ASSERT_NO_THROW(this->server = new Server());
+
+ for (auto const &plugin : _systemPlugins)
+ {
+ gazebo::addPlugin(plugin);
+ }
+
+ this->server->PreLoad();
+
+ if (_physics.length())
+ ASSERT_NO_THROW(this->server->LoadFile(_worldFilename,
+ _physics));
+ else
+ ASSERT_NO_THROW(this->server->LoadFile(_worldFilename));
+
+ if (!rendering::get_scene(
+ gazebo::physics::get_world()->GetName()))
+ {
+ ASSERT_NO_THROW(rendering::create_scene(
+ gazebo::physics::get_world()->GetName(), false, true));
+ }
+
+ ASSERT_NO_THROW(this->SetPause(_paused));
+
+ ASSERT_NO_THROW(this->server->Run());
+
+ ASSERT_NO_THROW(this->server->Fini());
+
+ ASSERT_NO_THROW(delete this->server);
+ this->server = NULL;
+}
+
+/////////////////////////////////////////////////
+rendering::ScenePtr ServerFixture::GetScene(
+ const std::string &_sceneName)
+{
+ // Wait for the scene to get loaded.
+ int i = 0;
+ int timeoutDS = 20;
+ while (rendering::get_scene(_sceneName) == NULL && i < timeoutDS)
+ {
+ common::Time::MSleep(100);
+ ++i;
+ }
+
+ if (i >= timeoutDS)
+ {
+ gzerr << "Unable to load the rendering scene.\n"
+ << "Test will fail";
+ this->launchTimeoutFailure(
+ "while waiting to load rendering scene", i);
+ }
+
+ return rendering::get_scene(_sceneName);
+}
+
+
+/////////////////////////////////////////////////
+void ServerFixture::OnStats(ConstWorldStatisticsPtr &_msg)
+{
+ this->simTime = msgs::Convert(_msg->sim_time());
+ this->realTime = msgs::Convert(_msg->real_time());
+ this->pauseTime = msgs::Convert(_msg->pause_time());
+ this->paused = _msg->paused();
+
+ if (this->realTime == 0)
+ this->percentRealTime = 0;
+ else
+ this->percentRealTime =
+ (this->simTime / this->realTime).Double();
+
+ this->serverRunning = true;
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SetPause(bool _pause)
+{
+ physics::pause_worlds(_pause);
+}
+
+/////////////////////////////////////////////////
+double ServerFixture::GetPercentRealTime() const
+{
+ while (!this->serverRunning)
+ common::Time::MSleep(100);
+
+ return this->percentRealTime;
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::OnPose(ConstPosesStampedPtr &_msg)
+{
+ boost::mutex::scoped_lock lock(this->receiveMutex);
+ for (int i = 0; i < _msg->pose_size(); ++i)
+ {
+ this->poses[_msg->pose(i).name()] = msgs::ConvertIgn(_msg->pose(i));
+ }
+}
+
+/////////////////////////////////////////////////
+math::Pose ServerFixture::GetEntityPose(const std::string &_name)
+{
+ boost::mutex::scoped_lock lock(this->receiveMutex);
+
+ std::map<std::string, math::Pose>::iterator iter;
+ iter = this->poses.find(_name);
+ EXPECT_TRUE(iter != this->poses.end());
+ return iter->second;
+}
+
+/////////////////////////////////////////////////
+bool ServerFixture::HasEntity(const std::string &_name)
+{
+ boost::mutex::scoped_lock lock(this->receiveMutex);
+ std::map<std::string, math::Pose>::iterator iter;
+ iter = this->poses.find(_name);
+ return iter != this->poses.end();
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::PrintImage(const std::string &_name, unsigned char **_image,
+ unsigned int _width, unsigned int _height, unsigned int _depth)
+{
+ unsigned int count = _height * _width * _depth;
+ printf("\n");
+ printf("static unsigned char __%s[] = {", _name.c_str());
+ unsigned int i;
+ for (i = 0; i < count-1; i++)
+ {
+ if (i % 10 == 0)
+ printf("\n");
+ else
+ printf(" ");
+ printf("%d,", (*_image)[i]);
+ }
+ printf(" %d};\n", (*_image)[i]);
+ printf("static unsigned char *%s = __%s;\n", _name.c_str(),
+ _name.c_str());
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::PrintScan(const std::string &_name, double *_scan,
+ unsigned int _sampleCount)
+{
+ printf("static double __%s[] = {\n", _name.c_str());
+ for (unsigned int i = 0; i < _sampleCount-1; ++i)
+ {
+ if ((i+1) % 5 == 0)
+ printf("%13.10f,\n", math::precision(_scan[i], 10));
+ else
+ printf("%13.10f, ", math::precision(_scan[i], 10));
+ }
+ printf("%13.10f};\n",
+ math::precision(_scan[_sampleCount-1], 10));
+ printf("static double *%s = __%s;\n", _name.c_str(),
+ _name.c_str());
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::FloatCompare(float *_scanA, float *_scanB,
+ unsigned int _sampleCount, float &_diffMax,
+ float &_diffSum, float &_diffAvg)
+{
+ _diffMax = 0;
+ _diffSum = 0;
+ _diffAvg = 0;
+ for (unsigned int i = 0; i < _sampleCount; ++i)
+ {
+ double diff = fabs(math::precision(_scanA[i], 10) -
+ math::precision(_scanB[i], 10));
+ _diffSum += diff;
+ if (diff > _diffMax)
+ {
+ _diffMax = diff;
+ }
+ }
+ _diffAvg = _diffSum / _sampleCount;
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::DoubleCompare(double *_scanA, double *_scanB,
+ unsigned int _sampleCount, double &_diffMax,
+ double &_diffSum, double &_diffAvg)
+{
+ _diffMax = 0;
+ _diffSum = 0;
+ _diffAvg = 0;
+ for (unsigned int i = 0; i < _sampleCount; ++i)
+ {
+ double diff;
+
+ // set diff = 0 if both values are same-sign infinite, as inf - inf = nan
+ if (std::isinf(_scanA[i]) && std::isinf(_scanB[i]) &&
+ _scanA[i] * _scanB[i] > 0)
+ {
+ diff = 0;
+ }
+ else
+ {
+ diff = fabs(math::precision(_scanA[i], 10) -
+ math::precision(_scanB[i], 10));
+ }
+
+ _diffSum += diff;
+ if (diff > _diffMax)
+ {
+ _diffMax = diff;
+ }
+ }
+ _diffAvg = _diffSum / _sampleCount;
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::ImageCompare(unsigned char *_imageA,
+ unsigned char *_imageB,
+ unsigned int _width, unsigned int _height, unsigned int _depth,
+ unsigned int &_diffMax, unsigned int &_diffSum,
+ double &_diffAvg)
+{
+ _diffMax = 0;
+ _diffSum = 0;
+
+ for (unsigned int y = 0; y < _height; ++y)
+ {
+ for (unsigned int x = 0; x < _width*_depth; ++x)
+ {
+ int a = _imageA[(y*_width*_depth)+x];
+ int b = _imageB[(y*_width*_depth)+x];
+
+ unsigned int absDiff = abs(a - b);
+
+ if (absDiff > _diffMax)
+ _diffMax = absDiff;
+
+ _diffSum += absDiff;
+ }
+ }
+ _diffAvg = _diffSum / (_height*_width*_depth);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::OnNewFrame(const unsigned char *_image,
+ unsigned int _width, unsigned int _height,
+ unsigned int _depth,
+ const std::string &/*_format*/)
+{
+ memcpy(*this->imgData, _image, _width * _height * _depth);
+ this->gotImage+= 1;
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::GetFrame(const std::string &_cameraName,
+ unsigned char **_imgData, unsigned int &_width,
+ unsigned int &_height)
+{
+ sensors::SensorPtr sensor = sensors::get_sensor(_cameraName);
+ EXPECT_TRUE(sensor != NULL);
+ sensors::CameraSensorPtr camSensor =
+ boost::dynamic_pointer_cast<sensors::CameraSensor>(sensor);
+
+ _width = camSensor->GetImageWidth();
+ _height = camSensor->GetImageHeight();
+
+ if (*_imgData)
+ {
+ delete *_imgData;
+ *_imgData = NULL;
+ }
+ (*_imgData) = new unsigned char[_width *_height*3];
+ this->imgData = _imgData;
+
+ this->gotImage = 0;
+ event::ConnectionPtr c =
+ camSensor->GetCamera()->ConnectNewImageFrame(
+ boost::bind(&ServerFixture::OnNewFrame,
+ this, _1, _2, _3, _4, _5));
+
+ while (this->gotImage < 20)
+ common::Time::MSleep(100);
+
+ camSensor->GetCamera()->DisconnectNewImageFrame(c);
+}
+
+/////////////////////////////////////////////////
+physics::ModelPtr ServerFixture::SpawnModel(const msgs::Model &_msg)
+{
+ physics::WorldPtr world = physics::get_world();
+ ServerFixture::CheckPointer(world);
+ world->InsertModelString(
+ "<sdf version='" + std::string(SDF_VERSION) + "'>"
+ + msgs::ModelToSDF(_msg)->ToString("")
+ + "</sdf>");
+
+ common::Time wait(10, 0);
+ common::Time wallStart = common::Time::GetWallTime();
+ unsigned int waitCount = 0;
+ while (wait > (common::Time::GetWallTime() - wallStart) &&
+ !this->HasEntity(_msg.name()))
+ {
+ common::Time::MSleep(10);
+ if (++waitCount % 100 == 0)
+ {
+ gzwarn << "Waiting " << waitCount / 100 << " seconds for "
+ << "box to spawn." << std::endl;
+ }
+ }
+
+ return world->GetModel(_msg.name());
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnCamera(const std::string &_modelName,
+ const std::string &_cameraName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ unsigned int _width, unsigned int _height, double _rate,
+ const std::string &_noiseType, double _noiseMean, double _noiseStdDev,
+ bool _distortion, double _distortionK1, double _distortionK2,
+ double _distortionK3, double _distortionP1, double _distortionP2,
+ double _cx, double _cy)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _modelName << "'>"
+ << "<static>true</static>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<link name ='body'>"
+ << " <sensor name ='" << _cameraName
+ << "' type ='camera'>"
+ << " <always_on>1</always_on>"
+ << " <update_rate>" << _rate << "</update_rate>"
+ << " <visualize>true</visualize>"
+ << " <camera>"
+ << " <horizontal_fov>0.78539816339744828</horizontal_fov>"
+ << " <image>"
+ << " <width>" << _width << "</width>"
+ << " <height>" << _height << "</height>"
+ << " <format>R8G8B8</format>"
+ << " </image>"
+ << " <clip>"
+ << " <near>0.1</near><far>100</far>"
+ << " </clip>";
+ // << " <save enabled ='true' path ='/tmp/camera/'/>"
+
+ if (_noiseType.size() > 0)
+ newModelStr << " <noise>"
+ << " <type>" << _noiseType << "</type>"
+ << " <mean>" << _noiseMean << "</mean>"
+ << " <stddev>" << _noiseStdDev << "</stddev>"
+ << " </noise>";
+
+ if (_distortion)
+ newModelStr << " <distortion>"
+ << " <k1>" << _distortionK1 << "</k1>"
+ << " <k2>" << _distortionK2 << "</k2>"
+ << " <k3>" << _distortionK3 << "</k3>"
+ << " <p1>" << _distortionP1 << "</p1>"
+ << " <p2>" << _distortionP2 << "</p2>"
+ << " <center>" << _cx << " " << _cy << "</center>"
+ << " </distortion>";
+
+ newModelStr << " </camera>"
+ << " </sensor>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ WaitUntilEntitySpawn(_modelName, 100, 50);
+ WaitUntilSensorSpawn(_cameraName, 100, 100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnRaySensor(const std::string &_modelName,
+ const std::string &_raySensorName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ double _hMinAngle, double _hMaxAngle,
+ double _vMinAngle, double _vMaxAngle,
+ double _minRange, double _maxRange,
+ double _rangeResolution, unsigned int _samples,
+ unsigned int _vSamples, double _hResolution,
+ double _vResolution,
+ const std::string &_noiseType, double _noiseMean,
+ double _noiseStdDev)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _modelName << "'>"
+ << "<static>true</static>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<link name ='body'>"
+ << "<collision name='parent_collision'>"
+ << " <pose>0 0 0.0205 0 0 0</pose>"
+ << " <geometry>"
+ << " <cylinder>"
+ << " <radius>0.021</radius>"
+ << " <length>0.029</length>"
+ << " </cylinder>"
+ << " </geometry>"
+ << "</collision>"
+ << " <sensor name ='" << _raySensorName << "' type ='ray'>"
+ << " <ray>"
+ << " <scan>"
+ << " <horizontal>"
+ << " <samples>" << _samples << "</samples>"
+ << " <resolution>" << _hResolution << "</resolution>"
+ << " <min_angle>" << _hMinAngle << "</min_angle>"
+ << " <max_angle>" << _hMaxAngle << "</max_angle>"
+ << " </horizontal>"
+ << " <vertical>"
+ << " <samples>" << _vSamples << "</samples>"
+ << " <resolution>" << _vResolution << "</resolution>"
+ << " <min_angle>" << _vMinAngle << "</min_angle>"
+ << " <max_angle>" << _vMaxAngle << "</max_angle>"
+ << " </vertical>"
+ << " </scan>"
+ << " <range>"
+ << " <min>" << _minRange << "</min>"
+ << " <max>" << _maxRange << "</max>"
+ << " <resolution>" << _rangeResolution <<"</resolution>"
+ << " </range>";
+
+ if (_noiseType.size() > 0)
+ newModelStr << " <noise>"
+ << " <type>" << _noiseType << "</type>"
+ << " <mean>" << _noiseMean << "</mean>"
+ << " <stddev>" << _noiseStdDev << "</stddev>"
+ << " </noise>";
+
+ newModelStr << " </ray>"
+ << " <visualize>true</visualize>"
+ << " </sensor>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ WaitUntilEntitySpawn(_modelName, 100, 100);
+ WaitUntilSensorSpawn(_raySensorName, 100, 100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnGpuRaySensor(const std::string &_modelName,
+ const std::string &_raySensorName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ double _hMinAngle, double _hMaxAngle,
+ double _minRange, double _maxRange,
+ double _rangeResolution, unsigned int _samples,
+ const std::string &_noiseType, double _noiseMean,
+ double _noiseStdDev)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _modelName << "'>"
+ << "<static>true</static>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<link name ='body'>"
+ << "<collision name='parent_collision'>"
+ << " <pose>0 0 0.0205 0 0 0</pose>"
+ << " <geometry>"
+ << " <cylinder>"
+ << " <radius>0.021</radius>"
+ << " <length>0.029</length>"
+ << " </cylinder>"
+ << " </geometry>"
+ << "</collision>"
+ << " <sensor name ='" << _raySensorName
+ << "' type ='gpu_ray'>"
+ << " <ray>"
+ << " <scan>"
+ << " <horizontal>"
+ << " <samples>" << _samples << "</samples>"
+ << " <resolution> 1 </resolution>"
+ << " <min_angle>" << _hMinAngle << "</min_angle>"
+ << " <max_angle>" << _hMaxAngle << "</max_angle>"
+ << " </horizontal>"
+ << " </scan>"
+ << " <range>"
+ << " <min>" << _minRange << "</min>"
+ << " <max>" << _maxRange << "</max>"
+ << " <resolution>" << _rangeResolution <<"</resolution>"
+ << " </range>";
+
+ if (_noiseType.size() > 0)
+ newModelStr << " <noise>"
+ << " <type>" << _noiseType << "</type>"
+ << " <mean>" << _noiseMean << "</mean>"
+ << " <stddev>" << _noiseStdDev << "</stddev>"
+ << " </noise>";
+
+ newModelStr << " </ray>"
+ << " </sensor>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ WaitUntilEntitySpawn(_modelName, 100, 100);
+ WaitUntilSensorSpawn(_raySensorName, 100, 100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnImuSensor(const std::string &_modelName,
+ const std::string &_imuSensorName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ const std::string &_noiseType,
+ double _rateNoiseMean, double _rateNoiseStdDev,
+ double _rateBiasMean, double _rateBiasStdDev,
+ double _accelNoiseMean, double _accelNoiseStdDev,
+ double _accelBiasMean, double _accelBiasStdDev)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _modelName << "'>" << std::endl
+ << "<static>true</static>" << std::endl
+ << "<pose>" << _pos << " " << _rpy << "</pose>" << std::endl
+ << "<link name ='body'>" << std::endl
+ << "<inertial>" << std::endl
+ << "<mass>0.1</mass>" << std::endl
+ << "</inertial>" << std::endl
+ << "<collision name='parent_collision'>" << std::endl
+ << " <pose>0 0 0.0205 0 0 0</pose>" << std::endl
+ << " <geometry>" << std::endl
+ << " <cylinder>" << std::endl
+ << " <radius>0.021</radius>" << std::endl
+ << " <length>0.029</length>" << std::endl
+ << " </cylinder>" << std::endl
+ << " </geometry>" << std::endl
+ << "</collision>" << std::endl
+ << " <sensor name ='" << _imuSensorName
+ << "' type ='imu'>" << std::endl
+ << " <imu>" << std::endl;
+
+ if (_noiseType.size() > 0)
+ {
+ newModelStr << " <noise>" << std::endl
+ << " <type>" << _noiseType << "</type>" << std::endl
+ << " <rate>" << std::endl
+ << " <mean>" << _rateNoiseMean
+ << "</mean>" << std::endl
+ << " <stddev>" << _rateNoiseStdDev
+ << "</stddev>" << std::endl
+ << " <bias_mean>" << _rateBiasMean
+ << "</bias_mean>" << std::endl
+ << " <bias_stddev>" << _rateBiasStdDev
+ << "</bias_stddev>" << std::endl
+ << " </rate>" << std::endl
+ << " <accel>" << std::endl
+ << " <mean>" << _accelNoiseMean << "</mean>"
+ << std::endl
+ << " <stddev>" << _accelNoiseStdDev << "</stddev>"
+ << std::endl
+ << " <bias_mean>" << _accelBiasMean
+ << "</bias_mean>" << std::endl
+ << " <bias_stddev>" << _accelBiasStdDev
+ << "</bias_stddev>" << std::endl
+ << " </accel>" << std::endl
+ << " </noise>" << std::endl;
+ }
+
+ newModelStr << " </imu>" << std::endl
+ << " </sensor>" << std::endl
+ << "</link>" << std::endl
+ << "</model>" << std::endl
+ << "</sdf>" << std::endl;
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ WaitUntilEntitySpawn(_modelName, 100, 1000);
+ WaitUntilSensorSpawn(_imuSensorName, 100, 100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnUnitContactSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const std::string &_collisionType, const math::Vector3 &_pos,
+ const math::Vector3 &_rpy, bool _static)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+ std::ostringstream shapeStr;
+
+ if (_collisionType == "box")
+ {
+ shapeStr << " <box><size>1 1 1</size></box>";
+ }
+ else if (_collisionType == "cylinder")
+ {
+ shapeStr << "<cylinder>"
+ << " <radius>.5</radius><length>1.0</length>"
+ << "</cylinder>";
+ }
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _name << "'>"
+ << "<static>" << _static << "</static>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<link name ='body'>"
+ << " <collision name ='contact_collision'>"
+ << " <geometry>"
+ << shapeStr.str()
+ << " </geometry>"
+ << " <surface>"
+ << " <contact>"
+ << " <ode>"
+ << " <min_depth>0.005</min_depth>"
+ << " </ode>"
+ << " </contact>"
+ << " </surface>"
+ << " </collision>"
+ << " <visual name ='visual'>"
+ << " <geometry>"
+ << shapeStr.str()
+ << " </geometry>"
+ << " </visual>"
+ << " <sensor name='" << _sensorName << "' type='contact'>"
+ << " <contact>"
+ << " <collision>contact_collision</collision>"
+ << " </contact>"
+ << " </sensor>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ WaitUntilEntitySpawn(_name, 100, 100);
+ WaitUntilSensorSpawn(_sensorName, 100, 100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnUnitImuSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const std::string &_collisionType,
+ const std::string &_topic, const math::Vector3 &_pos,
+ const math::Vector3 &_rpy, bool _static)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+ std::ostringstream shapeStr;
+ if (_collisionType == "box")
+ shapeStr << " <box><size>1 1 1</size></box>";
+ else if (_collisionType == "cylinder")
+ {
+ shapeStr << "<cylinder>"
+ << " <radius>.5</radius><length>1.0</length>"
+ << "</cylinder>";
+ }
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _name << "'>"
+ << "<static>" << _static << "</static>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<link name ='body'>"
+ << " <collision name ='contact_collision'>"
+ << " <geometry>"
+ << shapeStr.str()
+ << " </geometry>"
+ << " <surface>"
+ << " <contact>"
+ << " <ode>"
+ << " <min_depth>0.01</min_depth>"
+ << " </ode>"
+ << " </contact>"
+ << " </surface>"
+ << " </collision>"
+ << " <visual name ='visual'>"
+ << " <geometry>"
+ << shapeStr.str()
+ << " </geometry>"
+ << " </visual>"
+ << " <sensor name='" << _sensorName << "' type='imu'>"
+ << " <imu>"
+ << " <topic>" << _topic << "</topic>"
+ << " </imu>"
+ << " </sensor>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ WaitUntilEntitySpawn(_name, 20, 50);
+ WaitUntilSensorSpawn(_sensorName, 100, 100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnUnitAltimeterSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const std::string &_collisionType,
+ const std::string &_topic,
+ const ignition::math::Vector3d &_pos,
+ const ignition::math::Vector3d &_rpy,
+ bool _static)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+ std::ostringstream shapeStr;
+
+ if (_collisionType == "box")
+ {
+ shapeStr << " <box><size>1 1 1</size></box>";
+ }
+ else if (_collisionType == "cylinder")
+ {
+ shapeStr << "<cylinder>"
+ << " <radius>.5</radius><length>1.0</length>"
+ << "</cylinder>";
+ }
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _name << "'>"
+ << "<static>" << _static << "</static>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<link name ='body'>"
+ << " <collision name ='contact_collision'>"
+ << " <geometry>"
+ << shapeStr.str()
+ << " </geometry>"
+ << " </collision>"
+ << " <visual name ='visual'>"
+ << " <geometry>"
+ << shapeStr.str()
+ << " </geometry>"
+ << " </visual>"
+ << " <sensor name='" << _sensorName << "' type='altimeter'>"
+ << " <topic>" << _topic << "</topic>"
+ << " <altimeter>"
+ << " </altimeter>"
+ << " </sensor>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ WaitUntilEntitySpawn(_name, 20, 50);
+ WaitUntilSensorSpawn(_sensorName, 100, 100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnUnitMagnetometerSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const std::string &_collisionType,
+ const std::string &_topic,
+ const ignition::math::Vector3d &_pos,
+ const ignition::math::Vector3d &_rpy, bool _static)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+ std::ostringstream shapeStr;
+ if (_collisionType == "box")
+ {
+ shapeStr << " <box><size>1 1 1</size></box>";
+ }
+ else if (_collisionType == "cylinder")
+ {
+ shapeStr << "<cylinder>"
+ << " <radius>.5</radius><length>1.0</length>"
+ << "</cylinder>";
+ }
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _name << "'>"
+ << "<static>" << _static << "</static>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<link name ='body'>"
+ << " <collision name ='contact_collision'>"
+ << " <geometry>"
+ << shapeStr.str()
+ << " </geometry>"
+ << " </collision>"
+ << " <visual name ='visual'>"
+ << " <geometry>"
+ << shapeStr.str()
+ << " </geometry>"
+ << " </visual>"
+ << " <sensor name='" << _sensorName << "' type='magnetometer'>"
+ << " <topic>" << _topic << "</topic>"
+ << " <magnetometer>"
+ << " </magnetometer>"
+ << " </sensor>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ WaitUntilEntitySpawn(_name, 20, 50);
+ WaitUntilSensorSpawn(_sensorName, 100, 100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::launchTimeoutFailure(const char *_logMsg,
+ const int _timeoutCS)
+{
+ FAIL() << "ServerFixture timeout (wait more than " << _timeoutCS / 100
+ << "s): " << _logMsg;
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnWirelessTransmitterSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const math::Vector3 &_pos,
+ const math::Vector3 &_rpy,
+ const std::string &_essid,
+ double _freq,
+ double _power,
+ double _gain,
+ bool _visualize)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _name << "'>"
+ << "<static>true</static>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<link name ='link'>"
+ << " <sensor name='" << _sensorName
+ << "' type='wireless_transmitter'>"
+ << " <always_on>1</always_on>"
+ << " <update_rate>1</update_rate>"
+ << " <visualize>" << _visualize << "</visualize>"
+ << " <transceiver>"
+ << " <essid>" << _essid << "</essid>"
+ << " <frequency>" << _freq << "</frequency>"
+ << " <power>" << _power << "</power>"
+ << " <gain>" << _gain << "</gain>"
+ << " </transceiver>"
+ << " </sensor>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ WaitUntilEntitySpawn(_name, 100, 100);
+ WaitUntilSensorSpawn(_sensorName, 100, 100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnWirelessReceiverSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const math::Vector3 &_pos,
+ const math::Vector3 &_rpy,
+ double _minFreq,
+ double _maxFreq,
+ double _power,
+ double _gain,
+ double _sensitivity,
+ bool _visualize)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _name << "'>"
+ << "<static>true</static>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<link name ='link'>"
+ << " <sensor name='" << _sensorName
+ << "' type='wireless_receiver'>"
+ << " <update_rate>1</update_rate>"
+ << " <visualize>" << _visualize << "</visualize>"
+ << " <transceiver>"
+ << " <min_frequency>" << _minFreq << "</min_frequency>"
+ << " <max_frequency>" << _maxFreq << "</max_frequency>"
+ << " <power>" << _power << "</power>"
+ << " <gain>" << _gain << "</gain>"
+ << " <sensitivity>" << _sensitivity << "</sensitivity>"
+ << " </transceiver>"
+ << " </sensor>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ WaitUntilEntitySpawn(_name, 100, 100);
+ WaitUntilSensorSpawn(_sensorName, 100, 100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::WaitUntilEntitySpawn(const std::string &_name,
+ unsigned int _sleepEach,
+ int _retries)
+{
+ int i = 0;
+ // Wait for the entity to spawn
+ while (!this->HasEntity(_name) && i < _retries)
+ {
+ common::Time::MSleep(_sleepEach);
+ ++i;
+ }
+ EXPECT_LT(i, _retries);
+
+ if (i >= _retries)
+ FAIL() << "ServerFixture timeout: max number of retries ("
+ << _retries
+ << ") exceeded while awaiting the spawn of " << _name;
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::WaitUntilSensorSpawn(const std::string &_name,
+ unsigned int _sleepEach,
+ int _retries)
+{
+ int i = 0;
+ // Wait for the sensor to spawn
+ while (!sensors::get_sensor(_name) && i < _retries)
+ {
+ common::Time::MSleep(_sleepEach);
+ ++i;
+ }
+ EXPECT_LT(i, _retries);
+
+ if (i >= _retries)
+ FAIL() << "ServerFixture timeout: max number of retries ("
+ << _retries
+ << ") exceeded while awaiting the spawn of " << _name;
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnLight(const std::string &_name,
+ const std::string &_type,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ const common::Color &_diffuse,
+ const common::Color &_specular,
+ const math::Vector3 &_direction,
+ double _attenuationRange,
+ double _attenuationConstant,
+ double _attenuationLinear,
+ double _attenuationQuadratic,
+ double _spotInnerAngle,
+ double _spotOuterAngle,
+ double _spotFallOff,
+ bool _castShadows)
+{
+ msgs::Factory msg;
+ std::ostringstream newLightStr;
+
+ newLightStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<light name ='" << _name << "' type = '" << _type << "'>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<diffuse>" << _diffuse << "</diffuse>"
+ << "<specular>" << _specular << "</specular>"
+ << "<direction>" << _direction << "</direction>"
+ << "<attenuation>"
+ << " <range>" << _attenuationRange << "</range>"
+ << " <constant>" << _attenuationConstant << "</constant>"
+ << " <linear>" << _attenuationLinear << "</linear>"
+ << " <quadratic>" << _attenuationQuadratic << "</quadratic>"
+ << "</attenuation>";
+
+ if (_type == "spot")
+ {
+ newLightStr << "<spot>"
+ << " <inner_angle>" << _spotInnerAngle << "</inner_angle>"
+ << " <outer_angle>" << _spotOuterAngle << "</outer_angle>"
+ << " <falloff>" << _spotFallOff << "</falloff>"
+ << "</spot>";
+ }
+
+ newLightStr << "<cast_shadows>" << _castShadows << "</cast_shadows>"
+ << "</light>"
+ << "</sdf>";
+
+ msg.set_sdf(newLightStr.str());
+ this->factoryPub->Publish(msg);
+
+ physics::WorldPtr world = physics::get_world();
+ msgs::Scene sceneMsg;
+ int timeOutCount = 0;
+ int maxTimeOut = 10;
+ while (timeOutCount < maxTimeOut)
+ {
+ sceneMsg = world->GetSceneMsg();
+ for (int i = 0; i < sceneMsg.light_size(); ++i)
+ {
+ if (sceneMsg.light(i).name() == _name)
+ break;
+ }
+ timeOutCount++;
+ common::Time::MSleep(100);
+ }
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnCylinder(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _static)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+ msgs::Model model;
+ model.set_name(_name);
+ model.set_is_static(_static);
+ msgs::Set(model.mutable_pose(),
+ ignition::math::Pose3d(_pos.Ign(), _rpy.Ign()));
+ msgs::AddCylinderLink(model, 1.0, 0.5, 1.0);
+ auto link = model.mutable_link(0);
+ link->set_name("body");
+ link->mutable_collision(0)->set_name("geom");
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << msgs::ModelToSDF(model)->ToString("")
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ // Wait for the entity to spawn
+ while (!this->HasEntity(_name))
+ common::Time::MSleep(100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnSphere(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _wait, bool _static)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _name << "'>"
+ << "<static>" << _static << "</static>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<link name ='body'>"
+ << " <collision name ='geom'>"
+ << " <geometry>"
+ << " <sphere><radius>.5</radius></sphere>"
+ << " </geometry>"
+ << " </collision>"
+ << " <visual name ='visual'>"
+ << " <geometry>"
+ << " <sphere><radius>.5</radius></sphere>"
+ << " </geometry>"
+ << " </visual>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ // Wait for the entity to spawn
+ while (_wait && !this->HasEntity(_name))
+ common::Time::MSleep(100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnSphere(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ const math::Vector3 &_cog, double _radius,
+ bool _wait, bool _static)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+ msgs::Model model;
+ model.set_name(_name);
+ model.set_is_static(_static);
+ msgs::Set(model.mutable_pose(),
+ ignition::math::Pose3d(_pos.Ign(), _rpy.Ign()));
+ msgs::AddSphereLink(model, 1.0, _radius);
+ auto link = model.mutable_link(0);
+ link->set_name("body");
+ link->mutable_collision(0)->set_name("geom");
+ msgs::Set(link->mutable_inertial()->mutable_pose(),
+ ignition::math::Pose3d(_cog.Ign(),
+ ignition::math::Quaterniond()));
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << msgs::ModelToSDF(model)->ToString("")
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ // Wait for the entity to spawn
+ while (_wait && !this->HasEntity(_name))
+ common::Time::MSleep(100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnBox(const std::string &_name,
+ const math::Vector3 &_size, const math::Vector3 &_pos,
+ const math::Vector3 &_rpy, bool _static)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+ msgs::Model model;
+ model.set_name(_name);
+ model.set_is_static(_static);
+ msgs::Set(model.mutable_pose(),
+ ignition::math::Pose3d(_pos.Ign(), _rpy.Ign()));
+ msgs::AddBoxLink(model, 1.0, _size.Ign());
+ auto link = model.mutable_link(0);
+ link->set_name("body");
+ link->mutable_collision(0)->set_name("geom");
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << msgs::ModelToSDF(model)->ToString("")
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ // Wait for the entity to spawn
+ while (!this->HasEntity(_name))
+ common::Time::MSleep(100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnTrimesh(const std::string &_name,
+ const std::string &_modelPath, const math::Vector3 &_scale,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _static)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='" << _name << "'>"
+ << "<static>" << _static << "</static>"
+ << "<pose>" << _pos << " " << _rpy << "</pose>"
+ << "<link name ='body'>"
+ << " <collision name ='geom'>"
+ << " <geometry>"
+ << " <mesh>"
+ << " <uri>" << _modelPath << "</uri>"
+ << " <scale>" << _scale << "</scale>"
+ << " </mesh>"
+ << " </geometry>"
+ << " </collision>"
+ << " <visual name ='visual'>"
+ << " <geometry>"
+ << " <mesh><uri>" << _modelPath << "</uri></mesh>"
+ << " </geometry>"
+ << " </visual>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ // Wait for the entity to spawn
+ while (!this->HasEntity(_name))
+ common::Time::MSleep(100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnEmptyLink(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _static)
+{
+ msgs::Factory msg;
+ std::ostringstream newModelStr;
+ msgs::Model model;
+ model.set_name(_name);
+ model.set_is_static(_static);
+ msgs::Set(model.mutable_pose(),
+ ignition::math::Pose3d(_pos.Ign(), _rpy.Ign()));
+ model.add_link();
+ model.mutable_link(0)->set_name("body");
+
+ newModelStr << "<sdf version='" << SDF_VERSION << "'>"
+ << msgs::ModelToSDF(model)->ToString("")
+ << "</sdf>";
+
+ msg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(msg);
+
+ // Wait for the entity to spawn
+ while (!this->HasEntity(_name))
+ common::Time::MSleep(100);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnModel(const std::string &_filename)
+{
+ msgs::Factory msg;
+ msg.set_sdf_filename(_filename);
+ this->factoryPub->Publish(msg);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::SpawnSDF(const std::string &_sdf)
+{
+ msgs::Factory msg;
+ msg.set_sdf(_sdf);
+ this->factoryPub->Publish(msg);
+
+ // The code above sends a message, but it will take some time
+ // before the message is processed.
+ //
+ // The code below parses the sdf string to find a model name,
+ // then this function will block until that model
+ // has been processed and recognized by the Server Fixture.
+ sdf::SDF sdfParsed;
+ sdfParsed.SetFromString(_sdf);
+ // Check that sdf contains a model
+ if (sdfParsed.Root()->HasElement("model"))
+ {
+ // Timeout of 30 seconds (3000 * 10 ms)
+ int waitCount = 0, maxWaitCount = 3000;
+ sdf::ElementPtr model = sdfParsed.Root()->GetElement("model");
+ std::string name = model->Get<std::string>("name");
+ while (!this->HasEntity(name) && ++waitCount < maxWaitCount)
+ common::Time::MSleep(100);
+ ASSERT_LT(waitCount, maxWaitCount);
+ }
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::LoadPlugin(const std::string &_filename,
+ const std::string &_name)
+{
+ // Get the first world...we assume it the only one running
+ physics::WorldPtr world = physics::get_world();
+ world->LoadPlugin(_filename, _name, sdf::ElementPtr());
+}
+
+/////////////////////////////////////////////////
+physics::ModelPtr ServerFixture::GetModel(const std::string &_name)
+{
+ // Get the first world...we assume it the only one running
+ physics::WorldPtr world = physics::get_world();
+ return world->GetModel(_name);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::RemoveModel(const std::string &_name)
+{
+ msgs::Request *msg = msgs::CreateRequest("entity_delete", _name);
+ this->requestPub->Publish(*msg);
+ delete msg;
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::RemovePlugin(const std::string &_name)
+{
+ // Get the first world...we assume it the only one running
+ physics::WorldPtr world = physics::get_world();
+ world->RemovePlugin(_name);
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::GetMemInfo(double &_resident, double &_share)
+{
+#ifdef __linux__
+ int totalSize, residentPages, sharePages;
+ totalSize = residentPages = sharePages = 0;
+
+ std::ifstream buffer("/proc/self/statm");
+ buffer >> totalSize >> residentPages >> sharePages;
+ buffer.close();
+
+ // in case x86-64 is configured to use 2MB pages
+ int64_t pageSizeKb = sysconf(_SC_PAGE_SIZE) / 1024;
+
+ _resident = residentPages * pageSizeKb;
+ _share = sharePages * pageSizeKb;
+#elif __MACH__
+ // /proc is only available on Linux
+ // for OSX, use task_info to get resident and virtual memory
+ struct task_basic_info t_info;
+ mach_msg_type_number_t t_info_count = TASK_BASIC_INFO_COUNT;
+ if (KERN_SUCCESS != task_info(mach_task_self(),
+ TASK_BASIC_INFO,
+ (task_info_t)&t_info,
+ &t_info_count))
+ {
+ gzerr << "failure calling task_info\n";
+ return;
+ }
+ _resident = static_cast<double>(t_info.resident_size/1024);
+ _share = static_cast<double>(t_info.virtual_size/1024);
+#else
+ gzerr << "Unsupported architecture\n";
+ return;
+#endif
+}
+
+/////////////////////////////////////////////////
+std::string ServerFixture::GetUniqueString(const std::string &_prefix)
+{
+ std::ostringstream stream;
+ stream << _prefix << this->uniqueCounter++;
+ return stream.str();
+}
diff --git a/gazebo/test/ServerFixture.hh b/gazebo/test/ServerFixture.hh
new file mode 100644
index 0000000..7828873
--- /dev/null
+++ b/gazebo/test/ServerFixture.hh
@@ -0,0 +1,664 @@
+/*
+ * Copyright (C) 2012-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_SERVER_FIXTURE_HH_
+#define _GAZEBO_SERVER_FIXTURE_HH_
+
+#pragma GCC diagnostic ignored "-Wswitch-default"
+#pragma GCC diagnostic ignored "-Wfloat-equal"
+#pragma GCC diagnostic ignored "-Wshadow"
+
+// The following is needed to enable the GetMemInfo function for OSX
+#ifdef __MACH__
+# include <mach/mach.h>
+#endif // __MACH__
+
+#include <sdf/sdf.hh>
+
+#include <gtest/gtest.h>
+#include <boost/thread.hpp>
+#include <boost/filesystem.hpp>
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include "gazebo/transport/transport.hh"
+
+#include "gazebo/common/CommonIface.hh"
+#include "gazebo/common/SystemPaths.hh"
+#include "gazebo/common/Console.hh"
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/PhysicsTypes.hh"
+#include "gazebo/physics/PhysicsIface.hh"
+#include "gazebo/sensors/sensors.hh"
+#include "gazebo/rendering/rendering.hh"
+#include "gazebo/msgs/msgs.hh"
+
+#include "gazebo/math/SignalStats.hh"
+#include "gazebo/math/Vector3Stats.hh"
+
+#include "gazebo/gazebo_config.h"
+#include "gazebo/Server.hh"
+#include "gazebo/util/system.hh"
+
+#include "test_config.h"
+
+namespace gazebo
+{
+ std::string custom_exec(std::string _cmd);
+
+ class GAZEBO_VISIBLE ServerFixture : public testing::Test
+ {
+ /// \brief Constructor
+ protected: ServerFixture();
+
+ /// \brief Destructor
+ protected: virtual ~ServerFixture();
+
+ /// \brief Tear down the test fixture. This gets called by gtest.
+ protected: virtual void TearDown();
+
+ /// \brief Unload the test fixture.
+ protected: virtual void Unload();
+
+ /// \brief Load a world based on a filename.
+ /// \param[in] _worldFilename Name of the world to load.
+ protected: virtual void Load(const std::string &_worldFilename);
+
+ /// \brief Load a world based on a filename and set simulation
+ /// paused/un-paused.
+ /// \param[in] _worldFilename Name of the world to load.
+ /// \param[in] _paused True to start the world paused.
+ protected: virtual void Load(const std::string &_worldFilename,
+ bool _paused);
+
+ /// \brief Load a world based on a filename and set simulation
+ /// paused/un-paused, and specify physics engine. This versions allows
+ /// plugins to be loaded (via the cmd line args)
+ /// \param[in] _worldFilename Name of the world to load.
+ /// \param[in] _paused True to start the world paused.
+ /// \param[in] _physics Name of the physics engine.
+ /// \param[in] _systemPlugins Array of system plugins to load.
+ protected: virtual void Load(const std::string &_worldFilename,
+ bool _paused, const std::string &_physics,
+ const std::vector<std::string> &_systemPlugins = {});
+
+ /// \brief Run the server.
+ /// \param[in] _worldFilename Name of the world to run in simulation.
+ protected: void RunServer(const std::string &_worldFilename);
+
+ /// \brief Run the server, start paused/unpaused, and specify the physics
+ /// engine.
+ /// \param[in] _worldFilename Name of the world to load.
+ /// \param[in] _paused True to start the world paused.
+ /// \param[in] _physics Name of the physics engine.
+ /// \param[in] _systemPlugins Array of system plugins to load.
+ protected: void RunServer(const std::string &_worldFilename, bool _paused,
+ const std::string &_physics,
+ const std::vector<std::string> &_systemPlugins = {});
+
+
+ /// \brief Get a pointer to the rendering scene.
+ /// \param[in] _sceneName Name of the scene to get.
+ protected: rendering::ScenePtr GetScene(
+ const std::string &_sceneName = "default");
+
+
+ /// \brief Function that received world stastics messages.
+ /// \param[in] _msg World statistics message.
+ protected: void OnStats(ConstWorldStatisticsPtr &_msg);
+
+ /// \brief Set a running simulation paused/unpaused.
+ protected: void SetPause(bool _pause);
+
+ /// \brief Get the real-time factor.
+ /// \return The percent real time simulation is running.
+ protected: double GetPercentRealTime() const;
+
+ /// \brief Function that received poses messages from a running
+ /// simulation.
+ /// \param[in] _msg Pose message.
+ protected: void OnPose(ConstPosesStampedPtr &_msg);
+
+ /// \brief Get the pose of an entity.
+ /// \param[in] _name Name of the entity.
+ /// \return Pose of the named entity.
+ protected: math::Pose GetEntityPose(const std::string &_name);
+
+ /// \brief Return true if the named entity exists.
+ /// \param[in] _name Name of the entity to check for.
+ /// \return True if the entity exists.
+ protected: bool HasEntity(const std::string &_name);
+
+ /// \brief Print image data to screen. This is used to generate test data.
+ /// \param[in] _name Name to associate with the printed data.
+ /// \param[in] _image The raw image data.
+ /// \param[in] _width Width of the image.
+ /// \param[in] _height Height of the image.
+ /// \param[in] _depth Pixel depth.
+ protected: void PrintImage(const std::string &_name, unsigned char **_image,
+ unsigned int _width, unsigned int _height,
+ unsigned int _depth);
+
+ /// \brief Print laser scan to screen. This is used to generate test data.
+ /// \param[in] _name Name to associate with the printed data.
+ /// \param[in] _scan The laser scan data.
+ /// \param[in] _sampleCount Number of samples in the scan data.
+ protected: void PrintScan(const std::string &_name, double *_scan,
+ unsigned int _sampleCount);
+
+ /// \brief Function to compare two float arrays (for example two laser
+ /// scans).
+ /// \param[in] _scanA First float array.
+ /// \param[in] _scanB Second float array.
+ /// \param[in] _sampleCount Number of samples in each float array.
+ /// \param[out] _diffMax Maximum difference between the two arrays.
+ /// \param[out] _diffSum Sum of the differences between the two arrays.
+ /// \param[out] _diffAvg Average difference between the two arrays.
+ protected: void FloatCompare(float *_scanA, float *_scanB,
+ unsigned int _sampleCount, float &_diffMax,
+ float &_diffSum, float &_diffAvg);
+
+ /// \brief Function to compare two double arrays (for example two laser
+ /// scans).
+ /// \param[in] _scanA First double array.
+ /// \param[in] _scanB Second double array.
+ /// \param[in] _sampleCount Number of samples in each double array.
+ /// \param[out] _diffMax Maximum difference between the two arrays.
+ /// \param[out] _diffSum Sum of the differences between the two arrays.
+ /// \param[out] _diffAvg Average difference between the two arrays.
+ protected: void DoubleCompare(double *_scanA, double *_scanB,
+ unsigned int _sampleCount, double &_diffMax,
+ double &_diffSum, double &_diffAvg);
+
+ /// \brief Function to compare two images.
+ /// \param[in] _imageA First image to compare.
+ /// \param[in] _imageB Second image to compare.
+ /// \param[in] _width Width of both images.
+ /// \param[in] _height Height of both images.
+ /// \param[in] _depth Depth of both images.
+ /// \param[out] _diffMax Maximum difference between the two arrays.
+ /// \param[out] _diffSum Sum of the differences between the two arrays.
+ /// \param[out] _diffAvg Average difference between the two arrays.
+ protected: void ImageCompare(unsigned char *_imageA,
+ unsigned char *_imageB,
+ unsigned int _width, unsigned int _height,
+ unsigned int _depth,
+ unsigned int &_diffMax, unsigned int &_diffSum,
+ double &_diffAvg);
+
+ /// \brief Function that receives new image frames.
+ /// \param[in] _image Image data.
+ /// \param[in] _width Width of the image frame.
+ /// \param[in] _height Height of the image frame.
+ /// \param[in] _depth Depth of the image frame.
+ /// \param[in] _format Pixel format.
+ private: void OnNewFrame(const unsigned char *_image,
+ unsigned int _width, unsigned int _height,
+ unsigned int _depth,
+ const std::string &/*_format*/);
+
+ /// \brief Get an image frame from a camera.
+ /// \param[in] _cameraName Name of the camera to get a frame from.
+ /// \param[out] _imgData Array that receives the image data.
+ /// \param[in] _width Width of the image frame.
+ /// \param[in] _height Height of the image frame.
+ protected: void GetFrame(const std::string &_cameraName,
+ unsigned char **_imgData, unsigned int &_width,
+ unsigned int &_height);
+
+ /// \brief Spawn a model from a msgs::Model and return ModelPtr.
+ /// \param[in] _msg Model message.
+ /// \return Pointer to model.
+ protected: physics::ModelPtr SpawnModel(const msgs::Model &_msg);
+
+ /// \brief Check that a pointer is not NULL. A function is created
+ /// for this purpose, since ASSERT's cannot be called from non-void
+ /// functions.
+ /// \param[in] _ptr Pointer to verify is not NULL.
+ protected: template<typename T>
+ static void CheckPointer(boost::shared_ptr<T> _ptr)
+ {
+ ASSERT_TRUE(_ptr != NULL);
+ }
+
+ /// \brief Spawn a camera.
+ /// \param[in] _modelName Name of the model.
+ /// \param[in] _cameraName Name of the camera.
+ /// \param[in] _pos Camera position.
+ /// \param[in] _rpy Camera roll, pitch, yaw.
+ /// \param[in] _width Output image width.
+ /// \param[in] _height Output image height.
+ /// \param[in] _rate Output Hz.
+ /// \param[in] _noiseType Type of noise to apply.
+ /// \param[in] _noiseMean Mean noise value.
+ /// \param[in] _noiseStdDev Standard deviation of the noise.
+ /// \param[in] _distortionK1 Distortion coefficient k1.
+ /// \param[in] _distortionK2 Distortion coefficient k2.
+ /// \param[in] _distortionK3 Distortion coefficient k3.
+ /// \param[in] _distortionP1 Distortion coefficient P1.
+ /// \param[in] _distortionP2 Distortion coefficient p2.
+ /// \param[in] _cx Normalized optical center x, used for distortion.
+ /// \param[in] _cy Normalized optical center y, used for distortion.
+ protected: void SpawnCamera(const std::string &_modelName,
+ const std::string &_cameraName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ unsigned int _width = 320, unsigned int _height = 240,
+ double _rate = 25,
+ const std::string &_noiseType = "",
+ double _noiseMean = 0.0, double _noiseStdDev = 0.0,
+ bool _distortion = false, double _distortionK1 = 0.0,
+ double _distortionK2 = 0.0, double _distortionK3 = 0.0,
+ double _distortionP1 = 0.0, double _distortionP2 = 0.0,
+ double _cx = 0.5, double _cy = 0.5);
+
+ /// \brief Spawn a laser.
+ /// \param[in] _modelName Name of the model.
+ /// \param[in] _raySensorName Name of the laser.
+ /// \param[in] _pos Camera position.
+ /// \param[in] _rpy Camera roll, pitch, yaw.
+ /// \param[in] _hMinAngle Horizontal min angle
+ /// \param[in] _hMaxAngle Horizontal max angle
+ /// \param[in] _minRange Min range
+ /// \param[in] _maxRange Max range
+ /// \param[in] _rangeResolution Resolution of the scan
+ /// \param[in] _samples Number of samples.
+ /// \param[in] _rate Output Hz.
+ /// \param[in] _noiseType Type of noise to apply.
+ /// \param[in] _noiseMean Mean noise value.
+ /// \param[in] _noiseStdDev Standard deviation of the noise.
+ protected: void SpawnRaySensor(const std::string &_modelName,
+ const std::string &_raySensorName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
+ double _vMinAngle = -1.0, double _vMaxAngle = 1.0,
+ double _minRange = 0.08, double _maxRange = 10,
+ double _rangeResolution = 0.01, unsigned int _samples = 640,
+ unsigned int _vSamples = 1, double _hResolution = 1.0,
+ double _vResolution = 1.0,
+ const std::string &_noiseType = "", double _noiseMean = 0.0,
+ double _noiseStdDev = 0.0);
+
+ /// \brief Spawn a gpu laser.
+ /// \param[in] _modelName Name of the model.
+ /// \param[in] _raySensorName Name of the laser.
+ /// \param[in] _pos Camera position.
+ /// \param[in] _rpy Camera roll, pitch, yaw.
+ /// \param[in] _hMinAngle Horizontal min angle
+ /// \param[in] _hMaxAngle Horizontal max angle
+ /// \param[in] _minRange Min range
+ /// \param[in] _maxRange Max range
+ /// \param[in] _rangeResolution Resolution of the scan
+ /// \param[in] _samples Number of samples.
+ /// \param[in] _rate Output Hz.
+ /// \param[in] _noiseType Type of noise to apply.
+ /// \param[in] _noiseMean Mean noise value.
+ /// \param[in] _noiseStdDev Standard deviation of the noise.
+ protected: void SpawnGpuRaySensor(const std::string &_modelName,
+ const std::string &_raySensorName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
+ double _minRange = 0.08, double _maxRange = 10,
+ double _rangeResolution = 0.01, unsigned int _samples = 640,
+ const std::string &_noiseType = "", double _noiseMean = 0.0,
+ double _noiseStdDev = 0.0);
+
+ /// \brief Spawn an imu sensor laser.
+ /// \param[in] _modelName Name of the model.
+ /// \param[in] _imuSensorName Name of the imu sensor.
+ /// \param[in] _pos Camera position.
+ /// \param[in] _rpy Camera roll, pitch, yaw.
+ /// \param[in] _noiseType Type of noise to apply.
+ /// \param[in] _noiseMean Mean noise value.
+ /// \param[in] _noiseStdDev Standard deviation of the noise.
+ /// \param[in] _accelNoiseMean Acceleration based noise mean.
+ /// \param[in] _accelNoiseStdDev Acceleration based noise standard
+ /// deviation.
+ /// \param[in] _accelBiasMean Acceleration mean bias
+ /// \param[in] _accelBiasStdDev Acceleration standard deviation bias
+ protected: void SpawnImuSensor(const std::string &_modelName,
+ const std::string &_imuSensorName,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ const std::string &_noiseType = "",
+ double _rateNoiseMean = 0.0, double _rateNoiseStdDev = 0.0,
+ double _rateBiasMean = 0.0, double _rateBiasStdDev = 0.0,
+ double _accelNoiseMean = 0.0, double _accelNoiseStdDev = 0.0,
+ double _accelBiasMean = 0.0, double _accelBiasStdDev = 0.0);
+
+ /// \brief Spawn a contact sensor with the specified collision geometry
+ /// \param[in] _name Model name
+ /// \param[in] _sensorName Sensor name
+ /// \param[in] _collisionType Type of collision, box or cylinder
+ /// \param[in] _pos World position
+ /// \param[in] _rpy World rotation in Euler angles
+ /// \param[in] _static True to make the model static
+ protected: void SpawnUnitContactSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const std::string &_collisionType, const math::Vector3 &_pos,
+ const math::Vector3 &_rpy, bool _static = false);
+
+ /// \brief Spawn an IMU sensor on a link
+ /// \param[in] _name Model name
+ /// \param[in] _sensorName Sensor name
+ /// \param[in] _collisionType Type of collision, box or cylinder
+ /// \param[in] _topic Topic to publish IMU data to
+ /// \param[in] _pos World position
+ /// \param[in] _rpy World rotation in Euler angles
+ /// \param[in] _static True to make the model static
+ protected: void SpawnUnitImuSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const std::string &_collisionType,
+ const std::string &_topic, const math::Vector3 &_pos,
+ const math::Vector3 &_rpy, bool _static = false);
+
+ /// \brief Spawn an altimeter sensor on a link
+ /// \param[in] _name Model name
+ /// \param[in] _sensorName Sensor name
+ /// \param[in] _collisionType Type of collision, box or cylinder
+ /// \param[in] _topic Topic to publish altimeter data to
+ /// \param[in] _pos World position
+ /// \param[in] _rpy World rotation in Euler angles
+ /// \param[in] _static True to make the model static
+ protected: void SpawnUnitAltimeterSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const std::string &_collisionType,
+ const std::string &_topic,
+ const ignition::math::Vector3d &_pos,
+ const ignition::math::Vector3d &_rpy,
+ bool _static = false);
+
+ /// \brief Spawn a magnetometer sensor on a link
+ /// \param[in] _name Model name
+ /// \param[in] _sensorName Sensor name
+ /// \param[in] _collisionType Type of collision, box or cylinder
+ /// \param[in] _topic Topic to publish magnetometer data to
+ /// \param[in] _pos World position
+ /// \param[in] _rpy World rotation in Euler angles
+ /// \param[in] _static True to make the model static
+ protected: void SpawnUnitMagnetometerSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const std::string &_collisionType,
+ const std::string &_topic,
+ const ignition::math::Vector3d &_pos,
+ const ignition::math::Vector3d &_rpy,
+ bool _static = false);
+
+ /// \brief generate a gtest failure from a timeout error and display a
+ /// log message about the problem.
+ /// \param[in] log_msg: error msg related to the timeout
+ /// \param[in] timeoutCS: failing period (in centiseconds)
+ private: void launchTimeoutFailure(const char *_logMsg,
+ const int _timeoutCS);
+
+ /// \brief Spawn an Wireless transmitter sensor on a link
+ /// \param[in] _name Model name
+ /// \param[in] _sensorName Sensor name
+ /// \param[in] _pos World position
+ /// \param[in] _rpy World rotation in Euler angles
+ /// \param[in] _essid Service set identifier (network name)
+ /// \param[in] _freq Frequency of transmission (MHz)
+ /// \param[in] _power Transmission power (dBm)
+ /// \param[in] _gain Antenna gain (dBi)
+ /// \param[in] _visualize Enable sensor visualization
+ protected: void SpawnWirelessTransmitterSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const math::Vector3 &_pos,
+ const math::Vector3 &_rpy,
+ const std::string &_essid,
+ double _freq,
+ double _power,
+ double _gain,
+ bool _visualize = true);
+
+ /// \brief Spawn an Wireless receiver sensor on a link
+ /// \param[in] _name Model name
+ /// \param[in] _sensorName Sensor name
+ /// \param[in] _pos World position
+ /// \param[in] _rpy World rotation in Euler angles
+ /// \param[in] _minFreq Minimum frequency to be filtered (MHz)
+ /// \param[in] _maxFreq Maximum frequency to be filtered (MHz)
+ /// \param[in] _power Transmission power (dBm)
+ /// \param[in] _gain Antenna gain (dBi)
+ /// \param[in] _sensitivity Receiver sensitibity (dBm)
+ /// \param[in] _visualize Enable sensor visualization
+ protected: void SpawnWirelessReceiverSensor(const std::string &_name,
+ const std::string &_sensorName,
+ const math::Vector3 &_pos,
+ const math::Vector3 &_rpy,
+ double _minFreq,
+ double _maxFreq,
+ double _power,
+ double _gain,
+ double _sensitivity,
+ bool _visualize = true);
+
+ /// \brief Wait for a number of ms. and attempts until the entity is spawned
+ /// \param[in] _name Model name
+ /// \param[in] _sleepEach Number of milliseconds to sleep in each iteration
+ /// \param[in] _retries Number of iterations until give up
+ protected: void WaitUntilEntitySpawn(const std::string &_name,
+ unsigned int _sleepEach,
+ int _retries);
+
+ /// \brief Wait for a number of ms. and attempts until the sensor is spawned
+ /// \param[in] _name Sensor name
+ /// \param[in] _sleepEach Number of milliseconds to sleep in each iteration
+ /// \param[in] _retries Number of iterations until give up
+ protected: void WaitUntilSensorSpawn(const std::string &_name,
+ unsigned int _sleepEach,
+ int _retries);
+
+ /// \brief Spawn a light.
+ /// \param[in] _name Name for the light.
+ /// \param[in] _size Type of light - "spot", "directional", or "point".
+ /// \param[in] _pos Position for the light.
+ /// \param[in] _rpy Roll, pitch, yaw for the light.
+ /// \param[in] _diffuse Diffuse color of the light.
+ /// \param[in] _specular Specular color of the light.
+ /// \param[in] _direction Direction of the light ("spot" and "directional").
+ /// \param[in] _attenuationRange Range of attenuation.
+ /// \param[in] _attenuationConstant Constant component of attenuation
+ /// \param[in] _attenuationLinear Linear component of attenuation
+ /// \param[in] _attenuationQuadratic Quadratic component of attenuation
+ /// \param[in] _spotInnerAngle Inner angle ("spot" only).
+ /// \param[in] _spotOuterAngle Outer angle ("spot" only).
+ /// \param[in] _spotFallOff Fall off ("spot" only).
+ /// \param[in] _castShadows True to cast shadows.
+ protected: void SpawnLight(const std::string &_name,
+ const std::string &_type,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ const common::Color &_diffuse = common::Color::White,
+ const common::Color &_specular = common::Color::White,
+ const math::Vector3 &_direction = -math::Vector3::UnitZ,
+ double _attenuationRange = 20,
+ double _attenuationConstant = 0.5,
+ double _attenuationLinear = 0.01,
+ double _attenuationQuadratic = 0.001,
+ double _spotInnerAngle = 0,
+ double _spotOuterAngle = 0,
+ double _spotFallOff = 0,
+ bool _castShadows = true);
+
+ /// \brief Spawn a cylinder
+ /// \param[in] _name Name for the model.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _static True to make the model static.
+ protected: void SpawnCylinder(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _static = false);
+
+ /// \brief Spawn a sphere
+ /// \param[in] _name Name for the model.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _static True to make the model static.
+ /// \param[in] _wait True to wait for the sphere to spawn before
+ /// returning.
+ protected: void SpawnSphere(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _wait = true, bool _static = false);
+
+ /// \brief Spawn a sphere
+ /// \param[in] _name Name for the model.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _cog Center of gravity.
+ /// \param[in] _radius Sphere radius.
+ /// \param[in] _static True to make the model static.
+ /// \param[in] _wait True to wait for the sphere to spawn before
+ /// returning.
+ protected: void SpawnSphere(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ const math::Vector3 &_cog, double _radius,
+ bool _wait = true, bool _static = false);
+
+ /// \brief Spawn a box.
+ /// \param[in] _name Name for the model.
+ /// \param[in] _size Size of the box.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _static True to make the model static.
+ protected: void SpawnBox(const std::string &_name,
+ const math::Vector3 &_size, const math::Vector3 &_pos,
+ const math::Vector3 &_rpy, bool _static = false);
+
+ /// \brief Spawn a triangle mesh.
+ /// \param[in] _name Name for the model.
+ /// \param[in] _modelPath Path to the mesh file.
+ /// \param[in] _scale Scaling factor.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _static True to make the model static.
+ protected: void SpawnTrimesh(const std::string &_name,
+ const std::string &_modelPath, const math::Vector3 &_scale,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _static = false);
+
+ /// \brief Spawn an empty link.
+ /// \param[in] _name Name for the model.
+ /// \param[in] _pos Position for the model.
+ /// \param[in] _rpy Roll, pitch, yaw for the model.
+ /// \param[in] _static True to make the model static.
+ protected: void SpawnEmptyLink(const std::string &_name,
+ const math::Vector3 &_pos, const math::Vector3 &_rpy,
+ bool _static = false);
+
+ /// \brief Spawn a model from file.
+ /// \param[in] _filename File to load a model from.
+ protected: void SpawnModel(const std::string &_filename);
+
+ /// \brief Send a factory message based on an SDF string.
+ /// \param[in] _sdf SDF string to publish.
+ protected: void SpawnSDF(const std::string &_sdf);
+
+ /// \brief Load a plugin.
+ /// \param[in] _filename Plugin filename to load.
+ /// \param[in] _name Name to associate with with the plugin.
+ protected: void LoadPlugin(const std::string &_filename,
+ const std::string &_name);
+
+ /// \brief Get a pointer to a model.
+ /// \param[in] _name Name of the model to get.
+ /// \return Pointer to the model, or NULL if the model was not found.
+ protected: physics::ModelPtr GetModel(const std::string &_name);
+
+ /// \brief Remove a model by name.
+ /// \param[in] _name Name of the model to remove.
+ protected: void RemoveModel(const std::string &_name);
+
+ /// \brief Remove a plugin.
+ /// \param[in] _name Name of the plugin to remove.
+ protected: void RemovePlugin(const std::string &_name);
+
+ /// \brief Get the current memory information.
+ /// \param[out] _resident Resident memory.
+ /// \param[out] _share Shared memory.
+ protected: void GetMemInfo(double &_resident, double &_share);
+
+ /// \brief Get unique string with a specified prefix.
+ /// \param[in] _prefix Prefix for unique string.
+ /// \return String with prefix and unique number as suffix.
+ protected: std::string GetUniqueString(const std::string &_prefix);
+
+ /// \brief Helper to record data to gtest xml output.
+ /// \param[in] _name Name of data.
+ /// \param[in] _data Floating point number to store.
+ protected: void Record(const std::string &_name, const double _data);
+
+ /// \brief Helper to record signal statistics to gtest xml output.
+ /// \param[in] _prefix Prefix string for data names.
+ /// \param[in] _stats Signal statistics to store.
+ protected: void Record(const std::string &_prefix,
+ const math::SignalStats &_stats);
+
+ /// \brief Helper to record Vector3 signal statistics to gtest xml output.
+ /// \param[in] _prefix Prefix string for data names.
+ /// \param[in] _stats Vector3 signal statistics to store.
+ protected: void Record(const std::string &_prefix,
+ const math::Vector3Stats &_stats);
+
+ /// \brief Pointer the Gazebo server.
+ protected: Server *server;
+
+ /// \brief Pointer the thread the runs the server.
+ protected: boost::thread *serverThread;
+
+ /// \brief Pointer to a node for communication.
+ protected: transport::NodePtr node;
+
+ /// \brief Pose subscription.
+ protected: transport::SubscriberPtr poseSub;
+
+ /// \brief World statistics subscription.
+ protected: transport::SubscriberPtr statsSub;
+
+ /// \brief Factory publisher.
+ protected: transport::PublisherPtr factoryPub;
+
+ /// \brief Request publisher.
+ protected: transport::PublisherPtr requestPub;
+
+ /// \brief Map of received poses.
+ protected: std::map<std::string, math::Pose> poses;
+
+ /// \brief Mutex to protect data structures that store messages.
+ protected: boost::mutex receiveMutex;
+
+ /// \brief Image data
+ private: unsigned char **imgData;
+
+ /// \brief Increments when images are received.
+ private: int gotImage;
+
+ /// \brief Current simulation time, real time, and pause time.
+ protected: common::Time simTime, realTime, pauseTime;
+
+ /// \brief Current percent realtime.
+ private: double percentRealTime;
+
+ /// \brief True if paused.
+ private: bool paused;
+
+ /// \brief True if server is running.
+ private: bool serverRunning;
+
+ /// \brief Counter for unique name generation.
+ private: int uniqueCounter;
+ };
+} // namespace gazebo
+#endif // define _GAZEBO_SERVER_FIXTURE_HH_
diff --git a/gazebo/test/ServerFixtureRecord.cc b/gazebo/test/ServerFixtureRecord.cc
new file mode 100644
index 0000000..dba4a4f
--- /dev/null
+++ b/gazebo/test/ServerFixtureRecord.cc
@@ -0,0 +1,58 @@
+/*
+ * 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.
+ *
+*/
+#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 "gazebo/math/SignalStats.hh"
+#include "gazebo/math/Vector3Stats.hh"
+#include "ServerFixture.hh"
+
+using namespace gazebo;
+
+/////////////////////////////////////////////////
+void ServerFixture::Record(const std::string &_name, const double _data)
+{
+ std::ostringstream stream;
+ stream << _data;
+ RecordProperty(_name, stream.str());
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::Record(const std::string &_prefix,
+ const math::SignalStats &_stats)
+{
+ auto map = _stats.Map();
+ for (auto const &stat : map)
+ {
+ this->Record(_prefix + stat.first, stat.second);
+ }
+}
+
+/////////////////////////////////////////////////
+void ServerFixture::Record(const std::string &_prefix,
+ const math::Vector3Stats &_stats)
+{
+ this->Record(_prefix + "_x_", _stats.X());
+ this->Record(_prefix + "_y_", _stats.Y());
+ this->Record(_prefix + "_z_", _stats.Z());
+ this->Record(_prefix + "_mag_", _stats.Mag());
+}
diff --git a/gazebo/test/helper_physics_generator.hh b/gazebo/test/helper_physics_generator.hh
new file mode 100644
index 0000000..68984f1
--- /dev/null
+++ b/gazebo/test/helper_physics_generator.hh
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2012-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 _HELPER_PHYSICS_GENERATOR_HH_
+#define _HELPER_PHYSICS_GENERATOR_HH_
+
+#include "gazebo/gazebo_config.h"
+
+#define BULLET_SUPPORT
+
+#ifdef HAVE_BULLET
+# undef BULLET_SUPPORT
+# define BULLET_SUPPORT , "bullet"
+#endif
+
+#define SIMBODY_SUPPORT
+#define DART_SUPPORT
+#define WORLD_STEP_DART_PGS
+
+#ifdef HAVE_SIMBODY
+# undef SIMBODY_SUPPORT
+# define SIMBODY_SUPPORT , "simbody"
+#endif
+#ifdef HAVE_DART
+# undef DART_SUPPORT
+# define DART_SUPPORT , "dart"
+# undef WORLD_STEP_DART_PGS
+# define WORLD_STEP_DART_PGS , "DART_PGS"
+#endif
+
+/// \brief Helper macro to instantiate gtest for different physics engines
+#define PHYSICS_ENGINE_VALUES ::testing::Values("ode" \
+ BULLET_SUPPORT \
+ SIMBODY_SUPPORT \
+ DART_SUPPORT \
+ )
+
+/// \brief Helper macro to instantiate gtest for different solvers
+#define WORLD_STEP_SOLVERS ::testing::Values("ODE_DANTZIG" \
+ WORLD_STEP_DART_PGS \
+ )
+
+#endif
diff --git a/gazebo/transport/CMakeLists.txt b/gazebo/transport/CMakeLists.txt
index 3fb2bff..3802f15 100644
--- a/gazebo/transport/CMakeLists.txt
+++ b/gazebo/transport/CMakeLists.txt
@@ -1,5 +1,7 @@
include (${gazebo_cmake_dir}/GazeboUtils.cmake)
+include_directories(${TBB_INCLUDEDIR})
+
set (sources
CallbackHelper.cc
Connection.cc
@@ -47,14 +49,23 @@ set_property(
GAZEBO_DEFAULT_MASTER_PORT=${GAZEBO_DEFAULT_MASTER_PORT}
)
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBUILDING_DLL_GZ_TRANSPORT")
+if (WIN32)
+ # 0x601 = Windows 7. TBB needs to know this.
+ set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D_WIN32_WINNT=0x601")
+endif ()
gz_add_library(gazebo_transport ${sources})
target_link_libraries(gazebo_transport
gazebo_common
gazebo_msgs
+ ${IGNITION-MATH_LIBRARIES}
${Boost_LIBRARIES}
${TBB_LIBRARIES}
)
+if (WIN32)
+ target_link_libraries(gazebo_transport ws2_32 Iphlpapi)
+endif()
gz_install_library(gazebo_transport)
gz_install_includes("transport" ${headers} ${CMAKE_CURRENT_BINARY_DIR}/transport.hh)
diff --git a/gazebo/transport/CallbackHelper.hh b/gazebo/transport/CallbackHelper.hh
index fda3793..bf5c589 100644
--- a/gazebo/transport/CallbackHelper.hh
+++ b/gazebo/transport/CallbackHelper.hh
@@ -40,7 +40,7 @@ namespace gazebo
/// \class CallbackHelper CallbackHelper.hh transport/transport.hh
/// \brief A helper class to handle callbacks when messages arrive
- class GAZEBO_VISIBLE CallbackHelper
+ class GZ_TRANSPORT_VISIBLE CallbackHelper
{
/// \brief Constructor
/// \param[in] _latching Set to true to make the callback helper
@@ -104,7 +104,7 @@ namespace gazebo
/// \class CallbackHelperT CallbackHelper.hh transport/transport.hh
/// \brief Callback helper Template
template<class M>
- class GAZEBO_VISIBLE CallbackHelperT : public CallbackHelper
+ class CallbackHelperT : public CallbackHelper
{
/// \brief Constructor
/// \param[in] _cb boost function to call on incoming messages
@@ -168,7 +168,7 @@ namespace gazebo
/// \brief Used to connect publishers to subscribers, where the
/// subscriber wants the raw data from the publisher. Raw means that the
/// data has not been converted into a protobuf message.
- class GAZEBO_VISIBLE RawCallbackHelper : public CallbackHelper
+ class GZ_TRANSPORT_VISIBLE RawCallbackHelper : public CallbackHelper
{
/// \brief Constructor
/// \param[in] _cb boost function to call on incoming messages
diff --git a/gazebo/transport/Connection.cc b/gazebo/transport/Connection.cc
index 8cdc0e8..257ab7e 100644
--- a/gazebo/transport/Connection.cc
+++ b/gazebo/transport/Connection.cc
@@ -15,13 +15,36 @@
*
*/
-#include <arpa/inet.h>
-#include <sys/socket.h>
-#include <netdb.h>
-#include <ifaddrs.h>
+#ifdef _WIN32
+ // For socket(), connect(), send(), and recv().
+ #include <Winsock2.h>
+ #include <Ws2def.h>
+ #include <Ws2ipdef.h>
+ #include <Ws2tcpip.h>
+ #include <iphlpapi.h>
+ // Type used for raw data on this platform.
+ typedef char raw_type;
+ #define snprintf _snprintf
+#else
+ // For data types
+ #include <sys/types.h>
+ // For socket(), connect(), send(), and recv()
+ #include <sys/socket.h>
+ // For gethostbyname()
+ #include <netdb.h>
+ // For inet_addr()
+ #include <arpa/inet.h>
+ // For close()
+ #include <unistd.h>
+ // For sockaddr_in
+ #include <netinet/in.h>
+ // Type used for raw data on this platform
+ typedef void raw_type;
+ #include <ifaddrs.h>
+#endif
+
#include <stdio.h>
#include <stdlib.h>
-#include <unistd.h>
#include <boost/lexical_cast.hpp>
@@ -700,6 +723,7 @@ boost::asio::ip::tcp::endpoint Connection::GetLocalEndpoint()
// GAZEBO_HOSTNAME have failed.
if (addressIsUnspecified(address))
{
+#ifndef _WIN32
// the following is *nix implementation to get the external IP of the
// current machine.
@@ -757,6 +781,80 @@ boost::asio::ip::tcp::endpoint Connection::GetLocalEndpoint()
}
freeifaddrs(ifaddr);
+// _WIN32
+#else
+ // Establish our default return value, in case everything below fails.
+ std::string retAddr("127.0.0.1");
+
+ // Look up our address.
+ ULONG outBufLen = 0;
+ PIP_ADAPTER_ADDRESSES addrs = NULL;
+
+ // Not sure whether these are the right flags, but they work
+ // on Windows 7
+ ULONG flags = (GAA_FLAG_SKIP_ANYCAST | GAA_FLAG_SKIP_MULTICAST |
+ GAA_FLAG_SKIP_DNS_SERVER | GAA_FLAG_SKIP_FRIENDLY_NAME);
+
+ // The first time, it'll fail; we're asking how much space is needed to
+ // store the result.
+ GetAdaptersAddresses(AF_INET, flags, NULL, addrs, &outBufLen);
+
+ // Allocate the required space.
+ addrs = new IP_ADAPTER_ADDRESSES[outBufLen];
+ ULONG ret;
+
+ // Now the call should succeed.
+ if ((ret = GetAdaptersAddresses(AF_INET, flags, NULL, addrs, &outBufLen)) ==
+ NO_ERROR)
+ {
+ // Iterate over all returned adapters, arbitrarily sticking with the
+ // last non-loopback one that we find.
+ for (PIP_ADAPTER_ADDRESSES curr = addrs; curr; curr = curr->Next)
+ {
+ // Iterate over all unicast addresses for this adapter
+ for (PIP_ADAPTER_UNICAST_ADDRESS unicast = curr->FirstUnicastAddress;
+ unicast; unicast = unicast->Next)
+ {
+ // Cast to get an IPv4 numeric address (the AF_INET flag used above
+ // ensures that we're only going to get IPv4 address here).
+ sockaddr_in* sockaddress =
+ reinterpret_cast<sockaddr_in*>(unicast->Address.lpSockaddr);
+
+ // Make it a dotted quad
+ char ipv4Str[3*4+3+1];
+
+ snprintf(ipv4Str, sizeof(ipv4Str), "%d.%d.%d.%d",
+ sockaddress->sin_addr.S_un.S_un_b.s_b1,
+ sockaddress->sin_addr.S_un.S_un_b.s_b2,
+ sockaddress->sin_addr.S_un.S_un_b.s_b3,
+ sockaddress->sin_addr.S_un.S_un_b.s_b4);
+
+ // Ignore loopback address (that's our default anyway)
+ if (!strcmp(ipv4Str, "127.0.0.1"))
+ continue;
+
+ retAddr = ipv4Str;
+ }
+ }
+ }
+ else
+ {
+ gzerr << "GetAdaptersAddresses() failed: " << ret << std::endl;
+ }
+
+ delete [] addrs;
+
+ if (retAddr == "127.0.0.1")
+ {
+ gzwarn <<
+ "Couldn't find a preferred IP via the GetAdaptersAddresses() call; "
+ "I'm assuming that your IP "
+ "address is 127.0.0.1. This should work for local processes, "
+ "but will almost certainly not work if you have remote processes."
+ "Report to the disc-zmq development team to seek a fix." << std::endl;
+ }
+ address = boost::asio::ip::address_v4::from_string(retAddr);
+#endif
}
// Complain if we were unable to find a valid address
@@ -771,7 +869,12 @@ boost::asio::ip::tcp::endpoint Connection::GetLocalEndpoint()
bool Connection::ValidateIP(const std::string &_ip)
{
struct sockaddr_in sa;
+
+#ifdef _WIN32
+ int result = InetPton(AF_INET, _ip.c_str(), &(sa.sin_addr));
+#else
int result = inet_pton(AF_INET, _ip.c_str(), &(sa.sin_addr));
+#endif
return result != 0;
}
diff --git a/gazebo/transport/Connection.hh b/gazebo/transport/Connection.hh
index 2ccd3a7..f34c238 100644
--- a/gazebo/transport/Connection.hh
+++ b/gazebo/transport/Connection.hh
@@ -44,7 +44,7 @@ namespace gazebo
{
namespace transport
{
- extern bool is_stopped();
+ extern GZ_TRANSPORT_VISIBLE bool is_stopped();
class IOManager;
class Connection;
@@ -53,7 +53,7 @@ namespace gazebo
/// \cond
/// \brief A task instance that is created when data is read from
/// a socket and used by TBB
- class GAZEBO_VISIBLE ConnectionReadTask : public tbb::task
+ class GZ_TRANSPORT_VISIBLE ConnectionReadTask : public tbb::task
{
/// \brief Constructor
/// \param[_in] _func Boost function pointer, which is the function
@@ -97,7 +97,7 @@ namespace gazebo
///
/// \class Connection Connection.hh transport/transport.hh
/// \brief Single TCP/IP connection manager
- class GAZEBO_VISIBLE Connection :
+ class GZ_TRANSPORT_VISIBLE Connection :
public boost::enable_shared_from_this<Connection>
{
/// \brief Constructor
diff --git a/gazebo/transport/ConnectionManager.cc b/gazebo/transport/ConnectionManager.cc
index 727290d..95bb8f7 100644
--- a/gazebo/transport/ConnectionManager.cc
+++ b/gazebo/transport/ConnectionManager.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "gazebo/msgs/msgs.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Events.hh"
diff --git a/gazebo/transport/ConnectionManager.hh b/gazebo/transport/ConnectionManager.hh
index 0dc3253..fc25b88 100644
--- a/gazebo/transport/ConnectionManager.hh
+++ b/gazebo/transport/ConnectionManager.hh
@@ -40,7 +40,7 @@ namespace gazebo
/// \class ConnectionManager ConnectionManager.hh transport/transport.hh
/// \brief Manager of connections
- class GAZEBO_VISIBLE ConnectionManager :
+ class GZ_TRANSPORT_VISIBLE ConnectionManager :
public SingletonT<ConnectionManager>
{
/// \brief Constructor
diff --git a/gazebo/transport/Connection_TEST.cc b/gazebo/transport/Connection_TEST.cc
index 01225e9..638d612 100644
--- a/gazebo/transport/Connection_TEST.cc
+++ b/gazebo/transport/Connection_TEST.cc
@@ -32,7 +32,6 @@ TEST_F(Connection, IPWhiteList)
transport::Connection *connection = new transport::Connection();
EXPECT_TRUE(connection->GetIPWhiteList().empty());
delete connection;
- connection = NULL;
// Get original value
char *ipEnv = getenv("GAZEBO_IP_WHITE_LIST");
diff --git a/gazebo/transport/IOManager.hh b/gazebo/transport/IOManager.hh
index b643ff1..53030ea 100644
--- a/gazebo/transport/IOManager.hh
+++ b/gazebo/transport/IOManager.hh
@@ -30,7 +30,7 @@ namespace gazebo
/// \class IOManager IOManager.hh transport/transport.hh
/// \brief Manages boost::asio IO
- class GAZEBO_VISIBLE IOManager
+ class GZ_TRANSPORT_VISIBLE IOManager
{
/// \brief Constructor
public: IOManager();
diff --git a/gazebo/transport/Node.cc b/gazebo/transport/Node.cc
index 0186340..419d5d1 100644
--- a/gazebo/transport/Node.cc
+++ b/gazebo/transport/Node.cc
@@ -15,6 +15,12 @@
*
*/
+#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/TransportIface.hh"
#include "gazebo/transport/Node.hh"
@@ -306,7 +312,7 @@ void Node::InsertLatchedMsg(const std::string &_topic, MessagePtr _msg)
std::string Node::GetMsgType(const std::string &_topic) const
{
Callback_M::const_iterator iter = this->callbacks.find(_topic);
- if (iter != this->callbacks.end())
+ if (iter != this->callbacks.end() && !iter->second.empty())
return iter->second.front()->GetMsgType();
return std::string();
diff --git a/gazebo/transport/Node.hh b/gazebo/transport/Node.hh
index b5fa951..ffd9a1e 100644
--- a/gazebo/transport/Node.hh
+++ b/gazebo/transport/Node.hh
@@ -35,7 +35,7 @@ namespace gazebo
{
/// \cond
/// \brief Task used by Node::Publish to publish on a one-time publisher
- class GAZEBO_VISIBLE PublishTask : public tbb::task
+ class GZ_TRANSPORT_VISIBLE PublishTask : public tbb::task
{
/// \brief Constructor
/// \param[in] _pub Publisher to publish the message on.
@@ -74,7 +74,8 @@ namespace gazebo
/// \class Node Node.hh transport/transport.hh
/// \brief A node can advertise and subscribe topics, publish on
/// advertised topics and listen to subscribed topics.
- class GAZEBO_VISIBLE Node : public boost::enable_shared_from_this<Node>
+ class GZ_TRANSPORT_VISIBLE Node :
+ public boost::enable_shared_from_this<Node>
{
/// \brief Constructor
public: Node();
diff --git a/gazebo/transport/Publication.hh b/gazebo/transport/Publication.hh
index 9c4763f..d51a749 100644
--- a/gazebo/transport/Publication.hh
+++ b/gazebo/transport/Publication.hh
@@ -41,7 +41,7 @@ namespace gazebo
/// \class Publication Publication.hh transport/transport.hh
/// \brief A publication for a topic. This facilitates transport of
/// messages
- class GAZEBO_VISIBLE Publication
+ class GZ_TRANSPORT_VISIBLE Publication
{
/// \brief Constructor
/// \param[in] _topic The topic we're publishing
diff --git a/gazebo/transport/PublicationTransport.cc b/gazebo/transport/PublicationTransport.cc
index 564456e..abfb6ee 100644
--- a/gazebo/transport/PublicationTransport.cc
+++ b/gazebo/transport/PublicationTransport.cc
@@ -15,6 +15,12 @@
*
*/
+#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 "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 d810032..f8d5708 100644
--- a/gazebo/transport/PublicationTransport.hh
+++ b/gazebo/transport/PublicationTransport.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// transport/transport.hh
/// \brief Reads data from a remote advertiser, and passes the data
/// along to local subscribers
- class GAZEBO_VISIBLE PublicationTransport
+ class GZ_TRANSPORT_VISIBLE PublicationTransport
{
/// \brief Constructor
/// \param[in] _topic Topic that we're publishing
diff --git a/gazebo/transport/Publisher.cc b/gazebo/transport/Publisher.cc
index 04275da..b6f46d5 100644
--- a/gazebo/transport/Publisher.cc
+++ b/gazebo/transport/Publisher.cc
@@ -18,6 +18,14 @@
* 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 <ignition/math/Helpers.hh>
+
#include "gazebo/common/Exception.hh"
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/TopicManager.hh"
@@ -34,7 +42,7 @@ Publisher::Publisher(const std::string &_topic, const std::string &_msgType,
: topic(_topic), msgType(_msgType), queueLimit(_limit),
updatePeriod(0)
{
- if (!math::equal(_hzRate, 0.0))
+ if (!ignition::math::equal(_hzRate, 0.0))
this->updatePeriod = 1.0 / _hzRate;
this->queueLimitWarned = false;
diff --git a/gazebo/transport/Publisher.hh b/gazebo/transport/Publisher.hh
index 02fcfae..ae2c0d3 100644
--- a/gazebo/transport/Publisher.hh
+++ b/gazebo/transport/Publisher.hh
@@ -40,7 +40,7 @@ namespace gazebo
/// \class Publisher Publisher.hh transport/transport.hh
/// \brief A publisher of messages on a topic
- class GAZEBO_VISIBLE Publisher
+ class GZ_TRANSPORT_VISIBLE Publisher
{
/// \brief Constructor
/// \param[in] _topic Name of topic to be published
diff --git a/gazebo/transport/SubscribeOptions.hh b/gazebo/transport/SubscribeOptions.hh
index 525790b..113e158 100644
--- a/gazebo/transport/SubscribeOptions.hh
+++ b/gazebo/transport/SubscribeOptions.hh
@@ -32,7 +32,7 @@ namespace gazebo
/// \class SubscribeOptions SubscribeOptions.hh transport/transport.hh
/// \brief Options for a subscription
- class GAZEBO_VISIBLE SubscribeOptions
+ class GZ_TRANSPORT_VISIBLE SubscribeOptions
{
/// \brief Constructor
public: SubscribeOptions()
diff --git a/gazebo/transport/Subscriber.cc b/gazebo/transport/Subscriber.cc
index d4c8052..beb0463 100644
--- a/gazebo/transport/Subscriber.cc
+++ b/gazebo/transport/Subscriber.cc
@@ -18,6 +18,12 @@
* 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 "gazebo/transport/TopicManager.hh"
#include "gazebo/transport/Node.hh"
#include "gazebo/transport/Subscriber.hh"
diff --git a/gazebo/transport/Subscriber.hh b/gazebo/transport/Subscriber.hh
index 0ebff22..1827bf7 100644
--- a/gazebo/transport/Subscriber.hh
+++ b/gazebo/transport/Subscriber.hh
@@ -36,7 +36,7 @@ namespace gazebo
/// \class Subscriber Subscriber.hh transport/transport.hh
/// \brief A subscriber to a topic
- class GAZEBO_VISIBLE Subscriber
+ class GZ_TRANSPORT_VISIBLE Subscriber
{
/// \brief Constructor
/// \param[in] _topic The topic we're subscribing to
diff --git a/gazebo/transport/SubscriptionTransport.cc b/gazebo/transport/SubscriptionTransport.cc
index efbf354..0036be4 100644
--- a/gazebo/transport/SubscriptionTransport.cc
+++ b/gazebo/transport/SubscriptionTransport.cc
@@ -14,6 +14,13 @@
* 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 "gazebo/transport/ConnectionManager.hh"
#include "gazebo/transport/SubscriptionTransport.hh"
diff --git a/gazebo/transport/SubscriptionTransport.hh b/gazebo/transport/SubscriptionTransport.hh
index 00b66fe..f66cfc3 100644
--- a/gazebo/transport/SubscriptionTransport.hh
+++ b/gazebo/transport/SubscriptionTransport.hh
@@ -35,7 +35,7 @@ namespace gazebo
/// transport/transport.hh
/// \brief Handles sending data over the wire to
/// remote subscribers
- class GAZEBO_VISIBLE SubscriptionTransport : public CallbackHelper
+ class GZ_TRANSPORT_VISIBLE SubscriptionTransport : public CallbackHelper
{
/// \brief Constructor
public: SubscriptionTransport();
diff --git a/gazebo/transport/TopicManager.cc b/gazebo/transport/TopicManager.cc
index f3ca8d4..850fbe7 100644
--- a/gazebo/transport/TopicManager.cc
+++ b/gazebo/transport/TopicManager.cc
@@ -15,6 +15,12 @@
*
*/
+#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 <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
diff --git a/gazebo/transport/TopicManager.hh b/gazebo/transport/TopicManager.hh
index a003c82..fe41139 100644
--- a/gazebo/transport/TopicManager.hh
+++ b/gazebo/transport/TopicManager.hh
@@ -48,7 +48,7 @@ namespace gazebo
/// \class TopicManager TopicManager.hh transport/transport.hh
/// \brief Manages topics and their subscriptions
- class GAZEBO_VISIBLE TopicManager : public SingletonT<TopicManager>
+ class GZ_TRANSPORT_VISIBLE TopicManager : public SingletonT<TopicManager>
{
private: TopicManager();
private: virtual ~TopicManager();
diff --git a/gazebo/transport/TransportIface.cc b/gazebo/transport/TransportIface.cc
index 6ebc503..3b34ea2 100644
--- a/gazebo/transport/TransportIface.cc
+++ b/gazebo/transport/TransportIface.cc
@@ -14,6 +14,12 @@
* 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 <list>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
diff --git a/gazebo/transport/TransportIface.hh b/gazebo/transport/TransportIface.hh
index b8f3791..7f313fd 100644
--- a/gazebo/transport/TransportIface.hh
+++ b/gazebo/transport/TransportIface.hh
@@ -41,7 +41,7 @@ namespace gazebo
/// \param[out] _master_port The port of the master is set to this param
/// \return true if GAZEBO_MASTER_URI was successfully parsed; false
/// otherwise (in which case output params are not set)
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
bool get_master_uri(std::string &_master_host, unsigned int &_master_port);
/// \brief Initialize the transport system
@@ -52,7 +52,7 @@ namespace gazebo
/// \param[in] _timeoutIterations Number of times to wait for
/// a connection to master.
/// \return true if initialization succeeded; false otherwise
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
bool init(const std::string &_masterHost = "",
unsigned int _masterPort = 0,
uint32_t _timeoutIterations = 30);
@@ -60,35 +60,35 @@ namespace gazebo
/// \brief Run the transport component. Creates a thread to handle
/// message passing. This call will block until the master can
/// be contacted or until a retry limit is reached
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
void run();
/// \brief Return all the namespace (world names) on the master
/// \param[out] _namespaces The list of namespace will be written here
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
void get_topic_namespaces(std::list<std::string> &_namespaces);
/// \brief Is the transport system stopped?
/// \return true if the transport system is stopped; false otherwise
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
bool is_stopped();
/// \brief Stop the transport component from running.
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
void stop();
/// \brief Cleanup the transport component
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
void fini();
/// \brief Clear any remaining communication buffers
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
void clear_buffers();
/// \brief Pause or unpause incoming messages. When paused, messages
/// are queued for later delivery
/// \param[in] _pause If true, pause; otherwise unpause
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
void pause_incoming(bool _pause);
/// \brief Send a request and receive a response. This call will block
@@ -98,7 +98,7 @@ namespace gazebo
/// \param[in] _request The type request.
/// \param[in] _data Optional data string.
/// \return The response to the request. Can be empty.
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
boost::shared_ptr<msgs::Response> request(const std::string &_worldName,
const std::string &_request,
const std::string &_data = "");
@@ -109,7 +109,7 @@ namespace gazebo
/// should be sent.
/// \param[in] _request The type request.
/// \param[in] _data Optional data string.
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
void requestNoReply(const std::string &_worldName,
const std::string &_request,
const std::string &_data = "");
@@ -119,7 +119,7 @@ namespace gazebo
/// \param[in] _node Pointer to a node that provides communication.
/// \param[in] _request The type request.
/// \param[in] _data Optional data string.
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
void requestNoReply(NodePtr _node, const std::string &_request,
const std::string &_data = "");
@@ -130,7 +130,6 @@ namespace gazebo
/// \param[in] _topic The topic to advertise
/// \param[in] _message Message to be published
template<typename M>
- GAZEBO_VISIBLE
void publish(const std::string &_topic,
const google::protobuf::Message &_message)
{
@@ -142,7 +141,7 @@ namespace gazebo
/// \brief Get a list of all the topics and their message types.
/// \return A map where keys are message types, and values are a list
/// of topic names.
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
std::map<std::string, std::list<std::string> > getAdvertisedTopics();
/// \brief Get a list of all the unique advertised topic names.
@@ -150,34 +149,34 @@ namespace gazebo
/// empty, then a list of all the topics is returned.
/// \return A list of the advertised topics that publish messages
/// of the type specified by _msgType.
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
std::list<std::string> getAdvertisedTopics(const std::string &_msgType);
/// \brief Get the message typename that is published on the given topic.
/// \param[in] _topicName Name of the topic to query.
/// \return The message type, or empty string if the topic is not valid.
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
std::string getTopicMsgType(const std::string &_topicName);
/// \brief Set whether minimal comms should be used. This will be used
/// to reduce network traffic.
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
void setMinimalComms(bool _enabled);
/// \brief Get whether minimal comms has been enabled.
/// \return True if minimal comms is enabled.
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
bool getMinimalComms();
/// \brief Create a connection to master.
/// \return Connection to the master, NULL on error.
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
transport::ConnectionPtr connectToMaster();
/// \brief Blocks while waiting for topic namespaces from the Master.
/// This function will wait a maximum of _maxWait.
/// \return True if namespaces were found before _maxWait time.
- GAZEBO_VISIBLE
+ GZ_TRANSPORT_VISIBLE
bool waitForNamespaces(const gazebo::common::Time &_maxWait);
/// \}
}
diff --git a/gazebo/util/CMakeLists.txt b/gazebo/util/CMakeLists.txt
index e2f017f..8d499ab 100644
--- a/gazebo/util/CMakeLists.txt
+++ b/gazebo/util/CMakeLists.txt
@@ -1,16 +1,30 @@
include (${gazebo_cmake_dir}/GazeboUtils.cmake)
+add_definitions(-DBUILDING_DLL_GZ_UTIL)
+
if (HAVE_OPENAL)
include_directories(${OPENAL_INCLUDE_DIR})
endif()
-set (sources
+include_directories(${TBB_INCLUDEDIR})
+
+include_directories(${tinyxml_INCLUDE_DIRS})
+set (sources
Diagnostics.cc
LogPlay.cc
LogRecord.cc
OpenAL.cc
)
+if (NOT USE_EXTERNAL_TINYXML)
+ include_directories (${CMAKE_SOURCE_DIR}/deps/win/tinyxml)
+ set (sources ${sources}
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinystr.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxml.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxmlerror.cpp
+ ${CMAKE_SOURCE_DIR}/deps/win/tinyxml/tinyxmlparser.cpp)
+endif()
+
set (headers
Diagnostics.hh
LogPlay.hh
@@ -22,6 +36,7 @@ set (headers
set (gtest_sources
Diagnostics_TEST.cc
+ LogPlay_TEST.cc
LogRecord_TEST.cc
OpenAL_TEST.cc
)
diff --git a/gazebo/util/Diagnostics.cc b/gazebo/util/Diagnostics.cc
index e2c5d3d..eff89db 100644
--- a/gazebo/util/Diagnostics.cc
+++ b/gazebo/util/Diagnostics.cc
@@ -14,12 +14,15 @@
* limitations under the License.
*
*/
-/* Desc: A diagnostic class
- * Author: Nate Koenig
- * Date: 2 Feb 2011
- */
+#ifdef _WIN32
+ #include <algorithm>
+ // Ensure that Winsock2.h is included before Windows.h, which can get
+ // pulled in by anybody (e.g., Boost).
+ #include <Winsock2.h>
+#endif
#include "gazebo/common/Assert.hh"
+#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/Events.hh"
#include "gazebo/common/SystemPaths.hh"
#include "gazebo/transport/transport.hh"
@@ -31,8 +34,15 @@ using namespace util;
//////////////////////////////////////////////////
DiagnosticManager::DiagnosticManager()
{
+#ifndef _WIN32
+ const char *homePath = common::getEnv("HOME");
+#else
+ const char *homePath = common::getEnv("HOMEPATH");
+#endif
+ this->logPath = homePath;
+
// Get the base of the time logging path
- if (!getenv("HOME"))
+ if (!homePath)
{
common::SystemPaths *paths = common::SystemPaths::Instance();
gzwarn << "HOME environment variable missing. Diagnostic timing " <<
@@ -41,12 +51,16 @@ DiagnosticManager::DiagnosticManager()
}
else
{
- this->logPath = getenv("HOME");
this->logPath /= ".gazebo";
}
- this->logPath = this->logPath / "diagnostics" /
- common::Time::GetWallTimeAsISOString();
+ std::string timeStr = common::Time::GetWallTimeAsISOString();
+
+#ifdef _WIN32
+ std::replace(timeStr.begin(), timeStr.end(), ':', '_');
+#endif
+
+ this->logPath = this->logPath / "diagnostics" / timeStr;
// Make sure the path exists.
if (!boost::filesystem::exists(this->logPath))
diff --git a/gazebo/util/Diagnostics.hh b/gazebo/util/Diagnostics.hh
index dc06874..23ee4c6 100644
--- a/gazebo/util/Diagnostics.hh
+++ b/gazebo/util/Diagnostics.hh
@@ -14,10 +14,6 @@
* limitations under the License.
*
*/
-/* Desc: A diagnostic class
- * Author: Nate Koenig
- * Date: 2 Feb 2011
- */
#ifndef _DIAGNOSTICMANAGER_HH_
#define _DIAGNOSTICMANAGER_HH_
@@ -73,7 +69,7 @@ namespace gazebo
/// \class DiagnosticManager Diagnostics.hh util/util.hh
/// \brief A diagnostic manager class
- class GAZEBO_VISIBLE DiagnosticManager :
+ class GZ_UTIL_VISIBLE DiagnosticManager :
public SingletonT<DiagnosticManager>
{
/// \brief Constructor
@@ -168,7 +164,7 @@ namespace gazebo
/// \class DiagnosticTimer Diagnostics.hh util/util.hh
/// \brief A timer designed for diagnostics
- class GAZEBO_VISIBLE DiagnosticTimer : public common::Timer
+ class GZ_UTIL_VISIBLE DiagnosticTimer : public common::Timer
{
/// \brief Constructor
/// \param[in] _name Name of the timer
diff --git a/gazebo/util/LogPlay.cc b/gazebo/util/LogPlay.cc
index 3ae867d..2a58489 100644
--- a/gazebo/util/LogPlay.cc
+++ b/gazebo/util/LogPlay.cc
@@ -14,6 +14,11 @@
* 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/filesystem.hpp>
#include <boost/lexical_cast.hpp>
@@ -27,7 +32,7 @@
#include <boost/archive/iterators/istream_iterator.hpp>
#include <boost/archive/iterators/transform_width.hpp>
-#include "gazebo/math/Rand.hh"
+#include <ignition/math/Rand.hh>
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Console.hh"
@@ -54,7 +59,10 @@ void LogPlay::Open(const std::string &_logFile)
{
boost::filesystem::path path(_logFile);
if (!boost::filesystem::exists(path))
- gzthrow("Invalid logfile[" + _logFile + "]. Does not exist.");
+ gzthrow("Invalid logfile [" + _logFile + "]. Does not exist.");
+
+ if (boost::filesystem::is_directory(path))
+ gzthrow("Invalid logfile [" + _logFile + "]. This is a directory.");
// Parse the log file
if (!this->xmlDoc.LoadFile(_logFile))
@@ -74,6 +82,12 @@ void LogPlay::Open(const std::string &_logFile)
this->logCurrXml = this->logStartXml;
this->encoding.clear();
+
+ // Extract the start/end log times from the log.
+ this->ReadLogTimes();
+
+ // Extract the initial "iterations" value from the log.
+ this->iterationsFound = this->ReadIterations();
}
/////////////////////////////////////////////////
@@ -86,15 +100,30 @@ std::string LogPlay::GetHeader() const
<< "<log_version>" << this->logVersion << "</log_version>\n"
<< "<gazebo_version>" << this->gazeboVersion << "</gazebo_version>\n"
<< "<rand_seed>" << this->randSeed << "</rand_seed>\n"
+ << "<log_start>" << this->logStartTime << "</log_start>\n"
+ << "<log_end>" << this->logEndTime << "</log_end>\n"
<< "</header>\n";
return stream.str();
}
/////////////////////////////////////////////////
+uint64_t LogPlay::GetInitialIterations() const
+{
+ return this->initialIterations;
+}
+
+/////////////////////////////////////////////////
+bool LogPlay::HasIterations() const
+{
+ return this->iterationsFound;
+}
+
+
+/////////////////////////////////////////////////
void LogPlay::ReadHeader()
{
- this->randSeed = math::Rand::GetSeed();
+ this->randSeed = ignition::math::Rand::Seed();
TiXmlElement *headerXml, *childXml;
this->logVersion.clear();
@@ -112,6 +141,15 @@ void LogPlay::ReadHeader()
else
this->logVersion = childXml->GetText();
+ if (this->logVersion != GZ_LOG_VERSION)
+ {
+ gzwarn << "Log version[" << this->logVersion << "] in file["
+ << this->filename
+ << "] does not match Gazebo's log version["
+ << GZ_LOG_VERSION << "]\n";
+ return;
+ }
+
// Get the gazebo version
childXml = headerXml->FirstChildElement("gazebo_version");
if (!childXml)
@@ -126,14 +164,101 @@ void LogPlay::ReadHeader()
else
this->randSeed = boost::lexical_cast<uint32_t>(childXml->GetText());
- if (this->logVersion != GZ_LOG_VERSION)
- gzwarn << "Log version[" << this->logVersion << "] in file["
- << this->filename
- << "] does not match Gazebo's log version["
- << GZ_LOG_VERSION << "]\n";
-
/// Set the random number seed for simulation
- math::Rand::SetSeed(this->randSeed);
+ ignition::math::Rand::Seed(this->randSeed);
+}
+
+/////////////////////////////////////////////////
+void LogPlay::ReadLogTimes()
+{
+ if (this->GetChunkCount() < 2u)
+ {
+ gzwarn << "Unable to extract log timing information. No chunks available "
+ << "with <sim_time> information." << std::endl;
+ return;
+ }
+
+ const std::string kStartDelim = "<sim_time>";
+ const std::string kEndDelim = "</sim_time>";
+ std::string chunk;
+
+ // Read the start time of the log from the first chunk.
+ this->GetChunk(1, chunk);
+
+ // Find the first <sim_time> of the log.
+ auto from = chunk.find(kStartDelim);
+ auto to = chunk.find(kEndDelim, from + kStartDelim.size());
+ if (from != std::string::npos && to != std::string::npos)
+ {
+ auto length = to - from - kStartDelim.size();
+ std::string startTime = chunk.substr(from + kStartDelim.size(), length);
+ std::stringstream ss(startTime);
+ ss >> this->logStartTime;
+ }
+ else
+ {
+ gzwarn << "Unable to find <sim_time>...</sim_time> tags in the first chunk."
+ << std::endl;
+ return;
+ }
+
+ this->GetChunk(this->GetChunkCount() - 1, chunk);
+
+ // Update the last <sim_time> of the log.
+ to = chunk.rfind(kEndDelim);
+ from = chunk.rfind(kStartDelim, to - 1);
+
+ if (from != std::string::npos && to != std::string::npos)
+ {
+ auto length = to - from - kStartDelim.size();
+ std::string endTime = chunk.substr(from + kStartDelim.size(), length);
+ std::stringstream ss(endTime);
+ ss >> this->logEndTime;
+ }
+ else
+ {
+ gzwarn << "Unable to find <sim_time>...</sim_time> tags in the last chunk."
+ << std::endl;
+ return;
+ }
+}
+
+/////////////////////////////////////////////////
+bool LogPlay::ReadIterations()
+{
+ if (this->GetChunkCount() < 2u)
+ {
+ gzwarn << "Unable to extract iteration information. No chunks available "
+ << "with <iterations> information. Assuming that the first "
+ << "<iterations> value is 0." << std::endl;
+ return false;
+ }
+
+ const std::string kStartDelim = "<iterations>";
+ const std::string kEndDelim = "</iterations>";
+ std::string chunk;
+
+ // Read the first "iterations" value of the log from the first chunk.
+ this->GetChunk(1, chunk);
+
+ // Find the first <iterations> of the log.
+ auto from = chunk.find(kStartDelim);
+ auto to = chunk.find(kEndDelim, from + kStartDelim.size());
+ if (from != std::string::npos && to != std::string::npos)
+ {
+ auto length = to - from - kStartDelim.size();
+ std::string iterations = chunk.substr(from + kStartDelim.size(), length);
+ std::stringstream ss(iterations);
+ ss >> this->initialIterations;
+ return true;
+ }
+ else
+ {
+ gzwarn << "Unable to find <iterations>...</iterations> tags in the first "
+ << "chunk. Assuming that the first <iterations> value is 0."
+ << std::endl;
+ return false;
+ }
}
/////////////////////////////////////////////////
@@ -161,8 +286,42 @@ uint32_t LogPlay::GetRandSeed() const
}
/////////////////////////////////////////////////
+common::Time LogPlay::GetLogStartTime() const
+{
+ return this->logStartTime;
+}
+
+/////////////////////////////////////////////////
+common::Time LogPlay::GetLogEndTime() const
+{
+ return this->logEndTime;
+}
+
+/////////////////////////////////////////////////
+std::string LogPlay::GetFilename() const
+{
+ return boost::filesystem::basename(this->filename) +
+ boost::filesystem::extension(this->filename);
+}
+
+/////////////////////////////////////////////////
+std::string LogPlay::GetFullPathFilename() const
+{
+ const boost::filesystem::path logFilename(this->filename);
+ return boost::filesystem::canonical(logFilename).string();
+}
+
+/////////////////////////////////////////////////
+uintmax_t LogPlay::GetFileSize() const
+{
+ return boost::filesystem::file_size(this->filename);
+}
+
+/////////////////////////////////////////////////
bool LogPlay::Step(std::string &_data)
{
+ std::lock_guard<std::mutex> lock(this->mutex);
+
std::string startMarker = "<sdf ";
std::string endMarker = "</sdf>";
size_t start = this->currentChunk.find(startMarker);
@@ -203,6 +362,22 @@ bool LogPlay::Step(std::string &_data)
}
/////////////////////////////////////////////////
+bool LogPlay::Rewind()
+{
+ std::lock_guard<std::mutex> lock(this->mutex);
+
+ this->currentChunk.clear();
+ this->logCurrXml = this->logStartXml->FirstChildElement("chunk");
+ if (!logCurrXml)
+ {
+ gzerr << "Unable to jump to the beginning of the log file\n";
+ return false;
+ }
+
+ return true;
+}
+
+/////////////////////////////////////////////////
bool LogPlay::GetChunk(unsigned int _index, std::string &_data)
{
unsigned int count = 0;
diff --git a/gazebo/util/LogPlay.hh b/gazebo/util/LogPlay.hh
index 9144c3a..c4e8e84 100644
--- a/gazebo/util/LogPlay.hh
+++ b/gazebo/util/LogPlay.hh
@@ -14,17 +14,17 @@
* limitations under the License.
*
*/
-
-#ifndef _LOGPLAY_HH_
-#define _LOGPLAY_HH_
+#ifndef _GAZEBO_LOGPLAY_HH_
+#define _GAZEBO_LOGPLAY_HH_
#include <tinyxml.h>
#include <list>
+#include <mutex>
#include <string>
-#include <fstream>
#include "gazebo/common/SingletonT.hh"
+#include "gazebo/common/Time.hh"
#include "gazebo/util/system.hh"
namespace gazebo
@@ -44,7 +44,7 @@ namespace gazebo
/// state information to a World.
///
/// \sa LogRecord, State
- class GAZEBO_VISIBLE LogPlay : public SingletonT<LogPlay>
+ class GZ_UTIL_VISIBLE LogPlay : public SingletonT<LogPlay>
{
/// \brief Constructor
private: LogPlay();
@@ -56,7 +56,8 @@ namespace gazebo
///
/// Open a log file that was previously recorded.
/// \param[in] _logFile The file to load
- /// \throws Exception
+ /// \throws Exception When the log file does not exist, is a directory
+ /// instead of a regular file, or Gazebo was unable to parse it.
public: void Open(const std::string &_logFile);
/// \brief Return true if a file is open.
@@ -75,13 +76,38 @@ namespace gazebo
/// \brief Get the random number seed of the open log file.
/// \return The random number seed the open log file. The current
- /// random number seed, as defined in math::Rand::GetSeed.
+ /// random number seed, as defined in ignition::math::Rand::Seed.
public: uint32_t GetRandSeed() const;
+ /// \brief Get the log start time of the open log file.
+ /// \return Start time of the log.
+ public: common::Time GetLogStartTime() const;
+
+ /// \brief Get the log end time of the open log file.
+ /// \return End time of the log.
+ public: common::Time GetLogEndTime() const;
+
+ /// \brief Get the name of the log file.
+ /// \return The filename.
+ public: std::string GetFilename() const;
+
+ /// \brief Get the full path of the log file.
+ /// \return The full path of the log file.
+ public: std::string GetFullPathFilename() const;
+
+ /// \brief Get the size of the log file.
+ /// \return The size of the file in bytes.
+ public: uintmax_t GetFileSize() const;
+
/// \brief Step through the open log file.
/// \param[out] _data Data from next entry in the log file.
public: bool Step(std::string &_data);
+ /// \brief Jump to the beginning of the log file. The next step() call
+ /// will return the first data "chunk".
+ /// \return True If the function succeed or false otherwise.
+ public: bool Rewind();
+
/// \brief Get the number of chunks (steps) in the open log file.
/// \return The number of recorded states in the log file.
public: unsigned int GetChunkCount() const;
@@ -103,6 +129,16 @@ namespace gazebo
/// \return Header of the open log file.
public: std::string GetHeader() const;
+ /// \brief Get the initial simulation iterations from a log file.
+ /// \return Initial simulation iteration contained in the log file.
+ public: uint64_t GetInitialIterations() const;
+
+ /// \brief Return if the log file contains the <iterations> tag. Old log
+ /// files may not have the <iterations> tag.
+ /// \return True if <iterations> was found in the log file or
+ /// false otherwise.
+ public: bool HasIterations() const;
+
/// \brief Helper function to get chunk data from XML.
/// \param[in] _xml Pointer to an xml block that has state data.
/// \param[out] _data Storage for the chunk's data.
@@ -112,6 +148,14 @@ namespace gazebo
/// \brief Read the header from the log file.
private: void ReadHeader();
+ /// \brief Update the internal variables that keep track of the times
+ /// where the log started and finished (simulation time).
+ private: void ReadLogTimes();
+
+ /// \brief Update the internal variable that keeps track of the initial
+ /// "iterations" value.
+ private: bool ReadIterations();
+
/// \brief The XML document of the log file.
private: TiXmlDocument xmlDoc;
@@ -134,11 +178,27 @@ namespace gazebo
/// \brief The random number seed recorded in the open log file.
private: uint32_t randSeed;
+ /// \brief Log start time (simulation time).
+ private: common::Time logStartTime;
+
+ /// \brief Log end time (simulation time).
+ private: common::Time logEndTime;
+
/// \brief The encoding for the current chunk in the log file.
private: std::string encoding;
private: std::string currentChunk;
+ /// \brief Initial simulation iteration contained in the log file.
+ private: uint64_t initialIterations;
+
+ /// \brief True if <iterations> is found in the log file. Old log versions
+ /// may not include this tag in the log files.
+ private: bool iterationsFound;
+
+ /// \brief A mutex to avoid race conditions.
+ private: std::mutex mutex;
+
/// \brief This is a singleton
private: friend class SingletonT<LogPlay>;
};
diff --git a/gazebo/util/LogPlay_TEST.cc b/gazebo/util/LogPlay_TEST.cc
new file mode 100644
index 0000000..b26a499
--- /dev/null
+++ b/gazebo/util/LogPlay_TEST.cc
@@ -0,0 +1,186 @@
+/*
+ * 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 <boost/filesystem.hpp>
+#include <string>
+#include "gazebo/common/CommonIface.hh"
+#include "gazebo/common/Time.hh"
+#include "gazebo/util/LogPlay.hh"
+#include "test_config.h"
+#include "test/util.hh"
+
+class LogPlay_TEST : public gazebo::testing::AutoLogFixture { };
+
+/////////////////////////////////////////////////
+/// \brief Test LogPlay Open.
+TEST_F(LogPlay_TEST, Open)
+{
+ gazebo::util::LogPlay *player = gazebo::util::LogPlay::Instance();
+ EXPECT_FALSE(player->IsOpen());
+
+ // Open a file that does not exist.
+ EXPECT_ANY_THROW(player->Open("non-existing-file"));
+ EXPECT_FALSE(player->IsOpen());
+
+ boost::filesystem::path logFilePath(TEST_PATH);
+
+ // Open a file that is a directory.
+ EXPECT_ANY_THROW(player->Open(logFilePath.string()));
+ EXPECT_FALSE(player->IsOpen());
+
+ logFilePath = TEST_PATH / boost::filesystem::path("logs");
+ logFilePath /= boost::filesystem::path("invalidHeader.log");
+
+ // Open a malformed log file (incorrect header).
+ EXPECT_ANY_THROW(player->Open(logFilePath.string()));
+ EXPECT_FALSE(player->IsOpen());
+
+ logFilePath = TEST_PATH / boost::filesystem::path("logs");
+ logFilePath /= boost::filesystem::path("state.log");
+
+ // Open a correct log file.
+ EXPECT_NO_THROW(player->Open(logFilePath.string()));
+ EXPECT_TRUE(player->IsOpen());
+}
+
+/////////////////////////////////////////////////
+/// \brief Test LogPlay accessors.
+TEST_F(LogPlay_TEST, Accessors)
+{
+ gazebo::common::Time expectedStartTime(28, 457000000);
+ gazebo::common::Time expectedEndTime(31, 745000000);
+ std::ostringstream expectedHeader;
+
+ expectedHeader << "<?xml version='1.0'?>\n"
+ << "<gazebo_log>\n"
+ << "<header>\n"
+ << "<log_version>1.0</log_version>\n"
+ << "<gazebo_version>6.0.0</gazebo_version>\n"
+ << "<rand_seed>27838</rand_seed>\n"
+ << "<log_start>" << expectedStartTime << "</log_start>\n"
+ << "<log_end>" << expectedEndTime << "</log_end>\n"
+ << "</header>\n";
+
+ gazebo::util::LogPlay *player = gazebo::util::LogPlay::Instance();
+
+ // Open a correct log file.
+ boost::filesystem::path logFilePath(TEST_PATH);
+ logFilePath /= boost::filesystem::path("logs");
+ logFilePath /= boost::filesystem::path("state.log");
+
+ EXPECT_NO_THROW(player->Open(logFilePath.string()));
+
+ // Test the accessors.
+ EXPECT_EQ(player->GetLogVersion(), "1.0");
+ EXPECT_EQ(player->GetGazeboVersion(), "6.0.0");
+ EXPECT_EQ(player->GetRandSeed(), 27838u);
+ EXPECT_EQ(player->GetLogStartTime(), expectedStartTime);
+ EXPECT_EQ(player->GetLogEndTime(), expectedEndTime);
+ EXPECT_EQ(player->GetFilename(), "state.log");
+ EXPECT_EQ(player->GetFullPathFilename(), logFilePath.string());
+ EXPECT_EQ(player->GetFileSize(), 341608u);
+ EXPECT_EQ(player->GetEncoding(), "zlib");
+ EXPECT_EQ(player->GetHeader(), expectedHeader.str());
+ EXPECT_EQ(player->GetChunkCount(), 5u);
+ EXPECT_FALSE(player->HasIterations());
+ EXPECT_EQ(player->GetInitialIterations(), 0u);
+
+ std::string chunk;
+ EXPECT_TRUE(player->GetChunk(0, chunk));
+
+ // Open a correct log file including <iterations>.
+ logFilePath = TEST_PATH;
+ logFilePath /= boost::filesystem::path("logs");
+ logFilePath /= boost::filesystem::path("state2.log");
+
+ EXPECT_NO_THROW(player->Open(logFilePath.string()));
+ EXPECT_TRUE(player->HasIterations());
+ EXPECT_EQ(player->GetInitialIterations(), 23700u);
+}
+
+/////////////////////////////////////////////////
+/// \brief Test LogPlay chunks.
+TEST_F(LogPlay_TEST, Chunks)
+{
+ std::string chunk;
+
+ gazebo::util::LogPlay *player = gazebo::util::LogPlay::Instance();
+
+ // Open a correct log file.
+ boost::filesystem::path logFilePath(TEST_PATH);
+ logFilePath /= boost::filesystem::path("logs");
+ logFilePath /= boost::filesystem::path("state.log");
+
+ EXPECT_NO_THROW(player->Open(logFilePath.string()));
+
+ // Verify the content of chunk #0.
+ player->GetChunk(0, chunk);
+ std::string shasum = gazebo::common::get_sha1<std::string>(chunk);
+ EXPECT_EQ(shasum, "aa227eee0554b8ace3a033e90b4f0c247909db33");
+
+ // Make sure that the chunks returned are not empty.
+ for (unsigned int i = 0; i < player->GetChunkCount(); ++i)
+ {
+ EXPECT_TRUE(player->GetChunk(i, chunk));
+ EXPECT_TRUE(!chunk.empty());
+ }
+
+ // Try incorrect chunk indexes.
+ EXPECT_FALSE(player->GetChunk(-1, chunk));
+ EXPECT_FALSE(player->GetChunk(player->GetChunkCount(), chunk));
+}
+
+/////////////////////////////////////////////////
+/// \brief Test Rewind().
+TEST_F(LogPlay_TEST, Rewind)
+{
+ gazebo::util::LogPlay *player = gazebo::util::LogPlay::Instance();
+
+ // Open a correct log file.
+ boost::filesystem::path logFilePath(TEST_PATH);
+ logFilePath /= boost::filesystem::path("logs");
+ logFilePath /= boost::filesystem::path("state.log");
+
+ EXPECT_NO_THROW(player->Open(logFilePath.string()));
+
+ // Read the first entry in the log file.
+ std::string firstEntry;
+ // Consume the first chunk because it does not have <state>
+ // ToDo: Fix this.
+ EXPECT_TRUE(player->Step(firstEntry));
+ // Read the first world state.
+ EXPECT_TRUE(player->Step(firstEntry));
+
+ // Step a few more times.
+ std::string logEntry;
+ for (int i = 0; i < 5; ++i)
+ EXPECT_TRUE(player->Step(logEntry));
+
+ // Rewind and read the first entry again.
+ EXPECT_TRUE(player->Rewind());
+ std::string entry;
+ EXPECT_TRUE(player->Step(entry));
+ EXPECT_EQ(entry, firstEntry);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/gazebo/util/LogRecord.cc b/gazebo/util/LogRecord.cc
index 7ec49e7..923cd71 100644
--- a/gazebo/util/LogRecord.cc
+++ b/gazebo/util/LogRecord.cc
@@ -15,6 +15,21 @@
*
*/
+#ifdef _WIN32
+ // Ensure that Winsock2.h is included before Windows.h, which can get
+ // pulled in by anybody (e.g., Boost).
+ #include <Winsock2.h>
+
+ #include <io.h>
+
+ // Seems like W_OK does not exists on Windows.
+ // Reading access function documentation, the value should be 2
+ // http://msdn.microsoft.com/en-us/library/1w06ktdy.aspx
+ // Test for write permission.
+ #define W_OK 2
+ #define access _access
+#endif
+
#include <boost/date_time.hpp>
#include <boost/filesystem.hpp>
#include <boost/iostreams/filter/bzip2.hpp>
@@ -23,6 +38,9 @@
#include <boost/iostreams/copy.hpp>
#include <iomanip>
+#include <ignition/math/Rand.hh>
+
+#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/Assert.hh"
#include "gazebo/common/Base64.hh"
#include "gazebo/common/Console.hh"
@@ -31,7 +49,6 @@
#include "gazebo/common/Time.hh"
#include "gazebo/common/SystemPaths.hh"
#include "gazebo/gazebo_config.h"
-#include "gazebo/math/Rand.hh"
#include "gazebo/transport/transport.hh"
#include "gazebo/util/LogRecord.hh"
@@ -50,9 +67,12 @@ LogRecord::LogRecord()
this->readyToStart = false;
// Get the user's home directory
- // \todo getenv is not portable, and there is no generic cross-platform
- // method. Must check OS and choose a method
- char *homePath = getenv("HOME");
+#ifndef _WIN32
+ const char *homePath = common::getEnv("HOME");
+#else
+ const char *homePath = common::getEnv("HOMEPATH");
+#endif
+
GZ_ASSERT(homePath, "HOME environment variable is missing");
if (!homePath)
@@ -552,7 +572,7 @@ unsigned int LogRecord::Log::Update()
std::string data = stream.str();
if (!data.empty())
{
- const std::string encodingLocal = this->parent->GetEncoding();
+ const std::string &encodingLocal = this->parent->GetEncoding();
this->buffer.append("<chunk encoding='");
this->buffer.append(encodingLocal);
@@ -652,8 +672,8 @@ void LogRecord::Log::Start(const boost::filesystem::path &_path)
// Make sure the file does not exist
if (boost::filesystem::exists(this->completePath))
- gzlog << "Filename[" + this->completePath.string() + "], already exists."
- << " The log file will be overwritten.\n";
+ gzlog << "Filename [" + this->completePath.string() + "], already exists."
+ << " The log file will be overwritten.\n";
std::ostringstream stream;
stream << "<?xml version='1.0'?>\n"
@@ -661,7 +681,7 @@ void LogRecord::Log::Start(const boost::filesystem::path &_path)
<< "<header>\n"
<< "<log_version>" << GZ_LOG_VERSION << "</log_version>\n"
<< "<gazebo_version>" << GAZEBO_VERSION_FULL << "</gazebo_version>\n"
- << "<rand_seed>" << math::Rand::GetSeed() << "</rand_seed>\n"
+ << "<rand_seed>" << ignition::math::Rand::Seed() << "</rand_seed>\n"
<< "</header>\n";
this->buffer.append(stream.str());
diff --git a/gazebo/util/LogRecord.hh b/gazebo/util/LogRecord.hh
index a19d534..3a4ed21 100644
--- a/gazebo/util/LogRecord.hh
+++ b/gazebo/util/LogRecord.hh
@@ -65,7 +65,7 @@ namespace gazebo
/// guarantees that all data is stored.
///
/// \sa Logplay, State
- class GAZEBO_VISIBLE LogRecord : public SingletonT<LogRecord>
+ class GZ_UTIL_VISIBLE LogRecord : public SingletonT<LogRecord>
{
/// \brief Constructor
private: LogRecord();
diff --git a/gazebo/util/LogRecord_TEST.cc b/gazebo/util/LogRecord_TEST.cc
index be825dc..b4e4893 100644
--- a/gazebo/util/LogRecord_TEST.cc
+++ b/gazebo/util/LogRecord_TEST.cc
@@ -18,6 +18,7 @@
#include <gtest/gtest.h>
#include <boost/filesystem.hpp>
+#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/SystemPaths.hh"
@@ -33,7 +34,12 @@ TEST_F(LogRecord_TEST, Constructor)
{
gazebo::util::LogRecord *recorder = gazebo::util::LogRecord::Instance();
- char *homePath = getenv("HOME");
+#ifndef _WIN32
+ const char *homePath = common::getEnv("HOME");
+#else
+ const char *homePath = common::getEnv("HOMEPATH");
+#endif
+
EXPECT_TRUE(homePath != NULL);
common::SystemPaths *paths = common::SystemPaths::Instance();
diff --git a/gazebo/util/OpenAL.cc b/gazebo/util/OpenAL.cc
index 9087d2f..c03a76b 100644
--- a/gazebo/util/OpenAL.cc
+++ b/gazebo/util/OpenAL.cc
@@ -14,9 +14,11 @@
* limitations under the License.
*
*/
-#include <stdio.h>
-#include <unistd.h>
-#include <iostream>
+#ifndef _WIN32
+ #include <stdio.h>
+ #include <unistd.h>
+ #include <iostream>
+#endif
#include <gazebo/gazebo_config.h>
@@ -176,15 +178,21 @@ OpenALSink::~OpenALSink()
{
}
-////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////
bool OpenALSink::SetPose(const math::Pose &_pose)
{
+ return this->SetPose(_pose.Ign());
+}
+
+/////////////////////////////////////////////////
+bool OpenALSink::SetPose(const ignition::math::Pose3d &_pose)
+{
ALenum error;
// Clear error state
alGetError();
- alListener3f(AL_POSITION, _pose.pos.x, _pose.pos.y, _pose.pos.z);
+ alListener3f(AL_POSITION, _pose.Pos().X(), _pose.Pos().Y(), _pose.Pos().Z());
if ((error = alGetError()) != AL_NO_ERROR)
{
@@ -192,16 +200,16 @@ bool OpenALSink::SetPose(const math::Pose &_pose)
return false;
}
- math::Matrix3 rot = _pose.rot.GetAsMatrix3();
+ ignition::math::Matrix3d rot(_pose.Rot());
// The first three values are the direction vector values.
// The second three value are the up vector values.
- ALfloat orient[] = {static_cast<ALfloat>(rot[0][0]),
- static_cast<ALfloat>(rot[0][1]),
- static_cast<ALfloat>(rot[0][2]),
- static_cast<ALfloat>(rot[2][0]),
- static_cast<ALfloat>(rot[2][1]),
- static_cast<ALfloat>(rot[2][2])};
+ ALfloat orient[] = {static_cast<ALfloat>(rot(0, 0)),
+ static_cast<ALfloat>(rot(0, 1)),
+ static_cast<ALfloat>(rot(0, 2)),
+ static_cast<ALfloat>(rot(2, 0)),
+ static_cast<ALfloat>(rot(2, 1)),
+ static_cast<ALfloat>(rot(2, 2))};
// Clear error state
alGetError();
@@ -217,15 +225,21 @@ bool OpenALSink::SetPose(const math::Pose &_pose)
return true;
}
-////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////
bool OpenALSink::SetVelocity(const math::Vector3 &_vel)
{
+ return this->SetVelocity(_vel.Ign());
+}
+
+/////////////////////////////////////////////////
+bool OpenALSink::SetVelocity(const ignition::math::Vector3d &_vel)
+{
ALenum error;
// Clear error state
alGetError();
- alListener3f(AL_VELOCITY, _vel.x, _vel.y, _vel.z);
+ alListener3f(AL_VELOCITY, _vel.X(), _vel.Y(), _vel.Z());
if ((error = alGetError()) != AL_NO_ERROR)
{
gzerr << " Unable to set velocity. Error code[" << error << "]\n";
@@ -303,8 +317,14 @@ bool OpenALSource::Load(sdf::ElementPtr _sdf)
/////////////////////////////////////////////////
bool OpenALSource::SetPose(const math::Pose &_pose)
{
- ALfloat p[3] = {static_cast<float>(_pose.pos.x),
- static_cast<float>(_pose.pos.y), static_cast<float>(_pose.pos.z)};
+ return this->SetPose(_pose.Ign());
+}
+
+/////////////////////////////////////////////////
+bool OpenALSource::SetPose(const ignition::math::Pose3d &_pose)
+{
+ ALfloat p[3] = {static_cast<float>(_pose.Pos().X()),
+ static_cast<float>(_pose.Pos().Y()), static_cast<float>(_pose.Pos().Z())};
ALenum error;
// Clear error state
@@ -324,9 +344,15 @@ bool OpenALSource::SetPose(const math::Pose &_pose)
/////////////////////////////////////////////////
bool OpenALSource::SetVelocity(const math::Vector3 &_vel)
{
+ return this->SetVelocity(_vel.Ign());
+}
+
+/////////////////////////////////////////////////
+bool OpenALSource::SetVelocity(const ignition::math::Vector3d &_vel)
+{
ALenum error;
- ALfloat v[3] = {static_cast<float>(_vel.x),
- static_cast<float>(_vel.y), static_cast<float>(_vel.z)};
+ ALfloat v[3] = {static_cast<float>(_vel.X()),
+ static_cast<float>(_vel.Y()), static_cast<float>(_vel.Z())};
// Clear error state
alGetError();
diff --git a/gazebo/util/OpenAL.hh b/gazebo/util/OpenAL.hh
index 246d294..d5b81ff 100644
--- a/gazebo/util/OpenAL.hh
+++ b/gazebo/util/OpenAL.hh
@@ -21,9 +21,13 @@
#include <vector>
#include <sdf/sdf.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Pose3.hh>
+
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Pose.hh"
#include "gazebo/common/SingletonT.hh"
+#include "gazebo/common/CommonTypes.hh"
#include "gazebo/util/UtilTypes.hh"
#include "gazebo/gazebo_config.h"
@@ -47,7 +51,7 @@ namespace gazebo
/// \class OpenAL OpenAL.hh util/util.hh
/// \brief 3D audio setup and playback.
- class GAZEBO_VISIBLE OpenAL : public SingletonT<OpenAL>
+ class GZ_UTIL_VISIBLE OpenAL : public SingletonT<OpenAL>
{
/// \brief Constructor
private: OpenAL();
@@ -88,7 +92,7 @@ namespace gazebo
/// \class OpenALSink OpenALSink.hh util/util.hh
/// \brief OpenAL Listener. This can be thought of as a microphone.
- class GAZEBO_VISIBLE OpenALSink
+ class GZ_UTIL_VISIBLE OpenALSink
{
/// \brief Constructor
public: OpenALSink();
@@ -99,17 +103,32 @@ namespace gazebo
/// \brief Set the position of the sink.
/// \param[in] _pose New pose of the sink.
/// \return True on success.
- public: bool SetPose(const math::Pose &_pose);
+ /// \deprecated See SetPose function that accepts an
+ /// ignition::math::Pose3d object.
+ public: bool SetPose(const math::Pose &_pose) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set the position of the sink.
+ /// \param[in] _pose New pose of the sink.
+ /// \return True on success.
+ public: bool SetPose(const ignition::math::Pose3d &_pose);
+
+ /// \brief Set the velocity of the sink
+ /// \param[in] _vel Velocity of the sink.
+ /// \return True on success.
+ /// \deprecated See SetVelocity function that accepts an
+ /// ignition::math::Vector3d object.
+ public: bool SetVelocity(const math::Vector3 &_vel)
+ GAZEBO_DEPRECATED(6.0);
/// \brief Set the velocity of the sink
/// \param[in] _vel Velocity of the sink.
/// \return True on success.
- public: bool SetVelocity(const math::Vector3 &_vel);
+ public: bool SetVelocity(const ignition::math::Vector3d &_vel);
};
/// \class OpenALSource OpenALSource.hh util/util.hh
/// \brief OpenAL Source. This can be thought of as a speaker.
- class GAZEBO_VISIBLE OpenALSource
+ class GZ_UTIL_VISIBLE OpenALSource
{
/// \brief Constructor.
public: OpenALSource();
@@ -125,12 +144,27 @@ namespace gazebo
/// \brief Set the position of the source.
/// \param[in] _pose New pose of the source.
/// \return True on success.
- public: bool SetPose(const math::Pose &_pose);
+ /// \deprecated See SetPose function that accepts an
+ /// ignition::math::Pose3d object.
+ public: bool SetPose(const math::Pose &_pose) GAZEBO_DEPRECATED(6.0);
+
+ /// \brief Set the position of the source.
+ /// \param[in] _pose New pose of the source.
+ /// \return True on success.
+ public: bool SetPose(const ignition::math::Pose3d &_pose);
+
+ /// \brief Set the velocity of the source.
+ /// \param[in] _vel New velocity of the source.
+ /// \return True on success.
+ /// \deprecated See SetVelocity function that accepts an
+ /// ignition::math::Vector3d object.
+ public: bool SetVelocity(const math::Vector3 &_vel)
+ GAZEBO_DEPRECATED(6.0);
/// \brief Set the velocity of the source.
/// \param[in] _vel New velocity of the source.
/// \return True on success.
- public: bool SetVelocity(const math::Vector3 &_vel);
+ public: bool SetVelocity(const ignition::math::Vector3d &_vel);
/// \brief Set the pitch of the source.
/// \param[in] _p Pitch value.
diff --git a/gazebo/util/OpenAL_TEST.cc b/gazebo/util/OpenAL_TEST.cc
index 12e19cb..4acd141 100644
--- a/gazebo/util/OpenAL_TEST.cc
+++ b/gazebo/util/OpenAL_TEST.cc
@@ -45,20 +45,20 @@ TEST_F(OpenAL, DefaultDevice)
common::load();
sdf::SDFPtr sdf(new sdf::SDF);
- sdf::initFile("world.sdf", sdf->root);
+ sdf::initFile("world.sdf", sdf->Root());
std::string sdfString = "<sdf version='1.4'>"
"<world name='default'>"
"<audio>"
"<device>default</device>"
- "</device>"
+ "</audio>"
"</world>"
"</sdf>";
- EXPECT_TRUE(sdf::readString(sdfString, sdf->root));
+ EXPECT_TRUE(sdf::readString(sdfString, sdf->Root()));
- EXPECT_TRUE(util::OpenAL::Instance()->Load(sdf->root));
- EXPECT_TRUE(util::OpenAL::Instance()->Load(sdf->root->GetElement("audio")));
+ EXPECT_TRUE(util::OpenAL::Instance()->Load(sdf->Root()));
+ EXPECT_TRUE(util::OpenAL::Instance()->Load(sdf->Root()->GetElement("audio")));
}
/////////////////////////////////////////////////
@@ -67,19 +67,20 @@ TEST_F(OpenAL, NonDefaultDevice)
common::load();
sdf::SDFPtr sdf(new sdf::SDF);
- sdf::initFile("world.sdf", sdf->root);
+ sdf::initFile("world.sdf", sdf->Root());
std::string sdfString = "<sdf version='1.4'>"
"<world name='default'>"
"<audio>"
"<device>garbage</device>"
- "</device>"
+ "</audio>"
"</world>"
"</sdf>";
- EXPECT_TRUE(sdf::readString(sdfString, sdf->root));
+ EXPECT_TRUE(sdf::readString(sdfString, sdf->Root()));
- EXPECT_FALSE(util::OpenAL::Instance()->Load(sdf->root->GetElement("audio")));
+ EXPECT_FALSE(
+ util::OpenAL::Instance()->Load(sdf->Root()->GetElement("audio")));
}
/////////////////////////////////////////////////
@@ -94,7 +95,7 @@ TEST_F(OpenAL, BadSDF)
sdf::SDFPtr sdf(new sdf::SDF);
// Bad SDF
- EXPECT_EQ(util::OpenAL::Instance()->CreateSource(sdf->root), source);
+ EXPECT_EQ(util::OpenAL::Instance()->CreateSource(sdf->Root()), source);
ASSERT_NO_THROW(util::OpenAL::Instance()->Fini());
}
@@ -109,7 +110,7 @@ TEST_F(OpenAL, BadValues)
util::OpenALSourcePtr source;
sdf::SDFPtr sdf(new sdf::SDF);
- sdf::initFile("audio_source.sdf", sdf->root);
+ sdf::initFile("audio_source.sdf", sdf->Root());
std::string sdfString = "<sdf version='1.4'>"
"<audio_source>"
@@ -120,9 +121,9 @@ TEST_F(OpenAL, BadValues)
"</audio_source>"
"</sdf>";
- EXPECT_TRUE(sdf::readString(sdfString, sdf->root));
+ EXPECT_TRUE(sdf::readString(sdfString, sdf->Root()));
- source = util::OpenAL::Instance()->CreateSource(sdf->root);
+ source = util::OpenAL::Instance()->CreateSource(sdf->Root());
EXPECT_TRUE(source != NULL);
ASSERT_NO_THROW(util::OpenAL::Instance()->Fini());
@@ -143,7 +144,7 @@ TEST_F(OpenAL, SourcePlay)
EXPECT_TRUE(util::OpenAL::Instance()->Load());
sdf::SDFPtr sdf(new sdf::SDF);
- sdf::initFile("audio_source.sdf", sdf->root);
+ sdf::initFile("audio_source.sdf", sdf->Root());
std::string sdfString = "<sdf version='1.4'>"
"<audio_source>"
@@ -156,9 +157,9 @@ TEST_F(OpenAL, SourcePlay)
"</sdf>";
- EXPECT_TRUE(sdf::readString(sdfString, sdf->root));
+ EXPECT_TRUE(sdf::readString(sdfString, sdf->Root()));
- source = util::OpenAL::Instance()->CreateSource(sdf->root);
+ source = util::OpenAL::Instance()->CreateSource(sdf->Root());
EXPECT_TRUE(source != NULL);
EXPECT_TRUE(source->GetOnContact());
@@ -206,7 +207,7 @@ TEST_F(OpenAL, SourceVelPose)
common::load();
sdf::SDFPtr sdf(new sdf::SDF);
- sdf::initFile("audio_source.sdf", sdf->root);
+ sdf::initFile("audio_source.sdf", sdf->Root());
std::string sdfString = "<sdf version='1.4'>"
"<audio_source>"
@@ -217,15 +218,15 @@ TEST_F(OpenAL, SourceVelPose)
"</audio_source>"
"</sdf>";
- EXPECT_TRUE(sdf::readString(sdfString, sdf->root));
+ EXPECT_TRUE(sdf::readString(sdfString, sdf->Root()));
EXPECT_TRUE(util::OpenAL::Instance()->Load());
- source = util::OpenAL::Instance()->CreateSource(sdf->root);
+ source = util::OpenAL::Instance()->CreateSource(sdf->Root());
EXPECT_TRUE(source != NULL);
EXPECT_FALSE(source->GetOnContact());
- EXPECT_TRUE(source->SetVelocity(math::Vector3(1, 1, 1)));
- EXPECT_TRUE(source->SetPose(math::Pose(1, 1, 1, 0, 0, 0)));
+ EXPECT_TRUE(source->SetVelocity(ignition::math::Vector3d(1, 1, 1)));
+ EXPECT_TRUE(source->SetPose(ignition::math::Pose3d(1, 1, 1, 0, 0, 0)));
}
/////////////////////////////////////////////////
@@ -238,7 +239,7 @@ TEST_F(OpenAL, Sourcevalid)
// Has pitch, gain, loop, contact
{
sdf::SDFPtr sdf(new sdf::SDF);
- sdf::initFile("audio_source.sdf", sdf->root);
+ sdf::initFile("audio_source.sdf", sdf->Root());
std::string sdfString = "<sdf version='1.4'>"
"<audio_source>"
@@ -250,15 +251,15 @@ TEST_F(OpenAL, Sourcevalid)
"</audio_source>"
"</sdf>";
- EXPECT_TRUE(sdf::readString(sdfString, sdf->root));
+ EXPECT_TRUE(sdf::readString(sdfString, sdf->Root()));
- EXPECT_TRUE(util::OpenAL::Instance()->CreateSource(sdf->root) != NULL);
+ EXPECT_TRUE(util::OpenAL::Instance()->CreateSource(sdf->Root()) != NULL);
}
// No Pitch, gain, loop, contact
{
sdf::SDFPtr sdf(new sdf::SDF);
- sdf::initFile("audio_source.sdf", sdf->root);
+ sdf::initFile("audio_source.sdf", sdf->Root());
std::string sdfString = "<sdf version='1.4'>"
"<audio_source>"
@@ -266,9 +267,9 @@ TEST_F(OpenAL, Sourcevalid)
"</audio_source>"
"</sdf>";
- EXPECT_TRUE(sdf::readString(sdfString, sdf->root));
+ EXPECT_TRUE(sdf::readString(sdfString, sdf->Root()));
- EXPECT_TRUE(util::OpenAL::Instance()->CreateSource(sdf->root) != NULL);
+ EXPECT_TRUE(util::OpenAL::Instance()->CreateSource(sdf->Root()) != NULL);
}
ASSERT_NO_THROW(util::OpenAL::Instance()->Fini());
@@ -303,12 +304,12 @@ TEST_F(OpenAL, SinkVelPose)
sink = util::OpenAL::Instance()->CreateSink(sdf::ElementPtr());
EXPECT_TRUE(sink != NULL);
- EXPECT_FALSE(sink->SetVelocity(math::Vector3(1, 1, 1)));
- EXPECT_FALSE(sink->SetPose(math::Pose(1, 1, 1, 0, 0, 0)));
+ EXPECT_FALSE(sink->SetVelocity(ignition::math::Vector3d(1, 1, 1)));
+ EXPECT_FALSE(sink->SetPose(ignition::math::Pose3d(1, 1, 1, 0, 0, 0)));
EXPECT_TRUE(util::OpenAL::Instance()->Load());
- EXPECT_TRUE(sink->SetVelocity(math::Vector3(1, 1, 1)));
- EXPECT_TRUE(sink->SetPose(math::Pose(1, 1, 1, 0, 0, 0)));
+ EXPECT_TRUE(sink->SetVelocity(ignition::math::Vector3d(1, 1, 1)));
+ EXPECT_TRUE(sink->SetPose(ignition::math::Pose3d(1, 1, 1, 0, 0, 0)));
}
#endif
diff --git a/gazebo/util/system.hh b/gazebo/util/system.hh
index b0de100..07888f0 100644
--- a/gazebo/util/system.hh
+++ b/gazebo/util/system.hh
@@ -17,37 +17,460 @@
#ifndef _GAZEBO_VISIBLE_HH_
#define _GAZEBO_VISIBLE_HH_
-/** \def GAZEBO_VISIBLE
- * Use to represent "symbol visible" if supported
- */
+/// \def GAZEBO_VISIBLE
+/// Use to represent "symbol visible" if supported
-/** \def GAZEBO_HIDDEN
- * Use to represent "symbol hidden" if supported
- */
+/// \def GAZEBO_HIDDEN
+/// Use to represent "symbol hidden" if supported
-#if defined _WIN32 || defined __CYGWIN__
- #ifdef BUILDING_DLL
- #ifdef __GNUC__
- #define GAZEBO_VISIBLE __attribute__ ((dllexport))
+#if defined BUILDING_STATIC_LIBS
+ #define GAZEBO_VISIBLE
+ #define GZ_COMMON_VISIBLE
+ #define GZ_MATH_VISIBLE
+ #define GZ_TRANSPORT_VISIBLE
+ #define GZ_MSGS_VISIBLE
+ #define GZ_SELECTION_BUFF_VISIBLE
+ #define GZ_RENDERING_VISIBLE
+ #define GZ_UTIL_VISIBLE
+ #define GZ_PLUGIN_VISIBLE
+ #define GZ_RENDERING_DEFERRED_VISIBLE
+ #define GZ_PHYSICS_VISIBLE
+ #define GZ_PHYSICS_ODE_VISIBLE
+ #define GZ_GUI_VISIBLE
+ #define GZ_GUI_BUILDING_VISIBLE
+ #define GZ_GUI_MODEL_VISIBLE
+ #define GZ_GUI_TERRAIN_VISIBLE
+ #define GZ_GUI_VIEWERS_VISIBLE
+ #define GAZEBO_HIDDEN
+#else
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL
+ #ifdef __GNUC__
+ #define GAZEBO_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GAZEBO_VISIBLE __declspec(dllexport)
+ #endif
#else
- #define GAZEBO_VISIBLE __declspec(dllexport)
+ #ifdef __GNUC__
+ #define GAZEBO_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GAZEBO_VISIBLE __declspec(dllimport)
+ #endif
#endif
+ #define GAZEBO_HIDDEN
#else
- #ifdef __GNUC__
- #define GAZEBO_VISIBLE __attribute__ ((dllimport))
+ #if __GNUC__ >= 4
+ #define GAZEBO_VISIBLE __attribute__ ((visibility ("default")))
+ #define GAZEBO_HIDDEN __attribute__ ((visibility ("hidden")))
#else
- #define GAZEBO_VISIBLE __declspec(dllimport)
+ #define GAZEBO_VISIBLE
+ #define GAZEBO_HIDDEN
#endif
#endif
- #define GAZEBO_HIDDEN
-#else
- #if __GNUC__ >= 4
- #define GAZEBO_VISIBLE __attribute__ ((visibility ("default")))
- #define GAZEBO_HIDDEN __attribute__ ((visibility ("hidden")))
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_COMMON
+ #ifdef __GNUC__
+ #define GZ_COMMON_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_COMMON_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_COMMON_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_COMMON_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_COMMON_HIDDEN
#else
- #define GAZEBO_VISIBLE
- #define GAZEBO_HIDDEN
+ #if __GNUC__ >= 4
+ #define GZ_COMMON_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_COMMON_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_COMMON_VISIBLE
+ #define GZ_COMMON_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_PLUGIN
+ #ifdef __GNUC__
+ #define GZ_PLUGIN_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_PLUGIN_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_PLUGIN_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_PLUGIN_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_PLUGIN_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_PLUGIN_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_PLUGIN_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_PLUGIN_VISIBLE
+ #define GZ_PLUGIN_HIDDEN
+ #endif
#endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_MATH
+ #ifdef __GNUC__
+ #define GZ_MATH_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_MATH_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_MATH_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_MATH_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_MATH_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_MATH_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_MATH_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_MATH_VISIBLE
+ #define GZ_MATH_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_TRANSPORT
+ #ifdef __GNUC__
+ #define GZ_TRANSPORT_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_TRANSPORT_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_TRANSPORT_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_TRANSPORT_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_TRANSPORT_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_TRANSPORT_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_TRANSPORT_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_TRANSPORT_VISIBLE
+ #define GZ_TRANSPORT_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_MSGS
+ #ifdef __GNUC__
+ #define GZ_MSGS_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_MSGS_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_MSGS_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_MSGS_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_MSGS_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_MSGS_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_MSGS_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_MSGS_VISIBLE
+ #define GZ_MSGS_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_SELECTION_BUFF
+ #ifdef __GNUC__
+ #define GZ_SELECTION_BUFF_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_SELECTION_BUFF_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_SELECTION_BUFF_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_SELECTION_BUFF_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_SELECTION_BUFF_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_SELECTION_BUFF_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_SELECTION_BUFF_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_SELECTION_BUFF_VISIBLE
+ #define GZ_SELECTION_BUFF_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_RENDERING
+ #ifdef __GNUC__
+ #define GZ_RENDERING_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_RENDERING_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_RENDERING_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_RENDERING_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_RENDERING_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_RENDERING_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_RENDERING_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_RENDERING_VISIBLE
+ #define GZ_RENDERING_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_UTIL
+ #ifdef __GNUC__
+ #define GZ_UTIL_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_UTIL_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_UTIL_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_UTIL_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_UTIL_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_UTIL_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_UTIL_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_UTIL_VISIBLE
+ #define GZ_UTIL_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_RENDERING_DEFERRED
+ #ifdef __GNUC__
+ #define GZ_RENDERING_DEFERRED_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_RENDERING_DEFERRED_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_RENDERING_DEFERRED_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_RENDERING_DEFERRED_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_RENDERING_DEFERRED_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_RENDERING_DEFERRED_VISIBLE __attribute__ ((visibility (\
+ "default")))
+ #define GZ_RENDERING_DEFERRED_HIDDEN __attribute__ ((visibility (\
+ "hidden")))
+ #else
+ #define GZ_RENDERING_DEFERRED_VISIBLE
+ #define GZ_RENDERING_DEFERRED_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_PHYSICS
+ #ifdef __GNUC__
+ #define GZ_PHYSICS_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_PHYSICS_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_PHYSICS_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_PHYSICS_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_PHYSICS_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_PHYSICS_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_PHYSICS_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_PHYSICS_VISIBLE
+ #define GZ_PHYSICS_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_PHYSICS_ODE
+ #ifdef __GNUC__
+ #define GZ_PHYSICS_ODE_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_PHYSICS_ODE_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_PHYSICS_ODE_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_PHYSICS_ODE_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_PHYSICS_ODE_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_PHYSICS_ODE_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_PHYSICS_ODE_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_PHYSICS_ODE_VISIBLE
+ #define GZ_PHYSICS_ODE_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_GUI
+ #ifdef __GNUC__
+ #define GZ_GUI_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_GUI_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_GUI_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_GUI_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_GUI_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_GUI_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_GUI_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_GUI_VISIBLE
+ #define GZ_GUI_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_GUI_BUILDING
+ #ifdef __GNUC__
+ #define GZ_GUI_BUILDING_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_GUI_BUILDING_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_GUI_BUILDING_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_GUI_BUILDING_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_GUI_BUILDING_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_GUI_BUILDING_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_GUI_BUILDING_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_GUI_BUILDING_VISIBLE
+ #define GZ_GUI_BUILDING_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_GUI_MODEL
+ #ifdef __GNUC__
+ #define GZ_GUI_MODEL_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_GUI_MODEL_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_GUI_MODEL_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_GUI_MODEL_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_GUI_MODEL_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_GUI_MODEL_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_GUI_MODEL_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_GUI_MODEL_VISIBLE
+ #define GZ_GUI_MODEL_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_GUI_TERRAIN
+ #ifdef __GNUC__
+ #define GZ_GUI_TERRAIN_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_GUI_TERRAIN_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_GUI_TERRAIN_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_GUI_TERRAIN_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_GUI_TERRAIN_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_GUI_TERRAIN_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_GUI_TERRAIN_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_GUI_TERRAIN_VISIBLE
+ #define GZ_GUI_TERRAIN_HIDDEN
+ #endif
+ #endif
+
+ #if defined _WIN32 || defined __CYGWIN__
+ #ifdef BUILDING_DLL_GZ_GUI_VIEWERS
+ #ifdef __GNUC__
+ #define GZ_GUI_VIEWERS_VISIBLE __attribute__ ((dllexport))
+ #else
+ #define GZ_GUI_VIEWERS_VISIBLE __declspec(dllexport)
+ #endif
+ #else
+ #ifdef __GNUC__
+ #define GZ_GUI_VIEWERS_VISIBLE __attribute__ ((dllimport))
+ #else
+ #define GZ_GUI_VIEWERS_VISIBLE __declspec(dllimport)
+ #endif
+ #endif
+ #define GZ_GUI_VIEWERS_HIDDEN
+ #else
+ #if __GNUC__ >= 4
+ #define GZ_GUI_VIEWERS_VISIBLE __attribute__ ((visibility ("default")))
+ #define GZ_GUI_VIEWERS_HIDDEN __attribute__ ((visibility ("hidden")))
+ #else
+ #define GZ_GUI_VIEWERS_VISIBLE
+ #define GZ_GUI_VIEWERS_HIDDEN
+ #endif
+ #endif
+// BUILDING_STATIC_LIBS
#endif
-#endif /* GAZEBO_VISIBLE_HH */
+// _GAZEBO_VISIBLE_HH_
+#endif
diff --git a/ign-math-migration.md b/ign-math-migration.md
new file mode 100644
index 0000000..9370df2
--- /dev/null
+++ b/ign-math-migration.md
@@ -0,0 +1,38 @@
+# Migration from `gazebo::math` to `ignition::math`
+
+`libigntion-math` is a dependency of Gazebo. See: [http://ignitionrobotics.org/libraries/math](http://ignitionrobotics.org/libraries/math)
+
+## gazebo/common
+
+### KeyFrame.hh
+1. ***Deprecated*** public: void SetTranslation(const math::Vector3 &_trans)
+ + ***Replacement*** public: void Translation(const ignition::math::Vector3d &_trans);
+1. ***Deprecated*** public: math::Vector3 GetTranslation() const
+ + ***Replacement*** public: ignition::math::Vector3d Translation() const
+1. ***Deprecated*** public: void SetRotation(const math::Quaternion &_rot)
+ + ***Replacement*** public: void Rotation(const ignition::math::Quaterniond &_rot)
+1. ***Deprecated*** public: math::Quaternion GetRotation() const
+ + ***Replacement*** public: ignition::math::Quaterniond Rotation() const
+
+## gazebo/math
+
+### Angle.hh
+1. ***Added*** public: Angle(const ignition::math::Angle &_angle)
+1. ***Added*** public: ignition::math::Angle Ign() const
+1. ***Added*** public: Angle &operator=(const double &_angle)
+1. ***Added*** public: Angle &operator=(const ignition::math::Angle &_angle)
+
+### Box.hh
+1. ***Added*** public: Box(const ignition::math::Box &_box)
+1. ***Added*** public: ignition::math::Box Ign() const
+1. ***Added*** public: Box &operator=(const ignition::math::Box &_b)
+
+### Quaternion.hh
+1. ***Added*** public: Quaternion(const ignition::math::Quaterniond &_qt)
+1. ***Added*** public: ignition::math::Quaterniond Ign() const
+1. ***Added*** public: Quaternion &operator =(const ignition::math::Quaterniond &_v)
+
+### Vector3.hh
+1. ***Added*** public: Vector3(const ignition::math::Vector3d &_v)
+1. ***Added*** public: ignition::math::Vector3d Ign() const
+1. ***Added*** public: Vector3 &operator=(const ignition::math::Vector3d &_v)
diff --git a/interfaces/player/ActarrayInterface.cc b/interfaces/player/ActarrayInterface.cc
index 5ef18a3..27c5d6d 100644
--- a/interfaces/player/ActarrayInterface.cc
+++ b/interfaces/player/ActarrayInterface.cc
@@ -45,7 +45,7 @@ boost::recursive_mutex *ActarrayInterface::mutex = NULL;
ActarrayInterface::ActarrayInterface(player_devaddr_t addr,
GazeboDriver *driver, ConfigFile *cf,
int section)
-: GazeboInterface(addr, driver, cf, section)
+: GazeboInterface(addr, driver, cf, section), iface(NULL), datatime(0.0)
{
/*
// Get the ID of the interface
diff --git a/interfaces/player/BumperInterface.cc b/interfaces/player/BumperInterface.cc
index cdd4fcf..82533ca 100644
--- a/interfaces/player/BumperInterface.cc
+++ b/interfaces/player/BumperInterface.cc
@@ -44,7 +44,8 @@ boost::recursive_mutex *BumperInterface::mutex = NULL;
BumperInterface::BumperInterface(player_devaddr_t addr,
GazeboDriver *driver, ConfigFile *cf,
int section)
-: GazeboInterface(addr, driver, cf, section)
+: GazeboInterface(addr, driver, cf, section), iface(NULL), gz_id(NULL),
+ datatime(0.0)
{
/*
// Get the ID of the interface
diff --git a/interfaces/player/CMakeLists.txt b/interfaces/player/CMakeLists.txt
index 7a0ff8b..127f5df 100644
--- a/interfaces/player/CMakeLists.txt
+++ b/interfaces/player/CMakeLists.txt
@@ -33,6 +33,7 @@ link_directories(${PLAYER_LINK_DIRS} ${Boost_LIBRARY_DIRS})
gz_add_library(gazebo_player ${sources})
target_link_libraries(gazebo_player ${PLAYER_LINK_LIBS}
+libgazebo_client
gazebo_common
gazebo_transport
gazebo_msgs
diff --git a/interfaces/player/FiducialInterface.cc b/interfaces/player/FiducialInterface.cc
index f7f124f..5204356 100644
--- a/interfaces/player/FiducialInterface.cc
+++ b/interfaces/player/FiducialInterface.cc
@@ -41,7 +41,8 @@ boost::recursive_mutex *FiducialInterface::mutex = NULL;
FiducialInterface::FiducialInterface(player_devaddr_t addr,
GazeboDriver *driver, ConfigFile *cf,
int section)
-: GazeboInterface(addr, driver, cf, section)
+: GazeboInterface(addr, driver, cf, section), iface(NULL), gz_id(NULL),
+ datatime(0.0)
{
/*
// Get the ID of the interface
diff --git a/interfaces/player/GazeboDriver.cc b/interfaces/player/GazeboDriver.cc
index 516a057..9c8cd7b 100644
--- a/interfaces/player/GazeboDriver.cc
+++ b/interfaces/player/GazeboDriver.cc
@@ -92,8 +92,6 @@ GazeboDriver::GazeboDriver(ConfigFile *_cf, int _section)
: Driver(_cf, _section, false, 4096)
{
printf("GazeboDriver::GazeboDriver\n");
- this->devices = NULL;
- this->deviceCount = 0;
this->deviceMaxCount = 0;
if (this->LoadDevices(_cf, _section) < 0)
@@ -179,14 +177,11 @@ int GazeboDriver::Unsubscribe(player_devaddr_t addr)
// Main function for device thread
void GazeboDriver::Update()
{
- int i;
-
Driver::ProcessMessages();
- for (i = 0; i < this->deviceCount; i++)
+ for (auto device: this->devices)
{
- GazeboInterface *iface = this->devices[i];
- iface->Update();
+ device->Update();
}
return;
@@ -196,10 +191,8 @@ void GazeboDriver::Update()
// Helper function to load all devices on startup
int GazeboDriver::LoadDevices(ConfigFile *_cf, int _section)
{
- // Get the device count, and create the device array
+ // Get the device count
this->deviceMaxCount = _cf->GetTupleCount(_section, "provides");
- this->devices = static_cast<GazeboInterface**>(realloc(this->devices,
- this->deviceMaxCount * sizeof(this->devices[0])));
if (!player_quiet_startup)
{
@@ -337,7 +330,7 @@ int GazeboDriver::LoadDevices(ConfigFile *_cf, int _section)
}
// store the Interaface in our device list
- this->devices[this->deviceCount++] = ifsrc;
+ this->devices.push_back(ifsrc);
}
else
{
@@ -359,12 +352,8 @@ int GazeboDriver::LoadDevices(ConfigFile *_cf, int _section)
// Find a device according to a player_devaddr
GazeboInterface *GazeboDriver::LookupDevice(player_devaddr_t _addr)
{
- int i;
-
- for (i = 0; i < static_cast<int>(this->deviceCount); ++i)
+ for (auto iface: this->devices)
{
- GazeboInterface *iface = static_cast<GazeboInterface*>(this->devices[i]);
-
if (iface->device_addr.robot == _addr.robot &&
iface->device_addr.interf == _addr.interf &&
iface->device_addr.index == _addr.index)
diff --git a/interfaces/player/GazeboDriver.hh b/interfaces/player/GazeboDriver.hh
index c445b50..2a98ab7 100644
--- a/interfaces/player/GazeboDriver.hh
+++ b/interfaces/player/GazeboDriver.hh
@@ -24,6 +24,7 @@
#include <unistd.h>
#include <string.h>
+#include <vector>
#include "player.h"
#include "gazebo/util/system.hh"
@@ -76,10 +77,7 @@ class GAZEBO_VISIBLE GazeboDriver : public Driver
private: GazeboInterface *LookupDevice(player_devaddr_t addr);
/// Array of interfaces associated with this driver
- protected: GazeboInterface **devices;
-
- /// \brief Number of devices
- protected: int deviceCount;
+ protected: std::vector<GazeboInterface *> devices;
/// \brief Max device count
protected: int deviceMaxCount;
diff --git a/interfaces/player/Graphics3dInterface.cc b/interfaces/player/Graphics3dInterface.cc
index c8451de..c82ef96 100644
--- a/interfaces/player/Graphics3dInterface.cc
+++ b/interfaces/player/Graphics3dInterface.cc
@@ -40,7 +40,7 @@ boost::recursive_mutex *Graphics3dInterface::mutex = NULL;
// Constructor
Graphics3dInterface::Graphics3dInterface(player_devaddr_t addr,
GazeboDriver *driver, ConfigFile *cf, int section)
-: GazeboInterface(addr, driver, cf, section)
+: GazeboInterface(addr, driver, cf, section), gz_id(NULL), iface(NULL)
{
/*
// Get the ID of the interface
diff --git a/interfaces/player/GripperInterface.cc b/interfaces/player/GripperInterface.cc
index 39f43ab..9bde0e0 100644
--- a/interfaces/player/GripperInterface.cc
+++ b/interfaces/player/GripperInterface.cc
@@ -44,7 +44,8 @@ boost::recursive_mutex *GripperInterface::mutex = NULL;
/////////////////////////////////////////////////
GripperInterface::GripperInterface(player_devaddr_t addr,
GazeboDriver *driver, ConfigFile *cf, int section)
-: GazeboInterface(addr, driver, cf, section)
+: GazeboInterface(addr, driver, cf, section), iface(NULL), gz_id(NULL),
+ datatime(0.0)
{
/*
// Get the ID of the interface
diff --git a/interfaces/player/IRInterface.cc b/interfaces/player/IRInterface.cc
index 1870afa..7d02e79 100644
--- a/interfaces/player/IRInterface.cc
+++ b/interfaces/player/IRInterface.cc
@@ -37,7 +37,8 @@ boost::recursive_mutex *IRInterface::mutex = NULL;
/////////////////////////////////////////////////
IRInterface::IRInterface(player_devaddr_t addr,
GazeboDriver *driver, ConfigFile *cf, int section)
-: GazeboInterface(addr, driver, cf, section)
+: GazeboInterface(addr, driver, cf, section), iface(NULL), gz_id(NULL),
+ datatime(0.0)
{
/*
// Get the ID of the interface
diff --git a/interfaces/player/LaserInterface.cc b/interfaces/player/LaserInterface.cc
index 7114e32..259cd2a 100644
--- a/interfaces/player/LaserInterface.cc
+++ b/interfaces/player/LaserInterface.cc
@@ -197,8 +197,8 @@ void LaserInterface::OnScan(ConstLaserScanStampedPtr &_msg)
this->data.pose.px = _msg->scan().world_pose().position().x();
this->data.pose.py = _msg->scan().world_pose().position().y();
- this->data.pose.pa = gazebo::msgs::Convert(
- _msg->scan().world_pose().orientation()).GetAsEuler().z;
+ this->data.pose.pa = gazebo::msgs::ConvertIgn(
+ _msg->scan().world_pose().orientation()).Euler().Z();
if (this->data.scan.ranges_count > 0)
{
diff --git a/interfaces/player/OpaqueInterface.cc b/interfaces/player/OpaqueInterface.cc
index 52cc43a..ff82815 100644
--- a/interfaces/player/OpaqueInterface.cc
+++ b/interfaces/player/OpaqueInterface.cc
@@ -44,7 +44,8 @@ boost::recursive_mutex *OpaqueInterface::mutex = NULL;
// Constructor
OpaqueInterface::OpaqueInterface(player_devaddr_t addr,
GazeboDriver *driver, ConfigFile *cf, int section)
-: GazeboInterface(addr, driver, cf, section)
+: GazeboInterface(addr, driver, cf, section), iface(NULL), gz_id(NULL),
+ datatime(0.0)
{
/*
// Get the ID of the interface
diff --git a/interfaces/player/PTZInterface.cc b/interfaces/player/PTZInterface.cc
index 15e503e..063d887 100644
--- a/interfaces/player/PTZInterface.cc
+++ b/interfaces/player/PTZInterface.cc
@@ -43,7 +43,8 @@ boost::recursive_mutex *PTZInterface::mutex = NULL;
/////////////////////////////////////////////////
PTZInterface::PTZInterface(player_devaddr_t addr,
GazeboDriver *driver, ConfigFile *cf, int section)
-: GazeboInterface(addr, driver, cf, section)
+: GazeboInterface(addr, driver, cf, section), iface(NULL), gz_id(NULL),
+ datatime(0.0)
{
/*
// Get the ID of the interface
diff --git a/interfaces/player/Position2dInterface.cc b/interfaces/player/Position2dInterface.cc
index 00e0ae1..4d8200a 100644
--- a/interfaces/player/Position2dInterface.cc
+++ b/interfaces/player/Position2dInterface.cc
@@ -77,9 +77,9 @@ int Position2dInterface::ProcessMessage(QueuePointer &_respQueue,
gazebo::msgs::Pose msg;
gazebo::msgs::Set(msg.mutable_position(),
- gazebo::math::Vector3(cmd->vel.px, cmd->vel.py, 0));
+ ignition::math::Vector3d(cmd->vel.px, cmd->vel.py, 0));
gazebo::msgs::Set(msg.mutable_orientation(),
- gazebo::math::Quaternion(0, 0, -cmd->vel.pa));
+ ignition::math::Quaterniond(0, 0, -cmd->vel.pa));
this->velPub->Publish(msg);
result = 0;
@@ -237,9 +237,9 @@ void Position2dInterface::OnPoseMsg(ConstPosesStampedPtr &_msg)
data.pos.px = _msg->pose(i).position().x();
data.pos.py = _msg->pose(i).position().y();
- gazebo::math::Quaternion quat =
- gazebo::msgs::Convert(_msg->pose(i).orientation());
- data.pos.pa = quat.GetYaw();
+ ignition::math::Quaterniond quat =
+ gazebo::msgs::ConvertIgn(_msg->pose(i).orientation());
+ data.pos.pa = quat.Yaw();
this->driver->Publish(this->device_addr,
PLAYER_MSGTYPE_DATA,
diff --git a/interfaces/player/Position3dInterface.cc b/interfaces/player/Position3dInterface.cc
index 4122b06..6098401 100644
--- a/interfaces/player/Position3dInterface.cc
+++ b/interfaces/player/Position3dInterface.cc
@@ -37,7 +37,8 @@ boost::recursive_mutex *Position3dInterface::mutex = NULL;
Position3dInterface::Position3dInterface(player_devaddr_t addr,
GazeboDriver *driver, ConfigFile *cf, int section)
-: GazeboInterface(addr, driver, cf, section)
+: GazeboInterface(addr, driver, cf, section), iface(NULL), gz_id(NULL),
+ datatime(0.0)
{
/*
// Get the ID of the interface
diff --git a/interfaces/player/SimulationInterface.cc b/interfaces/player/SimulationInterface.cc
index 171763f..e7bd1a5 100644
--- a/interfaces/player/SimulationInterface.cc
+++ b/interfaces/player/SimulationInterface.cc
@@ -23,7 +23,7 @@
#include "player.h"
#include "gazebo/transport/transport.hh"
-#include "gazebo/gazebo.hh"
+#include "gazebo/gazebo_client.hh"
#include "GazeboTime.hh"
#include "GazeboDriver.hh"
@@ -39,7 +39,7 @@ SimulationInterface::SimulationInterface(player_devaddr_t _addr,
GazeboDriver *_driver, ConfigFile *_cf, int _section)
: GazeboInterface(_addr, _driver, _cf, _section)
{
- gazebo::setupClient();
+ gazebo::client::setup();
worldName = _cf->ReadString(_section, "world_name", "default");
@@ -73,7 +73,7 @@ SimulationInterface::SimulationInterface(player_devaddr_t _addr,
// Destructor
SimulationInterface::~SimulationInterface()
{
- gazebo::shutdown();
+ gazebo::client::shutdown();
if (this->responseQueue)
{
delete this->responseQueue;
@@ -107,7 +107,7 @@ int SimulationInterface::ProcessMessage(QueuePointer &_respQueue,
gazebo::msgs::Model msg;
msg.set_name(req->name);
- gazebo::msgs::Set(msg.mutable_pose(), pose);
+ gazebo::msgs::Set(msg.mutable_pose(), pose.Ign());
this->modelPub->Publish(msg);
this->driver->Publish(this->device_addr, _respQueue,
@@ -129,7 +129,7 @@ int SimulationInterface::ProcessMessage(QueuePointer &_respQueue,
gazebo::msgs::Model msg;
msg.set_name(req->name);
- gazebo::msgs::Set(msg.mutable_pose(), pose);
+ gazebo::msgs::Set(msg.mutable_pose(), pose.Ign());
this->modelPub->Publish(msg);
this->driver->Publish(this->device_addr, _respQueue,
diff --git a/media/fonts/CMakeLists.txt b/media/fonts/CMakeLists.txt
index a7e6fd5..441dc95 100644
--- a/media/fonts/CMakeLists.txt
+++ b/media/fonts/CMakeLists.txt
@@ -3,9 +3,6 @@ SET (files arial.ttf
font_arial.png
font_matisse_itc.png
Gazebo.fontdef
- maiden.ttf
- sample.fontdef
- solo5.ttf
)
INSTALL(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/gazebo-${GAZEBO_VERSION}/media/fonts/)
diff --git a/media/fonts/maiden.ttf b/media/fonts/maiden.ttf
deleted file mode 100644
index cb4b9d9..0000000
Binary files a/media/fonts/maiden.ttf and /dev/null differ
diff --git a/media/fonts/sample.fontdef b/media/fonts/sample.fontdef
deleted file mode 100644
index 9bdc6e5..0000000
--- a/media/fonts/sample.fontdef
+++ /dev/null
@@ -1,121 +0,0 @@
-IronMaiden
-{
- // Blame cearny for this one!
- type truetype
- source maiden.ttf
- size 48
- resolution 96
-}
-
-StarWars
-{
- // Now this one I agree with ;)
- // A Star Wars font :)
- type truetype
- source solo5.ttf
- size 16
- resolution 96
-}
-
-Ogre
-{
- type image
- source font_matisse_itc.png
-
-glyph ! 0.0898438 0.125 0.0976563 0.1875
-glyph 0.152344 0.125 0.160156 0.1875
-glyph # 0.205078 0.125 0.232422 0.1875
-glyph $ 0.273438 0.125 0.289063 0.1875
-glyph % 0.330078 0.125 0.357422 0.1875
-glyph & 0.396484 0.125 0.416016 0.1875
-glyph ' 0.464844 0.125 0.472656 0.1875
-glyph ( 0.523438 0.125 0.539063 0.1875
-glyph ) 0.585938 0.125 0.601563 0.1875
-glyph * 0.650391 0.125 0.662109 0.1875
-glyph + 0.705078 0.125 0.732422 0.1875
-glyph , 0.777344 0.125 0.785156 0.1875
-glyph - 0.835938 0.125 0.851563 0.1875
-glyph . 0.902344 0.125 0.910156 0.1875
-glyph / 0.957031 0.125 0.980469 0.1875
-glyph 0 0.0195313 0.1875 0.0429688 0.25
-glyph 1 0.0859375 0.1875 0.101563 0.25
-glyph 2 0.146484 0.1875 0.166016 0.25
-glyph 3 0.210938 0.1875 0.226563 0.25
-glyph 4 0.271484 0.1875 0.291016 0.25
-glyph 5 0.333984 0.1875 0.353516 0.25
-glyph 6 0.396484 0.1875 0.416016 0.25
-glyph 7 0.458984 0.1875 0.478516 0.25
-glyph 8 0.521484 0.1875 0.541016 0.25
-glyph 9 0.583984 0.1875 0.603516 0.25
-glyph : 0.652344 0.1875 0.660156 0.25
-glyph ; 0.714844 0.1875 0.722656 0.25
-glyph < 0.771484 0.1875 0.791016 0.25
-glyph = 0.832031 0.1875 0.855469 0.25
-glyph > 0.896484 0.1875 0.916016 0.25
-glyph ? 0.960938 0.1875 0.976563 0.25
-glyph @ 0.0195313 0.25 0.0429688 0.3125
-glyph A 0.0839844 0.25 0.103516 0.3125
-glyph B 0.146484 0.25 0.166016 0.3125
-glyph C 0.210938 0.25 0.226563 0.3125
-glyph D 0.271484 0.25 0.291016 0.3125
-glyph E 0.335938 0.25 0.351563 0.3125
-glyph F 0.396484 0.25 0.416016 0.3125
-glyph G 0.458984 0.25 0.478516 0.3125
-glyph H 0.521484 0.25 0.541016 0.3125
-glyph I 0.589844 0.25 0.597656 0.3125
-glyph J 0.646484 0.25 0.666016 0.3125
-glyph K 0.705078 0.25 0.732422 0.3125
-glyph L 0.771484 0.25 0.791016 0.3125
-glyph M 0.824219 0.25 0.863281 0.3125
-glyph N 0.892578 0.25 0.919922 0.3125
-glyph O 0.958984 0.25 0.978516 0.3125
-glyph P 0.0195313 0.3125 0.0429688 0.375
-glyph Q 0.0820313 0.3125 0.105469 0.375
-glyph R 0.144531 0.3125 0.167969 0.375
-glyph S 0.210938 0.3125 0.226563 0.375
-glyph T 0.269531 0.3125 0.292969 0.375
-glyph U 0.333984 0.3125 0.353516 0.375
-glyph V 0.396484 0.3125 0.416016 0.375
-glyph W 0.453125 0.3125 0.484375 0.375
-glyph X 0.517578 0.3125 0.544922 0.375
-glyph Y 0.576172 0.3125 0.611328 0.375
-glyph Z 0.646484 0.3125 0.666016 0.375
-glyph [ 0.708984 0.3125 0.728516 0.375
-glyph \ 0.767578 0.3125 0.794922 0.375
-glyph ] 0.833984 0.3125 0.853516 0.375
-glyph ^ 0.896484 0.3125 0.916016 0.375
-glyph _ 0.957031 0.3125 0.980469 0.375
-glyph ` 0.0195313 0.375 0.0429688 0.4375
-glyph a 0.0839844 0.375 0.103516 0.4375
-glyph b 0.146484 0.375 0.166016 0.4375
-glyph c 0.207031 0.375 0.230469 0.4375
-glyph d 0.273438 0.375 0.289063 0.4375
-glyph e 0.333984 0.375 0.353516 0.4375
-glyph f 0.396484 0.375 0.416016 0.4375
-glyph g 0.458984 0.375 0.478516 0.4375
-glyph h 0.521484 0.375 0.541016 0.4375
-glyph i 0.587891 0.375 0.599609 0.4375
-glyph j 0.646484 0.375 0.666016 0.4375
-glyph k 0.705078 0.375 0.732422 0.4375
-glyph l 0.771484 0.375 0.791016 0.4375
-glyph m 0.816406 0.375 0.871094 0.4375
-glyph n 0.888672 0.375 0.923828 0.4375
-glyph o 0.958984 0.375 0.978516 0.4375
-glyph p 0.0195313 0.4375 0.0429688 0.5
-glyph q 0.0839844 0.4375 0.103516 0.5
-glyph r 0.146484 0.4375 0.166016 0.5
-glyph s 0.208984 0.4375 0.228516 0.5
-glyph t 0.271484 0.4375 0.291016 0.5
-glyph u 0.332031 0.4375 0.355469 0.5
-glyph v 0.386719 0.4375 0.425781 0.5
-glyph w 0.445313 0.4375 0.492188 0.5
-glyph x 0.521484 0.4375 0.541016 0.5
-glyph y 0.583984 0.4375 0.603516 0.5
-glyph z 0.646484 0.4375 0.666016 0.5
-glyph { 0.710938 0.4375 0.726563 0.5
-glyph | 0.773438 0.4375 0.789063 0.5
-glyph } 0.835938 0.4375 0.851563 0.5
-glyph ~ 0.884766 0.4375 0.927734 0.5
-
-}
-
diff --git a/media/fonts/solo5.ttf b/media/fonts/solo5.ttf
deleted file mode 100644
index c723496..0000000
Binary files a/media/fonts/solo5.ttf and /dev/null differ
diff --git a/media/gui/fonts/BITSTREAM-LICENSE b/media/gui/fonts/BITSTREAM-LICENSE
new file mode 100644
index 0000000..8f94c22
--- /dev/null
+++ b/media/gui/fonts/BITSTREAM-LICENSE
@@ -0,0 +1,40 @@
+Copyright (c) 2003 by Bitstream, Inc. All Rights Reserved. Bitstream Vera is a
+trademark of Bitstream, Inc.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of
+the fonts accompanying this license ("Fonts") and associated documentation
+files (the "Font Software"), to reproduce and distribute the Font Software,
+including without limitation the rights to use, copy, merge, publish,
+distribute, and/or sell copies of the Font Software, and to permit persons to
+whom the Font Software is furnished to do so, subject to the following
+conditions:
+
+The above copyright and trademark notices and this permission notice shall be
+included in all copies of one or more of the Font Software typefaces.
+
+The Font Software may be modified, altered, or added to, and in particular the
+designs of glyphs or characters in the Fonts may be modified and additional
+glyphs or characters may be added to the Fonts, only if the fonts are renamed
+to names not containing either the words "Bitstream" or the word "Vera".
+
+This License becomes null and void to the extent applicable to Fonts or Font
+Software that has been modified and is distributed under the "Bitstream Vera"
+names.
+
+The Font Software may be sold as part of a larger software package but no copy
+of one or more of the Font Software typefaces may be sold by itself.
+
+THE FONT SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF COPYRIGHT, PATENT,
+TRADEMARK, OR OTHER RIGHT. IN NO EVENT SHALL BITSTREAM OR THE GNOME FOUNDATION
+BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, INCLUDING ANY GENERAL,
+SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, WHETHER IN AN ACTION
+OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF THE USE OR INABILITY TO
+USE THE FONT SOFTWARE OR FROM OTHER DEALINGS IN THE FONT SOFTWARE.
+
+Except as contained in this notice, the names of Gnome, the Gnome Foundation,
+and Bitstream Inc., shall not be used in advertising or otherwise to promote
+the sale, use or other dealings in this Font Software without prior written
+authorization from the Gnome Foundation or Bitstream Inc., respectively. For
+further information, contact: fonts at gnome dot org.
diff --git a/media/gui/fonts/CMakeLists.txt b/media/gui/fonts/CMakeLists.txt
index 00fe63f..5e9ba24 100644
--- a/media/gui/fonts/CMakeLists.txt
+++ b/media/gui/fonts/CMakeLists.txt
@@ -1,5 +1,4 @@
set (files Font.xsd
- baemuk.txt
DejaVuSans-10.font
DejaVuSans.ttf
FairChar-30.font
diff --git a/media/gui/fonts/baemuk.txt b/media/gui/fonts/baemuk.txt
deleted file mode 100644
index 220407d..0000000
--- a/media/gui/fonts/baemuk.txt
+++ /dev/null
@@ -1,12 +0,0 @@
-Copyright (c) 1986-2002 Kim Jeong-Hwan
-All rights reserved.
-
-Permission to use, copy, modify and distribute this font is
-hereby granted, provided that both the copyright notice and
-this permission notice appear in all copies of the font,
-derivative works or modified versions, and that the following
-acknowledgement appear in supporting documentation:
- Baekmuk Batang, Baekmuk Dotum, Baekmuk Gulim, and
- Baekmuk Headline are registered trademarks owned by
- Kim Jeong-Hwan.
-
diff --git a/media/materials/programs/CMakeLists.txt b/media/materials/programs/CMakeLists.txt
index 19bf3b7..74f6efb 100644
--- a/media/materials/programs/CMakeLists.txt
+++ b/media/materials/programs/CMakeLists.txt
@@ -1,37 +1,41 @@
add_subdirectory(deferred_rendering)
set (files
- camera_distortion_map_fs.glsl
- camera_distortion_map_vs.glsl
- camera_noise_gaussian_fs.glsl
- camera_noise_gaussian_vs.glsl
- grid_fp.glsl
- grid_vp.glsl
- shadow_caster_fp.glsl
- shadow_caster_vp.glsl
- spot_shadow_receiver_fp.glsl
- spot_shadow_receiver_vp.glsl
- directional_shadow_receiver_fp.glsl
- directional_shadow_receiver_vp.glsl
- point_receiver_vp.glsl
- point_receiver_fp.glsl
- ambient_one_texture_vp.glsl
- blur.glsl
- perpixel_vp.glsl
- perpixel_fp.glsl
- depth_map.frag
- depth_map.vert
- depth_points_map.frag
- depth_points_map.vert
- laser_1st_pass.frag
- laser_1st_pass_dbg.frag
- laser_1st_pass.vert
- laser_2nd_pass.frag
- laser_2nd_pass.vert
- plain_color_vs.glsl
- plain_color_fs.glsl
- warp.vert
- warpWithChromeAb.frag
+ambient_one_texture_vp.glsl
+blur.glsl
+camera_distortion_map_fs.glsl
+camera_distortion_map_vs.glsl
+camera_noise_gaussian_fs.glsl
+camera_noise_gaussian_vs.glsl
+depth_map.frag
+depth_map.vert
+depth_points_map.frag
+depth_points_map.vert
+directional_shadow_receiver_fp.glsl
+directional_shadow_receiver_vp.glsl
+grid_fp.glsl
+grid_vp.glsl
+laser_1st_pass_dbg.frag
+laser_1st_pass.frag
+laser_1st_pass.vert
+laser_2nd_pass.frag
+laser_2nd_pass.vert
+oculus.cg
+OculusDistortionFragment.glsl
+OculusDistortionVertex.glsl
+perpixel_fp.glsl
+perpixel_vp.glsl
+plain_color_fs.glsl
+plain_color_vs.glsl
+point_receiver_fp.glsl
+point_receiver_vp.glsl
+projector.frag
+projector.vert
+shadow_caster_fp.glsl
+shadow_caster_vp.glsl
+spot_shadow_receiver_fp.glsl
+spot_shadow_receiver_vp.glsl
)
install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/gazebo-${GAZEBO_VERSION}/media/materials/programs/)
+
diff --git a/media/materials/programs/OculusDistortionFragment.glsl b/media/materials/programs/OculusDistortionFragment.glsl
new file mode 100644
index 0000000..29468b6
--- /dev/null
+++ b/media/materials/programs/OculusDistortionFragment.glsl
@@ -0,0 +1,14 @@
+
+uniform sampler2D diffuseMap;
+
+void main(void)
+{
+ float red = texture2D(diffuseMap, gl_TexCoord[0].xy).r;
+ float green = texture2D(diffuseMap, gl_TexCoord[1].xy).g;
+ float blue = texture2D(diffuseMap, gl_TexCoord[2].xy).b;
+
+ gl_FragColor = vec4( red, green, blue, 1.0 )*gl_Color;
+
+ //gl_FragColor = texture2D(diffuseMap, gl_TexCoord[0].xy);//vec4( red, green, blue, 1.0 );
+ //gl_FragColor = vec4( 1.0, 1.0, 1.0, 1.0 );
+}
diff --git a/media/materials/programs/OculusDistortionVertex.glsl b/media/materials/programs/OculusDistortionVertex.glsl
new file mode 100644
index 0000000..3a73cac
--- /dev/null
+++ b/media/materials/programs/OculusDistortionVertex.glsl
@@ -0,0 +1,45 @@
+#version 120
+
+// Values automatically defined by Ogre/OpenGL:
+attribute vec4 vertex;
+attribute vec2 uv0; // Red channel
+attribute vec2 uv1; // Green channel
+attribute vec2 uv2; // Blue channel
+attribute vec4 colour; // Vertex Colour
+
+// Load in values defined in the material:
+uniform mat4 worldViewProj;
+uniform vec2 eyeToSourceUVScale;
+uniform vec2 eyeToSourceUVOffset;
+uniform mat4 eyeRotationStart;
+uniform mat4 eyeRotationEnd;
+
+varying vec4 gl_FrontColor;
+
+vec2 timewarpTexCoord( vec2 texCoord, mat4 rotMat )
+{
+ vec3 transformed = (rotMat * vec4( texCoord.xy, 1, 1) ).xyz;
+
+ vec2 flattened = transformed.xy / transformed.z;
+
+ return eyeToSourceUVScale * flattened + eyeToSourceUVOffset;
+}
+
+void main(void)
+{
+ /*float timewarpLerpFactor = 0.0;
+ mat4 lerpedEyeRot = eyeRotationStart * (1 - timewarpLerpFactor) + eyeRotationEnd * timewarpLerpFactor;
+
+ gl_TexCoord[0] = vec4( timewarpTexCoord( uv0, lerpedEyeRot ), 0.0, 0.0 );
+ gl_TexCoord[1] = vec4( timewarpTexCoord( uv1, lerpedEyeRot ), 0.0, 0.0 );
+ gl_TexCoord[2] = vec4( timewarpTexCoord( uv2, lerpedEyeRot ), 0.0, 0.0 );*/
+
+ gl_TexCoord[0] = vec4( eyeToSourceUVScale * uv0 + eyeToSourceUVOffset, 0.0, 0.0 );
+ gl_TexCoord[1] = vec4( eyeToSourceUVScale * uv1 + eyeToSourceUVOffset, 0.0, 0.0 );
+ gl_TexCoord[2] = vec4( eyeToSourceUVScale * uv2 + eyeToSourceUVOffset, 0.0, 0.0 );
+
+ gl_Position = worldViewProj * vertex;
+
+ gl_FrontColor = colour;
+}
+
diff --git a/media/materials/programs/warp.vert b/media/materials/programs/warp.vert
deleted file mode 100644
index 96f09f1..0000000
--- a/media/materials/programs/warp.vert
+++ /dev/null
@@ -1,9 +0,0 @@
-#version 120
-
-varying vec2 Texcoord;
-
-void main( void )
-{
- gl_Position = ftransform();
- Texcoord = gl_MultiTexCoord0.xy;
-}
diff --git a/media/materials/programs/warpWithChromeAb.frag b/media/materials/programs/warpWithChromeAb.frag
deleted file mode 100644
index f775fa7..0000000
--- a/media/materials/programs/warpWithChromeAb.frag
+++ /dev/null
@@ -1,40 +0,0 @@
-#version 120
-
-uniform sampler2D WarpTexture;
-uniform vec2 LensCenter;
-uniform vec2 ScreenCenter;
-uniform vec2 Scale;
-uniform vec2 ScaleIn;
-uniform vec4 HmdWarpParam;
-uniform vec4 ChromAbParam;
-
-varying vec2 Texcoord;
-
-void main(void)
-{
- vec2 theta = (Texcoord - LensCenter) * ScaleIn; // Scales texture coordinates to [-1, 1]
- float rSq = theta.x * theta.x + theta.y * theta.y;
- vec2 theta1 = theta * ( HmdWarpParam.x +
- HmdWarpParam.y * rSq +
- HmdWarpParam.z * rSq * rSq +
- HmdWarpParam.w * rSq * rSq * rSq);
- // Detect whether blue texture coordinates are out of range since these will scaled out the furthest.
- vec2 thetaBlue = theta1 * (ChromAbParam.z + ChromAbParam.w * rSq);
- vec2 tcBlue = LensCenter + Scale * thetaBlue;
-
- if (any(bvec2(clamp(tcBlue, ScreenCenter - vec2(0.5, 0.5), ScreenCenter + vec2(0.5, 0.5)) - tcBlue))) {
- gl_FragColor = vec4(0, 0, 0, 0);
- return;
- }
-
- // Now do blue texture lookup.
- float blue = texture2D(WarpTexture, tcBlue).b;
- // Do green lookup (no scaling).
- vec2 tcGreen = LensCenter + Scale * theta1;
- float green = texture2D(WarpTexture, tcGreen).g;
- // Do red scale and lookup.
- vec2 thetaRed = theta1 * (ChromAbParam.x + ChromAbParam.y * rSq);
- vec2 tcRed = LensCenter + Scale * thetaRed;
- float red = texture2D(WarpTexture, tcRed).r;
- gl_FragColor = vec4(red, green, blue, 1);
-}
diff --git a/media/materials/scripts/CMakeLists.txt b/media/materials/scripts/CMakeLists.txt
index f7ed8ee..fc724c9 100644
--- a/media/materials/scripts/CMakeLists.txt
+++ b/media/materials/scripts/CMakeLists.txt
@@ -1,28 +1,26 @@
set (files
- blur.compositor
- blur.material
- CMakeLists.txt
- deferred.compositor
- deferred_post.material
- deferred_post_minilight.material
- deferred_post.program
- distortion.compositor
- gazebo.material
- grid.material
- kitchen.material
- noise.compositor
- glsl120.program
- oculus.compositor
- perpixel.program
- picker.material
- ShadowCaster.material
- ShadowCaster.program
- ssao.compositor
- ssao.material
- VPL.material
- VPL.program
- shadow_caster.program
- warp.material
+blur.compositor
+blur.material
+deferred.compositor
+deferred_post.material
+deferred_post_minilight.material
+deferred_post.program
+distortion.compositor
+gazebo.material
+grid.material
+kitchen.material
+noise.compositor
+oculus.material
+perpixel.program
+picker.material
+ShadowCaster.material
+shadow_caster.program
+ShadowCaster.program
+ssao.compositor
+ssao.material
+VPL.material
+VPL.program
)
install(FILES ${files} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/gazebo-${GAZEBO_VERSION}/media/materials/scripts/)
+
diff --git a/media/materials/scripts/gazebo.material b/media/materials/scripts/gazebo.material
index 0d2949b..ab53c14 100644
--- a/media/materials/scripts/gazebo.material
+++ b/media/materials/scripts/gazebo.material
@@ -509,6 +509,48 @@ material Gazebo/GreenTransparentOverlay
}
}
+material Gazebo/OrangeTransparentOverlay
+{
+ technique
+ {
+ pass
+ {
+ scene_blend alpha_blend
+ depth_write off
+ lighting off
+ depth_check off
+ ambient 1 0.5088 0.0468
+
+ texture_unit
+ {
+ colour_op_ex source1 src_manual src_current 1 0.5088 0.0468
+ alpha_op_ex source1 src_manual src_current 0.8
+ }
+ }
+ }
+}
+
+material Gazebo/DarkOrangeTransparentOverlay
+{
+ technique
+ {
+ pass
+ {
+ scene_blend alpha_blend
+ depth_write off
+ lighting off
+ depth_check off
+ ambient 0.6 0.305 0.028
+
+ texture_unit
+ {
+ colour_op_ex source1 src_manual src_current 0.6 0.305 0.028
+ alpha_op_ex source1 src_manual src_current 0.8
+ }
+ }
+ }
+}
+
material Gazebo/RedTransparent
{
technique
@@ -570,6 +612,27 @@ material Gazebo/BlueTransparent
}
}
+material Gazebo/DarkMagentaTransparent
+{
+ technique
+ {
+ pass
+ {
+ scene_blend alpha_blend
+ depth_write off
+
+ ambient 0.6 0.0 0.6 1
+ diffuse 0.6 0.0 0.6 1
+
+ texture_unit
+ {
+ colour_op_ex source1 src_current src_current 0.6 0 0.6
+ alpha_op_ex source1 src_manual src_current 0.5
+ }
+ }
+ }
+}
+
material Gazebo/GreyTransparent
{
technique
@@ -638,6 +701,31 @@ material Gazebo/LightOff
}
}
+material Gazebo/LightBlueLaser
+{
+ receive_shadows off
+
+ technique
+ {
+ pass
+ {
+ scene_blend alpha_blend
+ depth_write off
+ cull_hardware none
+
+ ambient 0.5 0.5 1.0 1
+ diffuse 0.5 0.5 1.0 1
+
+ texture_unit
+ {
+ colour_op_ex source1 src_current src_current 0 1 0
+ alpha_op_ex source1 src_manual src_current 0.4
+ }
+
+ }
+ }
+}
+
material Gazebo/BlueLaser
{
receive_shadows off
@@ -705,6 +793,21 @@ material Gazebo/JointAnchor
}
}
+material Gazebo/CoM
+{
+ technique
+ {
+ pass
+ {
+ ambient 0.5 0.5 0.5 1.000000
+
+ texture_unit
+ {
+ texture com.png
+ }
+ }
+ }
+}
material Gazebo/WoodFloor
{
@@ -1348,3 +1451,42 @@ material Gazebo/BuildingFrame
}
}
+material Gazebo/Runway
+{
+ receive_shadows on
+
+ technique
+ {
+ pass
+ {
+ ambient 0.5 0.5 0.5 1.000000
+
+ texture_unit
+ {
+ texture runway.png
+ }
+ }
+ }
+}
+
+material Gazebo/Grass
+{
+ receive_shadows on
+
+ technique
+ {
+ pass
+ {
+ ambient 0.5 0.5 0.5 1.000000
+
+ texture_unit
+ {
+ scale .02 .02
+ texture grass.jpg
+ filtering anisotropic
+ max_anisotropy 16
+ }
+ }
+ }
+}
+
diff --git a/media/materials/scripts/glsl120.program b/media/materials/scripts/glsl120.program
deleted file mode 100644
index d37c405..0000000
--- a/media/materials/scripts/glsl120.program
+++ /dev/null
@@ -1,20 +0,0 @@
-
-vertex_program oculus_gazebo_plugins/glsl120/warp.vert glsl
-{
- source warp.vert
-}
-
-fragment_program gazebo/glsl120/warpWithChromeAb.frag glsl
-{
- source warpWithChromeAb.frag
- default_params
- {
- param_named WarpTexture int 0.0
- param_named ScreenCenter float2 0.5 0.5
- param_named LensCenter float2 0.5 0.5
- param_named Scale float2 0.3 0.3
- param_named ScaleIn float2 2.0 2.0
- param_named HmdWarpParam float4 1.0 0.22 0.24 0.0
- param_named ChromAbParam float4 0.996, -0.004, 1.014, 0
- }
-}
diff --git a/media/materials/scripts/oculus.compositor b/media/materials/scripts/oculus.compositor
deleted file mode 100644
index 99819dd..0000000
--- a/media/materials/scripts/oculus.compositor
+++ /dev/null
@@ -1,43 +0,0 @@
-compositor OculusLeft
-{
- technique
- {
- texture rt0 1024 1024 PF_R8G8B8
-
- target rt0 { input previous }
-
- target_output
- {
- // Start with clear output
- input none
-
- pass render_quad
- {
- material Ogre/Compositor/Oculus
- input 0 rt0
- }
- }
- }
-}
-
-compositor OculusRight
-{
- technique
- {
- texture rt0 1024 1024 PF_R8G8B8
-
- target rt0 { input previous }
-
- target_output
- {
- // Start with clear output
- input none
-
- pass render_quad
- {
- material Ogre/Compositor/Oculus
- input 0 rt0
- }
- }
- }
-}
diff --git a/media/materials/scripts/oculus.material b/media/materials/scripts/oculus.material
new file mode 100644
index 0000000..f3fa435
--- /dev/null
+++ b/media/materials/scripts/oculus.material
@@ -0,0 +1,68 @@
+vertex_program OculusDistortionVertex glsl
+{
+ source OculusDistortionVertex.glsl
+ default_params
+ {
+ param_named eyeToSourceUVScale float2 1.0 1.0
+ param_named eyeToSourceUVOffset float2 0.0 0.0
+ param_named eyeRotationStart matrix4x4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
+ param_named eyeRotationEnd matrix4x4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
+ param_named_auto worldViewProj worldviewproj_matrix
+ }
+}
+fragment_program OculusDistortionFragment glsl
+{
+ source OculusDistortionFragment.glsl
+}
+
+
+material Oculus/LeftEye
+{
+ receive_shadows off
+
+ technique
+ {
+ pass Oculus/LeftEye
+ {
+ vertex_program_ref OculusDistortionVertex
+ {
+ }
+
+ fragment_program_ref OculusDistortionFragment
+ {
+ param_named diffuseMap int 0
+ }
+
+ lighting off
+ texture_unit
+ {
+ }
+ }
+ }
+}
+
+material Oculus/RightEye
+{
+ receive_shadows off
+
+ technique
+ {
+ pass Oculus/RightEye
+ {
+ vertex_program_ref OculusDistortionVertex
+ {
+ }
+
+ fragment_program_ref OculusDistortionFragment
+ {
+ param_named diffuseMap int 0
+ }
+
+ lighting off
+
+ texture_unit
+ {
+ }
+ }
+ }
+}
diff --git a/media/materials/scripts/warp.material b/media/materials/scripts/warp.material
deleted file mode 100644
index 7f3b970..0000000
--- a/media/materials/scripts/warp.material
+++ /dev/null
@@ -1,21 +0,0 @@
-material Ogre/Compositor/Oculus
-{
- technique
- {
- pass
- {
- depth_check off
-
- vertex_program_ref oculus_gazebo_plugins/glsl120/warp.vert {}
- fragment_program_ref gazebo/glsl120/warpWithChromeAb.frag {}
-
- texture_unit RT
- {
- tex_coord_set 0
- tex_address_mode border
- tex_border_colour 0 0 0
- filtering linear linear linear
- }
- }
- }
-}
diff --git a/media/materials/textures/CMakeLists.txt b/media/materials/textures/CMakeLists.txt
index ff8ed28..f167401 100644
--- a/media/materials/textures/CMakeLists.txt
+++ b/media/materials/textures/CMakeLists.txt
@@ -4,6 +4,7 @@ set (files
bricks.png
ceiling_tiled.jpg
clouds.jpg
+ com.png
dirt_diffusespecular.png
dirt_normal.png
flat_normal.png
@@ -24,6 +25,7 @@ set (files
random.png
residential.jpg
road1.jpg
+ runway.png
secondary.jpg
sidewalk.jpg
steps.jpeg
diff --git a/media/materials/textures/com.png b/media/materials/textures/com.png
new file mode 100644
index 0000000..3ed9e88
Binary files /dev/null and b/media/materials/textures/com.png differ
diff --git a/media/materials/textures/runway.png b/media/materials/textures/runway.png
new file mode 100644
index 0000000..2aafc7d
Binary files /dev/null and b/media/materials/textures/runway.png differ
diff --git a/models/ground_plane/model.sdf b/models/ground_plane/model.sdf
index 00fe113..ee331fc 100644
--- a/models/ground_plane/model.sdf
+++ b/models/ground_plane/model.sdf
@@ -11,6 +11,9 @@
</plane>
</geometry>
<surface>
+ <contact>
+ <collide_bitmask>0xffff</collide_bitmask>
+ </contact>
<friction>
<ode>
<mu>100</mu>
diff --git a/plugins/ActuatorPlugin.cc b/plugins/ActuatorPlugin.cc
new file mode 100644
index 0000000..bb6863a
--- /dev/null
+++ b/plugins/ActuatorPlugin.cc
@@ -0,0 +1,177 @@
+/*
+ * 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 "ActuatorPlugin.hh"
+
+using namespace gazebo;
+
+//////////////////////////////////////////////////
+/// \brief Calculate torque due to the input conditions for an electric motor
+/// model. Simplified from http://lancet.mit.edu/motors/motors3.html#power
+/// \param[in] _speed Input velocity.
+/// \param[in] _properties Static properties of this actuator
+/// \return Torque according to the model.
+float ElectricMotorModel(const float _speed, const float /*_torque*/,
+ const ActuatorProperties &_properties)
+{
+ if (_speed > _properties.maximumVelocity)
+ return _properties.power / _properties.maximumVelocity;
+
+ float torque = _properties.power / _speed;
+
+ if (torque > _properties.maximumTorque)
+ return _properties.maximumTorque;
+
+ return torque;
+}
+
+//////////////////////////////////////////////////
+/// \brief A simple velocity limiting motor model. Returns the maximum torque
+/// if speed is above the allowed limit, and returns the input torque otherwise.
+/// \param[in] _speed Input velocity.
+/// \param[in] _torque Input torque.
+/// \param[in] _properties Static properties of this actuator
+/// \return Torque according to the model.
+float VelocityLimiterModel(const float _speed, const float _torque,
+ const ActuatorProperties &_properties)
+{
+ if (_speed > _properties.maximumVelocity)
+ return _properties.maximumTorque;
+
+ return _torque;
+}
+
+//////////////////////////////////////////////////
+/// \brief The null motor model. Nothing exciting happening here.
+/// \return Torque according to the model, which will always be zero.
+float NullModel(const float /*_speed*/, const float /*_torque*/,
+ const ActuatorProperties &/*_properties*/)
+{
+ return 0;
+}
+
+//////////////////////////////////////////////////
+void ActuatorPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
+{
+ // Read the SDF
+ if (_sdf->HasElement("actuator"))
+ {
+ for (sdf::ElementPtr elem = _sdf->GetElement("actuator"); elem != NULL;
+ elem = elem->GetNextElement("actuator"))
+ {
+ // Get actuator properties
+ ActuatorProperties properties;
+
+ // actuator name is currently an optional property
+ if (elem->HasElement("name"))
+ properties.name = elem->Get<std::string>("name");
+
+ if (!elem->HasElement("joint"))
+ {
+ gzwarn << "Invalid SDF: got actuator element without joint."
+ << std::endl;
+ continue;
+ }
+ std::string jointName = elem->Get<std::string>("joint");
+
+ properties.modelFunction = NullModel;
+ if (elem->HasElement("type"))
+ {
+ std::string modelType = elem->Get<std::string>("type");
+ if (modelType.compare("electric_motor") == 0)
+ {
+ if (!elem->HasElement("power") ||
+ !elem->HasElement("max_velocity") ||
+ !elem->HasElement("max_torque"))
+ {
+ gzwarn << "Invalid SDF: Missing required elements for motor model "
+ << modelType << "." << std::endl;
+ continue;
+ }
+
+ properties.power = elem->Get<float>("power");
+ properties.maximumVelocity = elem->Get<float>("max_velocity");
+ properties.maximumTorque = elem->Get<float>("max_torque");
+
+ properties.modelFunction = ElectricMotorModel;
+ }
+ else if (modelType.compare("velocity_limiter") == 0)
+ {
+ if (!elem->HasElement("max_velocity") ||
+ !elem->HasElement("max_torque"))
+ {
+ gzwarn << "Invalid SDF: Missing required elements for motor model "
+ << modelType << "." << std::endl;
+ continue;
+ }
+ properties.maximumVelocity = elem->Get<float>("max_velocity");
+ properties.maximumTorque = elem->Get<float>("max_torque");
+ properties.modelFunction = VelocityLimiterModel;
+ }
+ else if (modelType.compare("null") != 0)
+ {
+ gzwarn << "Unknown motor model specified, selecting NullModel."
+ << std::endl;
+ }
+ }
+ else
+ {
+ gzwarn << "No motor model specified, selecting NullModel."
+ << std::endl;
+ }
+
+ if (elem->HasElement("index"))
+ {
+ properties.jointIndex = elem->Get<unsigned int>("index");
+ }
+ else
+ {
+ properties.jointIndex = 0;
+ }
+
+ // Store pointer to the joint we will actuate
+ physics::JointPtr joint = _parent->GetJoint(jointName);
+ if (!joint)
+ {
+ gzwarn << "Invalid SDF: actuator joint " << jointName << " does not "
+ << "exist!" << std::endl;
+ continue;
+ }
+ joint->SetEffortLimit(properties.jointIndex, properties.maximumTorque);
+ this->joints.push_back(joint);
+ this->actuators.push_back(properties);
+ }
+ // Set up a physics update callback
+ this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&ActuatorPlugin::WorldUpdateCallback, this)));
+ }
+}
+
+//////////////////////////////////////////////////
+void ActuatorPlugin::WorldUpdateCallback()
+{
+ // Update the stored joints according to the desired model.
+ for (unsigned int i = 0; i < this->joints.size(); i++)
+ {
+ const int index = this->actuators[i].jointIndex;
+ const float velocity = this->joints[i]->GetVelocity(index);
+ float curForce = this->joints[i]->GetForce(index);
+ float maxForce = this->actuators[i].modelFunction(velocity, curForce,
+ this->actuators[i]);
+ this->joints[i]->SetEffortLimit(index, maxForce);
+ }
+}
diff --git a/plugins/ActuatorPlugin.hh b/plugins/ActuatorPlugin.hh
new file mode 100644
index 0000000..90a88f0
--- /dev/null
+++ b/plugins/ActuatorPlugin.hh
@@ -0,0 +1,106 @@
+/*
+ * 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_ACTUATOR_PLUGIN_
+#define _GAZEBO_ACTUATOR_PLUGIN_
+
+#include <vector>
+#include <string>
+#include <gazebo/common/Events.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/gazebo.hh>
+
+/// Example SDF:
+/// <plugin name="actuator_plugin" filename="libActuatorPlugin.so">
+/// <actuator>
+/// <name>actuator_0</name> <!-- optional -->
+/// <joint>JOINT_0</joint> <!-- name of joint to actuate -->
+/// <index>0</index> <!-- needed for multi-DOF joints -->
+/// <type>electric_motor</type> <!-- motor model type -->
+/// <power>20</power> <!-- parameters for motor model -->
+/// <max_velocity>6</max_velocity>
+/// <max_torque>10.0</max_torque>
+/// </actuator>
+/// </plugin>
+/// </model>
+///
+/// Required fields:
+/// - name
+/// - joint
+/// - index (can be 0 in most cases)
+/// - type: current options are electric_motor, velocity_limiter or null
+/// Required for motor model electric_motor:
+/// - power
+/// - max_velocity
+/// - max_torque
+/// Required for motor model velocity_limiter:
+/// - max_velocity
+/// - max_torque
+
+namespace gazebo
+{
+ /// \brief Properties for a model of a rotational actuator
+ class ActuatorProperties
+ {
+ /// \brief An identifier for the actuator.
+ public: std::string name;
+
+ /// \brief Which joint index is actuated by this actuator.
+ public: int jointIndex;
+
+ /// \brief Mechanical power output of the actuator (Watts)
+ public: float power;
+
+ /// \brief Maximum velocity of the actuator (radians per second)
+ public: float maximumVelocity;
+
+ /// \brief Maximum torque of the actuator (Newton-meters)
+ public: float maximumTorque;
+
+ /// \brief Function used to calculate motor output.
+ /// \param[in] float1 Input velocity.
+ /// \param[in] float2 Input torque.
+ /// \param[in] ActuatorProperties Static properties of this actuator
+ /// \return Torque according to the model.
+ public: boost::function<float (float, float, const ActuatorProperties&)>
+ modelFunction;
+ };
+
+ /// \brief Plugin for simulating a torque-speed curve for actuators.
+ class GAZEBO_VISIBLE ActuatorPlugin : public ModelPlugin
+ {
+ /// Documentation inherited
+ public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+
+ /// \brief Callback on world update event.
+ private: void WorldUpdateCallback();
+
+ /// \brief The joints we want to actuate
+ private: std::vector<physics::JointPtr> joints;
+
+ /// \brief Corresponding actuator properties (power, max torque, etc.)
+ private: std::vector<ActuatorProperties> actuators;
+
+ /// \brief Connections to events associated with this class.
+ private: std::vector<event::ConnectionPtr> connections;
+ };
+
+ // Register this plugin with the simulator
+ GZ_REGISTER_MODEL_PLUGIN(ActuatorPlugin)
+}
+
+#endif
diff --git a/plugins/ArrangePlugin.cc b/plugins/ArrangePlugin.cc
index b9f581c..f7f9837 100644
--- a/plugins/ArrangePlugin.cc
+++ b/plugins/ArrangePlugin.cc
@@ -146,7 +146,7 @@ void ArrangePlugin::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
this->node = transport::NodePtr(new transport::Node());
this->node->Init(_world->GetName());
- sub = this->node->Subscribe(this->eventTopicName,
+ this->sub = this->node->Subscribe(this->eventTopicName,
&ArrangePlugin::ArrangementCallback, this);
}
diff --git a/plugins/BreakableJointPlugin.cc b/plugins/BreakableJointPlugin.cc
index 244c5c6..928e0a6 100644
--- a/plugins/BreakableJointPlugin.cc
+++ b/plugins/BreakableJointPlugin.cc
@@ -14,6 +14,11 @@
* 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 "gazebo/physics/PhysicsIface.hh"
#include "BreakableJointPlugin.hh"
@@ -52,8 +57,8 @@ void BreakableJointPlugin::OnUpdate(msgs::WrenchStamped _msg)
{
if (this->parentJoint)
{
- math::Vector3 force = msgs::Convert(_msg.wrench().force());
- if (force.GetLength() > this->breakingForce)
+ ignition::math::Vector3d force = msgs::ConvertIgn(_msg.wrench().force());
+ if (force.Length() > this->breakingForce)
{
this->worldConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&BreakableJointPlugin::OnWorldUpdate, this));
diff --git a/plugins/BuoyancyPlugin.cc b/plugins/BuoyancyPlugin.cc
new file mode 100644
index 0000000..4229298
--- /dev/null
+++ b/plugins/BuoyancyPlugin.cc
@@ -0,0 +1,178 @@
+/*
+ * 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/common/Assert.hh"
+#include "gazebo/common/Events.hh"
+#include "plugins/BuoyancyPlugin.hh"
+
+using namespace gazebo;
+
+GZ_REGISTER_MODEL_PLUGIN(BuoyancyPlugin)
+
+/////////////////////////////////////////////////
+BuoyancyPlugin::BuoyancyPlugin()
+ // Density of liquid water at 1 atm pressure and 15 degrees Celsius.
+ : fluidDensity(999.1026)
+{
+}
+
+/////////////////////////////////////////////////
+void BuoyancyPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
+{
+ GZ_ASSERT(_model != NULL, "Received NULL model pointer");
+ this->model = _model;
+ physics::WorldPtr world = _model->GetWorld();
+ GZ_ASSERT(world != NULL, "Model is in a NULL world");
+ this->physicsEngine = world->GetPhysicsEngine();
+ GZ_ASSERT(this->physicsEngine != NULL, "Physics engine was NULL");
+
+ GZ_ASSERT(_sdf != NULL, "Received NULL SDF pointer");
+ this->sdf = _sdf;
+
+ if (this->sdf->HasElement("fluid_density"))
+ {
+ this->fluidDensity = this->sdf->Get<double>("fluid_density");
+ }
+
+ // Get "center of volume" and "volume" that were inputted in SDF
+ // SDF input is recommended for mesh or polylines collision shapes
+ if (this->sdf->HasElement("link"))
+ {
+ for (sdf::ElementPtr linkElem = this->sdf->GetElement("link"); linkElem;
+ linkElem = this->sdf->GetNextElement("link"))
+ {
+ int id = -1;
+ std::string name = "";
+ if (linkElem->HasAttribute("name"))
+ {
+ name = linkElem->Get<std::string>("name");
+ physics::LinkPtr link =
+ this->model->GetLink(name);
+ if (!link)
+ {
+ gzwarn << "Specified link [" << name << "] not found." << std::endl;
+ continue;
+ }
+ id = link->GetId();
+ }
+ else
+ {
+ gzwarn << "Required attribute name missing from link [" << name
+ << "] in BuoyancyPlugin SDF" << std::endl;
+ // Exit if we didn't set ID
+ continue;
+ }
+
+ if (this->volPropsMap.count(id) != 0)
+ {
+ gzwarn << "Properties for link [" << name << "] already set, skipping "
+ << "second property block" << std::endl;
+ }
+
+ if (linkElem->HasElement("center_of_volume"))
+ {
+ math::Vector3 cov =
+ linkElem->GetElement("center_of_volume")->Get<math::Vector3>();
+ this->volPropsMap[id].cov = cov;
+ }
+ else
+ {
+ gzwarn << "Required element center_of_volume missing from link ["
+ << name
+ << "] in BuoyancyPlugin SDF" << std::endl;
+ continue;
+ }
+
+ if (linkElem->HasElement("volume"))
+ {
+ double volume = linkElem->GetElement("volume")->Get<double>();
+ if (volume <= 0)
+ {
+ gzwarn << "Nonpositive volume specified in BuoyancyPlugin!"
+ << std::endl;
+ // Remove the element from the map since it's invalid.
+ this->volPropsMap.erase(id);
+ continue;
+ }
+ this->volPropsMap[id].volume = volume;
+ }
+ else
+ {
+ gzwarn << "Required element volume missing from element link [" << name
+ << "] in BuoyancyPlugin SDF" << std::endl;
+ continue;
+ }
+ }
+ }
+
+ // For links the user didn't input, precompute the center of volume and
+ // density. This will be accurate for simple shapes.
+ for (auto link : this->model->GetLinks())
+ {
+ int id = link->GetId();
+ if (this->volPropsMap.find(id) == this->volPropsMap.end())
+ {
+ double volumeSum = 0;
+ math::Vector3 weightedPosSum = math::Vector3::Zero;
+
+ // The center of volume of the link is a weighted average over the pose
+ // of each collision shape, where the weight is the volume of the shape
+ for (auto collision : link->GetCollisions())
+ {
+ double volume = collision->GetShape()->ComputeVolume();
+ volumeSum += volume;
+ weightedPosSum += volume*collision->GetWorldPose().pos;
+ }
+ // Subtract the center of volume into the link frame.
+ this->volPropsMap[id].cov =
+ weightedPosSum/volumeSum - link->GetWorldPose().pos;
+ this->volPropsMap[id].volume = volumeSum;
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void BuoyancyPlugin::Init()
+{
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ std::bind(&BuoyancyPlugin::OnUpdate, this));
+}
+
+/////////////////////////////////////////////////
+void BuoyancyPlugin::OnUpdate()
+{
+ for (auto link : this->model->GetLinks())
+ {
+ VolumeProperties volumeProperties = this->volPropsMap[link->GetId()];
+ double volume = volumeProperties.volume;
+ GZ_ASSERT(volume > 0, "Nonpositive volume found in volume properties!");
+
+ // By Archimedes' principle,
+ // buoyancy = -(mass*gravity)*fluid_density/object_density
+ // object_density = mass/volume, so the mass term cancels.
+ // Therefore,
+ math::Vector3 buoyancy =
+ -this->fluidDensity * volume * this->physicsEngine->GetGravity();
+
+ math::Pose linkFrame = link->GetWorldPose();
+ // rotate buoyancy into the link frame before applying the force.
+ math::Vector3 buoyancyLinkFrame =
+ linkFrame.rot.GetInverse().RotateVector(buoyancy);
+
+ link->AddLinkForce(buoyancyLinkFrame, volumeProperties.cov);
+ }
+}
diff --git a/plugins/BuoyancyPlugin.hh b/plugins/BuoyancyPlugin.hh
new file mode 100644
index 0000000..00b8536
--- /dev/null
+++ b/plugins/BuoyancyPlugin.hh
@@ -0,0 +1,96 @@
+/*
+ * 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_BUOYANCY_PLUGIN_HH_
+#define _GAZEBO_BUOYANCY_PLUGIN_HH_
+
+#include <map>
+#include "gazebo/common/Event.hh"
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/math/Vector3.hh"
+#include "gazebo/physics/physics.hh"
+
+namespace gazebo
+{
+ /// \brief A class for storing the volume properties of a link.
+ class VolumeProperties
+ {
+ /// \brief Default constructor.
+ public: VolumeProperties() : volume(0) {}
+
+ /// \brief Center of volume in the link frame.
+ public: math::Vector3 cov;
+
+ /// \brief Volume of this link.
+ public: double volume;
+ };
+
+ /// \brief A plugin that simulates buoyancy of an object immersed in fluid.
+ /// All SDF parameters are optional.
+ /// <fluid_density> sets the density of the fluid that surrounds the buoyant
+ /// object.
+ /// <link> elements describe the volume properties of individual links in the
+ /// model. For example:
+ /// <link name="body">
+ /// <center_of_volume>1 2 3</center_of_volume>
+ /// <volume>50</volume>
+ /// </link>
+ /// <center_of_volume> A point representing the volumetric center of the
+ /// link in the link frame. This is where the buoyancy force will be applied.
+ /// <volume> The volume of the link in kg/m^3.
+ /// If center of volume and volume are not specified, the plugin will attempt
+ /// to compute these properties from the link collision shapes. This
+ /// computation will not be accurate if the object is not composed of simple
+ /// collision shapes.
+ class GAZEBO_VISIBLE BuoyancyPlugin : public ModelPlugin
+ {
+ /// \brief Constructor.
+ public: BuoyancyPlugin();
+
+ /// \brief Read the model SDF to compute volume and center of volume for
+ /// each link, and store those properties in volPropsMap.
+ public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+ // Documentation inherited
+ public: virtual void Init();
+
+ /// \brief Callback for World Update events.
+ protected: virtual void OnUpdate();
+
+ /// \brief Connection to World Update events.
+ protected: event::ConnectionPtr updateConnection;
+
+ /// \brief Pointer to model containing the plugin.
+ protected: physics::ModelPtr model;
+
+ /// \brief Pointer to the physics engine (for accessing gravity).
+ protected: physics::PhysicsEnginePtr physicsEngine;
+
+ /// \brief Pointer to the plugin SDF.
+ protected: sdf::ElementPtr sdf;
+
+ /// \brief The density of the fluid in which the object is submerged in
+ /// kg/m^3. Defaults to 1000, the fluid density of water.
+ protected: double fluidDensity;
+
+ /// \brief Map of <link ID, point> pairs mapping link IDs to the CoV (center
+ /// of volume) and volume of the link.
+ protected: std::map<int, VolumeProperties> volPropsMap;
+ };
+}
+
+#endif
diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt
index c499200..2a869c8 100644
--- a/plugins/CMakeLists.txt
+++ b/plugins/CMakeLists.txt
@@ -1,8 +1,13 @@
+if (WIN32)
+ add_definitions(-DBUILDING_DLL -DBUILDING_DLL_GZ_PLUGIN)
+endif()
+
include_directories(
${PROJECT_SOURCE_DIR}
${PROTOBUF_INCLUDE_DIR}
${SDFormat_INCLUDE_DIRS}
${OGRE_INCLUDE_DIRS}
+ ${IGNITION-MATH_INCLUDE_DIRS}
)
# Tinyxml are used since the plugins are linking against gazebo_common
@@ -25,14 +30,24 @@ if (HAVE_DART)
link_directories(${DARTCore_LIBRARY_DIRS})
endif()
+if (HAVE_UUID)
+ include_directories(${uuid_INCLUDE_DIRS})
+ link_directories(${uuid_LIBRARY_DIRS})
+ add_definitions(${uuid_CFLAGS})
+endif()
+
set (plugins
+ ActuatorPlugin
ArrangePlugin
BreakableJointPlugin
+ BuoyancyPlugin
CameraPlugin
CartDemoPlugin
+ CessnaPlugin
ContactPlugin
DepthCameraPlugin
DiffDrivePlugin
+ ElevatorPlugin
ForceTorquePlugin
GpuRayPlugin
ImuSensorPlugin
@@ -48,6 +63,7 @@ set (plugins
SkidSteerDrivePlugin
SonarPlugin
SphereAtlasDemoPlugin
+ TransporterPlugin
VehiclePlugin)
if (HAVE_USB AND UNIX AND NOT APPLE)
@@ -57,6 +73,11 @@ if (HAVE_USB AND UNIX AND NOT APPLE)
)
endif()
+set (GUIplugins
+ CessnaGUIPlugin
+ TimerGUIPlugin
+)
+
set(GAZEBO_PLUGIN_INSTALL_DIR
${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR}/gazebo-${GAZEBO_VERSION}/plugins/
)
@@ -72,10 +93,16 @@ set_target_properties(BreakableJointPlugin PROPERTIES
INSTALL_RPATH ${GAZEBO_PLUGIN_INSTALL_DIR}
)
-QT4_WRAP_CPP(TimerGUIPlugin_MOC TimerGUIPlugin.hh)
-add_library(TimerGUIPlugin SHARED TimerGUIPlugin.cc ${TimerGUIPlugin_MOC})
-target_link_libraries(TimerGUIPlugin libgazebo gazebo_sensors ${ogre_libraries} ${QT_LIBRARIES})
-install (TARGETS TimerGUIPlugin DESTINATION ${GAZEBO_PLUGIN_INSTALL_DIR})
+foreach (src ${GUIplugins})
+ QT4_WRAP_CPP(${src}_MOC ${src}.hh)
+ add_library(${src} SHARED ${src}.cc ${${src}_MOC})
+ target_link_libraries(${src} libgazebo gazebo_sensors ${ogre_libraries} ${QT_LIBRARIES})
+ install (TARGETS ${src} DESTINATION ${GAZEBO_PLUGIN_INSTALL_DIR})
+ gz_install_includes("plugins" ${src}.hh)
+endforeach (src ${GUIplugins})
add_subdirectory(events)
+if (NOT WIN32)
+ add_subdirectory(rest_web)
+endif()
diff --git a/plugins/CameraPlugin.cc b/plugins/CameraPlugin.cc
index 3c71d0f..d03768a 100644
--- a/plugins/CameraPlugin.cc
+++ b/plugins/CameraPlugin.cc
@@ -14,6 +14,12 @@
* 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 "gazebo/sensors/DepthCameraSensor.hh"
#include "plugins/CameraPlugin.hh"
@@ -21,7 +27,8 @@ using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(CameraPlugin)
/////////////////////////////////////////////////
-CameraPlugin::CameraPlugin() : SensorPlugin()
+CameraPlugin::CameraPlugin()
+: SensorPlugin(), width(0), height(0), depth(0)
{
}
diff --git a/plugins/CessnaGUIPlugin.cc b/plugins/CessnaGUIPlugin.cc
new file mode 100644
index 0000000..91a5f01
--- /dev/null
+++ b/plugins/CessnaGUIPlugin.cc
@@ -0,0 +1,338 @@
+/*
+ * 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.
+ *
+*/
+#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 <algorithm>
+#include <mutex>
+#include <gazebo/gui/GuiPlugin.hh>
+#include <gazebo/gui/Actions.hh>
+#include "CessnaGUIPlugin.hh"
+
+using namespace gazebo;
+
+// Register this plugin with the simulator
+GZ_REGISTER_GUI_PLUGIN(CessnaGUIPlugin)
+
+/////////////////////////////////////////////////
+CessnaGUIPlugin::CessnaGUIPlugin()
+ : GUIPlugin()
+{
+ // This is needed to avoid the creation of a black widget with default size.
+ this->resize(0, 0);
+
+ // Set the increment or decrement in angle per key pressed.
+ this->angleStep.SetFromDegree(1.0);
+
+ // Initialize transport.
+ this->gzNode = transport::NodePtr(new transport::Node());
+ this->gzNode->Init();
+ this->controlPub =
+ this->gzNode->Advertise<msgs::Cessna>("~/cessna_c172/control");
+ this->stateSub = this->gzNode->Subscribe<msgs::Cessna>(
+ "~/cessna_c172/state", &CessnaGUIPlugin::OnState, this);
+
+ // Connect hotkeys.
+ QShortcut *increaseThrust = new QShortcut(QKeySequence("w"), this);
+ QObject::connect(increaseThrust, SIGNAL(activated()), this,
+ SLOT(OnIncreaseThrust()));
+
+ QShortcut *decreaseThrust = new QShortcut(QKeySequence("s"), this);
+ QObject::connect(decreaseThrust, SIGNAL(activated()), this,
+ SLOT(OnDecreaseThrust()));
+
+ QShortcut *increaseFlaps = new QShortcut(QKeySequence("g"), this);
+ QObject::connect(increaseFlaps, SIGNAL(activated()), this,
+ SLOT(OnIncreaseFlaps()));
+
+ QShortcut *decreaseFlaps = new QShortcut(QKeySequence("b"), this);
+ QObject::connect(decreaseFlaps, SIGNAL(activated()), this,
+ SLOT(OnDecreaseFlaps()));
+
+ QShortcut *increaseRoll = new QShortcut(QKeySequence(Qt::Key_Left), this);
+ QObject::connect(increaseRoll, SIGNAL(activated()), this,
+ SLOT(OnIncreaseRoll()));
+
+ QShortcut *decreaseRoll = new QShortcut(QKeySequence(Qt::Key_Right), this);
+ QObject::connect(decreaseRoll, SIGNAL(activated()), this,
+ SLOT(OnDecreaseRoll()));
+
+ QShortcut *increaseElevators =
+ new QShortcut(QKeySequence(Qt::Key_Down), this);
+ QObject::connect(increaseElevators, SIGNAL(activated()), this,
+ SLOT(OnIncreaseElevators()));
+
+ QShortcut *decreaseElevators = new QShortcut(QKeySequence(Qt::Key_Up), this);
+ QObject::connect(decreaseElevators, SIGNAL(activated()), this,
+ SLOT(OnDecreaseElevators()));
+
+ QShortcut *increaseRudder = new QShortcut(QKeySequence("d"), this);
+ QObject::connect(increaseRudder, SIGNAL(activated()), this,
+ SLOT(OnIncreaseRudder()));
+
+ QShortcut *decreaseRudder = new QShortcut(QKeySequence("a"), this);
+ QObject::connect(decreaseRudder, SIGNAL(activated()), this,
+ SLOT(OnDecreaseRudder()));
+
+ QShortcut *presetTakeOff = new QShortcut(QKeySequence('1'), this);
+ QObject::connect(presetTakeOff, SIGNAL(activated()), this,
+ SLOT(OnPresetTakeOff()));
+
+ QShortcut *presetCruise = new QShortcut(QKeySequence('2'), this);
+ QObject::connect(presetCruise, SIGNAL(activated()), this,
+ SLOT(OnPresetCruise()));
+
+ QShortcut *presetLanding = new QShortcut(QKeySequence('3'), this);
+ QObject::connect(presetLanding, SIGNAL(activated()), this,
+ SLOT(OnPresetLanding()));
+}
+
+/////////////////////////////////////////////////
+CessnaGUIPlugin::~CessnaGUIPlugin()
+{
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnState(ConstCessnaPtr &_msg)
+{
+ std::lock_guard<std::mutex> lock(this->mutex);
+
+ // Refresh the state.
+ this->state = *_msg;
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnIncreaseThrust()
+{
+ float thrust;
+ {
+ std::lock_guard<std::mutex> lock(this->mutex);
+ thrust = this->state.cmd_propeller_speed();
+ }
+
+ msgs::Cessna msg;
+ thrust = std::min(thrust + 0.1f, 1.0f);
+ msg.set_cmd_propeller_speed(thrust);
+ this->controlPub->Publish(msg);
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnDecreaseThrust()
+{
+ float thrust;
+ {
+ std::lock_guard<std::mutex> lock(this->mutex);
+ thrust = this->state.cmd_propeller_speed();
+ }
+
+ msgs::Cessna msg;
+ thrust = std::max(thrust - 0.1f, 0.0f);
+ msg.set_cmd_propeller_speed(thrust);
+ this->controlPub->Publish(msg);
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnIncreaseFlaps()
+{
+ math::Angle flap;
+ {
+ std::lock_guard<std::mutex> lock(this->mutex);
+ flap.SetFromRadian(this->state.cmd_left_flap());
+ }
+
+ msgs::Cessna msg;
+ if (flap.Degree() < 30)
+ {
+ flap += this->angleStep;
+ msg.set_cmd_left_flap(flap.Radian());
+ msg.set_cmd_right_flap(flap.Radian());
+ this->controlPub->Publish(msg);
+ }
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnDecreaseFlaps()
+{
+ math::Angle flap;
+ {
+ std::lock_guard<std::mutex> lock(this->mutex);
+ flap.SetFromRadian(this->state.cmd_left_flap());
+ }
+
+ msgs::Cessna msg;
+ if (flap.Degree() > -30)
+ {
+ flap -= this->angleStep;
+ msg.set_cmd_left_flap(flap.Radian());
+ msg.set_cmd_right_flap(flap.Radian());
+ this->controlPub->Publish(msg);
+ }
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnIncreaseRoll()
+{
+ math::Angle aileron;
+ {
+ std::lock_guard<std::mutex> lock(this->mutex);
+ aileron.SetFromRadian(this->state.cmd_left_aileron());
+ }
+
+ msgs::Cessna msg;
+ if (aileron.Degree() < 30)
+ {
+ aileron += this->angleStep;
+ msg.set_cmd_left_aileron(aileron.Radian());
+ msg.set_cmd_right_aileron(-aileron.Radian());
+ this->controlPub->Publish(msg);
+ }
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnDecreaseRoll()
+{
+ math::Angle aileron;
+ {
+ std::lock_guard<std::mutex> lock(this->mutex);
+ aileron.SetFromRadian(this->state.cmd_left_aileron());
+ }
+
+ msgs::Cessna msg;
+ if (aileron.Degree() > -30)
+ {
+ aileron -= this->angleStep;
+ msg.set_cmd_left_aileron(aileron.Radian());
+ msg.set_cmd_right_aileron(-aileron.Radian());
+ this->controlPub->Publish(msg);
+ }
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnIncreaseElevators()
+{
+ math::Angle elevators;
+ {
+ std::lock_guard<std::mutex> lock(this->mutex);
+ elevators.SetFromRadian(this->state.cmd_elevators());
+ }
+
+ msgs::Cessna msg;
+ if (elevators.Degree() < 30)
+ {
+ elevators += this->angleStep;
+ msg.set_cmd_elevators(elevators.Radian());
+ this->controlPub->Publish(msg);
+ }
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnDecreaseElevators()
+{
+ math::Angle elevators;
+ {
+ std::lock_guard<std::mutex> lock(this->mutex);
+ elevators.SetFromRadian(this->state.cmd_elevators());
+ }
+
+ msgs::Cessna msg;
+ if (elevators.Degree() > -30)
+ {
+ elevators -= this->angleStep;
+ msg.set_cmd_elevators(elevators.Radian());
+ this->controlPub->Publish(msg);
+ }
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnIncreaseRudder()
+{
+ math::Angle rudder;
+ {
+ std::lock_guard<std::mutex> lock(this->mutex);
+ rudder.SetFromRadian(this->state.cmd_rudder());
+ }
+
+ msgs::Cessna msg;
+ if (rudder.Degree() < 30)
+ {
+ rudder += this->angleStep;
+ msg.set_cmd_rudder(rudder.Radian());
+ this->controlPub->Publish(msg);
+ }
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnDecreaseRudder()
+{
+ math::Angle rudder;
+ {
+ std::lock_guard<std::mutex> lock(this->mutex);
+ rudder.SetFromRadian(this->state.cmd_rudder());
+ }
+
+ msgs::Cessna msg;
+ if (rudder.Degree() > -30)
+ {
+ rudder -= this->angleStep;
+ msg.set_cmd_rudder(rudder.Radian());
+ this->controlPub->Publish(msg);
+ }
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnPresetTakeOff()
+{
+ msgs::Cessna msg;
+ msg.set_cmd_propeller_speed(0.8);
+ msg.set_cmd_left_aileron(-0.017);
+ msg.set_cmd_right_aileron(0.017);
+ msg.set_cmd_left_flap(0);
+ msg.set_cmd_right_flap(0);
+ msg.set_cmd_elevators(0.033);
+ msg.set_cmd_rudder(-0.035);
+ this->controlPub->Publish(msg);
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnPresetCruise()
+{
+ msgs::Cessna msg;
+ msg.set_cmd_propeller_speed(0.6);
+ msg.set_cmd_left_aileron(0);
+ msg.set_cmd_right_aileron(0);
+ msg.set_cmd_left_flap(0);
+ msg.set_cmd_right_flap(0);
+ msg.set_cmd_elevators(0.12);
+ msg.set_cmd_rudder(-0.035);
+ this->controlPub->Publish(msg);
+}
+
+/////////////////////////////////////////////////
+void CessnaGUIPlugin::OnPresetLanding()
+{
+ msgs::Cessna msg;
+ msg.set_cmd_propeller_speed(0.3);
+ msg.set_cmd_left_aileron(0);
+ msg.set_cmd_right_aileron(0);
+ msg.set_cmd_left_flap(0);
+ msg.set_cmd_right_flap(0);
+ msg.set_cmd_elevators(0.16);
+ msg.set_cmd_rudder(-0.035);
+ this->controlPub->Publish(msg);
+}
diff --git a/plugins/CessnaGUIPlugin.hh b/plugins/CessnaGUIPlugin.hh
new file mode 100644
index 0000000..f41e3ae
--- /dev/null
+++ b/plugins/CessnaGUIPlugin.hh
@@ -0,0 +1,123 @@
+/*
+ * 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_GUI_CESSNA_PLUGIN_HH_
+#define _GAZEBO_GUI_CESSNA_PLUGIN_HH_
+
+#include <mutex>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/gui/GuiPlugin.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
+{
+ /// \brief A GUI plugin that controls the Cessna model using the keyboard.
+ /// If you are reading this, feel free to improve this plugin by adding
+ /// graphical widgets to make the demo more interesting and fun.
+ ///
+ /// Keyboard controls:
+ /// w Increase thrust (+10 %)
+ /// s Decrease thrust (-10 %)
+ /// d Increase rudder angle (+1 degree)
+ /// a Decrease rudder angle (-1 degree)
+ /// Left-Key Left roll (+1 degree)
+ /// Right-Key Right roll (+1 degree)
+ /// Up-Key Pitch down (+1 degree)
+ /// Down-Key Pitch up (+1 degree)
+ /// 1 Preset for take-off
+ /// 2 Preset for cruise
+ /// 3 Preset for landing
+ class GAZEBO_VISIBLE CessnaGUIPlugin : public GUIPlugin
+ {
+ Q_OBJECT
+
+ /// \brief Constructor.
+ public: CessnaGUIPlugin();
+
+ /// \brief Destructor.
+ public: virtual ~CessnaGUIPlugin();
+
+ /// \brief Callback that receives a control message from
+ /// the ~/cessna_c172/state topic.
+ /// \param[in] _msg State msg.
+ private: void OnState(ConstCessnaPtr &_msg);
+
+ /// \brief Increase the propeller RPMs.
+ private slots: void OnIncreaseThrust();
+
+ /// \brief Decrease the propeller RPMs.
+ private slots: void OnDecreaseThrust();
+
+ /// \brief Increase the flaps angle.
+ private slots: void OnIncreaseFlaps();
+
+ /// \brief Decrease the flaps angle.
+ private slots: void OnDecreaseFlaps();
+
+ /// \brief Increase Roll.
+ private slots: void OnIncreaseRoll();
+
+ /// \brief Decrease Roll.
+ private slots: void OnDecreaseRoll();
+
+ /// \brief Increase the elevators angle.
+ private slots: void OnIncreaseElevators();
+
+ /// \brief Decrease the elevators angle.
+ private slots: void OnDecreaseElevators();
+
+ /// \brief Increase the rudder angle.
+ private slots: void OnIncreaseRudder();
+
+ /// \brief Decrease the rudder angle.
+ private slots: void OnDecreaseRudder();
+
+ /// \brief Take-off preset.
+ private slots: void OnPresetTakeOff();
+
+ /// \brief Cruise preset.
+ private slots: void OnPresetCruise();
+
+ /// \brief Landing preset.
+ private slots: void OnPresetLanding();
+
+ /// \brief SDF for this plugin.
+ private: sdf::ElementPtr sdf;
+
+ /// \brief Pointer to a node for communication.
+ private: transport::NodePtr gzNode;
+
+ /// \brief Control publisher.
+ private: transport::PublisherPtr controlPub;
+
+ /// \brief State subscriber.
+ private: transport::SubscriberPtr stateSub;
+
+ /// \brief Angle increment/decrement each time a key is pressed;
+ private: math::Angle angleStep;
+
+ /// \brief State received from the Cessna plugin.
+ private: msgs::Cessna state;
+
+ /// \brief Protection.
+ private: std::mutex mutex;
+ };
+}
+
+#endif
diff --git a/plugins/CessnaPlugin.cc b/plugins/CessnaPlugin.cc
new file mode 100644
index 0000000..397f1f7
--- /dev/null
+++ b/plugins/CessnaPlugin.cc
@@ -0,0 +1,247 @@
+/*
+ * 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 <mutex>
+#include <string>
+#include <sdf/sdf.hh>
+#include <gazebo/common/Assert.hh>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+#include "plugins/CessnaPlugin.hh"
+
+using namespace gazebo;
+
+GZ_REGISTER_MODEL_PLUGIN(CessnaPlugin)
+
+////////////////////////////////////////////////////////////////////////////////
+CessnaPlugin::CessnaPlugin() : cmds {{0, 0, 0, 0, 0, 0, 0}}
+{
+ // PID default parameters.
+ this->propellerPID.Init(50.0, 0.1, 1, 0.0, 0.0, 20000.0, -20000.0);
+ this->propellerPID.SetCmd(0.0);
+
+ for (auto &pid : this->controlSurfacesPID)
+ {
+ pid.Init(50.0, 0.1, 1, 0.0, 0.0, 20.0, -20.0);
+ pid.SetCmd(0.0);
+ }
+}
+
+/////////////////////////////////////////////////
+CessnaPlugin::~CessnaPlugin()
+{
+ event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
+}
+
+/////////////////////////////////////////////////
+bool CessnaPlugin::FindJoint(const std::string &_sdfParam, sdf::ElementPtr _sdf,
+ physics::JointPtr &_joint)
+{
+ // Read the required plugin parameters.
+ if (!_sdf->HasElement(_sdfParam))
+ {
+ gzerr << "Unable to find the <" << _sdfParam << "> parameter." << std::endl;
+ return false;
+ }
+
+ std::string jointName = _sdf->Get<std::string>(_sdfParam);
+ _joint = this->model->GetJoint(jointName);
+ if (!_joint)
+ {
+ gzerr << "Failed to find joint [" << jointName
+ << "] aborting plugin load." << std::endl;
+ return false;
+ }
+ return true;
+}
+
+/////////////////////////////////////////////////
+void CessnaPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
+{
+ GZ_ASSERT(_model, "CessnaPlugin _model pointer is NULL");
+ GZ_ASSERT(_sdf, "CessnaPlugin _sdf pointer is NULL");
+ this->model = _model;
+
+ // Read the required parameter for the propeller max RPMs.
+ if (!_sdf->HasElement("propeller_max_rpm"))
+ {
+ gzerr << "Unable to find the <propeller_max_rpm> parameter." << std::endl;
+ return;
+ }
+ this->propellerMaxRpm = _sdf->Get<int32_t>("propeller_max_rpm");
+ if (this->propellerMaxRpm == 0)
+ {
+ gzerr << "Maximum propeller RPMs cannot be 0" << std::endl;
+ return;
+ }
+
+ // Read the required joint name parameters.
+ std::vector<std::string> requiredParams = {"left_aileron", "left_flap",
+ "right_aileron", "right_flap", "elevators", "rudder", "propeller"};
+
+ for (size_t i = 0; i < requiredParams.size(); ++i)
+ {
+ if (!this->FindJoint(requiredParams[i], _sdf, this->joints[i]))
+ return;
+ }
+
+ // Overload the PID parameters if they are available.
+ if (_sdf->HasElement("propeller_p_gain"))
+ this->propellerPID.SetPGain(_sdf->Get<double>("propeller_p_gain"));
+
+ if (_sdf->HasElement("propeller_i_gain"))
+ this->propellerPID.SetIGain(_sdf->Get<double>("propeller_i_gain"));
+
+ if (_sdf->HasElement("propeller_d_gain"))
+ this->propellerPID.SetDGain(_sdf->Get<double>("propeller_d_gain"));
+
+ if (_sdf->HasElement("surfaces_p_gain"))
+ {
+ for (auto &pid : this->controlSurfacesPID)
+ pid.SetPGain(_sdf->Get<double>("surfaces_p_gain"));
+ }
+
+ if (_sdf->HasElement("surfaces_i_gain"))
+ {
+ for (auto &pid : this->controlSurfacesPID)
+ pid.SetIGain(_sdf->Get<double>("surfaces_i_gain"));
+ }
+
+ if (_sdf->HasElement("surfaces_d_gain"))
+ {
+ for (auto &pid : this->controlSurfacesPID)
+ pid.SetDGain(_sdf->Get<double>("surfaces_d_gain"));
+ }
+
+ // Controller time control.
+ this->lastControllerUpdateTime = this->model->GetWorld()->GetSimTime();
+
+ // Listen to the update event. This event is broadcast every simulation
+ // iteration.
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&CessnaPlugin::Update, this, _1));
+
+ // Initialize transport.
+ this->node = transport::NodePtr(new transport::Node());
+ this->node->Init();
+ std::string prefix = "~/" + this->model->GetName() + "/";
+ this->statePub = this->node->Advertise<msgs::Cessna>(prefix + "state");
+ this->controlSub = this->node->Subscribe(prefix + "control",
+ &CessnaPlugin::OnControl, this);
+
+ gzlog << "Cessna ready to fly. The force will be with you" << std::endl;
+}
+
+/////////////////////////////////////////////////
+void CessnaPlugin::Update(const common::UpdateInfo &/*_info*/)
+{
+ std::lock_guard<std::mutex> lock(this->mutex);
+
+ gazebo::common::Time curTime = this->model->GetWorld()->GetSimTime();
+
+ if (curTime > this->lastControllerUpdateTime)
+ {
+ // Update the control surfaces and publish the new state.
+ this->UpdatePIDs((curTime - this->lastControllerUpdateTime).Double());
+ this->PublishState();
+
+ this->lastControllerUpdateTime = curTime;
+ }
+}
+
+/////////////////////////////////////////////////
+void CessnaPlugin::OnControl(ConstCessnaPtr &_msg)
+{
+ std::lock_guard<std::mutex> lock(this->mutex);
+
+ if (_msg->has_cmd_propeller_speed() &&
+ std::abs(_msg->cmd_propeller_speed()) <= 1)
+ {
+ this->cmds[kPropeller] = _msg->cmd_propeller_speed();
+ }
+ if (_msg->has_cmd_left_aileron())
+ this->cmds[kLeftAileron] = _msg->cmd_left_aileron();
+ if (_msg->has_cmd_left_flap())
+ this->cmds[kLeftFlap] = _msg->cmd_left_flap();
+ if (_msg->has_cmd_right_aileron())
+ this->cmds[kRightAileron] = _msg->cmd_right_aileron();
+ if (_msg->has_cmd_right_flap())
+ this->cmds[kRightFlap] = _msg->cmd_right_flap();
+ if (_msg->has_cmd_elevators())
+ this->cmds[kElevators] = _msg->cmd_elevators();
+ if (_msg->has_cmd_rudder())
+ this->cmds[kRudder] = _msg->cmd_rudder();
+}
+
+/////////////////////////////////////////////////
+void CessnaPlugin::UpdatePIDs(double _dt)
+{
+ // Velocity PID for the propeller.
+ double vel = this->joints[kPropeller]->GetVelocity(0);
+ double maxVel = this->propellerMaxRpm*2.0*M_PI/60.0;
+ double target = maxVel * this->cmds[kPropeller];
+ double error = vel - target;
+ double force = this->propellerPID.Update(error, _dt);
+ this->joints[kPropeller]->SetForce(0, force);
+
+ // Position PID for the control surfaces.
+ for (size_t i = 0; i < this->controlSurfacesPID.size(); ++i)
+ {
+ double pos = this->joints[i]->GetAngle(0).Radian();
+ error = pos - this->cmds[i];
+ force = this->controlSurfacesPID[i].Update(error, _dt);
+ this->joints[i]->SetForce(0, force);
+ }
+}
+
+/////////////////////////////////////////////////
+void CessnaPlugin::PublishState()
+{
+ // Read the current state.
+ double propellerRpms = this->joints[kPropeller]->GetVelocity(0)
+ /(2.0*M_PI)*60.0;
+ float propellerSpeed = propellerRpms / this->propellerMaxRpm;
+ float leftAileron = this->joints[kLeftAileron]->GetAngle(0).Radian();
+ float leftFlap = this->joints[kLeftFlap]->GetAngle(0).Radian();
+ float rightAileron = this->joints[kRightAileron]->GetAngle(0).Radian();
+ float rightFlap = this->joints[kRightFlap]->GetAngle(0).Radian();
+ float elevators = this->joints[kElevators]->GetAngle(0).Radian();
+ float rudder = this->joints[kRudder]->GetAngle(0).Radian();
+
+ msgs::Cessna msg;
+ // Set the observed state.
+ msg.set_propeller_speed(propellerSpeed);
+ msg.set_left_aileron(leftAileron);
+ msg.set_left_flap(leftFlap);
+ msg.set_right_aileron(rightAileron);
+ msg.set_right_flap(rightFlap);
+ msg.set_elevators(elevators);
+ msg.set_rudder(rudder);
+
+ // Set the target state.
+ msg.set_cmd_propeller_speed(this->cmds[kPropeller]);
+ msg.set_cmd_left_aileron(this->cmds[kLeftAileron]);
+ msg.set_cmd_left_flap(this->cmds[kLeftFlap]);
+ msg.set_cmd_right_aileron(this->cmds[kRightAileron]);
+ msg.set_cmd_right_flap(this->cmds[kRightFlap]);
+ msg.set_cmd_elevators(this->cmds[kElevators]);
+ msg.set_cmd_rudder(this->cmds[kRudder]);
+
+ this->statePub->Publish(msg);
+}
diff --git a/plugins/CessnaPlugin.hh b/plugins/CessnaPlugin.hh
new file mode 100644
index 0000000..3cc3961
--- /dev/null
+++ b/plugins/CessnaPlugin.hh
@@ -0,0 +1,146 @@
+/*
+ * 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_CESSNA_PLUGIN_HH_
+#define _GAZEBO_CESSNA_PLUGIN_HH_
+
+#include <array>
+#include <mutex>
+#include <string>
+#include <sdf/sdf.hh>
+#include <gazebo/common/PID.hh>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/common/UpdateInfo.hh>
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/physics/PhysicsTypes.hh>
+#include <gazebo/transport/TransportTypes.hh>
+
+namespace gazebo
+{
+ /// \brief Allow moving the control surfaces of a Cessna C-172 plane. This
+ /// plugin might be used with other models that have similar control surfaces.
+ ///
+ /// The plugin requires the following parameters:
+ /// <propeller> Name of the joint controlling the propeller spin.
+ /// <propeller_max_rpm> Maximum angular speed in rpm.
+ /// <left_aileron> Name of the joint controlling the left aileron.
+ /// <left_flap> Name of the joint controlling the left flap.
+ /// <right_aileron> Name of the joint controlling the right aileron.
+ /// <right_flap> Name of the joint controlling the right flap.
+ /// <elevators> Name of the joint controlling the rear elevators.
+ /// <rudder> Name of the joint controlling the rudder.
+ ///
+ /// The following parameters are optional:
+ /// <propeller_p_gain> P gain for the PID that controls the propeller's speed.
+ /// <propeller_i_gain> I gain for the PID that controls the propeller's speed.
+ /// <propeller_d_gain> D gain for the PID that controls the propeller's speed.
+ /// <surfaces_p_gain> P gain for the PID that controls the position of the
+ /// control surfaces.
+ /// <surfaces_i_gain> I gain for the PID that controls the position of the
+ /// control surfaces.
+ /// <surfaces_d_gain> D gain for the PID that controls the position of the
+ /// control surfaces.
+ ///
+ /// The plugin will be subscribed to the following topic:
+ /// "~/<model_name>/control" The expected value is a Cessna message.
+ ///
+ /// The plugin will advertise the following topic with the current state:
+ /// "~/<model_name>/state"
+ class GAZEBO_VISIBLE CessnaPlugin : public ModelPlugin
+ {
+ /// \brief Constructor.
+ public: CessnaPlugin();
+
+ /// \brief Destructor.
+ public: ~CessnaPlugin();
+
+ // Documentation Inherited.
+ public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+ /// \brief Read an SDF parameter with a joint name and initialize a pointer
+ /// to this joint.
+ /// \param[in] _sdfParam SDF parameter containing a joint name.
+ /// \param[in] _sdf Pointer to the SDF element containing the parameters.
+ /// \param[out] _joint Pointer to the joint to be initialized.
+ /// \return True if the SDF parameter is found and the joint name is found,
+ /// false otherwise.
+ private: bool FindJoint(const std::string &_sdfParam,
+ sdf::ElementPtr _sdf, physics::JointPtr &_joint);
+
+ /// \brief Update the control surfaces controllers.
+ /// \param[in] _info Update information provided by the server.
+ private: void Update(const common::UpdateInfo &_info);
+
+ /// \brief Callback executed when a new message containing control commands
+ /// is received.
+ /// \param[in] _msg New message containing control commands.
+ private: void OnControl(ConstCessnaPtr &_msg);
+
+ /// \brief Update PID Joint controllers.
+ /// \param[in] _dt time step size since last update.
+ private: void UpdatePIDs(double _dt);
+
+ /// \brief Publish Cessna state.
+ private: void PublishState();
+
+ /// \brief Joint indexes.
+ private: static const unsigned int kLeftAileron = 0;
+ private: static const unsigned int kLeftFlap = 1;
+ private: static const unsigned int kRightAileron = 2;
+ private: static const unsigned int kRightFlap = 3;
+ private: static const unsigned int kElevators = 4;
+ private: static const unsigned int kRudder = 5;
+ private: static const unsigned int kPropeller = 6;
+
+ /// \brief Pointer to the update event connection.
+ private: event::ConnectionPtr updateConnection;
+
+ /// \brief Node used for using Gazebo communications.
+ private: transport::NodePtr node;
+
+ /// \brief Subscriber pointer.
+ private: transport::SubscriberPtr controlSub;
+
+ /// \brief Publisher pointer.
+ private: transport::PublisherPtr statePub;
+
+ /// \brief Pointer to the model;
+ private: physics::ModelPtr model;
+
+ /// \brief Control surfaces joints.
+ private: std::array<physics::JointPtr, 7> joints;
+
+ /// \brief Max propeller RPM.
+ private: int32_t propellerMaxRpm = 2500;
+
+ /// \brief Next command to be applied to the propeller and control surfaces.
+ private: std::array<float, 7>cmds;
+
+ /// \brief Velocity PID for the propeller.
+ private: common::PID propellerPID;
+
+ /// \brief Position PID for the control surfaces.
+ private: std::array<common::PID, 6> controlSurfacesPID;
+
+ /// \brief keep track of controller update sim-time.
+ private: gazebo::common::Time lastControllerUpdateTime;
+
+ /// \brief Controller update mutex.
+ private: std::mutex mutex;
+ };
+}
+#endif
diff --git a/plugins/DepthCameraPlugin.cc b/plugins/DepthCameraPlugin.cc
index 4f1fd93..ce0f4cd 100644
--- a/plugins/DepthCameraPlugin.cc
+++ b/plugins/DepthCameraPlugin.cc
@@ -20,7 +20,8 @@ using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(DepthCameraPlugin)
/////////////////////////////////////////////////
-DepthCameraPlugin::DepthCameraPlugin() : SensorPlugin()
+DepthCameraPlugin::DepthCameraPlugin()
+: SensorPlugin(), width(0), height(0), depth(0)
{
}
diff --git a/plugins/DiffDrivePlugin.cc b/plugins/DiffDrivePlugin.cc
index c855047..0e89e89 100644
--- a/plugins/DiffDrivePlugin.cc
+++ b/plugins/DiffDrivePlugin.cc
@@ -28,6 +28,8 @@ enum {RIGHT, LEFT};
DiffDrivePlugin::DiffDrivePlugin()
{
this->wheelSpeed[LEFT] = this->wheelSpeed[RIGHT] = 0;
+ this->wheelSeparation = 1.0;
+ this->wheelRadius = 1.0;
}
/////////////////////////////////////////////////
@@ -53,14 +55,6 @@ void DiffDrivePlugin::Load(physics::ModelPtr _model,
this->rightJoint = _model->GetJoint(
_sdf->GetElement("right_joint")->Get<std::string>());
- if (_sdf->HasElement("torque"))
- {
- this->torque = _sdf->GetElement("torque")->Get<double>();
- gzwarn << "The MaxForce API is deprecated in Gazebo, "
- << "and the torque tag is no longer used in this plugin."
- << std::endl;
- }
-
if (!this->leftJoint)
gzerr << "Unable to find left joint["
<< _sdf->GetElement("left_joint")->Get<std::string>() << "]\n";
@@ -92,7 +86,7 @@ void DiffDrivePlugin::OnVelMsg(ConstPosePtr &_msg)
double vr, va;
vr = _msg->position().x();
- va = msgs::Convert(_msg->orientation()).GetAsEuler().z;
+ va = msgs::ConvertIgn(_msg->orientation()).Euler().Z();
this->wheelSpeed[LEFT] = vr + va * this->wheelSeparation / 2.0;
this->wheelSpeed[RIGHT] = vr - va * this->wheelSeparation / 2.0;
diff --git a/plugins/DiffDrivePlugin.hh b/plugins/DiffDrivePlugin.hh
index 8c33ed0..6b72d51 100644
--- a/plugins/DiffDrivePlugin.hh
+++ b/plugins/DiffDrivePlugin.hh
@@ -42,7 +42,6 @@ namespace gazebo
private: physics::JointPtr leftJoint, rightJoint;
private: event::ConnectionPtr updateConnection;
private: double wheelSpeed[2];
- private: double torque;
private: double wheelSeparation;
private: double wheelRadius;
private: common::Time prevUpdateTime;
diff --git a/plugins/ElevatorPlugin.cc b/plugins/ElevatorPlugin.cc
new file mode 100644
index 0000000..4530b5f
--- /dev/null
+++ b/plugins/ElevatorPlugin.cc
@@ -0,0 +1,494 @@
+/*
+ * 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/common/Events.hh>
+#include <gazebo/common/Assert.hh>
+#include <gazebo/common/Console.hh>
+
+#include <gazebo/physics/World.hh>
+#include <gazebo/physics/Model.hh>
+#include <gazebo/physics/Joint.hh>
+
+#include "plugins/ElevatorPluginPrivate.hh"
+#include "plugins/ElevatorPlugin.hh"
+
+using namespace gazebo;
+
+GZ_REGISTER_MODEL_PLUGIN(ElevatorPlugin)
+
+/////////////////////////////////////////////////
+ElevatorPlugin::ElevatorPlugin()
+ : dataPtr(new ElevatorPluginPrivate)
+{
+ this->dataPtr->doorController = NULL;
+ this->dataPtr->liftController = NULL;
+ this->dataPtr->doorWaitTime = common::Time(5, 0);
+}
+
+/////////////////////////////////////////////////
+ElevatorPlugin::~ElevatorPlugin()
+{
+ event::Events::DisconnectWorldUpdateBegin(this->dataPtr->updateConnection);
+
+ delete this->dataPtr->doorController;
+ this->dataPtr->doorController = NULL;
+
+ delete this->dataPtr->liftController;
+ this->dataPtr->liftController = NULL;
+
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+void ElevatorPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
+{
+ GZ_ASSERT(_model, "ElevatorPlugin model pointer is NULL");
+ GZ_ASSERT(_sdf, "ElevatorPlugin sdf pointer is NULL");
+ this->dataPtr->model = _model;
+ this->dataPtr->sdf = _sdf;
+
+ // Get the time to hold the door open.
+ if (this->dataPtr->sdf->HasElement("door_wait_time"))
+ {
+ this->dataPtr->doorWaitTime.Set(
+ this->dataPtr->sdf->Get<double>("door_wait_time"));
+ }
+
+ // Get the elevator topic.
+ std::string elevatorTopic = "~/elevator";
+ if (this->dataPtr->sdf->HasElement("topic"))
+ elevatorTopic = this->dataPtr->sdf->Get<std::string>("topic");
+
+ float floorHeight = 3.0;
+ if (this->dataPtr->sdf->HasElement("floor_height"))
+ floorHeight = this->dataPtr->sdf->Get<float>("floor_height");
+ else
+ {
+ gzwarn << "No <floor_height> specified for elevator plugin. "
+ << "Using a height of 3 meters. This value may cause "
+ << "the elevator to move incorrectly.\n";
+ }
+
+ // Get the lift joint
+ std::string liftJointName =
+ this->dataPtr->sdf->Get<std::string>("lift_joint");
+
+ this->dataPtr->liftJoint = this->dataPtr->model->GetJoint(liftJointName);
+ if (!this->dataPtr->liftJoint)
+ {
+ gzerr << "Unable to find lift joint[" << liftJointName << "].\n";
+ gzerr << "The elevator plugin is disabled.\n";
+ return;
+ }
+
+ // Get the door joint
+ std::string doorJointName =
+ this->dataPtr->sdf->Get<std::string>("door_joint");
+
+ this->dataPtr->doorJoint = this->dataPtr->model->GetJoint(doorJointName);
+ if (!this->dataPtr->doorJoint)
+ {
+ gzerr << "Unable to find door joint[" << doorJointName << "].\n";
+ gzerr << "The elevator plugin is disabled.\n";
+ return;
+ }
+
+ // Create the door and lift controllers.
+ this->dataPtr->doorController = new ElevatorPluginPrivate::DoorController(
+ this->dataPtr->doorJoint);
+ this->dataPtr->liftController = new ElevatorPluginPrivate::LiftController(
+ this->dataPtr->liftJoint, floorHeight);
+
+ // Connect to the update event.
+ this->dataPtr->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&ElevatorPlugin::Update, this, _1));
+
+ // Create the node for communication
+ this->dataPtr->node = transport::NodePtr(new transport::Node());
+ this->dataPtr->node->Init(this->dataPtr->model->GetWorld()->GetName());
+
+ // Subscribe to the elevator topic.
+ this->dataPtr->elevatorSub = this->dataPtr->node->Subscribe(elevatorTopic,
+ &ElevatorPlugin::OnElevator, this);
+}
+
+/////////////////////////////////////////////////
+void ElevatorPlugin::OnElevator(ConstGzStringPtr &_msg)
+{
+ // Currently we only expect the message to contain a floor to move to.
+ try
+ {
+ this->MoveToFloor(std::stoi(_msg->data()));
+ }
+ catch(...)
+ {
+ gzerr << "Unable to process elevator message["
+ << _msg->data() << "]\n";
+ }
+}
+
+/////////////////////////////////////////////////
+void ElevatorPlugin::MoveToFloor(const int _floor)
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->stateMutex);
+
+ // Ignore messages when the elevator is currently busy.
+ if (!this->dataPtr->states.empty())
+ return;
+
+ // Step 1: close the door.
+ this->dataPtr->states.push_back(new ElevatorPluginPrivate::CloseState(
+ this->dataPtr->doorController));
+
+ // Step 2: Move to the correct floor.
+ this->dataPtr->states.push_back(new ElevatorPluginPrivate::MoveState(
+ _floor, this->dataPtr->liftController));
+
+ // Step 3: Open the door
+ this->dataPtr->states.push_back(new ElevatorPluginPrivate::OpenState(
+ this->dataPtr->doorController));
+
+ // Step 4: Wait
+ this->dataPtr->states.push_back(new ElevatorPluginPrivate::WaitState(
+ this->dataPtr->doorWaitTime));
+
+ // Step 5: Close the door
+ this->dataPtr->states.push_back(new ElevatorPluginPrivate::CloseState(
+ this->dataPtr->doorController));
+}
+
+/////////////////////////////////////////////////
+void ElevatorPlugin::Update(const common::UpdateInfo &_info)
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->stateMutex);
+
+ // Process the states
+ if (!this->dataPtr->states.empty())
+ {
+ // Update the front state, and remove it if the state is done
+ if (this->dataPtr->states.front()->Update())
+ {
+ delete this->dataPtr->states.front();
+ this->dataPtr->states.pop_front();
+ }
+ }
+
+ // Update the controllers
+ this->dataPtr->doorController->Update(_info);
+ this->dataPtr->liftController->Update(_info);
+}
+
+////////////////////////////////////////////////
+void ElevatorPlugin::Reset()
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->stateMutex);
+ for (auto s: this->dataPtr->states)
+ delete s;
+ this->dataPtr->states.clear();
+ this->dataPtr->doorController->Reset();
+ this->dataPtr->liftController->Reset();
+}
+
+////////////////////////////////////////////////
+// ElevatorPluginPrivate Class
+
+/////////////////////////////////////////////////
+ElevatorPluginPrivate::~ElevatorPluginPrivate()
+{
+ delete this->doorController;
+ this->doorController = NULL;
+
+ delete this->liftController;
+ this->liftController = NULL;
+
+ for (auto s: this->states)
+ delete s;
+ this->states.clear();
+}
+
+////////////////////////////////////////////////
+// CloseState Class
+
+/////////////////////////////////////////////////
+ElevatorPluginPrivate::CloseState::CloseState(DoorController *_ctrl)
+ : State(), ctrl(_ctrl)
+{
+}
+
+/////////////////////////////////////////////////
+void ElevatorPluginPrivate::CloseState::Start()
+{
+ this->ctrl->SetTarget(ElevatorPluginPrivate::DoorController::CLOSE);
+ this->started = true;
+}
+
+/////////////////////////////////////////////////
+bool ElevatorPluginPrivate::CloseState::Update()
+{
+ if (!this->started)
+ {
+ this->Start();
+ return false;
+ }
+ else
+ {
+ return this->ctrl->GetTarget() ==
+ ElevatorPluginPrivate::DoorController::CLOSE &&
+ this->ctrl->GetState() ==
+ ElevatorPluginPrivate::DoorController::STATIONARY;
+ }
+}
+
+////////////////////////////////////////////////
+// OpenState Class
+
+/////////////////////////////////////////////////
+ElevatorPluginPrivate::OpenState::OpenState(DoorController *_ctrl)
+ : State(), ctrl(_ctrl)
+{
+}
+
+/////////////////////////////////////////////////
+void ElevatorPluginPrivate::OpenState::Start()
+{
+ this->ctrl->SetTarget(ElevatorPluginPrivate::DoorController::OPEN);
+ this->started = true;
+}
+
+/////////////////////////////////////////////////
+bool ElevatorPluginPrivate::OpenState::Update()
+{
+ if (!this->started)
+ {
+ this->Start();
+ return false;
+ }
+ else
+ {
+ return this->ctrl->GetTarget() ==
+ ElevatorPluginPrivate::DoorController::OPEN &&
+ this->ctrl->GetState() ==
+ ElevatorPluginPrivate::DoorController::STATIONARY;
+ }
+}
+
+////////////////////////////////////////////////
+// MoveState Class
+
+/////////////////////////////////////////////////
+ElevatorPluginPrivate::MoveState::MoveState(int _floor, LiftController *_ctrl)
+ : State(), floor(_floor), ctrl(_ctrl)
+{
+}
+
+/////////////////////////////////////////////////
+void ElevatorPluginPrivate::MoveState::Start()
+{
+ this->ctrl->SetFloor(this->floor);
+ this->started = true;
+}
+
+/////////////////////////////////////////////////
+bool ElevatorPluginPrivate::MoveState::Update()
+{
+ if (!this->started)
+ {
+ this->Start();
+ return false;
+ }
+ else
+ {
+ return this->ctrl->GetState() ==
+ ElevatorPluginPrivate::LiftController::STATIONARY;
+ }
+}
+
+////////////////////////////////////////////////
+// WaitState Class
+
+/////////////////////////////////////////////////
+ElevatorPluginPrivate::WaitState::WaitState(const common::Time &_waitTime)
+ : State(), waitTimer(_waitTime, true)
+{
+}
+
+/////////////////////////////////////////////////
+void ElevatorPluginPrivate::WaitState::Start()
+{
+ this->waitTimer.Reset();
+ this->waitTimer.Start();
+ this->started = true;
+}
+
+/////////////////////////////////////////////////
+bool ElevatorPluginPrivate::WaitState::Update()
+{
+ if (!this->started)
+ {
+ this->Start();
+ return false;
+ }
+ else
+ {
+ if (this->waitTimer.GetElapsed() == common::Time::Zero)
+ return true;
+ else
+ return false;
+ }
+}
+
+////////////////////////////////////////////////
+// DoorController Class
+
+/////////////////////////////////////////////////
+ElevatorPluginPrivate::DoorController::DoorController(
+ physics::JointPtr _doorJoint)
+ : doorJoint(_doorJoint), state(STATIONARY), target(CLOSE)
+{
+ this->doorPID.Init(2, 0, 1.0);
+}
+
+/////////////////////////////////////////////////
+void ElevatorPluginPrivate::DoorController::SetTarget(
+ ElevatorPluginPrivate::DoorController::Target _target)
+{
+ this->target = _target;
+}
+
+/////////////////////////////////////////////////
+ElevatorPluginPrivate::DoorController::Target
+ElevatorPluginPrivate::DoorController::GetTarget() const
+{
+ return this->target;
+}
+
+/////////////////////////////////////////////////
+ElevatorPluginPrivate::DoorController::State
+ElevatorPluginPrivate::DoorController::GetState() const
+{
+ return this->state;
+}
+
+/////////////////////////////////////////////////
+void ElevatorPluginPrivate::DoorController::Reset()
+{
+ this->prevSimTime = common::Time::Zero;
+}
+
+/////////////////////////////////////////////////
+bool ElevatorPluginPrivate::DoorController::Update(
+ const common::UpdateInfo &_info)
+{
+ // Bootstrap the time.
+ if (this->prevSimTime == common::Time::Zero)
+ {
+ this->prevSimTime = _info.simTime;
+ return false;
+ }
+
+ double errorTarget = this->target == OPEN ? 1.0 : 0.0;
+
+ double doorError = this->doorJoint->GetAngle(0).Radian() -
+ errorTarget;
+
+ double doorForce = this->doorPID.Update(doorError,
+ _info.simTime - this->prevSimTime);
+
+ this->doorJoint->SetForce(0, doorForce);
+
+ if (std::abs(doorError) < 0.05)
+ {
+ this->state = STATIONARY;
+ return true;
+ }
+ else
+ {
+ this->state = MOVING;
+ return false;
+ }
+}
+
+////////////////////////////////////////////////
+// LiftController Class
+
+/////////////////////////////////////////////////
+ElevatorPluginPrivate::LiftController::LiftController(
+ physics::JointPtr _liftJoint, float _floorHeight)
+ : state(STATIONARY), floor(0), floorHeight(_floorHeight),
+ liftJoint(_liftJoint)
+{
+ this->liftPID.Init(100000, 0, 100000.0);
+}
+
+/////////////////////////////////////////////////
+void ElevatorPluginPrivate::LiftController::Reset()
+{
+ this->prevSimTime = common::Time::Zero;
+}
+
+/////////////////////////////////////////////////
+bool ElevatorPluginPrivate::LiftController::Update(
+ const common::UpdateInfo &_info)
+{
+ // Bootstrap the time.
+ if (this->prevSimTime == common::Time::Zero)
+ {
+ this->prevSimTime = _info.simTime;
+ return false;
+ }
+
+ double error = this->liftJoint->GetAngle(0).Radian() -
+ (this->floor * this->floorHeight);
+
+ double force = this->liftPID.Update(error, _info.simTime - this->prevSimTime);
+ this->prevSimTime = _info.simTime;
+
+ this->liftJoint->SetForce(0, force);
+
+ if (std::abs(error) < 0.15)
+ {
+ this->state = ElevatorPluginPrivate::LiftController::STATIONARY;
+ return true;
+ }
+ else
+ {
+ this->state = ElevatorPluginPrivate::LiftController::MOVING;
+ return false;
+ }
+}
+
+/////////////////////////////////////////////////
+void ElevatorPluginPrivate::LiftController::SetFloor(int _floor)
+{
+ this->floor = _floor;
+}
+
+/////////////////////////////////////////////////
+int ElevatorPluginPrivate::LiftController::GetFloor() const
+{
+ return this->floor;
+}
+
+/////////////////////////////////////////////////
+ElevatorPluginPrivate::LiftController::State
+ElevatorPluginPrivate::LiftController::GetState() const
+{
+ return this->state;
+}
+
+
diff --git a/plugins/ElevatorPlugin.hh b/plugins/ElevatorPlugin.hh
new file mode 100644
index 0000000..8c7dd77
--- /dev/null
+++ b/plugins/ElevatorPlugin.hh
@@ -0,0 +1,82 @@
+/*
+ * 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_ELEVATOR_PLUGIN_HH_
+#define _GAZEBO_ELEVATOR_PLUGIN_HH_
+
+#include <string>
+
+#include <sdf/sdf.hh>
+
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/common/UpdateInfo.hh>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/util/system.hh>
+
+namespace gazebo
+{
+ /// Forward declare private data.
+ class ElevatorPluginPrivate;
+
+ /// \brief Plugin to control a elevator. This plugin will listen for
+ /// door and lift events on a specified topic.
+ ///
+ /// \verbatim
+ /// <plugin filename="libElevatorPlugin.so" name="elevator_plugin">
+ /// <lift_joint>elevator::lift</lift_joint>
+ /// <door_joint>elevator::door</door_joint>
+ /// <floor_height>3.075</floor_height>
+ ///
+ /// <!-- Time the elevator door will stay open in seconds -->
+ /// <door_wait_time>5</door_wait_time>
+ ///
+ /// <topic>~/elevator</topic>
+ /// </plugin>
+ /// \endverbatim
+ ///
+ /// See worlds/elevator.world for a complete example.
+ class GAZEBO_VISIBLE ElevatorPlugin : public ModelPlugin
+ {
+ /// \brief Constructor.
+ public: ElevatorPlugin();
+
+ /// \brief Destructor.
+ public: ~ElevatorPlugin();
+
+ // Documentation inherited
+ public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+ // Documentation inherited
+ public: virtual void Reset();
+
+ /// \brief Move to a particular floor.
+ /// \param[in] _floor Number of the floor to move the elevator to.
+ public: void MoveToFloor(const int _floor);
+
+ /// \brief Update the plugin once every iteration of simulation.
+ /// \param[in] _info Update information provided by the server.
+ private: void Update(const common::UpdateInfo &_info);
+
+ /// \brief Receives messages on the elevator's topic.
+ /// \param[in] _msg The string message that contains a command.
+ private: void OnElevator(ConstGzStringPtr &_msg);
+
+ /// \brief Private data pointer
+ private: ElevatorPluginPrivate *dataPtr;
+ };
+}
+#endif
diff --git a/plugins/ElevatorPluginPrivate.hh b/plugins/ElevatorPluginPrivate.hh
new file mode 100644
index 0000000..a954585
--- /dev/null
+++ b/plugins/ElevatorPluginPrivate.hh
@@ -0,0 +1,307 @@
+/*
+ * 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_ELEVATOR_PLUGIN_PRIVATE_HH_
+#define _GAZEBO_ELEVATOR_PLUGIN_PRIVATE_HH_
+
+#include <list>
+#include <mutex>
+#include <string>
+
+#include <sdf/sdf.hh>
+
+#include <gazebo/transport/Node.hh>
+#include <gazebo/transport/Subscriber.hh>
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/common/PID.hh>
+#include <gazebo/common/Timer.hh>
+
+namespace gazebo
+{
+ /// \internal
+ /// \brief Private data for the ElevatorPlugin
+ class ElevatorPluginPrivate
+ {
+ /// \brief Constructor
+ public: ElevatorPluginPrivate() = default;
+
+ /// \brief Destructor
+ public: virtual ~ElevatorPluginPrivate();
+
+ /// \brief Controller for opening and closing the elevator door.
+ public: class DoorController
+ {
+ /// \enum Target
+ /// \brief Door targets.
+ public: enum Target {
+ /// \brief Open the door
+ OPEN,
+
+ /// \brief Close the door
+ CLOSE
+ };
+
+ /// \enum State
+ /// \brief Door motion states
+ public: enum State {
+ /// \brief The door is moving
+ MOVING,
+
+ /// \brief The door is stationary
+ STATIONARY
+ };
+
+ /// \brief Constructor
+ /// \param[in] _doorJoint Pointer to the joint that should be
+ /// controlled.
+ public: DoorController(physics::JointPtr _doorJoint);
+
+ /// \brief Destructor
+ public: virtual ~DoorController() = default;
+
+ /// \brief Set the target for the door (OPEN or CLOSE).
+ /// \param[in] _target The target for the door.
+ public: void SetTarget(
+ ElevatorPluginPrivate::DoorController::Target _target);
+
+ /// \brief Get the current state.
+ /// \return Current state.
+ public: ElevatorPluginPrivate::DoorController::State GetState() const;
+
+ /// \brief Get the current target.
+ /// \return Current target.
+ public: ElevatorPluginPrivate::DoorController::Target GetTarget() const;
+
+ /// \brief Reset the controller
+ public: void Reset();
+
+ /// \brief Update the controller.
+ /// \param[in] _info Update information provided by the server.
+ /// \return True if the target has been reached.
+ public: virtual bool Update(const common::UpdateInfo &_info);
+
+ /// \brief Pointer to the door joint.
+ public: physics::JointPtr doorJoint;
+
+ /// \brief Current door state
+ public: State state;
+
+ /// \brief Current door target
+ public: Target target;
+
+ /// \brief PID controller for the door.
+ public: common::PID doorPID;
+
+ /// \brief Previous simulation time.
+ public: common::Time prevSimTime;
+ };
+
+ /// \brief Controller for raising and lowering the elevator.
+ public: class LiftController
+ {
+ /// \enum State
+ /// \brief Lift state
+ public: enum State {
+ /// \brief The lift is moving
+ MOVING,
+
+ /// \brief The lift is stationary
+ STATIONARY
+ };
+
+ /// \brief Constructor
+ /// \param[in] _liftJoint Pointer to the joint that should be
+ /// controlled.
+ /// \param[in] _floorHeight Height of each floor.
+ public: LiftController(physics::JointPtr _liftJoint,
+ float _floorHeight);
+
+ /// \brief Destructor
+ public: virtual ~LiftController() = default;
+
+ /// \brief Set the current floor to move to.
+ /// \param[in] _floor Floor number.
+ public: void SetFloor(int _floor);
+
+ /// \brief Get the current floor.
+ /// \return Floor number
+ public: int GetFloor() const;
+
+ /// \brief Get the current state.
+ /// \return Current lift state.
+ public: ElevatorPluginPrivate::LiftController::State GetState() const;
+
+ /// \brief Reset the controller
+ public: void Reset();
+
+ /// \brief Update the controller.
+ /// \param[in] _info Update information provided by the server.
+ /// \return True if the target has been reached.
+ public: virtual bool Update(const common::UpdateInfo &_info);
+
+ /// \brief State of the controller.
+ public: State state;
+
+ /// \brief Floor the elevator is on or moving to.
+ public: int floor;
+
+ /// \brief Height of each floor.
+ public: float floorHeight;
+
+ /// \brief Joint to control
+ public: physics::JointPtr liftJoint;
+
+ /// \brief PID controller.
+ public: common::PID liftPID;
+
+ /// \brief Previous simulation time.
+ public: common::Time prevSimTime;
+ };
+
+ /// \brief State base class
+ public: class State
+ {
+ /// \brief Constructor
+ public: State() : started(false) {}
+
+ /// \brief Destructor
+ public: virtual ~State() = default;
+
+ /// \brief State name
+ public: std::string name;
+
+ /// \brief Used to start a state.
+ public: virtual void Start() {}
+
+ /// \brief Used to update a state.
+ public: virtual bool Update() {return true;}
+
+ /// \brief True when started.
+ protected: bool started;
+ };
+
+ /// \brief State used to close the elevator door.
+ public: class CloseState : public State
+ {
+ /// \brief Constructor.
+ /// \param[in] _ctrl Elevator door controller
+ public: CloseState(ElevatorPluginPrivate::DoorController *_ctrl);
+
+ // Documentation inherited
+ public: virtual void Start();
+
+ // Documentation inherited
+ public: virtual bool Update();
+
+ /// \brief Pointer to the door controller.
+ public: ElevatorPluginPrivate::DoorController *ctrl;
+ };
+
+ /// \brief State used to open the elevator door.
+ public: class OpenState : public State
+ {
+ /// \brief Constructor.
+ /// \param[in] _ctrl Elevator door controller
+ public: OpenState(ElevatorPluginPrivate::DoorController *_ctrl);
+
+ // Documentation inherited
+ public: virtual void Start();
+
+ // Documentation inherited
+ public: virtual bool Update();
+
+ /// \brief Pointer to the door controller.
+ public: ElevatorPluginPrivate::DoorController *ctrl;
+ };
+
+ /// \brief State used to move the elevator to a floor.
+ public: class MoveState : public State
+ {
+ /// \brief Constructor
+ /// \param[in] _floor Floor index to move to.
+ /// \param[in] _ctrl Lift controller pointer.
+ public: MoveState(int _floor, LiftController *_ctrl);
+
+ // Documentation inherited
+ public: virtual void Start();
+
+ // Documentation inherited
+ public: virtual bool Update();
+
+ /// \brief Target floor number.
+ public: int floor;
+
+ /// \brief Lift controller.
+ public: LiftController *ctrl;
+ };
+
+ /// \brief State used to make the elevator wait.
+ public: class WaitState : public State
+ {
+ /// \brief Constructor
+ /// \param[in] _waitTime Length of the wait state
+ public: WaitState(const common::Time &_waitTime);
+
+ // Documentation inherited
+ public: virtual void Start();
+
+ // Documentation inherited
+ public: virtual bool Update();
+
+ /// \brief Timer to hold the door open.
+ public: common::Timer waitTimer;
+ };
+
+ /// \brief Pointer to the elevator model.
+ public: physics::ModelPtr model;
+
+ /// \brief Pointer to the joint that lifts the elevator
+ public: physics::JointPtr liftJoint;
+
+ /// \brief Pointer to the joint that opens the door
+ public: physics::JointPtr doorJoint;
+
+ /// \brief SDF pointer.
+ public: sdf::ElementPtr sdf;
+
+ /// \brief Pointer to the update event connection
+ public: event::ConnectionPtr updateConnection;
+
+ /// \brief Node for communication
+ public: transport::NodePtr node;
+
+ /// \brief Used to subscribe to command message. This will call the
+ /// OnElevator function when a message arrives.
+ public: transport::SubscriberPtr elevatorSub;
+
+ /// \brief Door controller.
+ public: DoorController *doorController;
+
+ /// \brief Lift controller.
+ public: LiftController *liftController;
+
+ /// \brief List of states that should be executed.
+ public: std::list<State *> states;
+
+ /// \brief Mutex to protect states.
+ public: std::mutex stateMutex;
+
+ /// \brief Time to hold the door in the open state.
+ public: common::Time doorWaitTime;
+ };
+}
+#endif
diff --git a/plugins/ForceTorquePlugin.cc b/plugins/ForceTorquePlugin.cc
index 2915759..8f9b858 100644
--- a/plugins/ForceTorquePlugin.cc
+++ b/plugins/ForceTorquePlugin.cc
@@ -14,6 +14,11 @@
* 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 "ForceTorquePlugin.hh"
diff --git a/plugins/GpuRayPlugin.cc b/plugins/GpuRayPlugin.cc
index 09336f8..4f2c9c3 100644
--- a/plugins/GpuRayPlugin.cc
+++ b/plugins/GpuRayPlugin.cc
@@ -21,7 +21,8 @@ using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(GpuRayPlugin)
/////////////////////////////////////////////////
-GpuRayPlugin::GpuRayPlugin() : SensorPlugin()
+GpuRayPlugin::GpuRayPlugin()
+: SensorPlugin(), width(0), height(0)
{
}
diff --git a/plugins/HydraDemoPlugin.hh b/plugins/HydraDemoPlugin.hh
index fcf0164..5fa8ae3 100644
--- a/plugins/HydraDemoPlugin.hh
+++ b/plugins/HydraDemoPlugin.hh
@@ -27,7 +27,7 @@
namespace gazebo
{
- class HydraDemoPlugin : public ModelPlugin
+ class GAZEBO_VISIBLE HydraDemoPlugin : public ModelPlugin
{
/// \brief Constructor
public: HydraDemoPlugin();
diff --git a/plugins/HydraPlugin.cc b/plugins/HydraPlugin.cc
index b744a73..de4102e 100644
--- a/plugins/HydraPlugin.cc
+++ b/plugins/HydraPlugin.cc
@@ -62,10 +62,28 @@ GZ_REGISTER_WORLD_PLUGIN(RazerHydra)
/////////////////////////////////////////////////
RazerHydra::RazerHydra()
-: hidrawFd(0)
+: hidrawFd(0), lastCycleStart(common::Time::GetWallTime())
{
this->stop = false;
- this->lastCycleStart = common::Time::GetWallTime();
+ this->pollThread = NULL;
+
+ for (auto &v: this->analog)
+ v = 0;
+
+ for (auto &v: this->rawAnalog)
+ v = 0;
+
+ for (auto &v: this->rawButtons)
+ v = 0;
+
+ for (auto &v: this->rawQuat)
+ v = 0;
+
+ for (auto &v: this->rawPos)
+ v = 0;
+
+ for (auto &v: this->buttons)
+ v = 0;
// magic number for 50% mix at each step
this->periodEstimate.SetFc(0.11, 1.0);
@@ -79,7 +97,8 @@ RazerHydra::~RazerHydra()
event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
this->stop = true;
- this->pollThread->join();
+ if (this->pollThread)
+ this->pollThread->join();
}
/////////////////////////////////////////////////
@@ -224,8 +243,8 @@ void RazerHydra::Update(const common::UpdateInfo & /*_info*/)
rightPaddle->set_button_center(this->buttons[12]);
rightPaddle->set_button_joy(this->buttons[13]);
- msgs::Set(rightPaddle->mutable_pose(), grabRight);
- msgs::Set(leftPaddle->mutable_pose(), grabLeft);
+ msgs::Set(rightPaddle->mutable_pose(), grabRight.Ign());
+ msgs::Set(leftPaddle->mutable_pose(), grabLeft.Ign());
this->pub->Publish(msg);
}
diff --git a/plugins/HydraPlugin.hh b/plugins/HydraPlugin.hh
index 1ec8a0f..213d565 100644
--- a/plugins/HydraPlugin.hh
+++ b/plugins/HydraPlugin.hh
@@ -24,7 +24,7 @@
namespace gazebo
{
- class RazerHydra : public WorldPlugin
+ class GAZEBO_VISIBLE RazerHydra : public WorldPlugin
{
/// \brief Constructor.
public: RazerHydra();
diff --git a/plugins/ImuSensorPlugin.hh b/plugins/ImuSensorPlugin.hh
index 25d16c4..d108aa7 100644
--- a/plugins/ImuSensorPlugin.hh
+++ b/plugins/ImuSensorPlugin.hh
@@ -18,14 +18,14 @@
#ifndef _GAZEBO_IMU_SENSOR_PLUGIN_HH_
#define _GAZEBO_IMU_SENSOR_PLUGIN_HH_
-#include "gazebo/sensors/sensors.hh"
#include "gazebo/common/Plugin.hh"
+#include "gazebo/sensors/sensors.hh"
#include "gazebo/common/Events.hh"
namespace gazebo
{
/// \brief An base class plugin for custom imu sensor processing.
- class ImuSensorPlugin : public SensorPlugin
+ class GAZEBO_VISIBLE ImuSensorPlugin : public SensorPlugin
{
/// \brief Constructor
public: ImuSensorPlugin();
diff --git a/plugins/LiftDragPlugin.cc b/plugins/LiftDragPlugin.cc
index af8dac8..3b43fed 100644
--- a/plugins/LiftDragPlugin.cc
+++ b/plugins/LiftDragPlugin.cc
@@ -36,23 +36,36 @@ LiftDragPlugin::LiftDragPlugin() : cla(1.0), cda(0.01), cma(0.01), rho(1.2041)
this->upward = math::Vector3(0, 0, 1);
this->area = 1.0;
this->alpha0 = 0.0;
+ this->alpha = 0.0;
+ this->sweep = 0.0;
+ this->velocityStall = 0.0;
// 90 deg stall
this->alphaStall = 0.5*M_PI;
this->claStall = 0.0;
+ this->radialSymmetry = false;
+
/// \TODO: what's flat plate drag?
this->cdaStall = 1.0;
this->cmaStall = 0.0;
+
+ /// how much to change CL per every radian of the control joint value
+ this->controlJointRadToCL = 4.0;
}
/////////////////////////////////////////////////
-void LiftDragPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
+LiftDragPlugin::~LiftDragPlugin()
+{
+}
+
+/////////////////////////////////////////////////
+void LiftDragPlugin::Load(physics::ModelPtr _model,
+ sdf::ElementPtr _sdf)
{
GZ_ASSERT(_model, "LiftDragPlugin _model pointer is NULL");
GZ_ASSERT(_sdf, "LiftDragPlugin _sdf pointer is NULL");
this->model = _model;
- this->modelName = _model->GetName();
this->sdf = _sdf;
this->world = this->model->GetWorld();
@@ -63,6 +76,9 @@ void LiftDragPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
GZ_ASSERT(_sdf, "LiftDragPlugin _sdf pointer is NULL");
+ if (_sdf->HasElement("radial_symmetry"))
+ this->radialSymmetry = _sdf->Get<bool>("radial_symmetry");
+
if (_sdf->HasElement("a0"))
this->alpha0 = _sdf->Get<double>("a0");
@@ -107,30 +123,50 @@ void LiftDragPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
if (_sdf->HasElement("link_name"))
{
sdf::ElementPtr elem = _sdf->GetElement("link_name");
- this->linkName = elem->Get<std::string>();
- this->link = this->model->GetLink(this->linkName);
+ GZ_ASSERT(elem, "Element link_name doesn't exist!");
+ std::string linkName = elem->Get<std::string>();
+ this->link = this->model->GetLink(linkName);
+ GZ_ASSERT(this->link, "Link was NULL");
+
+ if (!this->link)
+ {
+ gzerr << "Link with name[" << linkName << "] not found. "
+ << "The LiftDragPlugin will not generate forces\n";
+ }
+ else
+ {
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&LiftDragPlugin::OnUpdate, this));
+ }
}
-}
-/////////////////////////////////////////////////
-void LiftDragPlugin::Init()
-{
- this->updateConnection = event::Events::ConnectWorldUpdateBegin(
- boost::bind(&LiftDragPlugin::OnUpdate, this));
+ if (_sdf->HasElement("control_joint_name"))
+ {
+ std::string controlJointName = _sdf->Get<std::string>("control_joint_name");
+ this->controlJoint = this->model->GetJoint(controlJointName);
+ if (!this->controlJoint)
+ {
+ gzerr << "Joint with name[" << controlJointName << "] does not exist.\n";
+ }
+ }
+
+ if (_sdf->HasElement("control_joint_rad_to_cl"))
+ this->controlJointRadToCL = _sdf->Get<double>("control_joint_rad_to_cl");
}
/////////////////////////////////////////////////
void LiftDragPlugin::OnUpdate()
{
+ GZ_ASSERT(this->link, "Link was NULL");
// get linear velocity at cp in inertial frame
- math::Vector3 vel = this->link->GetWorldLinearVel(this->cp);
+ math::Vector3 velI = this->link->GetWorldLinearVel(this->cp);
// smoothing
// double e = 0.8;
- // this->velSmooth = e*vel + (1.0 - e)*velSmooth;
- // vel = this->velSmooth;
+ // this->velSmooth = e*velI + (1.0 - e)*velSmooth;
+ // velI = this->velSmooth;
- if (vel.GetLength() <= 0.01)
+ if (velI.GetLength() <= 0.01)
return;
// pose of body
@@ -138,16 +174,31 @@ void LiftDragPlugin::OnUpdate()
// rotate forward and upward vectors into inertial frame
math::Vector3 forwardI = pose.rot.RotateVector(this->forward);
- math::Vector3 upwardI = pose.rot.RotateVector(this->upward);
- // ldNormal vector to lift-drag-plane described in inertial frame
- math::Vector3 ldNormal = forwardI.Cross(upwardI).Normalize();
+ math::Vector3 upwardI;
+ if (this->radialSymmetry)
+ {
+ // use inflow velocity to determine upward direction
+ // which is the component of inflow perpendicular to forward direction.
+ math::Vector3 tmp = forwardI.Cross(velI);
+ upwardI = forwardI.Cross(tmp).Normalize();
+ }
+ else
+ {
+ upwardI = pose.rot.RotateVector(this->upward);
+ }
- // check sweep (angle between vel and lift-drag-plane)
- double sinSweepAngle = ldNormal.Dot(vel) / vel.GetLength();
+ // spanwiseI: a vector normal to lift-drag-plane described in inertial frame
+ math::Vector3 spanwiseI = forwardI.Cross(upwardI).Normalize();
+
+ const double minRatio = -1.0;
+ const double maxRatio = 1.0;
+ // check sweep (angle between velI and lift-drag-plane)
+ double sinSweepAngle = math::clamp(
+ spanwiseI.Dot(velI) / velI.GetLength(), minRatio, maxRatio);
// get cos from trig identity
- double cosSweepAngle2 = (1.0 - sinSweepAngle * sinSweepAngle);
+ double cosSweepAngle = 1.0 - sinSweepAngle * sinSweepAngle;
this->sweep = asin(sinSweepAngle);
// truncate sweep to within +/-90 deg
@@ -156,40 +207,40 @@ void LiftDragPlugin::OnUpdate()
: this->sweep + M_PI;
// angle of attack is the angle between
- // vel projected into lift-drag plane
+ // velI projected into lift-drag plane
// and
// forward vector
//
- // projected = ldNormal Xcross ( vector Xcross ldNormal)
+ // projected = spanwiseI Xcross ( vector Xcross spanwiseI)
//
// so,
// velocity in lift-drag plane (expressed in inertial frame) is:
- math::Vector3 velInLDPlane = ldNormal.Cross(vel.Cross(ldNormal));
+ math::Vector3 velInLDPlane = spanwiseI.Cross(velI.Cross(spanwiseI));
// get direction of drag
math::Vector3 dragDirection = -velInLDPlane;
dragDirection.Normalize();
// get direction of lift
- math::Vector3 liftDirection = ldNormal.Cross(velInLDPlane);
+ math::Vector3 liftDirection = spanwiseI.Cross(velInLDPlane);
liftDirection.Normalize();
// get direction of moment
- math::Vector3 momentDirection = ldNormal;
+ math::Vector3 momentDirection = spanwiseI;
+ double forwardVelocity = forwardI.GetLength() * velInLDPlane.GetLength();
double cosAlpha = math::clamp(
- forwardI.Dot(velInLDPlane) /
- (forwardI.GetLength() * velInLDPlane.GetLength()), -1.0, 1.0);
+ forwardI.Dot(velInLDPlane) / forwardVelocity, minRatio, maxRatio);
+
// gzerr << "ca " << forwardI.Dot(velInLDPlane) /
// (forwardI.GetLength() * velInLDPlane.GetLength()) << "\n";
// get sign of alpha
// take upwards component of velocity in lift-drag plane.
// if sign == upward, then alpha is negative
- double alphaSign = -upwardI.Dot(velInLDPlane)/
- (upwardI.GetLength() + velInLDPlane.GetLength());
+ double upwardVelocity = upwardI.GetLength() + velInLDPlane.GetLength();
+ double alphaSign = -upwardI.Dot(velInLDPlane)/upwardVelocity;
- // double sinAlpha = sqrt(1.0 - cosAlpha * cosAlpha);
if (alphaSign > 0.0)
this->alpha = this->alpha0 + acos(cosAlpha);
else
@@ -210,7 +261,7 @@ void LiftDragPlugin::OnUpdate()
{
cl = (this->cla * this->alphaStall +
this->claStall * (this->alpha - this->alphaStall))
- * cosSweepAngle2;
+ * cosSweepAngle;
// make sure cl is still great than 0
cl = std::max(0.0, cl);
}
@@ -218,12 +269,20 @@ void LiftDragPlugin::OnUpdate()
{
cl = (-this->cla * this->alphaStall +
this->claStall * (this->alpha + this->alphaStall))
- * cosSweepAngle2;
+ * cosSweepAngle;
// make sure cl is still less than 0
cl = std::min(0.0, cl);
}
else
- cl = this->cla * this->alpha * cosSweepAngle2;
+ cl = this->cla * this->alpha * cosSweepAngle;
+
+ // modify cl per control joint value
+ if (this->controlJoint)
+ {
+ double controlAngle = this->controlJoint->GetAngle(0).Radian();
+ cl = cl + this->controlJointRadToCL * controlAngle;
+ /// \TODO: also change cm and cd
+ }
// compute lift force at cp
math::Vector3 lift = cl * q * this->area * liftDirection;
@@ -234,16 +293,16 @@ void LiftDragPlugin::OnUpdate()
{
cd = (this->cda * this->alphaStall +
this->cdaStall * (this->alpha - this->alphaStall))
- * cosSweepAngle2;
+ * cosSweepAngle;
}
else if (this->alpha < -this->alphaStall)
{
cd = (-this->cda * this->alphaStall +
this->cdaStall * (this->alpha + this->alphaStall))
- * cosSweepAngle2;
+ * cosSweepAngle;
}
else
- cd = (this->cda * this->alpha) * cosSweepAngle2;
+ cd = (this->cda * this->alpha) * cosSweepAngle;
// make sure drag is positive
cd = fabs(cd);
@@ -257,7 +316,7 @@ void LiftDragPlugin::OnUpdate()
{
cm = (this->cma * this->alphaStall +
this->cmaStall * (this->alpha - this->alphaStall))
- * cosSweepAngle2;
+ * cosSweepAngle;
// make sure cm is still great than 0
cm = std::max(0.0, cm);
}
@@ -265,14 +324,15 @@ void LiftDragPlugin::OnUpdate()
{
cm = (-this->cma * this->alphaStall +
this->cmaStall * (this->alpha + this->alphaStall))
- * cosSweepAngle2;
+ * cosSweepAngle;
// make sure cm is still less than 0
cm = std::min(0.0, cm);
}
else
- cm = this->cma * this->alpha * cosSweepAngle2;
+ cm = this->cma * this->alpha * cosSweepAngle;
- // reset cm to zero, as cm needs testing
+ /// \TODO: implement cm
+ /// for now, reset cm to zero, as cm needs testing
cm = 0.0;
// compute moment (torque) at cp
@@ -294,21 +354,22 @@ void LiftDragPlugin::OnUpdate()
//
// if ((this->link->GetName() == "wing_1" ||
// this->link->GetName() == "wing_2") &&
- // (vel.GetLength() > 50.0 &&
- // vel.GetLength() < 50.0))
+ // (velI.GetLength() > 50.0 &&
+ // velI.GetLength() < 50.0))
if (0)
{
gzerr << "=============================\n";
gzerr << "Link: [" << this->link->GetName()
<< "] pose: [" << pose
<< "] dynamic pressure: [" << q << "]\n";
- gzerr << "spd: [" << vel.GetLength() << "] vel: [" << vel << "]\n";
+ gzerr << "spd: [" << velI.GetLength()
+ << "] velI: [" << velI << "]\n";
gzerr << "spd sweep: [" << velInLDPlane.GetLength()
- << "] vel in LD: [" << velInLDPlane << "]\n";
+ << "] velI in LD plane: [" << velInLDPlane << "]\n";
gzerr << "forward (inertial): " << forwardI << "\n";
gzerr << "upward (inertial): " << upwardI << "\n";
gzerr << "lift dir (inertial): " << liftDirection << "\n";
- gzerr << "LD Normal: " << ldNormal << "\n";
+ gzerr << "Span direction (normal to LD plane): " << spanwiseI << "\n";
gzerr << "sweep: " << this->sweep << "\n";
gzerr << "alpha: " << this->alpha << "\n";
gzerr << "lift: " << lift << "\n";
@@ -320,6 +381,11 @@ void LiftDragPlugin::OnUpdate()
gzerr << "torque: " << torque << "\n";
}
+ // Correct for nan or inf
+ force.Correct();
+ this->cp.Correct();
+ torque.Correct();
+
// apply forces at cg (with torques for position shift)
this->link->AddForceAtRelativePosition(force, this->cp);
this->link->AddTorque(torque);
diff --git a/plugins/LiftDragPlugin.hh b/plugins/LiftDragPlugin.hh
index c3f0c4c..8d9ade5 100644
--- a/plugins/LiftDragPlugin.hh
+++ b/plugins/LiftDragPlugin.hh
@@ -27,16 +27,16 @@
namespace gazebo
{
/// \brief A plugin that simulates lift and drag.
- class LiftDragPlugin : public ModelPlugin
+ class GAZEBO_VISIBLE LiftDragPlugin : public ModelPlugin
{
/// \brief Constructor.
public: LiftDragPlugin();
- // Documentation Inherited.
- public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+ /// \brief Destructor.
+ public: ~LiftDragPlugin();
// Documentation Inherited.
- public: virtual void Init();
+ public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
/// \brief Callback for World Update events.
protected: virtual void OnUpdate();
@@ -53,9 +53,6 @@ namespace gazebo
/// \brief Pointer to model containing plugin.
protected: physics::ModelPtr model;
- /// \brief Name of model containing plugin.
- protected: std::string modelName;
-
/// \brief Coefficient of Lift / alpha slope.
/// Lift = C_L * q * S
/// where q (dynamic pressure) = 0.5 * rho * v^2
@@ -91,6 +88,12 @@ namespace gazebo
/// At 20 °C and 101.325 kPa, dry air has a density of 1.2041 kg/m3.
protected: double rho;
+ /// \brief if the shape is aerodynamically radially symmetric about
+ /// the forward direction. Defaults to false for wing shapes.
+ /// If set to true, the upward direction is determined by the
+ /// angle of attack.
+ protected: bool radialSymmetry;
+
/// \brief effective planeform surface area
protected: double area;
@@ -106,22 +109,29 @@ namespace gazebo
/// \brief center of pressure in link local coordinates
protected: math::Vector3 cp;
- /// \brief forward flight direction in link local coordinates
+ /// \brief Normally, this is taken as a direction parallel to the chord
+ /// of the airfoil in zero angle of attack forward flight.
protected: math::Vector3 forward;
- /// \brief A vector in the lift/drag plane, anything orthogonal to it
- /// is considered wing sweep.
+ /// \brief A vector in the lift/drag plane, perpendicular to the forward
+ /// vector. Inflow velocity orthogonal to forward and upward vectors
+ /// is considered flow in the wing sweep direction.
protected: math::Vector3 upward;
- /// \brief Smooth velocity
+ /// \brief Smoothed velocity
protected: math::Vector3 velSmooth;
- /// \brief Names of allowed target links, specified in sdf parameters.
- protected: std::string linkName;
-
/// \brief Pointer to link currently targeted by mud joint.
protected: physics::LinkPtr link;
+ /// \brief Pointer to a joint that actuates a control surface for
+ /// this lifting body
+ protected: physics::JointPtr controlJoint;
+
+ /// \brief how much to change CL per radian of control surface joint
+ /// value.
+ protected: double controlJointRadToCL;
+
/// \brief SDF for this plugin;
protected: sdf::ElementPtr sdf;
};
diff --git a/plugins/ModelPropShop.cc b/plugins/ModelPropShop.cc
index cdb126c..5d7f8d8 100644
--- a/plugins/ModelPropShop.cc
+++ b/plugins/ModelPropShop.cc
@@ -14,6 +14,12 @@
* 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/program_options.hpp>
#include "gazebo/msgs/msgs.hh"
@@ -98,7 +104,7 @@ void ModelPropShop::Load(int _argc, char **_argv)
return;
}
- sdf::ElementPtr modelElem = this->sdf->root->GetElement("model");
+ sdf::ElementPtr modelElem = this->sdf->Root()->GetElement("model");
this->modelName = modelElem->Get<std::string>("name");
}
@@ -164,7 +170,7 @@ void ModelPropShop::Update()
gazebo::msgs::Set(lightMsg.mutable_specular(),
gazebo::common::Color(.2, .2, .2, 1));
gazebo::msgs::Set(lightMsg.mutable_direction(),
- gazebo::math::Vector3(-0.5, 0.1, -0.9));
+ ignition::math::Vector3d(-0.5, 0.1, -0.9));
lightMsg.set_cast_shadows(false);
lightMsg.set_range(1000);
lightMsg.set_attenuation_constant(1);
diff --git a/plugins/MudPlugin.cc b/plugins/MudPlugin.cc
index f535882..7a5fa4c 100644
--- a/plugins/MudPlugin.cc
+++ b/plugins/MudPlugin.cc
@@ -238,7 +238,8 @@ void MudPlugin::OnUpdate()
for (unsigned int j = 0; j < pc; ++j)
{
contactPositionAverage +=
- msgs::Convert(this->newestContactsMsg.contact(i).position(j));
+ msgs::ConvertIgn(
+ this->newestContactsMsg.contact(i).position(j));
}
// Then divide by numer of contact points
contactPositionAverage /= static_cast<double>(pc);
diff --git a/plugins/RaySensorNoisePlugin.cc b/plugins/RaySensorNoisePlugin.cc
index 8b39fee..342321f 100644
--- a/plugins/RaySensorNoisePlugin.cc
+++ b/plugins/RaySensorNoisePlugin.cc
@@ -45,7 +45,7 @@ void RaySensorNoisePlugin::Load(sensors::SensorPtr _parent,
return;
}
- sensors::NoisePtr noise = _parent->GetNoise();
+ sensors::NoisePtr noise = _parent->GetNoise(sensors::RAY_NOISE);
if (noise)
{
diff --git a/plugins/RubblePlugin.cc b/plugins/RubblePlugin.cc
index 2389b94..f7ab5d9 100644
--- a/plugins/RubblePlugin.cc
+++ b/plugins/RubblePlugin.cc
@@ -270,7 +270,7 @@ void RubblePlugin::MakeBox(const std::string &_name, math::Pose &_pose,
}
/////////////////////////////////////////////////
-void RubblePlugin::MakeCylinder(const std::string &_name, math::Vector3 &_pos,
+/*void RubblePlugin::MakeCylinder(const std::string &_name, math::Vector3 &_pos,
math::Vector3 &_size, double _mass)
{
std::ostringstream newModelStr;
@@ -313,10 +313,10 @@ void RubblePlugin::MakeCylinder(const std::string &_name, math::Vector3 &_pos,
"</sdf>";
this->world->InsertModelString(newModelStr.str());
-}
+}*/
/////////////////////////////////////////////////
-void RubblePlugin::MakeCompound(const std::string &_name, CompoundObj &_obj)
+/*void RubblePlugin::MakeCompound(const std::string &_name, CompoundObj &_obj)
{
std::ostringstream newModelStr, geomStr, inertiaStr;
@@ -393,4 +393,4 @@ void RubblePlugin::MakeCompound(const std::string &_name, CompoundObj &_obj)
<< "</sdf>";
this->world->InsertModelString(newModelStr.str());
-}
+}*/
diff --git a/plugins/RubblePlugin.hh b/plugins/RubblePlugin.hh
index 03764d6..781e972 100644
--- a/plugins/RubblePlugin.hh
+++ b/plugins/RubblePlugin.hh
@@ -45,8 +45,8 @@ namespace gazebo
private: void MakeCinderBlock(const std::string &_name, math::Pose &_pose,
math::Vector3 &_size, double _mass);
- private: void MakeCylinder(const std::string &_name, math::Vector3 &_pos,
- math::Vector3 &_size, double _mass);
+ // private: void MakeCylinder(const std::string &_name, math::Vector3 &_pos,
+ // math::Vector3 &_size, double _mass);
private: class Obj
{
@@ -65,7 +65,7 @@ namespace gazebo
public: std::vector<Obj> objects;
};
- private: void MakeCompound(const std::string &_name, CompoundObj &_obj);
+ // private: void MakeCompound(const std::string &_name, CompoundObj &_obj);
private: physics::WorldPtr world;
};
}
diff --git a/plugins/SkidSteerDrivePlugin.cc b/plugins/SkidSteerDrivePlugin.cc
index 9b27177..8e249ea 100644
--- a/plugins/SkidSteerDrivePlugin.cc
+++ b/plugins/SkidSteerDrivePlugin.cc
@@ -37,7 +37,7 @@ SkidSteerDrivePlugin::SkidSteerDrivePlugin()
int SkidSteerDrivePlugin::RegisterJoint(int _index, const std::string &_name)
{
// Bounds checking on index
- if (_index < 0 or _index >= NUMBER_OF_WHEELS)
+ if (_index < 0 || _index >= NUMBER_OF_WHEELS)
{
gzerr << "Joint index " << _index << " out of bounds [0, "
<< NUMBER_OF_WHEELS << "] in model " << this->model->GetName()
@@ -130,7 +130,7 @@ void SkidSteerDrivePlugin::OnVelMsg(ConstPosePtr &_msg)
// << msgs::Convert(msg->orientation()).GetAsEuler().z << std::endl;
double vel_lin = _msg->position().x() / this->wheelRadius;
- double vel_rot = -1 * msgs::Convert(_msg->orientation()).GetAsEuler().z
+ double vel_rot = -1 * msgs::ConvertIgn(_msg->orientation()).Euler().Z()
* (this->wheelSeparation / this->wheelRadius);
this->joints[RIGHT_FRONT]->SetVelocity(0, vel_lin - vel_rot);
diff --git a/plugins/TimerGUIPlugin.cc b/plugins/TimerGUIPlugin.cc
index 9fdecae..07f0d02 100644
--- a/plugins/TimerGUIPlugin.cc
+++ b/plugins/TimerGUIPlugin.cc
@@ -14,6 +14,12 @@
* 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 <sstream>
#include <gazebo/msgs/msgs.hh>
#include "TimerGUIPlugin.hh"
@@ -42,16 +48,91 @@ TimerGUIPlugin::TimerGUIPlugin()
QFrame *mainFrame = new QFrame();
// Create the layout that sits inside the frame
- QHBoxLayout *frameLayout = new QHBoxLayout();
+ QVBoxLayout *frameLayout = new QVBoxLayout();
// Create a time label
QLabel *timeLabel = new QLabel(tr("00:00:00.00"));
// Add the label to the frame's layout
frameLayout->addWidget(timeLabel);
+ frameLayout->setAlignment(timeLabel, Qt::AlignCenter);
connect(this, SIGNAL(SetTime(QString)),
timeLabel, SLOT(setText(QString)), Qt::QueuedConnection);
+ // Create a start/stop button
+ this->startStopButton = new QPushButton();
+ this->startStopButton->installEventFilter(this);
+ this->startStopButton->setFocusPolicy(Qt::NoFocus);
+ this->startStopButton->setText(QString("Start"));
+
+ this->startStyle =
+ "QPushButton {\
+ background: qradialgradient(cx: 0.3, cy: -0.4, fx: 0.3, fy: -0.4, \
+ radius: 1.35, stop: 0 #ddd, stop: 1 #59b353);\
+ border: 2px solid #8bca88;\
+ border-radius: 4px;\
+ font: bold 20px;\
+ color: #eee;\
+ margin-right: 10px;\
+ margin-left: 10px;\
+ }\
+ QPushButton:hover {\
+ background: qradialgradient(cx: 0.3, cy: -0.4, fx: 0.3, fy: -0.4, \
+ radius: 1.35, stop: 0 #ddd, stop: 1 #70c464);\
+ }";
+
+ this->stopStyle =
+ "QPushButton {\
+ background: qradialgradient(cx: 0.3, cy: -0.4, fx: 0.3, fy: -0.4, \
+ radius: 1.35, stop: 0 #ddd, stop: 1 #D85C48);\
+ border: 2px solid #e18071;\
+ border-radius: 4px;\
+ font: bold 20px;\
+ color: #eee;\
+ margin-right: 10px;\
+ margin-left: 10px;\
+ }\
+ QPushButton:hover {\
+ background: qradialgradient(cx: 0.3, cy: -0.4, fx: 0.3, fy: -0.4, \
+ radius: 1.35, stop: 0 #ddd, stop: 1 #bf5140);\
+ }";
+
+ this->startStopButton->setStyleSheet(this->startStyle.c_str());
+ this->startStopButton->hide();
+
+ // Add the button to the frame's layout
+ frameLayout->addWidget(this->startStopButton);
+ connect(this->startStopButton, SIGNAL(clicked()), this,
+ SLOT(OnStartStopButton()));
+ connect(this, SIGNAL(SetStartStopButton(QString)),
+ this, SLOT(OnSetStartStopButton(QString)), Qt::QueuedConnection);
+
+ // Create a reset button
+ this->resetButton = new QPushButton();
+ this->resetButton->installEventFilter(this);
+ this->resetButton->setFocusPolicy(Qt::NoFocus);
+ this->resetButton->setText(QString("Reset"));
+ this->resetButton->setStyleSheet(
+ "QPushButton {\
+ background: qradialgradient(cx: 0.3, cy: -0.4, fx: 0.3, fy: -0.4, \
+ radius: 1.35, stop: 0 #ddd, stop: 1 #666);\
+ border: 2px solid #ccc;\
+ border-radius: 4px;\
+ font: bold 20px;\
+ color: #eee;\
+ margin-right: 10px;\
+ margin-left: 10px;\
+ }\
+ QPushButton:hover {\
+ background: qradialgradient(cx: 0.3, cy: -0.4, fx: 0.3, fy: -0.4, \
+ radius: 1.35, stop: 0 #ddd, stop: 1 #777);\
+ }");
+ this->resetButton->hide();
+
+ // Add the button to the frame's layout
+ frameLayout->addWidget(this->resetButton);
+ connect(this->resetButton, SIGNAL(clicked()), this, SLOT(OnResetButton()));
+
// Add frameLayout to the frame
mainFrame->setLayout(frameLayout);
@@ -67,6 +148,10 @@ TimerGUIPlugin::TimerGUIPlugin()
// Connect to the PreRender Gazebo signal
this->connections.push_back(event::Events::ConnectPreRender(
boost::bind(&TimerGUIPlugin::PreRender, this)));
+
+ // Initialize variables
+ this->posX = 0;
+ this->posY = 0;
}
/////////////////////////////////////////////////
@@ -77,50 +162,114 @@ TimerGUIPlugin::~TimerGUIPlugin()
/////////////////////////////////////////////////
void TimerGUIPlugin::Load(sdf::ElementPtr _elem)
{
+ bool hasStartButton = false;
+ bool hasResetButton = false;
+
+ // If a countdown time was given in SDF, read the countdown time and
+ // initialize the Timer object as a countdown timer.
+ // Time is read in SDF as (seconds nanosecnds)
+ if (_elem->HasElement("countdown_time"))
+ {
+ common::Time maxTime =
+ _elem->GetElement("countdown_time")->Get<common::Time>();
+ this->timer = common::Timer(maxTime, true);
+ }
+
+ // Check if there is a start button
+ if (_elem->HasElement("start_stop_button"))
+ {
+ hasStartButton = _elem->Get<bool>("start_stop_button");
+ if (hasStartButton)
+ this->startStopButton->show();
+ }
+
+ // Check if there is a reset button
+ if (_elem->HasElement("reset_button"))
+ {
+ hasResetButton = _elem->Get<bool>("reset_button");
+ if (hasResetButton)
+ this->resetButton->show();
+ }
+
// Size this widget
+ math::Vector2d s;
if (_elem->HasElement("size"))
{
- math::Vector2d s = _elem->Get<math::Vector2d>("size");
- this->resize(s.x, s.y);
+ s = _elem->Get<math::Vector2d>("size");
}
+
+ // Minumum horizontal size
+ s.x = std::max(s.x, 150.0);
+
+ // Minimum vertical size according to the elements present
+ if (hasStartButton && hasResetButton)
+ s.y = std::max(s.y, 120.0);
+ else if (hasStartButton || hasResetButton)
+ s.y = std::max(s.y, 80.0);
else
- {
- this->resize(200, 30);
- }
+ s.y = std::max(s.y, 30.0);
+
+ this->resize(s.x, s.y);
// Position this widget
if (_elem->HasElement("pos"))
{
math::Vector2d p = _elem->Get<math::Vector2d>("pos");
- // Check for negative x position
+ // Negative positions are counted from the ends
+ // If there are negative positions, we need to filter window resize
+ // events to reposition the timer
+ if (p.x < 0 || p.y < 0)
+ {
+ this->parent()->installEventFilter(this);
+ }
+
if (p.x < 0)
{
- gzwarn << "GUI widget x pos < 0, clamping to 0.\n";
- p.x = 0;
+ if (this->parent())
+ {
+ this->posX = p.x - s.x;
+ p.x = static_cast<QWidget *>(this->parent())->width() + this->posX;
+ }
+ else
+ {
+ gzwarn << "Couldn't get parent, setting position x to zero" <<
+ std::endl;
+ p.x = 0;
+ }
}
- // Check for negative y position
if (p.y < 0)
{
- gzwarn << "GUI widget y pos < 0, clamping to 0.\n";
- p.y = 0;
+ if (this->parent())
+ {
+ this->posY = p.y - s.y;
+ p.y = static_cast<QWidget *>(this->parent())->height() + this->posY;
+ }
+ else
+ {
+ gzwarn << "Couldn't get parent, setting position y to zero" <<
+ std::endl;
+ p.y = 0;
+ }
}
// Check for x position greater than parent width
- if (parent() && p.x > static_cast<QWidget*>(parent())->width())
+ if (this->parent() && p.x > static_cast<QWidget *>(this->parent())->width())
{
gzwarn << "GUI widget x pos > parent width, "
<< "clamping to parent width - this widget's width.\n";
- p.x = static_cast<QWidget*>(parent())->width() - this->width();
+ p.x = static_cast<QWidget *>(this->parent())->width() - this->width();
}
// Check for y position greater than parent height
- if (parent() && p.y > static_cast<QWidget*>(parent())->height())
+ if (this->parent() &&
+ p.y > static_cast<QWidget *>(this->parent())->height())
{
gzwarn << "GUI widget y pos > parent height, "
<< "clamping to parent height - this widget's height.\n";
- p.y = static_cast<QWidget*>(parent())->height() - this->height();
+ p.y = static_cast<QWidget *>(this->parent())->height() -
+ this->height();
}
this->move(p.x, p.y);
@@ -128,8 +277,11 @@ void TimerGUIPlugin::Load(sdf::ElementPtr _elem)
else
{
int xPos, yPos;
- if (parent())
- xPos = static_cast<QWidget*>(parent())->width() - this->width() - 10;
+ if (this->parent())
+ {
+ xPos = static_cast<QWidget *>(this->parent())->width() - this->width() -
+ 10;
+ }
else
xPos = 600;
@@ -137,7 +289,6 @@ void TimerGUIPlugin::Load(sdf::ElementPtr _elem)
this->move(xPos, yPos);
}
-
// Create a node for transportation
this->node = transport::NodePtr(new transport::Node());
this->node->Init();
@@ -156,55 +307,112 @@ void TimerGUIPlugin::Load(sdf::ElementPtr _elem)
}
/////////////////////////////////////////////////
+common::Time TimerGUIPlugin::GetCurrentTime() const
+{
+ return this->timer.GetElapsed();
+}
+
+/////////////////////////////////////////////////
void TimerGUIPlugin::PreRender()
{
boost::mutex::scoped_lock lock(this->timerMutex);
this->SetTime(QString::fromStdString(
- this->FormatTime(this->timer.GetElapsed())));
+ this->timer.GetElapsed().FormattedString(
+ common::Time::FormatOption::HOURS)));
}
/////////////////////////////////////////////////
void TimerGUIPlugin::OnTimerCtrl(ConstGzStringPtr &_msg)
{
- boost::mutex::scoped_lock lock(this->timerMutex);
if (_msg->data() == "start")
- this->timer.Start();
+ this->Start();
else if (_msg->data() == "stop")
- this->timer.Stop();
+ this->Stop();
else if (_msg->data() == "reset")
- this->timer.Reset();
+ this->Reset();
else
gzwarn << "Unable to process command[" << _msg->data() << "]\n";
}
/////////////////////////////////////////////////
-std::string TimerGUIPlugin::FormatTime(const common::Time &_time) const
+void TimerGUIPlugin::Start()
{
- std::ostringstream stream;
- unsigned int day, hour, min, sec, msec;
+ boost::mutex::scoped_lock lock(this->timerMutex);
+ this->timer.Start();
+
+ this->SetStartStopButton("Stop");
+}
- stream.str("");
+/////////////////////////////////////////////////
+void TimerGUIPlugin::Stop()
+{
+ boost::mutex::scoped_lock lock(this->timerMutex);
+ this->timer.Stop();
- sec = _time.sec;
+ this->SetStartStopButton("Start");
+}
- day = sec / 86400;
- sec -= day * 86400;
+/////////////////////////////////////////////////
+void TimerGUIPlugin::OnSetStartStopButton(QString _state)
+{
+ if (!this->startStopButton->isVisible())
+ return;
+
+ this->startStopButton->setText(_state);
+
+ if (_state == "Start")
+ this->startStopButton->setStyleSheet(this->startStyle.c_str());
+ else if (_state == "Stop")
+ this->startStopButton->setStyleSheet(this->stopStyle.c_str());
+}
+
+/////////////////////////////////////////////////
+void TimerGUIPlugin::Reset()
+{
+ // stop before resetting
+ this->Stop();
+ {
+ boost::mutex::scoped_lock lock(this->timerMutex);
+ this->timer.Reset();
+ }
+}
- hour = sec / 3600;
- sec -= hour * 3600;
+////////////////////////////////////////////////
+void TimerGUIPlugin::OnStartStopButton()
+{
+ if (!timer.GetRunning())
+ this->Start();
+ else
+ this->Stop();
+}
- min = sec / 60;
- sec -= min * 60;
+////////////////////////////////////////////////
+void TimerGUIPlugin::OnResetButton()
+{
+ this->Reset();
+}
- msec = rint(_time.nsec * 1e-6);
+/////////////////////////////////////////////////
+bool TimerGUIPlugin::eventFilter(QObject *_obj, QEvent *_event)
+{
+ QWidget *widget = qobject_cast<QWidget *>(_obj);
+ if (widget == this->parent() && _event->type() == QEvent::Resize)
+ {
+ int pX = this->posX;
+ int pY = this->posY;
- // \todo Add in ability to specify time format in SDF.
- // stream << std::setw(2) << std::setfill('0') << day << " ";
+ // Zero values mean that was a positive position, so keep the same
+ if (pX == 0)
+ pX = this->pos().x();
+ else
+ pX = widget->width() + pX;
- stream << std::setw(2) << std::setfill('0') << hour << ":";
- stream << std::setw(2) << std::setfill('0') << min << ":";
- stream << std::setw(2) << std::setfill('0') << sec << ".";
- stream << std::setw(3) << std::setfill('0') << msec;
+ if (pY == 0)
+ pY = this->pos().y();
+ else
+ pY = widget->height() + pY;
- return stream.str();
+ this->move(pX, pY);
+ }
+ return QObject::eventFilter(_obj, _event);
}
diff --git a/plugins/TimerGUIPlugin.hh b/plugins/TimerGUIPlugin.hh
index 1e0dab0..1ff5efa 100644
--- a/plugins/TimerGUIPlugin.hh
+++ b/plugins/TimerGUIPlugin.hh
@@ -45,7 +45,11 @@ namespace gazebo
/// <topic>~/my_timer_control</topic>
/// <pos>pixel_x_pos pixel_y_pos</pos>
/// <size>pixel_width pixel_height</size>
+ /// <start_stop_button>visible</start_stop_button>
+ /// <reset_button>visible</reset_button>
/// </plugin>
+ ///
+ /// Note that the size has to be large enough to fit all widgets.
class GAZEBO_VISIBLE TimerGUIPlugin : public GUIPlugin
{
Q_OBJECT
@@ -59,22 +63,50 @@ namespace gazebo
// Documentation inherited
public: void Load(sdf::ElementPtr _elem);
+ /// \brief Get the current time listed in the timer.
+ /// \return Gazebo time object representing the time.
+ public: common::Time GetCurrentTime() const;
+
/// \brief A signal used to set the sim time line edit.
/// \param[in] _string String representation of sim time.
signals: void SetTime(QString _string);
+ /// \brief A signal used to set the style of the start/stop button.
+ /// \param[in] _string State ("Start"/"Stop").
+ signals: void SetStartStopButton(QString _state);
+
/// \brief Callback that receives timer control message.
/// \param[in] _msg "start" = start timer, "stop" = stop timer, "reset"
/// = reset timer.
private: void OnTimerCtrl(ConstGzStringPtr &_msg);
+ /// \brief Start the timer.
+ private: void Start();
+
+ /// \brief Stop the timer.
+ private: void Stop();
+
+ /// \brief Reset the timer.
+ private: void Reset();
+
+ /// \brief Callback when the start/stop button is pressed.
+ private slots: void OnStartStopButton();
+
+ /// \brief Callback to set the start/stop button style.
+ /// \param[in] _string State ("Start"/"Stop").
+ private slots: void OnSetStartStopButton(QString _state);
+
+ /// \brief Callback when the reset button is pressed.
+ private slots: void OnResetButton();
+
/// \brief Handles the prerender callback
private: void PreRender();
- /// \brief Helper function to format time string.
- /// \param[in] _msg Time message.
- /// \return Time formatted as a string.
- private: std::string FormatTime(const common::Time &_time) const;
+ /// \brief Qt event filter currently used to filter resize events.
+ /// \param[in] _obj Object that is watched by the event filter.
+ /// \param[in] _event Qt event.
+ /// \return True if the event is handled.
+ private: bool eventFilter(QObject *_obj, QEvent *_event);
/// \brief Node used to establish communication with gzserver.
private: transport::NodePtr node;
@@ -90,6 +122,26 @@ namespace gazebo
/// \brief Mutex to protect timer updates.
private: boost::mutex timerMutex;
+
+ /// \brief Start/stop button.
+ private: QPushButton *startStopButton;
+
+ /// \brief QT style for the start setting of the start/stop button
+ private: std::string startStyle;
+
+ /// \brief QT style for the stop setting of the start/stop button
+ private: std::string stopStyle;
+
+ /// \brief Reset button.
+ private: QPushButton *resetButton;
+
+ /// \brief Number of pixels from the timer's left side and the
+ /// window's right side. Only needed for negative positions.
+ private: int posX;
+
+ /// \brief Number of pixels from the timer's top and the window's
+ /// bottom. Only needed for negative positions.
+ private: int posY;
};
}
diff --git a/plugins/TransporterPlugin.cc b/plugins/TransporterPlugin.cc
new file mode 100644
index 0000000..6e54820
--- /dev/null
+++ b/plugins/TransporterPlugin.cc
@@ -0,0 +1,186 @@
+/*
+ * 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/common/Events.hh>
+#include <gazebo/common/Assert.hh>
+#include <gazebo/common/Console.hh>
+
+#include <gazebo/physics/World.hh>
+#include <gazebo/physics/Model.hh>
+
+#include "plugins/TransporterPluginPrivate.hh"
+#include "plugins/TransporterPlugin.hh"
+
+using namespace gazebo;
+
+// Register the plugin
+GZ_REGISTER_WORLD_PLUGIN(TransporterPlugin)
+
+/////////////////////////////////////////////////
+TransporterPlugin::TransporterPlugin()
+ : dataPtr(new TransporterPluginPrivate)
+{
+}
+
+/////////////////////////////////////////////////
+TransporterPlugin::~TransporterPlugin()
+{
+ delete this->dataPtr;
+ this->dataPtr = NULL;
+}
+
+/////////////////////////////////////////////////
+void TransporterPlugin::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
+{
+ bool hasManualActivation = false;
+
+ GZ_ASSERT(_world, "TransporterPlugin world pointer is NULL");
+ GZ_ASSERT(_sdf, "TransporterPlugin sdf pointer is NULL");
+
+ this->dataPtr->world = _world;
+
+ // Read each pad element
+ sdf::ElementPtr padElem = _sdf->GetElement("pad");
+ while (padElem)
+ {
+ // Create a new pad
+ auto pad = std::make_shared<TransporterPluginPrivate::Pad>();
+
+ // Set the pad's name and destination
+ pad->name = padElem->Get<std::string>("name");
+ pad->dest = padElem->Get<std::string>("destination");
+
+ // Check that a pad does not exist
+ if (this->dataPtr->pads.find(pad->name) != this->dataPtr->pads.end())
+ {
+ gzerr << "Transporter pad with name[" << pad->name << "] already exists."
+ << "The duplicate pad will not be loaded\n";
+ continue;
+ }
+
+ // Set the pad's activation type. The default is auto activation.
+ if (padElem->HasElement("activation"))
+ {
+ pad->autoActivation = padElem->Get<std::string>("activation") == "auto";
+
+ // Store that the user has at least one pad with manual activation.
+ // This info is used to trigger a warning message below.
+ if (!pad->autoActivation)
+ hasManualActivation = true;
+ }
+ else
+ {
+ pad->autoActivation = true;
+ }
+
+ // Read the outgoing information
+ sdf::ElementPtr outElem = padElem->GetElement("outgoing");
+ pad->outgoingBox.min = outElem->Get<math::Vector3>("min");
+ pad->outgoingBox.max = outElem->Get<math::Vector3>("max");
+
+ // Read the incoming information
+ sdf::ElementPtr inElem = padElem->GetElement("incoming");
+ pad->incomingPose = inElem->Get<math::Pose>("pose");
+
+ // Store the pad
+ this->dataPtr->pads[pad->name] = pad;
+
+ // Get the next pad element
+ padElem = padElem->GetNextElement("pad");
+ }
+
+ // Connect to the update event
+ this->dataPtr->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&TransporterPlugin::Update, this));
+
+ // Listen on the activation topic, if present. This topic is used for
+ // manual activation.
+ if (_sdf->HasElement("activation_topic"))
+ {
+ // Create and initialize the node.
+ this->dataPtr->node = transport::NodePtr(new transport::Node());
+ this->dataPtr->node->Init(_world->GetName());
+
+ // Subscribe to the activation topic.
+ this->dataPtr->activationSub = this->dataPtr->node->Subscribe(
+ _sdf->Get<std::string>("activation_topic"),
+ &TransporterPlugin::OnActivation, this);
+ }
+ else if (hasManualActivation)
+ {
+ gzerr << "Manual activation of a transporter pad has been requested, "
+ << "but no <activation_topic> is present in the plugin's SDF.\n";
+ }
+}
+
+/////////////////////////////////////////////////
+void TransporterPlugin::OnActivation(ConstGzStringPtr &_msg)
+{
+ // Find the pad
+ auto const &iter = this->dataPtr->pads.find(_msg->data());
+ if (iter != this->dataPtr->pads.end())
+ {
+ // Activate the pad.
+ std::lock_guard<std::mutex> lock(this->dataPtr->padMutex);
+ iter->second->activated = true;
+ }
+ else
+ {
+ gzwarn << "Unknown transporter pad[" << _msg->data() << "]\n";
+ }
+}
+
+/////////////////////////////////////////////////
+void TransporterPlugin::Update()
+{
+ // Get all the models
+ physics::Model_V models = this->dataPtr->world->GetModels();
+
+ std::lock_guard<std::mutex> lock(this->dataPtr->padMutex);
+
+ // Process each model.
+ for (auto const &model : models)
+ {
+ // Skip models that are static
+ if (model->IsStatic())
+ continue;
+
+ // Get the model's pose
+ math::Pose modelPose = model->GetWorldPose();
+
+ // Iterate over all pads
+ for (auto const &padIter : this->dataPtr->pads)
+ {
+ // Check if the model is in the pad's outgoing box.
+ if (padIter.second->outgoingBox.Contains(modelPose.pos))
+ {
+ // Get the destination pad
+ auto const &destIter = this->dataPtr->pads.find(padIter.second->dest);
+
+ // Make sure we can transport the model
+ if (destIter != this->dataPtr->pads.end() &&
+ (padIter.second->autoActivation || padIter.second->activated))
+ {
+ // Move the model
+ model->SetWorldPose(destIter->second->incomingPose);
+
+ // Deactivate the pad. This is used by manually activated pads.
+ padIter.second->activated = false;
+ }
+ }
+ }
+ }
+}
diff --git a/plugins/TransporterPlugin.hh b/plugins/TransporterPlugin.hh
new file mode 100644
index 0000000..5ea9a2e
--- /dev/null
+++ b/plugins/TransporterPlugin.hh
@@ -0,0 +1,109 @@
+/*
+ * 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_TRANSPORTER_PLUGIN_HH_
+#define _GAZEBO_TRANSPORTER_PLUGIN_HH_
+
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/util/system.hh>
+
+namespace gazebo
+{
+ // Forward declare private data class
+ class TransporterPluginPrivate;
+
+ /// \brief A plugin that allows models to transport (teleport) to
+ /// a new location. A transporter plugin uses multiple named <pads>, where
+ /// each <pad> defines an outgoing region, an incoming pose, and
+ /// a destination pad.
+ ///
+ /// When a model enters a pad's outgoing region it is moved to the
+ /// destination pad's incoming pose. This means the transporter plugin is
+ /// only useful if at least two pads are defined.
+ ///
+ /// The following is example usage in SDF:
+ ///
+ /** \verbatim
+ <plugin filename="libTransporterPlugin.so" name="transporter">
+ <!-- Topic that facilitates manual activation of a pad. An
+ activation message is only meaningful for pad's with manual
+ activation. An activation message consists of
+ a string message on this topic with a pad's name to activate.
+ See examples/stand_alone/transporter.cc for an example for
+ triggering a manual pad. -->
+ <activation_topic>~/transporter</activation_topic>
+
+ <!-- Pad 1, which is automatically updated. This means any model
+ that enter the outgoing region will be moved. -->
+ <pad name="pad1">
+ <destination>pad2</destination>
+ <activation>auto</activation>
+
+ <outgoing>
+ <min>-.5 -.5 0</min>
+ <max>.5 .5 1</max>
+ </outgoing>
+
+ <incoming>
+ <pose>2 3.5 0 0 0 0</pose>
+ </incoming>
+ </pad>
+
+ <!-- Pad 2 is manually updated. This means a model will be
+ moved only if it is in the outgoing region and an activation
+ message has been received. An activation message consists of
+ a string message on the <activation_topic> topic with
+ the pad's name. -->
+ <pad name="pad2">
+ <destination>pad1</destination>
+ <activation>manual</activation>
+
+ <outgoing>
+ <min>-.5 3.0 0</min>
+ <max>.5 4.0 1</max>
+ </outgoing>
+
+ <incoming>
+ <pose>2 0 0 0 0 0</pose>
+ </incoming>
+ </pad>
+ </plugin>
+ \endverbatim */
+ class GAZEBO_VISIBLE TransporterPlugin : public WorldPlugin
+ {
+ /// \brief Constructor.
+ public: TransporterPlugin();
+
+ /// \brief Destructor.
+ public: virtual ~TransporterPlugin();
+
+ // Documentation inherited
+ public: virtual void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf);
+
+ /// \brief Update the plugin. This is updated every iteration of
+ /// simulation.
+ private: void Update();
+
+ /// \brief Callback that receives activation messages.
+ /// \param[in] _msg String message that indicates what transporter pad
+ /// was activated.
+ private: void OnActivation(ConstGzStringPtr &_msg);
+
+ /// \brief Private data pointer.
+ private: TransporterPluginPrivate *dataPtr;
+ };
+}
+#endif
diff --git a/plugins/TransporterPluginPrivate.hh b/plugins/TransporterPluginPrivate.hh
new file mode 100644
index 0000000..1b13859
--- /dev/null
+++ b/plugins/TransporterPluginPrivate.hh
@@ -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.
+ *
+*/
+#ifndef _GAZEBO_TRANSPORTER_PLUGIN_PRIVATE_HH_
+#define _GAZEBO_TRANSPORTER_PLUGIN_PRIVATE_HH_
+
+#include <map>
+#include <string>
+#include <mutex>
+#include <memory>
+
+#include <gazebo/transport/Node.hh>
+#include <gazebo/transport/Subscriber.hh>
+
+namespace gazebo
+{
+ class TransporterPluginPrivate
+ {
+ /// \brief Definition of a transported pad
+ public: class Pad
+ {
+ /// \brief Name of the pad.
+ public: std::string name;
+
+ /// \brief Name of the destination pad.
+ public: std::string dest;
+
+ /// \brief Pose of the incoming pad. This is where incoming models
+ /// appear.
+ public: math::Pose incomingPose;
+
+ /// \brief Box that defines the activation region of the transporter.
+ public: math::Box outgoingBox;
+
+ /// \brief True if the pad should automatically teleport.
+ /// False will cause the pad to wait for an activation
+ /// signal. See this plugin's <activation_topic> xml element.
+ public: bool autoActivation;
+
+ /// \brief This flag is used for manual activation of a pad.
+ /// It is set to true when a string message that contains
+ /// the name of the pad is sent over the activation topic.
+ public: bool activated;
+ };
+
+ /// \brief World pointer.
+ public: physics::WorldPtr world;
+
+ /// \brief Map of all the transporter pads
+ public: std::map<std::string, std::shared_ptr<Pad> > pads;
+
+ /// \brief Pointer to the update event connection
+ public: event::ConnectionPtr updateConnection;
+
+ /// \brief Node for communication.
+ public: transport::NodePtr node;
+
+ /// \brief Subscriber to the activation topic.
+ public: transport::SubscriberPtr activationSub;
+
+ /// \brief Mutex to protect pad data.
+ public: std::mutex padMutex;
+ };
+}
+#endif
diff --git a/plugins/VehiclePlugin.cc b/plugins/VehiclePlugin.cc
index fca94bb..e1443fa 100644
--- a/plugins/VehiclePlugin.cc
+++ b/plugins/VehiclePlugin.cc
@@ -34,6 +34,10 @@ VehiclePlugin::VehiclePlugin()
this->frontPower = 50;
this->rearPower = 50;
this->wheelRadius = 0.3;
+ this->maxBrake = 0.0;
+ this->maxGas = 0.0;
+ this->steeringRatio = 1.0;
+ this->tireAngleRange = 1.0;
}
/////////////////////////////////////////////////
diff --git a/plugins/events/CMakeLists.txt b/plugins/events/CMakeLists.txt
index a66b865..006d5df 100644
--- a/plugins/events/CMakeLists.txt
+++ b/plugins/events/CMakeLists.txt
@@ -1,3 +1,7 @@
+if (WIN32)
+ add_definitions(-DBUILDING_DLL)
+endif()
+
link_directories(
${GAZEBO_LIBRARY_DIRS}
${CMAKE_CURRENT_BINARY_DIR}
@@ -7,6 +11,8 @@ set (src
EventSource.cc
ExistenceEventSource.cc
InRegionEventSource.cc
+ OccupiedEventSource.cc
+ Region.cc
SimEventsPlugin.cc
SimStateEventSource.cc
)
@@ -15,6 +21,8 @@ set (inc
EventSource.hh
ExistenceEventSource.hh
InRegionEventSource.hh
+ OccupiedEventSource.hh
+ Region.hh
SimEventsException.hh
SimEventsPlugin.hh
SimStateEventSource.hh
@@ -24,7 +32,7 @@ include_directories(
${GAZEBO_INCLUDE_DIRS}
)
-add_library(SimEventsPlugin SHARED ${src} )
+add_library(SimEventsPlugin SHARED ${src})
target_link_libraries(SimEventsPlugin ${GAZEBO_libraries} gazebo_msgs)
install (TARGETS SimEventsPlugin DESTINATION ${GAZEBO_PLUGIN_INSTALL_DIR})
-gz_install_includes("plugins" ${inc})
+gz_install_includes("plugins/events" ${inc})
diff --git a/plugins/events/EventSource.cc b/plugins/events/EventSource.cc
index aac61b6..ceb9e08 100644
--- a/plugins/events/EventSource.cc
+++ b/plugins/events/EventSource.cc
@@ -33,6 +33,8 @@ EventSource::EventSource(transport::PublisherPtr _pub,
physics::WorldPtr _world)
:type(_type), world(_world), active(true), pub(_pub)
{
+ GZ_ASSERT(_world, "EventSource world pointer is NULL");
+ GZ_ASSERT(_pub, "EventSource pub pointer is NULL");
}
////////////////////////////////////////////////////////////////////////////////
@@ -41,9 +43,10 @@ void EventSource::Init()
}
////////////////////////////////////////////////////////////////////////////////
-void EventSource::Load(const sdf::ElementPtr &_sdf)
+void EventSource::Load(const sdf::ElementPtr _sdf)
{
this->name = _sdf->GetElement("name")->Get<std::string>();
+
// active by default, but this can be set in the world file
if (_sdf->HasElement("active"))
{
@@ -54,7 +57,7 @@ void EventSource::Load(const sdf::ElementPtr &_sdf)
////////////////////////////////////////////////////////////////////////////////
-void EventSource::Emit(const std::string& _data)
+void EventSource::Emit(const std::string &_data) const
{
if (this->IsActive())
{
@@ -72,12 +75,12 @@ void EventSource::Emit(const std::string& _data)
msgs::Set(worldStatsMsg->mutable_real_time(), this->world->GetRealTime());
msgs::Set(worldStatsMsg->mutable_pause_time(), this->world->GetPauseTime());
// send it on the publisher we got in the ctor
- pub->Publish(msg);
+ this->pub->Publish(msg);
}
}
///////////////////////////////////////////////////////////////////////////////
-bool EventSource::IsActive()
+bool EventSource::IsActive() const
{
// inactive events do not fire.
return this->active;
diff --git a/plugins/events/EventSource.hh b/plugins/events/EventSource.hh
index 17e63c8..8a69e14 100644
--- a/plugins/events/EventSource.hh
+++ b/plugins/events/EventSource.hh
@@ -15,8 +15,14 @@
*
*/
-#ifndef _EVENTSOURCE_HH_
-#define _EVENTSOURCE_HH_
+#ifndef _GAZEBO_PLUGIN_EVENTSOURCE_HH_
+#define _GAZEBO_PLUGIN_EVENTSOURCE_HH_
+
+#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>
@@ -37,7 +43,7 @@ namespace gazebo
/// \param[in] _type the type of event
/// \param[in] _world Pointer to the world (in order to get model refs, etc)
public: EventSource(transport::PublisherPtr _pub,
- const std::string& _type,
+ const std::string &_type,
physics::WorldPtr _world);
/// \brief Destructor
@@ -46,11 +52,11 @@ namespace gazebo
/// \brief emit an event with data to the internal publisher
/// (and using the internal type)
/// \param[in] _data the JSON data related to this event.
- public: void Emit(const std::string& _data);
+ public: void Emit(const std::string& _data) const;
/// \brief Load from an sdf element (with possible configuration data)
/// \param[in] _sdf the sdf element for the event in the world file
- public: virtual void Load(const sdf::ElementPtr &_sdf);
+ public: virtual void Load(const sdf::ElementPtr _sdf);
/// \brief Initialize the event
public: virtual void Init();
@@ -58,7 +64,7 @@ namespace gazebo
/// \brief An event source can be used to enable other events. Inactive
/// events do not generate an message when Emit is called.
/// \return true if the event is active
- public: virtual bool IsActive();
+ public: virtual bool IsActive() const;
/// \brief Name of the event.
protected: std::string name;
@@ -80,7 +86,7 @@ namespace gazebo
typedef boost::shared_ptr<EventSource> EventSourcePtr;
/// \brief Gazebo events to detect model creation/deletion
- class SimEventConnector
+ class SimEventConnector
{
/// \brief Connect a boost::slot to the spawn model event
/// \param[in] _subscriber the subscriber to this event
diff --git a/plugins/events/ExistenceEventSource.cc b/plugins/events/ExistenceEventSource.cc
index 2bf59e4..9105e60 100644
--- a/plugins/events/ExistenceEventSource.cc
+++ b/plugins/events/ExistenceEventSource.cc
@@ -28,7 +28,7 @@ ExistenceEventSource::ExistenceEventSource(transport::PublisherPtr _pub,
}
////////////////////////////////////////////////////////////////////////////////
-void ExistenceEventSource::Load(const sdf::ElementPtr &_sdf)
+void ExistenceEventSource::Load(const sdf::ElementPtr _sdf)
{
EventSource::Load(_sdf);
if (_sdf->HasElement("model"))
diff --git a/plugins/events/ExistenceEventSource.hh b/plugins/events/ExistenceEventSource.hh
index e7893e3..51aacd1 100644
--- a/plugins/events/ExistenceEventSource.hh
+++ b/plugins/events/ExistenceEventSource.hh
@@ -15,8 +15,8 @@
*
*/
-#ifndef _EXISTENCEVENTSOURCE_HH_
-#define _EXISTENCEVENTSOURCE_HH_
+#ifndef _GAZEBO_EXISTENCEVENTSOURCE_HH_
+#define _GAZEBO_EXISTENCEVENTSOURCE_HH_
#include <string>
@@ -34,7 +34,7 @@ namespace gazebo
/// \brief Reads the model filter and event name.
/// \param[in] _sdf the element for this event source
- public: virtual void Load(const sdf::ElementPtr &_sdf);
+ public: virtual void Load(const sdf::ElementPtr _sdf);
/// \brief callback for gazebo event
/// \param[in] _model the name of the model
diff --git a/plugins/events/InRegionEventSource.cc b/plugins/events/InRegionEventSource.cc
index a175549..a64edc3 100644
--- a/plugins/events/InRegionEventSource.cc
+++ b/plugins/events/InRegionEventSource.cc
@@ -21,24 +21,22 @@ using namespace gazebo;
////////////////////////////////////////////////////////////////////////////////
InRegionEventSource::InRegionEventSource(transport::PublisherPtr _pub,
- physics::WorldPtr _world,
- const std::map<std::string, RegionPtr>
- &_regions)
- :EventSource(_pub, "region", _world), regions(_regions), isInside(false)
+ physics::WorldPtr _world, const std::map<std::string, RegionPtr> &_regions)
+ : EventSource(_pub, "region", _world), regions(_regions), isInside(false)
{
}
////////////////////////////////////////////////////////////////////////////////
-void InRegionEventSource::Load(const sdf::ElementPtr &_sdf)
+void InRegionEventSource::Load(const sdf::ElementPtr _sdf)
{
EventSource::Load(_sdf);
if (_sdf->HasElement("model"))
- this->modelName = _sdf->GetElement("model")->Get<std::string>();
+ this->modelName = _sdf->Get<std::string>("model");
else
gzerr << this->name << " is missing a model element" << std::endl;
if (_sdf->HasElement("region"))
- this->regionName = _sdf->GetElement("region")->Get<std::string>();
+ this->regionName = _sdf->Get<std::string>("region");
else
gzerr << this->name << " is missing a region element" << std::endl;
@@ -69,6 +67,28 @@ void InRegionEventSource::Init()
gzerr << this->name << ": Region '" << this->regionName
<< "' does not exist" << std::endl;
}
+
+ this->Info();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void InRegionEventSource::Info() const
+{
+ std::stringstream ss;
+ ss << "InRegionEventSource "
+ << " model " << this->modelName << " region [" << this->regionName
+ << "]" << std::endl;
+
+ for (auto v: this->region->boxes)
+ {
+ ss << " Min ";
+ ss << "[" << v.min.x << ", " << v.min.y << ", " << v.min.z << "]";
+ ss << std::endl;
+ ss << " Max ";
+ ss << "[" << v.max.x << ", " << v.max.y << ", " << v.max.z << "]\n";
+ }
+ ss << " inside: " << this->isInside << std::endl;
+ gzmsg << ss.str();
}
////////////////////////////////////////////////////////////////////////////////
@@ -77,15 +97,18 @@ void InRegionEventSource::Update()
// model must exist
if (!this->model)
return;
+
// region must exist
if (!this->region)
return;
math::Vector3 point = this->model->GetWorldPose().pos;
bool oldState = this->isInside;
- this->isInside = this->region->PointInRegion(point);
- if (oldState != this->isInside)
+ bool currentState = this->region->Contains(point);
+
+ if (oldState != currentState)
{
+ this->isInside = currentState;
std::string json = "{";
if (this->isInside)
{
@@ -101,58 +124,3 @@ void InRegionEventSource::Update()
this->Emit(json);
}
}
-
-////////////////////////////////////////////////////////////////////////////////
-Volume::~Volume()
-{
-}
-
-////////////////////////////////////////////////////////////////////////////////
-bool Volume::PointInVolume(const math::Vector3 &_p) const
-{
- return _p.x >= this->min.x && _p.x <= this->max.x &&
- _p.y >= this->min.y && _p.y <= this->max.y &&
- _p.z >= this->min.z && _p.z <= this->max.z;
-}
-
-////////////////////////////////////////////////////////////////////////////////
-bool Region::PointInRegion(const math::Vector3 &_p) const
-{
- for (unsigned int i = 0; i < this->volumes.size(); ++i)
- {
- if (this->volumes[i]->PointInVolume(_p))
- {
- return true;
- }
- }
- return false;
-}
-
-////////////////////////////////////////////////////////////////////////////////
-void Region::Load(const sdf::ElementPtr &_sdf)
-{
- sdf::ElementPtr child = _sdf->GetFirstElement();
- while (child)
- {
- std::string ename = child->GetName();
- if (ename == "volume")
- {
- VolumePtr volume = VolumePtr(new Volume());
- volume->min = child->GetElement("min")->Get<math::Vector3>();
- volume->max = child->GetElement("max")->Get<math::Vector3>();
- this->volumes.push_back(volume);
- }
- else if (ename == "name")
- {
- this->name = child->Get<std::string>();
- }
- else
- {
- std::string m;
- m += "Unexpected element \"" + ename + "\" in Region element";
- throw SimEventsException(m.c_str());
- }
- child = child->GetNextElement();
- }
-}
-
diff --git a/plugins/events/InRegionEventSource.hh b/plugins/events/InRegionEventSource.hh
index 4b70ba0..082a22d 100644
--- a/plugins/events/InRegionEventSource.hh
+++ b/plugins/events/InRegionEventSource.hh
@@ -15,70 +15,28 @@
*
*/
-#ifndef _INREGIONEVENTSOURCE_HH_
-#define _INREGIONEVENTSOURCE_HH_
+#ifndef _GAZEBO_PLUGINS_EVENTS_INREGIONEVENTSOURCE_HH_
+#define _GAZEBO_PLUGINS_EVENTS_INREGIONEVENTSOURCE_HH_
#include <map>
#include <string>
#include <vector>
-#include "EventSource.hh"
+#include "plugins/events/Region.hh"
+#include "plugins/events/EventSource.hh"
namespace gazebo
{
- /// \brief A Volume base class with an inclusion query PointInVoume.
- /// The default implementation is a simple axis aligned bounding box.
- class Volume
- {
- /// \brief dtor
- public: virtual ~Volume();
-
- /// \brief Checks if a point lies inside the box
- /// \param[in] _p point
- public: virtual bool PointInVolume(const math::Vector3 &_p) const;
-
- /// \brief The volume extent
- public: math::Vector3 min, max;
- };
-
- typedef boost::shared_ptr<Volume> VolumePtr;
-
- /// \brief A region, made of a list of volumes
- class Region
- {
- /// \brief Load from a world file (inside a SimEvent plugin element)
- /// \param[in] _sdf the region element
- public: void Load(const sdf::ElementPtr &_sdf);
-
- /// \brief Checks if a point lies inside the region
- /// \param[in] _p point
- public: bool PointInRegion(const math::Vector3 &p) const;
-
- /// \brief name of the region (as defined in the world file)
- public: std::string name;
-
- /// \brief The list of volumes inside this region
- public: std::vector<VolumePtr> volumes;
- };
-
- typedef boost::shared_ptr<Region> RegionPtr;
-
- /// \brief convenience function to print a region to the console
- /// \param[in] _out the output stream
- /// \param[in] _region the instance to write out
- std::ostream& operator << (std::ostream &_out, const Region &_region);
-
/// \brief The event generator class
- class InRegionEventSource: public EventSource
+ class InRegionEventSource: public EventSource
{
/// \brief Constructor
/// \param[in] _pub the publisher for the SimEvents
/// \param[in] _world Pointer to the world.
/// \param[in] _regions dictionary of regions in the world
public: InRegionEventSource(transport::PublisherPtr _pub,
- physics::WorldPtr _world,
- const std::map<std::string, RegionPtr>
- &_regions);
+ physics::WorldPtr _world,
+ const std::map<std::string, RegionPtr> &_regions);
/// \brief Initialize the event
public: virtual void Init();
@@ -86,10 +44,13 @@ namespace gazebo
/// \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 region from the world
/// file.
/// \param[in] _sdf
- public: virtual void Load(const sdf::ElementPtr &_sdf);
+ public: virtual void Load(const sdf::ElementPtr _sdf);
/// \brief Pointer to the update event connection
private: event::ConnectionPtr updateConnection;
@@ -114,6 +75,4 @@ namespace gazebo
private: bool isInside;
};
}
-
-
#endif
diff --git a/plugins/events/OccupiedEventSource.cc b/plugins/events/OccupiedEventSource.cc
new file mode 100644
index 0000000..98499fa
--- /dev/null
+++ b/plugins/events/OccupiedEventSource.cc
@@ -0,0 +1,127 @@
+/*
+ * 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.
+ *
+*/
+#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 <gazebo/common/Events.hh>
+#include <gazebo/common/Assert.hh>
+#include <gazebo/common/Console.hh>
+
+#include <gazebo/msgs/msgs.hh>
+
+#include <gazebo/transport/Node.hh>
+#include <gazebo/transport/Subscriber.hh>
+
+#include <gazebo/physics/World.hh>
+#include <gazebo/physics/Model.hh>
+
+#include "plugins/events/OccupiedEventSource.hh"
+
+using namespace gazebo;
+
+////////////////////////////////////////////////////////////////////////////////
+OccupiedEventSource::OccupiedEventSource(transport::PublisherPtr _pub,
+ physics::WorldPtr _world, const std::map<std::string, RegionPtr> &_regions)
+ : EventSource(_pub, "occupied", _world), regions(_regions)
+{
+}
+
+/////////////////////////////////////////////////
+void OccupiedEventSource::Load(const sdf::ElementPtr _sdf)
+{
+ std::string topic;
+ std::string data;
+
+ GZ_ASSERT(_sdf, "OccupiedEventSource sdf pointer is NULL");
+ EventSource::Load(_sdf);
+
+ if (_sdf->HasElement("region"))
+ this->regionName = _sdf->Get<std::string>("region");
+ else
+ {
+ gzerr << "SimEventPlugin event[" << this->name << "] "
+ << "is missing a region element. This event will be ignored.\n";
+ }
+
+ // Get the topic name
+ if (_sdf->HasElement("topic"))
+ topic = _sdf->Get<std::string>("topic");
+ else
+ {
+ gzerr << "Missing <topic>, child of <event> with name[" << this->name
+ << "]. This event will be skipped.\n";
+ }
+
+ // Get message data
+ if (_sdf->HasElement("msg_data"))
+ {
+ data = _sdf->Get<std::string>("msg_data");
+ }
+ else
+ {
+ gzerr << "Missing <msg_data>, child of <event> with name[" << this->name
+ << "]. This event will be skipped.\n";
+ }
+
+ auto regionIter = this->regions.find(this->regionName);
+ if (regionIter == this->regions.end())
+ {
+ gzerr << "Unkown region with name[" << this->regionName << "] "
+ << "in <event> with name[" << this->name << "]. "
+ << "This event will be skipped.\n";
+ }
+
+ // Setup communications and connect to world update if everything is okay.
+ if (!topic.empty() && !data.empty() && regionIter != this->regions.end())
+ {
+ // Setup communication
+ this->node = transport::NodePtr(new transport::Node());
+ this->node->Init(this->world->GetName());
+ this->msgPub = this->node->Advertise<gazebo::msgs::GzString>(topic);
+
+ this->msg.set_data(data);
+
+ // Connect to the update event.
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&OccupiedEventSource::Update, this));
+ }
+}
+
+/////////////////////////////////////////////////
+void OccupiedEventSource::Update()
+{
+ // Get all the models.
+ physics::Model_V models = this->world->GetModels();
+
+ // Process each model.
+ for (physics::Model_V::iterator iter = models.begin();
+ iter != models.end(); ++iter)
+ {
+ // Skip models that are static
+ if ((*iter)->IsStatic())
+ continue;
+
+ // If inside, then transmit the desired message.
+ if (this->regions[this->regionName]->Contains((*iter)->GetWorldPose().pos))
+ {
+ this->msgPub->Publish(this->msg);
+ }
+ }
+}
diff --git a/plugins/events/OccupiedEventSource.hh b/plugins/events/OccupiedEventSource.hh
new file mode 100644
index 0000000..c498970
--- /dev/null
+++ b/plugins/events/OccupiedEventSource.hh
@@ -0,0 +1,96 @@
+/*
+ * 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_OCCUPIED_EVENT_SOURCE_HH_
+#define _GAZEBO_PLUGINS_EVENTS_OCCUPIED_EVENT_SOURCE_HH_
+
+#include <string>
+#include <map>
+
+#include <sdf/sdf.hh>
+
+#include <gazebo/transport/TransportTypes.hh>
+
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/util/system.hh>
+
+#include "Region.hh"
+#include "EventSource.hh"
+
+namespace gazebo
+{
+ /// \brief A plugin that transmits a message when an in-region event occurs.
+ /// Events are specified in SDF. The following is example usage:
+ //
+ /// \verbatim
+ /// <plugin filename="libSimEventsPlugin.so" name="event_plugin">
+ /// <region>
+ /// <name>region1</name>
+ /// <volume>
+ /// <min>1.5 -1 0</min>
+ /// <max>2.5 1 1</max>
+ /// </volume>
+ /// </region>
+ ///
+ /// <event>
+ /// <name>region1_event</name>
+ /// <type>occupied</type>
+ /// <topic>~/elevator</topic>
+ /// <region>region1</region>
+ /// <msg_data>0</msg_data>
+ /// </event>
+ /// </plugin>
+ /// \endverbatim
+ class GAZEBO_VISIBLE OccupiedEventSource : public EventSource
+ {
+ // Documentation inherited
+ public: OccupiedEventSource(transport::PublisherPtr _pub,
+ physics::WorldPtr _world,
+ const std::map<std::string, RegionPtr> &_regions);
+
+ /// \brief Destructor.
+ public: ~OccupiedEventSource() = default;
+
+ // Documentation inherited
+ public: virtual void Load(const sdf::ElementPtr _sdf);
+
+ /// \brief Update function called once every cycle
+ private: void Update();
+
+ /// \brief SDF pointer.
+ private: sdf::ElementPtr sdf;
+
+ /// \brief Map of region names to regions.
+ private: std::map<std::string, RegionPtr> regions;
+
+ /// \brief String message that is transmitted when an event occurs.
+ public: msgs::GzString msg;
+
+ /// \brief Publisher that transmits the message when an event occurs.
+ public: transport::PublisherPtr msgPub;
+
+ /// \brief Pointer to the update event connection
+ private: event::ConnectionPtr updateConnection;
+
+ /// \brief Pointer to a transport node.
+ private: transport::NodePtr node;
+
+ /// \brief The region used for the in region check.
+ private: std::string regionName;
+ };
+}
+#endif
diff --git a/plugins/events/Region.cc b/plugins/events/Region.cc
new file mode 100644
index 0000000..cb70bf8
--- /dev/null
+++ b/plugins/events/Region.cc
@@ -0,0 +1,58 @@
+/*
+ * 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/common/Console.hh>
+#include "plugins/events/Region.hh"
+
+using namespace gazebo;
+
+////////////////////////////////////////////////////////////////////////////////
+bool Region::Contains(const math::Vector3 &_p) const
+{
+ for (auto v: this->boxes)
+ {
+ if (v.Contains(_p))
+ {
+ return true;
+ }
+ }
+ return false;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void Region::Load(const sdf::ElementPtr &_sdf)
+{
+ sdf::ElementPtr child = _sdf->GetFirstElement();
+ while (child)
+ {
+ std::string ename = child->GetName();
+ if (ename == "volume")
+ {
+ this->boxes.push_back(math::Box(child->Get<math::Vector3>("min"),
+ child->Get<math::Vector3>("max")));
+ }
+ else if (ename == "name")
+ {
+ this->name = child->Get<std::string>();
+ }
+ else
+ {
+ gzwarn << "Unexpected element \"" + ename + "\" in Region element.";
+ }
+ child = child->GetNextElement();
+ }
+}
diff --git a/plugins/events/Region.hh b/plugins/events/Region.hh
new file mode 100644
index 0000000..101390d
--- /dev/null
+++ b/plugins/events/Region.hh
@@ -0,0 +1,66 @@
+/*
+ * 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_PLUGIN_EVENTS_REGION_HH_
+#define _GAZEBO_PLUGIN_EVENTS_REGION_HH_
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <sdf/sdf.hh>
+#include "gazebo/math/Vector3.hh"
+#include "gazebo/math/Box.hh"
+
+namespace gazebo
+{
+ /// \brief A region, made of a list of boxes
+ class Region
+ {
+ /// \brief Constructor
+ public: Region() = default;
+
+ /// \brief Destructor
+ public: virtual ~Region() = default;
+
+ /// \brief Load from a world file (inside a SimEvent plugin element)
+ /// \param[in] _sdf The <region> element
+ public: void Load(const sdf::ElementPtr &_sdf);
+
+ /// \brief Check if a point lies inside the region
+ /// \param[in] _p Point to check
+ /// \return True if point is in region
+ public: bool Contains(const math::Vector3 &_p) const;
+
+ /// \brief Output operator to print a region to the console.
+ /// \param[in] _out The output stream.
+ /// \param[in] _region The instance to write out.
+ /// \return the stream
+ public: friend std::ostream& operator<<(std::ostream &_out,
+ const Region &_region);
+
+ /// \brief Name of the region (as defined in the world file)
+ public: std::string name;
+
+ /// \brief The list of volumes inside this region
+ public: std::vector<math::Box> boxes;
+ };
+
+ /// \def RegionPtr
+ /// \brief Shared pointer to a region
+ typedef std::shared_ptr<Region> RegionPtr;
+}
+#endif
diff --git a/plugins/events/SimEventsPlugin.cc b/plugins/events/SimEventsPlugin.cc
index b1db899..3e11409 100644
--- a/plugins/events/SimEventsPlugin.cc
+++ b/plugins/events/SimEventsPlugin.cc
@@ -15,15 +15,16 @@
*
*/
+#include "InRegionEventSource.hh"
+#include "ExistenceEventSource.hh"
+#include "OccupiedEventSource.hh"
#include "SimEventsPlugin.hh"
-
using namespace gazebo;
using namespace sdf;
using namespace physics;
-
////////////////////////////////////////////////////////////////////////////////
void SimEventsPlugin::OnModelInfo(ConstModelPtr &_msg)
{
@@ -63,17 +64,22 @@ void SimEventsPlugin::Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
this->sdf = _sdf;
// Create a new transport node
- transport::NodePtr node(new transport::Node());
+ this->node = transport::NodePtr(new transport::Node());
+
// Initialize the node with the world name
- node->Init(_parent->GetName());
+ this->node->Init(_parent->GetName());
+
// Create a publisher on the Rest plugin topic
- pub = node->Advertise<gazebo::msgs::SimEvent>("/gazebo/sim_events");
+ this->pub = this->node->Advertise<gazebo::msgs::SimEvent>(
+ "/gazebo/sim_events");
+
// Subscribe to model spawning
- spawnSub = node->Subscribe("~/model/info",
- &SimEventsPlugin::OnModelInfo,
- this);
+ this->spawnSub = this->node->Subscribe("~/model/info",
+ &SimEventsPlugin::OnModelInfo, this);
+
// detect model deletion
- requestSub = node->Subscribe("~/request", &SimEventsPlugin::OnRequest, this);
+ this->requestSub = this->node->Subscribe("~/request",
+ &SimEventsPlugin::OnRequest, this);
// regions are defined outside of events, so that they can be shared
// between events....
@@ -81,7 +87,7 @@ void SimEventsPlugin::Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
sdf::ElementPtr child = this->sdf->GetElement("region");
while (child)
{
- Region* r = new Region;
+ Region *r = new Region;
r->Load(child);
RegionPtr region;
region.reset(r);
@@ -110,6 +116,11 @@ void SimEventsPlugin::Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
this->world,
this->regions));
}
+ else if (eventType == "occupied")
+ {
+ event.reset(new OccupiedEventSource(this->pub,
+ this->world, this->regions));
+ }
else if (eventType == "existence" )
{
event.reset(new ExistenceEventSource(this->pub, this->world) );
@@ -121,6 +132,7 @@ void SimEventsPlugin::Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
m += "\" of type: \"" + eventType + "\" in SimEvents plugin";
throw SimEventsException(m.c_str());
}
+
if (event)
{
event->Load(child);
@@ -147,7 +159,5 @@ void SimEventsPlugin::Init()
}
}
-
-
// Register this plugin with the simulator
GZ_REGISTER_WORLD_PLUGIN(SimEventsPlugin)
diff --git a/plugins/events/SimEventsPlugin.hh b/plugins/events/SimEventsPlugin.hh
index 306b9b2..081135f 100644
--- a/plugins/events/SimEventsPlugin.hh
+++ b/plugins/events/SimEventsPlugin.hh
@@ -24,8 +24,6 @@
#include <vector>
#include "SimEventsException.hh"
-#include "InRegionEventSource.hh"
-#include "ExistenceEventSource.hh"
#include "SimStateEventSource.hh"
namespace gazebo
@@ -66,6 +64,9 @@ namespace gazebo
/// \brief List of all sim event emitters
private: std::vector<EventSourcePtr> events;
+ /// \brief Node for communication.
+ private: transport::NodePtr node;
+
/// \brief the publisher for SimEvents
private: transport::PublisherPtr pub;
diff --git a/plugins/events/SimStateEventSource.cc b/plugins/events/SimStateEventSource.cc
index fd532eb..86a16a0 100644
--- a/plugins/events/SimStateEventSource.cc
+++ b/plugins/events/SimStateEventSource.cc
@@ -33,17 +33,35 @@ SimStateEventSource::~SimStateEventSource()
}
////////////////////////////////////////////////////////////////////////////////
-void SimStateEventSource::Load(const sdf::ElementPtr &_sdf)
+void SimStateEventSource::Load(const sdf::ElementPtr _sdf)
{
EventSource::Load(_sdf);
+
// Listen to the pause event. This event is broadcast every
// simulation iteration.
this->pauseConnection = event::Events::ConnectPause(
boost::bind(&SimStateEventSource::OnPause, this, _1));
+
+ // Listen to the update event. This event is broadcast every
+ // simulation iteration.
+ this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&SimStateEventSource::OnUpdate, this, _1));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void SimStateEventSource::OnUpdate(const common::UpdateInfo &_info)
+{
+ if (_info.simTime < this->simTime)
+ {
+ std::string json;
+ json = "{\"state\": \"reset\" }";
+ this->Emit(json);
+ }
+ this->simTime = _info.simTime;
}
////////////////////////////////////////////////////////////////////////////////
-void SimStateEventSource::OnPause(bool _pause)
+void SimStateEventSource::OnPause(const bool _pause)
{
std::string json;
if (_pause)
@@ -55,4 +73,5 @@ void SimStateEventSource::OnPause(bool _pause)
json = "{\"state\": \"running\" }";
}
this->Emit(json);
+ this->hasPaused = _pause;
}
diff --git a/plugins/events/SimStateEventSource.hh b/plugins/events/SimStateEventSource.hh
index c872a12..5702b55 100644
--- a/plugins/events/SimStateEventSource.hh
+++ b/plugins/events/SimStateEventSource.hh
@@ -37,7 +37,11 @@ namespace gazebo
/// \brief Load the name of the event from the world file
/// \param[in] _sdf the event element in the world file
- public: virtual void Load(const sdf::ElementPtr &_sdf);
+ public: virtual void Load(const sdf::ElementPtr _sdf);
+
+ /// \brief Update for every time step
+ /// \param[in] _info Update information provided by the server.
+ public: virtual void OnUpdate(const common::UpdateInfo &_info);
/// \brief Callback for the pause event
/// \param[in] _p true if the sim has been paused
@@ -48,6 +52,12 @@ namespace gazebo
/// \brief Pointer to the Gazebo pause event connection
private: event::ConnectionPtr pauseConnection;
+
+ /// \brief Pointer to the update event connection
+ private: event::ConnectionPtr updateConnection;
+
+ /// \brief Simulation time at the previous step
+ private: common::Time simTime;
};
}
diff --git a/plugins/rest_web/CMakeLists.txt b/plugins/rest_web/CMakeLists.txt
new file mode 100644
index 0000000..4dd4e66
--- /dev/null
+++ b/plugins/rest_web/CMakeLists.txt
@@ -0,0 +1,61 @@
+link_directories(
+ ${GAZEBO_LIBRARY_DIRS}
+ ${CMAKE_CURRENT_BINARY_DIR}
+ )
+
+set (server_inc
+ RestApi.hh
+ RestException.hh
+ RestWebPlugin.hh
+ )
+
+
+set (server_src
+ RestApi.cc
+ RestWebPlugin.cc
+ )
+
+set (client_inc
+ RestUiLoginDialog.hh
+ RestUiLogoutDialog.hh
+ RestUiPlugin.hh
+ RestUiWidget.hh
+ )
+
+
+set (client_src
+ RestUiLoginDialog.cc
+ RestUiLogoutDialog.cc
+ RestUiPlugin.cc
+ RestUiWidget.cc
+ )
+
+set (qt_headers
+ RestUiWidget.hh
+ RestUiLoginDialog.hh
+ RestUiLogoutDialog.hh
+)
+
+include ( ${QT_USE_FILE} )
+
+add_definitions(${QT_DEFINITIONS})
+
+QT4_WRAP_CPP(headers_MOC ${qt_headers})
+
+
+
+include_directories(
+ ${GAZEBO_INCLUDE_DIRS}
+ )
+
+
+
+add_library(RestWebPlugin SHARED ${server_src} )
+target_link_libraries(RestWebPlugin ${GAZEBO_libraries} gazebo_msgs)
+install (TARGETS RestWebPlugin DESTINATION ${GAZEBO_PLUGIN_INSTALL_DIR})
+gz_install_includes("plugins" ${server_inc})
+
+add_library(RestUiPlugin SHARED ${client_src} ${headers_MOC})
+target_link_libraries(RestUiPlugin ${GAZEBO_libraries} gazebo_msgs)
+install (TARGETS RestUiPlugin DESTINATION ${GAZEBO_PLUGIN_INSTALL_DIR})
+gz_install_includes("plugins" ${client_inc})
diff --git a/plugins/rest_web/RestApi.cc b/plugins/rest_web/RestApi.cc
new file mode 100644
index 0000000..128541b
--- /dev/null
+++ b/plugins/rest_web/RestApi.cc
@@ -0,0 +1,380 @@
+/*
+ * 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 <iostream>
+#include <cstring>
+#include <stdlib.h>
+#include <curl/curl.h>
+#include <inttypes.h>
+
+#include "RestApi.hh"
+
+using namespace gazebo;
+
+// This code is adapted from the curl C examples
+// (http://curl.haxx.se/libcurl/c/)
+// mostly modified to adhere to the Gazebo code check tool.
+// It implements the following features:
+// - HTML POST, sending and receiving data
+// - Authentication (Basic Auth)
+// - https (SSL, but accepting non signed certificates)
+
+// You can enable this flag to get more curl details
+// about each request (will provide SSL negociation and TCP dumps of the
+// requests and responses)
+bool trace_requests = false;
+
+struct data
+{
+ // 1 or 0
+ char trace_ascii;
+};
+
+/////////////////////////////////////////////////
+static void DumpRequest(const char *_text,
+ FILE *_stream,
+ unsigned char *_ptr,
+ size_t _size,
+ char _nohex)
+{
+ size_t i;
+ size_t c;
+ unsigned int width = 0x10;
+
+ if (_nohex)
+ {
+ // without the hex output, we can fit more on screen
+ width = 0x40;
+ }
+ int64_t s = _size;
+ fprintf(_stream,
+ "%s, %10.10" PRId64 " bytes (0x%8.8" PRIx64 ")\n", _text, s, s);
+
+ for (i = 0; i < _size; i += width)
+ {
+ fprintf(_stream, "%4.4lx: ", i);
+ if (!_nohex)
+ {
+ // hex not disabled, show it
+ for (c = 0; c < width; ++c)
+ {
+ if (i+c < _size)
+ fprintf(_stream, "%02x ", _ptr[i+c]);
+ else
+ fputs(" ", _stream);
+ }
+ }
+
+ for (c = 0; (c < width) && (i+c < _size); ++c)
+ {
+ // check for 0D0A; if found, skip past and start a new line of output
+ if (_nohex && (i+c+1 < _size) && _ptr[i+c] == 0x0D && _ptr[i+c+1] == 0x0A)
+ {
+ i+=(c+2-width);
+ break;
+ }
+ fprintf(_stream, "%c",
+ (_ptr[i+c] >= 0x20) && (_ptr[i+c] < 0x80)?_ptr[i+c]:'.');
+ // check again for 0D0A, to avoid an extra \n if it's at width
+ if (_nohex && (i+c+2 < _size) && _ptr[i+c+1] == 0x0D &&
+ _ptr[i+c+2] == 0x0A)
+ {
+ i+=(c+3-width);
+ break;
+ }
+ }
+ fputc('\n', _stream);
+ }
+ fflush(_stream);
+}
+
+/////////////////////////////////////////////////
+// Callback given to curl that outputs data about the request
+static int TraceRequest(CURL *_handle,
+ curl_infotype _type,
+ char *_data,
+ size_t _size,
+ void *_userp)
+{
+ struct data *config = (struct data *)_userp;
+ const char *text;
+
+ // prevent compiler warning
+ (void)_handle;
+
+ switch (_type)
+ {
+ case CURLINFO_TEXT:
+ if (trace_requests)
+ fprintf(stderr, "== Info: %s", _data);
+ // in case a new one is introduced to shock us
+ default:
+ return 0;
+ case CURLINFO_HEADER_OUT:
+ text = "=> Send header";
+ break;
+ case CURLINFO_DATA_OUT:
+ text = "=> Send data";
+ break;
+ case CURLINFO_SSL_DATA_OUT:
+ text = "=> Send SSL data";
+ break;
+ case CURLINFO_HEADER_IN:
+ text = "<= Recv header";
+ break;
+ case CURLINFO_DATA_IN:
+ text = "<= Recv data";
+ break;
+ case CURLINFO_SSL_DATA_IN:
+ text = "<= Recv SSL data";
+ break;
+ }
+
+ if (trace_requests)
+ {
+ DumpRequest(text, stderr,
+ (unsigned char *)(_data), _size, config->trace_ascii);
+ }
+ return 0;
+}
+
+// private data structure used to
+// read libcurl response
+struct MemoryStruct {
+ char *memory;
+ size_t size;
+};
+
+/////////////////////////////////////////////////
+// callback for libcurl when data is read from http response
+static size_t WriteMemoryCallback(void *_contents,
+ size_t _size,
+ size_t _nmemb,
+ void *_userp)
+{
+ size_t realsize = _size * _nmemb;
+ struct MemoryStruct *mem = (struct MemoryStruct *)_userp;
+ size_t newsize = mem->size + realsize + 1;
+ mem->memory = static_cast<char *> (realloc(mem->memory, newsize));
+ if (mem->memory == NULL)
+ {
+ // out of memory!
+ gzerr << "not enough memory (realloc returned NULL)" << std::endl;
+ return 0;
+ }
+ memcpy(&(mem->memory[mem->size]), _contents, realsize);
+ mem->size += realsize;
+ mem->memory[mem->size] = 0;
+ return realsize;
+}
+
+/////////////////////////////////////////////////
+RestApi::RestApi()
+ :isLoggedIn(false)
+{
+}
+
+/////////////////////////////////////////////////
+RestApi::~RestApi()
+{
+ curl_global_cleanup();
+}
+
+/////////////////////////////////////////////////
+void RestApi::PostJsonData(const char *_route, const char *_json)
+{
+ Post post;
+ post.route = _route;
+ post.json = _json;
+ {
+ boost::mutex::scoped_lock lock(this->postsMutex);
+ this->posts.push_back(post);
+ }
+ SendUnpostedPosts();
+}
+
+/////////////////////////////////////////////////
+std::string RestApi::Login(const std::string &_urlStr,
+ const std::string &_route,
+ const std::string &_userStr,
+ const std::string &_passStr)
+{
+ this->isLoggedIn = false;
+ this->url = _urlStr;
+ this->user = _userStr;
+ this->pass = _passStr;
+
+ // at this point we want to test the (user supplied) login data
+ // so we're hitting the server on the login route ('/login')
+ this->loginRoute = _route;
+ std::string resp;
+
+ gzmsg << "login route: " << this->loginRoute << std::endl;
+ resp = this->Request(loginRoute, "");
+ gzmsg << "login response: " << resp << std::endl;
+
+ this->isLoggedIn = true;
+ this->SendUnpostedPosts();
+ return resp;
+}
+
+/////////////////////////////////////////////////
+void RestApi::Logout()
+{
+ this->isLoggedIn = false;
+ gzmsg << "Logout" << std::endl;
+}
+
+/////////////////////////////////////////////////
+void RestApi::SendUnpostedPosts()
+{
+ if (this->isLoggedIn)
+ {
+ while (!this->posts.empty())
+ {
+ Post post;
+ {
+ boost::mutex::scoped_lock lock(this->postsMutex);
+ post = this->posts.front();
+
+ // You can generate a similar request on the cmd line like so:
+ // curl --verbose --connect-timeout 5 -X POST
+ // -H \"Content-Type: application/json \" -k --user"
+ this->Request(post.route, post.json);
+ this->posts.pop_front();
+ }
+ }
+ }
+ else
+ {
+ gzmsg << posts.size() << " post(s) queued to be sent" << std::endl;
+ }
+}
+
+/////////////////////////////////////////////////
+std::string RestApi::GetUser() const
+{
+ return this->user;
+}
+
+/////////////////////////////////////////////////
+std::string RestApi::Request(const std::string &_reqUrl,
+ const std::string &_postJsonStr)
+{
+ if (this->url.empty())
+ throw RestException("A URL must be specified for web service");
+
+ if (this->user.empty())
+ {
+ std::string e = "No user specified for the web service. Please login.";
+ throw RestException(e.c_str());
+ }
+ // build full url (with server)
+ std::string path = url + _reqUrl;
+ CURL *curl = curl_easy_init();
+ curl_easy_setopt(curl, CURLOPT_URL, path.c_str() );
+
+ // in case things go wrong
+ if (trace_requests)
+ {
+ gzmsg << "RestApi::Request" << std::endl;
+ gzmsg << " path: " << path << std::endl;
+ gzmsg << " data: " << _postJsonStr << std::endl;
+ gzmsg << std::endl;
+
+ struct data config;
+ config.trace_ascii = 1; // enable ascii tracing
+ curl_easy_setopt(curl, CURLOPT_DEBUGFUNCTION, TraceRequest);
+ curl_easy_setopt(curl, CURLOPT_DEBUGDATA, &config);
+
+ curl_easy_setopt(curl, CURLOPT_VERBOSE, 1L);
+ curl_easy_setopt(curl, CURLOPT_HEADER, 1L);
+ }
+
+ struct MemoryStruct chunk;
+ // will be grown as needed by the realloc above
+ chunk.memory = static_cast<char*>(malloc(1));
+ chunk.size = 0; // no data at this point
+ bool secure = false;
+ if (!secure)
+ {
+ // skip peer verification
+ curl_easy_setopt(curl, CURLOPT_SSL_VERIFYPEER, 0L);
+ // skip host verification
+ curl_easy_setopt(curl, CURLOPT_SSL_VERIFYHOST, 0L);
+ }
+
+ // send all data to this function
+ curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, WriteMemoryCallback);
+ // we pass our 'chunk' struct to the callback function
+ curl_easy_setopt(curl,
+ CURLOPT_WRITEDATA,
+ static_cast<void *>(&chunk));
+
+ // some servers don't like requests that are made without a user-agent
+ // field, so we provide one
+ curl_easy_setopt(curl, CURLOPT_USERAGENT, "libcurl-agent/1.0");
+
+ // set user name and password for the authentication
+ curl_easy_setopt(curl, CURLOPT_HTTPAUTH, CURLAUTH_BASIC);
+ std::string userpass = this->user + ":" + this->pass;
+ curl_easy_setopt(curl, CURLOPT_USERPWD, userpass.c_str());
+
+ // connection timeout 10 sec
+ curl_easy_setopt(curl, CURLOPT_CONNECTTIMEOUT, 10);
+
+ // is this a POST?
+ struct curl_slist *slist = NULL;
+ if (!_postJsonStr.empty())
+ {
+ curl_easy_setopt(curl, CURLOPT_UPLOAD, 0L); // disable PUT
+ curl_easy_setopt(curl, CURLOPT_POST, 1); // enable POST
+ curl_easy_setopt(curl, CURLOPT_POSTFIELDS, _postJsonStr.c_str());
+
+ slist = curl_slist_append(slist, "Content-Type: application/json");
+ slist = curl_slist_append(slist, "charsets: utf-8");
+ curl_easy_setopt(curl, CURLOPT_HTTPHEADER, slist);
+ }
+
+ CURLcode res;
+ res = curl_easy_perform(curl);
+
+ // get HTTP response code
+ int64_t http_code = 0;
+
+ curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &http_code);
+
+ curl_easy_cleanup(curl);
+ if (res != CURLE_OK)
+ {
+ gzerr << "Request to " << url << " failed: ";
+ gzerr << curl_easy_strerror(res) << std::endl;
+ throw RestException(curl_easy_strerror(res));
+ }
+ // copy the data into a string
+ std::string response(chunk.memory, chunk.size);
+
+ if (http_code != 200)
+ {
+ gzerr << "Request to " << url << " error: " << response << std::endl;
+ throw RestException(response.c_str());
+ }
+ // clean up
+ curl_slist_free_all(slist);
+ if (chunk.memory)
+ free(chunk.memory);
+ return response;
+}
diff --git a/plugins/rest_web/RestApi.hh b/plugins/rest_web/RestApi.hh
new file mode 100644
index 0000000..71617c5
--- /dev/null
+++ b/plugins/rest_web/RestApi.hh
@@ -0,0 +1,106 @@
+/*
+ * 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_REST_API_HH_
+#define _GAZEBO_REST_API_HH_
+
+#include <string>
+#include <list>
+#include <gazebo/common/Console.hh>
+
+#include "RestException.hh"
+
+namespace gazebo
+{
+ /// \class RestApi RestApi.hh RestApi.hh
+ /// \brief REST interface
+ class RestApi
+ {
+ /// \brief Constructor
+ public: RestApi();
+
+ /// \brief Destructor
+ public: virtual ~RestApi();
+
+ /// \brief Connects to the REST service.
+ /// \param[in] _url The web service url
+ /// \param[in] _route The route on the server
+ /// \param[in] _user The user name
+ /// \param[in] _pass The user password
+ /// \return The response message from the REST server
+ public: std::string Login(const std::string &_url,
+ const std::string &_route,
+ const std::string &_user,
+ const std::string &_pass);
+
+ /// \brief Stops sending messages to the REST service.
+ /// Does not do anything if not logged in. Once logged out,
+ /// a new call to Login has to be made to resume sending messages.
+ public: void Logout();
+
+ /// \brief Notify the service with a http POST
+ /// \param[in] _route on the web server
+ /// \param[in] _json the data to send to the server
+ public: void PostJsonData(const char *_route, const char *_json);
+
+ /// \brief Returns the username
+ /// \return The user name
+ public: std::string GetUser() const;
+
+ /// \brief A Request/Respone (can be used for GET and POST)
+ /// \param[in] _requestUrl The request url.
+ /// \param[in] _postStr The data to post
+ /// \throws RestException When this->url or this->user are empty, and
+ /// when the request failed.
+ /// \return The web server response
+ private: std::string Request(const std::string &_requestUrl,
+ const std::string &_postStr);
+
+ /// \brief Sends unposted posts
+ private: void SendUnpostedPosts();
+
+ /// \brief Login information: REST service host url
+ private: std::string url;
+
+ /// \brief Login information: REST service username
+ private: std::string user;
+
+ /// \brief Login information: REST service password
+ private: std::string pass;
+
+ /// \brief Login information: login route
+ private: std::string loginRoute;
+
+ /// \brief True when a previous Login attempt was successful
+ private: bool isLoggedIn;
+
+ /// \brief A post: what (json) and where (route)
+ private: struct Post
+ {
+ std::string route;
+ std::string json;
+ };
+
+ /// \brief List of unposted posts. Posts await when isLoggedIn is false
+ private: std::list<Post> posts;
+
+ /// \brief A mutex to ensure integrity of the post list
+ private: boost::mutex postsMutex;
+ };
+}
+
+#endif
diff --git a/plugins/rest_web/RestException.hh b/plugins/rest_web/RestException.hh
new file mode 100644
index 0000000..64a9468
--- /dev/null
+++ b/plugins/rest_web/RestException.hh
@@ -0,0 +1,33 @@
+/*
+ * 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 _REST_EXCEPTION_HH_
+#define _REST_EXCEPTION_HH_
+
+#include <stdexcept>
+
+namespace gazebo
+{
+ // Basic exception class that inherits from the standard runtime error.
+ class RestException : public std::runtime_error
+ {
+ public: RestException(const char *_m):std::runtime_error(_m) {}
+ };
+}
+
+#endif
+
diff --git a/plugins/rest_web/RestUiLoginDialog.cc b/plugins/rest_web/RestUiLoginDialog.cc
new file mode 100644
index 0000000..06cd185
--- /dev/null
+++ b/plugins/rest_web/RestUiLoginDialog.cc
@@ -0,0 +1,124 @@
+/*
+ * 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 _REST_UI_LOGIN_DIALOG_HH_
+#define _REST_UI_LOGIN_DIALOG_HH_
+
+#include <iostream>
+#include <curl/curl.h>
+
+#include "RestUiLoginDialog.hh"
+#include "RestUiWidget.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+
+/////////////////////////////////////////////////
+RestUiLoginDialog::RestUiLoginDialog(QWidget *_parent,
+ const std::string &_title,
+ const std::string &_urlLabel,
+ const std::string &_defaultUrl)
+ : QDialog(_parent), url(_defaultUrl.c_str())
+{
+ setWindowTitle(tr(_title.c_str()));
+ setModal(true);
+
+ labelUrl = new QLabel(this);
+ labelUrl->setText(tr(_urlLabel.c_str()));
+ editUrl = new QLineEdit(this);
+ editUrl->setText(tr(url.c_str()));
+ editUrl->setFixedWidth(400);
+ labelUrl->setBuddy(editUrl);
+
+ labelUsername = new QLabel(this);
+ labelUsername->setText(tr("Username"));
+ editUsername = new QLineEdit(this);
+ editUsername->setFocus();
+ labelUsername->setBuddy(editUsername);
+
+ labelPassword = new QLabel(this);
+ labelPassword->setText(tr("Password"));
+ editPassword = new QLineEdit(this);
+ editPassword->setEchoMode(QLineEdit::Password);
+ labelPassword->setBuddy(editPassword);
+
+ labelInfo = new QLabel(this);
+
+ buttons = new QDialogButtonBox(this);
+ buttons->addButton(QDialogButtonBox::Ok);
+ buttons->button(QDialogButtonBox::Ok)->setText("Login");
+ buttons->button(QDialogButtonBox::Ok)->setDefault(true);
+ buttons->addButton(QDialogButtonBox::Cancel);
+
+ // place components
+ QGridLayout *formGridLayout = new QGridLayout(this);
+
+ formGridLayout->addWidget(labelUrl, 0, 0, 1, 2);
+ formGridLayout->addWidget(editUrl, 1, 0, 1, 2);
+ formGridLayout->addWidget(labelUsername, 2, 0);
+ formGridLayout->addWidget(editUsername, 2, 1);
+ formGridLayout->addWidget(labelPassword, 3, 0);
+ formGridLayout->addWidget(editPassword, 3, 1);
+ formGridLayout->addWidget(labelInfo, 4, 0, 1, 2);
+ formGridLayout->addWidget(buttons, 5, 0, 1, 2);
+
+ setLayout(formGridLayout);
+
+ connect(buttons->button(QDialogButtonBox::Cancel),
+ SIGNAL(clicked()),
+ this,
+ SLOT(close()));
+
+ connect(buttons->button(QDialogButtonBox::Ok),
+ SIGNAL(clicked()),
+ this,
+ SLOT(SlotAcceptLogin()));
+}
+
+/////////////////////////////////////////////////
+std::string RestUiLoginDialog::GetUsername() const
+{
+ return this->username;
+}
+
+/////////////////////////////////////////////////
+std::string RestUiLoginDialog::GetPassword() const
+{
+ return this->password;
+}
+
+/////////////////////////////////////////////////
+std::string RestUiLoginDialog::GetUrl() const
+{
+ return this->url;
+}
+
+/////////////////////////////////////////////////
+void RestUiLoginDialog::SlotAcceptLogin()
+{
+ QString user = editUsername->text();
+ QString pass = editPassword->text();
+ QString u = editUrl->text();
+
+ this->username = user.toStdString();
+ this->password = pass.toStdString();
+ this->url = u.toStdString();
+ accept();
+}
+
+#endif
diff --git a/plugins/rest_web/RestUiLoginDialog.hh b/plugins/rest_web/RestUiLoginDialog.hh
new file mode 100644
index 0000000..05e15f9
--- /dev/null
+++ b/plugins/rest_web/RestUiLoginDialog.hh
@@ -0,0 +1,108 @@
+/*
+ * 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 _REST_LOGIN_DIALOG_HH_
+#define _REST_LOGIN_DIALOG_HH_
+
+#include <string>
+
+#include <gazebo/gui/qt.h>
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ class RestUiWidget;
+
+ /// \class RestUiLoginDialog RestUiLoginDialog.hh RestUiLoginDialog.hh
+ /// \brief Provides a means to login to a webservice
+ class GAZEBO_VISIBLE RestUiLoginDialog : public QDialog
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ /// \param[in] _parent Parent QWidget
+ /// \param[in] _title The dialog window title bar text
+ /// \param[in] _url Label the title of the url (ex: super webservice url)
+ /// \param[in] _defaultUrl Url text for the url (ex: https://superweb.com)
+ public: RestUiLoginDialog(QWidget *_parent,
+ const std::string &_title,
+ const std::string &_urlLabel,
+ const std::string &_defautlUrl);
+
+ /// \brief Emitted when the user presses the login button
+ /// \param[in] _url The web server url
+ /// \param[in] _username The user name
+ /// \param[in] _password The user password
+ signals: void AcceptLogin(QString &_url,
+ QString &_username,
+ QString &_password);
+
+ /// \brief Getter for User name (of the basic auth REST service)
+ /// \return User name
+ public: std::string GetUsername() const;
+
+ /// \brief Getter for the password
+ /// \return The password
+ public: std::string GetPassword() const;
+
+ /// \brief Getter for the Url
+ /// \return The url for the site (ex: https://yoursite.com:4000)
+ public: std::string GetUrl() const;
+
+ /// \brief Slot for the AcceptLogin event
+ public slots: void SlotAcceptLogin();
+
+ /// \brief A label for the url component that appears on the
+ /// login widget above
+ private: QLabel *labelUrl;
+
+ /// \brief A label for the username component
+ private: QLabel *labelUsername;
+
+ /// \brief A label for the password
+ private: QLabel *labelPassword;
+
+ /// \brief A text field for the default url
+ private: QLineEdit *editUrl;
+
+ /// \brief A text field to enter the user name
+ private: QLineEdit *editUsername;
+
+ /// \brief A text field to enter the password
+ private: QLineEdit *editPassword;
+
+ /// \brief A label to display errors and information
+ private: QLabel *labelInfo;
+
+ /// \brief The standard dialog buttons
+ private: QDialogButtonBox *buttons;
+
+ /// \brief The username
+ private: std::string username;
+
+ /// \brief The user password
+ private: std::string password;
+
+ /// \brief The web server url
+ private: std::string url;
+ };
+ }
+}
+
+#endif
diff --git a/plugins/rest_web/RestUiLogoutDialog.cc b/plugins/rest_web/RestUiLogoutDialog.cc
new file mode 100644
index 0000000..e336ecf
--- /dev/null
+++ b/plugins/rest_web/RestUiLogoutDialog.cc
@@ -0,0 +1,75 @@
+/*
+ * 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_REST_UI_LOGIN_DIALOG_HH_
+#define _GAZEBO_REST_UI_LOGIN_DIALOG_HH_
+
+#include "RestUiLogoutDialog.hh"
+#include "RestUiWidget.hh"
+
+using namespace gazebo;
+using namespace gui;
+
+/////////////////////////////////////////////////
+RestUiLogoutDialog::RestUiLogoutDialog(QWidget *_parent,
+ const std::string &_defaultUrl)
+ : QDialog(_parent)
+{
+ this->setWindowTitle(tr("Logout"));
+ this->setModal(true);
+
+ /// A label for the url component that appears on the
+ /// logout dialog
+ QLabel *labelUrl;
+ /// The standard dialog buttons
+ QDialogButtonBox *buttons;
+ labelUrl = new QLabel(this);
+ labelUrl->setText(tr(_defaultUrl.c_str()));
+
+ buttons = new QDialogButtonBox(this);
+ buttons->addButton(QDialogButtonBox::Ok);
+ buttons->button(QDialogButtonBox::Ok)->setText("Logout");
+ buttons->button(QDialogButtonBox::Ok)->setDefault(true);
+ buttons->addButton(QDialogButtonBox::Cancel);
+
+ // place components
+ QGridLayout *formGridLayout = new QGridLayout(this);
+
+ formGridLayout->addWidget(labelUrl, 0, 0, 1, 2);
+ formGridLayout->addWidget(buttons, 5, 0, 1, 2);
+
+ this->setLayout(formGridLayout);
+
+ this->connect(buttons->button(QDialogButtonBox::Cancel),
+ SIGNAL(clicked()),
+ this,
+ SLOT(close()));
+
+ this->connect(buttons->button(QDialogButtonBox::Ok),
+ SIGNAL(clicked()),
+ this,
+ SLOT(SlotAcceptLogout()));
+}
+
+/////////////////////////////////////////////////
+void RestUiLogoutDialog::SlotAcceptLogout()
+{
+ // close the dialog with success exit code
+ accept();
+}
+
+#endif
diff --git a/plugins/rest_web/RestUiLogoutDialog.hh b/plugins/rest_web/RestUiLogoutDialog.hh
new file mode 100644
index 0000000..5b16db4
--- /dev/null
+++ b/plugins/rest_web/RestUiLogoutDialog.hh
@@ -0,0 +1,47 @@
+/*
+ * 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_REST_LOGOUT_DIALOG_HH_
+#define _GAZEBO_REST_LOGOUT_DIALOG_HH_
+
+#include <string>
+
+#include <gazebo/gui/qt.h>
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace gui
+ {
+ /// \brief Provides a means to logout from a webservice
+ class GAZEBO_VISIBLE RestUiLogoutDialog : public QDialog
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ /// \param[in] _parent Parent QWidget
+ /// \param[in] _defaultUrl Url text for the url (ex: https://superweb.com)
+ public: RestUiLogoutDialog(QWidget *_parent,
+ const std::string &_defautlUrl);
+
+ /// \brief Slot for the AcceptLogin event
+ public slots: void SlotAcceptLogout();
+ };
+ }
+}
+
+#endif
diff --git a/plugins/rest_web/RestUiPlugin.cc b/plugins/rest_web/RestUiPlugin.cc
new file mode 100644
index 0000000..9d5f85d
--- /dev/null
+++ b/plugins/rest_web/RestUiPlugin.cc
@@ -0,0 +1,126 @@
+/*
+ * Copyright 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "gazebo/gui/MainWindow.hh"
+#include "gazebo/gui/GuiIface.hh"
+#include "gazebo/gui/GuiEvents.hh"
+
+#include "RestUiPlugin.hh"
+
+using namespace gazebo;
+
+/////////////////////////////////////////////////
+RestUiPlugin::RestUiPlugin()
+: menuTitle("Web service"),
+ loginTitle("Web service login"),
+ urlLabel("url"),
+ defaultUrl("https://"),
+ widget(NULL)
+{
+}
+
+/////////////////////////////////////////////////
+void RestUiPlugin::Load(int _argc, char ** _argv)
+{
+ gzmsg << "RestUiPlugin: cmd line arguments (menu=, title=, label=, url=)\n";
+ for (int i = 0; i < _argc; ++i)
+ {
+ std::string arg = _argv[i];
+ if (arg.find("menu=") == 0)
+ {
+ this->menuTitle = arg.substr(5);
+ }
+ else if (arg.find("title=") == 0 )
+ {
+ this->loginTitle = arg.substr(6);
+ }
+ else if (arg.find("label=") == 0 )
+ {
+ this->urlLabel = arg.substr(6);
+ }
+ else if (arg.find("url=") == 0 )
+ {
+ this->defaultUrl = arg.substr(4);
+ }
+ }
+ gzmsg << " menu title: " << this->menuTitle << std::endl;
+ gzmsg << " Login window title: " << this->loginTitle << std::endl;
+ gzmsg << " Login window label: " << this->urlLabel << std::endl;
+ gzmsg << " Web servide URL: " << this->defaultUrl << std::endl;
+}
+
+/////////////////////////////////////////////////
+void RestUiPlugin::Init()
+{
+ // Connect to the sensor update event.
+ this->connections.push_back(
+ gui::Events::ConnectMainWindowReady(
+ boost::bind(&RestUiPlugin::OnMainWindowReady, this)));
+
+ this->connections.push_back(
+ event::Events::ConnectPreRender(
+ boost::bind(&RestUiPlugin::Update, this)));
+}
+
+/////////////////////////////////////////////////
+void RestUiPlugin::Update()
+{
+ if (this->widget)
+ {
+ this->widget->Update();
+ }
+}
+
+/////////////////////////////////////////////////
+void RestUiPlugin::OnMainWindowReady()
+{
+ // add menu for this plugin
+ std::string menuStr("&");
+ menuStr += this->menuTitle;
+ QMenu *menu = new QMenu(QString(menuStr.c_str()));
+
+ QAction *loginAct = new QAction(QString("&Login"), menu);
+ loginAct->setStatusTip(QString("Login to web service"));
+ QAction *logoutAct = new QAction(QString("Log&out"), menu);
+ logoutAct->setStatusTip(QString("Logout from web service"));
+ logoutAct->setEnabled(false);
+
+ gui::MainWindow *mainWindow = gui::get_main_window();
+ // create a global widget instance, to act as a global QT object
+ // the RestUiPlugin class is not a QT object
+ this->widget = new RestUiWidget(mainWindow,
+ *loginAct,
+ *logoutAct,
+ this->menuTitle.c_str(),
+ this->loginTitle.c_str(),
+ this->urlLabel.c_str(),
+ this->defaultUrl.c_str());
+
+ QObject::connect(loginAct, SIGNAL(triggered()),
+ this->widget, SLOT(Login()));
+ menu->addAction(loginAct);
+
+ QObject::connect(logoutAct, SIGNAL(triggered()),
+ this->widget, SLOT(Logout()));
+
+ menu->addAction(logoutAct);
+
+ mainWindow->AddMenu(menu);
+}
+
+// Register this plugin with the simulator
+GZ_REGISTER_SYSTEM_PLUGIN(RestUiPlugin)
diff --git a/plugins/rest_web/RestUiPlugin.hh b/plugins/rest_web/RestUiPlugin.hh
new file mode 100644
index 0000000..6e95c36
--- /dev/null
+++ b/plugins/rest_web/RestUiPlugin.hh
@@ -0,0 +1,77 @@
+/*
+ * Copyright 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef _REST_UI_PLUGIN_HH_
+#define _REST_UI_PLUGIN_HH_
+
+#include <string>
+#include <vector>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/gui/qt.h>
+#include <gazebo/util/system.hh>
+
+#include "RestUiWidget.hh"
+
+namespace gazebo
+{
+ /// \class RestUiPlugin RestUiPlugin.hh RestUiPlugin.hh
+ /// \brief REST user interface plugin
+ class GAZEBO_VISIBLE RestUiPlugin : public SystemPlugin
+ {
+ /// \brief Constructor
+ public: RestUiPlugin();
+
+ /// \brief Destructor
+ public: virtual ~RestUiPlugin() = default;
+
+ /// \brief Called when plugin is loaded.
+ /// \param[in] _argc Arguments count
+ /// \param[in] _argv Argument vector
+ public: virtual void Load(int _argc, char **_argv);
+
+ /// \brief Plugin initialization.
+ private: virtual void Init();
+
+ /// \brief Called by Gazebo after the main window has been setup.
+ private: void OnMainWindowReady();
+
+ /// \brief Called from the GUI thread before rendering.
+ private: void Update();
+
+ /// \brief Callbacks (to connect to the main window ready event).
+ private: std::vector<event::ConnectionPtr> connections;
+
+ /// \brief The Title, used for the menu item.
+ private: std::string menuTitle;
+
+ /// \brief The login title.
+ private: std::string loginTitle;
+
+ /// \brief The url description.
+ private: std::string urlLabel;
+
+ /// \brief The default url (if fixed).
+ private: std::string defaultUrl;
+
+ /// \brief The widget.
+ private: RestUiWidget *widget;
+ };
+}
+
+#endif
+
diff --git a/plugins/rest_web/RestUiWidget.cc b/plugins/rest_web/RestUiWidget.cc
new file mode 100644
index 0000000..7ce729c
--- /dev/null
+++ b/plugins/rest_web/RestUiWidget.cc
@@ -0,0 +1,121 @@
+/*
+ * Copyright 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <QMessageBox>
+#include "RestUiWidget.hh"
+
+using namespace gazebo;
+
+/////////////////////////////////////////////////
+RestUiWidget::RestUiWidget(QWidget *_parent,
+ QAction &_login,
+ QAction &_logout,
+ const std::string &_menuTitle,
+ const std::string &_loginTitle,
+ const std::string &_urlLabel,
+ const std::string &_defautlUrl)
+ : QWidget(_parent),
+ loginMenuAction(_login),
+ logoutMenuAction(_logout),
+ title(_menuTitle),
+ node(new gazebo::transport::Node()),
+ logoutDialog(this, _defautlUrl),
+ loginDialog(this, _loginTitle, _urlLabel, _defautlUrl)
+{
+ this->node->Init();
+ this->loginPub = node->Advertise<gazebo::msgs::RestLogin>(
+ "/gazebo/rest/rest_login");
+ this->logoutPub = node->Advertise<gazebo::msgs::RestLogout>(
+ "/gazebo/rest/rest_logout");
+ this->errorSub = node->Subscribe("/gazebo/rest/rest_error",
+ &RestUiWidget::OnResponse,
+ this);
+}
+
+/////////////////////////////////////////////////
+void RestUiWidget::Logout()
+{
+ if (this->logoutDialog.exec() != QDialog::Rejected)
+ {
+ gazebo::msgs::RestLogout msg;
+ std::string url = this->loginDialog.GetUrl();
+ msg.set_url(url);
+ gzmsg << "Logging out from: " << url << std::endl;
+ this->logoutPub->Publish(msg);
+ this->loginMenuAction.setEnabled(true);
+ this->logoutMenuAction.setEnabled(false);
+ }
+}
+
+/////////////////////////////////////////////////
+void RestUiWidget::Login()
+{
+ if (this->loginDialog.exec() != QDialog::Rejected)
+ {
+ gazebo::msgs::RestLogin msg;
+ msg.set_url(this->loginDialog.GetUrl());
+ msg.set_username(this->loginDialog.GetUsername());
+ msg.set_password(this->loginDialog.GetPassword());
+ this->loginPub->Publish(msg);
+ this->loginMenuAction.setEnabled(false);
+ this->logoutMenuAction.setEnabled(true);
+ }
+}
+
+/////////////////////////////////////////////////
+void RestUiWidget::OnResponse(ConstRestErrorPtr &_msg)
+{
+ gzerr << "Error received:" << std::endl;
+ gzerr << " type: " << _msg->type() << std::endl;
+ gzerr << " msg: " << _msg->msg() << std::endl;
+ // add msg to queue for later processing from the GUI thread
+ this->msgRespQ.push_back(_msg);
+}
+
+/////////////////////////////////////////////////
+void RestUiWidget::Update()
+{
+ // Login problem?
+ while (!this->msgRespQ.empty())
+ {
+ ConstRestErrorPtr msg = this->msgRespQ.front();
+ std::string msgStr = msg->msg();
+ this->msgRespQ.pop_front();
+
+ // look for login error, and reenable the login menu if necessary
+ if (msgStr.find("There was a problem trying to login to the server") == 0)
+ {
+ this->loginMenuAction.setEnabled(true);
+ this->logoutMenuAction.setEnabled(false);
+ }
+
+ if (msg->type() == "Error")
+ {
+ msgStr += "\n\nIf the server is not available, ";
+ msgStr += "logout to hide theses messages.";
+ QMessageBox::critical(this,
+ tr(this->title.c_str()),
+ tr(msgStr.c_str()));
+ }
+ else
+ {
+ QMessageBox::information(this,
+ tr(this->title.c_str()),
+ tr(msgStr.c_str()));
+ }
+ }
+}
diff --git a/plugins/rest_web/RestUiWidget.hh b/plugins/rest_web/RestUiWidget.hh
new file mode 100644
index 0000000..8bc7527
--- /dev/null
+++ b/plugins/rest_web/RestUiWidget.hh
@@ -0,0 +1,105 @@
+/*
+ * Copyright 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef _GAZEBO_RESTUI_WIDGET_HH_
+#define _GAZEBO_RESTUI_WIDGET_HH_
+
+#include <string>
+#include <list>
+
+// See: https://bugreports.qt-project.org/browse/QTBUG-22829
+#ifndef Q_MOC_RUN
+# include <gazebo/gazebo.hh>
+#endif
+#include <gazebo/util/system.hh>
+#include "RestUiLoginDialog.hh"
+#include "RestUiLogoutDialog.hh"
+
+namespace gazebo
+{
+ /// \class RestUiWidget RestUiWidget.hh RestUiWidget.hh
+ /// \brief REST user interface widget
+ class GAZEBO_VISIBLE RestUiWidget : public QWidget
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ /// \param[in] _parent Parent widget.
+ /// \param[in] _menuTitle Menu title.
+ /// \param[in] _loginTitle Login title.
+ /// \param[in] _urlLabel Url label.
+ /// \param[in] _defaultUrl Default url.
+ public: RestUiWidget(QWidget *_parent,
+ QAction &_login,
+ QAction &_logout,
+ const std::string &_menuTitle,
+ const std::string &_loginTitle,
+ const std::string &_urlLabel,
+ const std::string &_defautlUrl);
+
+ /// \brief Destructor
+ public: virtual ~RestUiWidget() = default;
+
+ /// \brief QT callback (from the window menu)
+ public slots: void Login();
+
+ /// \brief QT callback (from the window menu)
+ public slots: void Logout();
+
+ /// \brief Called before rendering, from the GUI thread this is called from
+ /// the plugin's update.
+ public: void Update();
+
+ /// \brief Called everytime a response message is received.
+ /// \param[in] _msg Rest error message.
+ private: void OnResponse(ConstRestErrorPtr &_msg);
+
+ /// \brief Login menu item
+ private: QAction &loginMenuAction;
+
+ /// \brief Logout menu item
+ private: QAction &logoutMenuAction;
+
+ /// \brief The title to use when displaying dialog/message windows
+ private: std::string title;
+
+ /// \brief Pub/sub node to communicate with gzserver.
+ private: gazebo::transport::NodePtr node;
+
+ /// \brief Login dialog.
+ private: gui::RestUiLogoutDialog logoutDialog;
+
+ /// \brief Login dialog.
+ private: gui::RestUiLoginDialog loginDialog;
+
+ /// \brief Gazebo login topic publisher
+ private: gazebo::transport::PublisherPtr loginPub;
+
+ /// \brief Gazebo logout topic publisher
+ private: gazebo::transport::PublisherPtr logoutPub;
+
+ /// \brief Gazebo error topic subscriber.
+ private: gazebo::transport::SubscriberPtr errorSub;
+
+ /// \brief List of unprocessed error messages to be displayed from the gui
+ /// thread.
+ private: std::list<boost::shared_ptr<const gazebo::msgs::RestError>>
+ msgRespQ;
+ };
+}
+
+#endif
diff --git a/plugins/rest_web/RestWebPlugin.cc b/plugins/rest_web/RestWebPlugin.cc
new file mode 100644
index 0000000..b7f994e
--- /dev/null
+++ b/plugins/rest_web/RestWebPlugin.cc
@@ -0,0 +1,344 @@
+/*
+ * 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.
+ *
+*/
+
+#ifdef _WIN32
+ #include <Rpc.h>
+ #pragma comment(lib, "Rpcrt4.lib")
+#else /* UNIX */
+
+#ifdef HAVE_UUID
+ #include <uuid/uuid.h>
+#endif
+
+#endif
+
+#include "RestWebPlugin.hh"
+
+
+using namespace gazebo;
+using namespace std;
+
+//////////////////////////////////////////////////
+RestWebPlugin::RestWebPlugin()
+: node(new gazebo::transport::Node()),
+ stopMsgProcessing(false),
+ requestQThread(NULL)
+{
+ // generate a unique session ID
+ // On Windows
+#ifdef _WIN32
+ UUID uuid;
+ C_STATUS Result = ::UuidCreate(uuid);
+ if (Result != RPC_S_OK)
+ {
+ gzerr << "Call to UuidCreate return a non success RPC call. " <<
+ "Return code: " << Result << std::endl;
+ }
+ char* szUuid = NULL;
+ if (::UuidToStringA(&this->data, reinterpret_cast<RPC_CSTR*>(&szUuid)) ==
+ RPC_S_OK)
+ {
+ this->session = szUuid;
+ ::RpcStringFreeA(reinterpret_cast<RPC_CSTR*>(&szUuid));
+ }
+ // or on UNIX
+#else
+
+#ifdef HAVE_UUID
+ uuid_t uuid;
+ uuid_generate_random(uuid);
+ char s[37];
+ uuid_unparse(uuid, s);
+ this->session = s;
+#endif
+
+#endif
+
+ gzmsg << "REST web Session : " << this->session << endl;
+}
+
+//////////////////////////////////////////////////
+RestWebPlugin::~RestWebPlugin()
+{
+ // tell the requestQ to stop precessing
+ this->stopMsgProcessing = true;
+ if (this->requestQThread->joinable())
+ {
+ this->requestQThread->join();
+ delete this->requestQThread;
+ }
+}
+
+//////////////////////////////////////////////////
+void RestWebPlugin::Init()
+{
+ // setup our node for communication
+ this->node->Init();
+ this->subLogin = node->Subscribe("/gazebo/rest/rest_login",
+ &RestWebPlugin::OnRestLoginRequest, this);
+
+ this->subLogout = node->Subscribe("/gazebo/rest/rest_logout",
+ &RestWebPlugin::OnRestLogoutRequest, this);
+
+ this->subEvent = node->Subscribe("/gazebo/rest/rest_post",
+ &RestWebPlugin::OnEventRestPost, this);
+
+ this->subSimEvent = node->Subscribe("/gazebo/sim_events",
+ &RestWebPlugin::OnSimEvent, this);
+
+ this->requestQThread = new boost::thread(
+ boost::bind(&RestWebPlugin::RunRequestQ, this));
+}
+
+//////////////////////////////////////////////////
+void RestWebPlugin::Load(int /*_argc*/, char ** /*_argv*/)
+{
+ // nothing to do for now
+}
+
+//////////////////////////////////////////////////
+void RestWebPlugin::OnSimEvent(ConstSimEventPtr &_msg)
+{
+ try
+ {
+ // where to post the data on the REST server
+ std::string route = "/events/new";
+ std::string eType = _msg->type();
+ std::string name = _msg->name();
+ std::string data = _msg->data();
+
+ msgs::WorldStatistics ws = _msg->world_statistics();
+ msgs::Time simT = ws.sim_time();
+ msgs::Time realT = ws.real_time();
+ msgs::Time pauseT = ws.pause_time();
+ bool paused = ws.paused();
+
+ std::string worldName = physics::get_world()->GetName();
+ std::string event = "{\n";
+
+ event += "\"session\": \"" + this->session + "\", ";
+ event += "\"name\": \"" + name + "\", ";
+ event += "\"type\": \"" + eType + "\",\n";
+ event += "\"data\": " + data + ", ";
+
+ event += "\"world\": {";
+ event += "\"name\": ";
+ event += "\"";
+ event += worldName;
+ event += "\", ";
+
+ event += "\"paused\": ";
+ event += "\"";
+ if (paused)
+ event += "true";
+ else
+ event += "false";
+ event += "\", ";
+
+ event += "\"clock_time\": ";
+ event += "\"";
+ event += common::Time::GetWallTimeAsISOString();
+ event += "\", ";
+
+ event += "\"real_time\": ";
+ event += "\"";
+ event += msgs::Convert(realT).FormattedString();
+ event += "\", ";
+
+ event += "\"sim_time\": ";
+ event += "\"";
+ event += msgs::Convert(simT).FormattedString();
+ event += "\", ";
+
+ event += "\"pause_time\": ";
+ event += "\"";
+ event += msgs::Convert(pauseT).FormattedString();
+ event += "\"";
+
+ event += "}\n"; // world element
+ event += "}"; // root element
+ // post it with curl
+ this->restApi.PostJsonData(route.c_str(), event.c_str());
+ }
+ catch(RestException &x)
+ {
+ gazebo::msgs::RestError msg;
+ std::string errorMsg;
+ errorMsg = "There was a problem trying to send data to the server: ";
+ errorMsg += x.what();
+ msg.set_type("Error");
+ msg.set_msg(errorMsg);
+ // alert the user via the gui plugin
+ gzerr << "ERROR in REST service POST request: " << errorMsg << std::endl;
+ this->pub->Publish(msg);
+ }
+}
+
+//////////////////////////////////////////////////
+void RestWebPlugin::OnEventRestPost(ConstRestPostPtr &_msg)
+{
+ gzmsg << "RestWebPlugin::OnRestPost";
+ gzmsg << "[" << _msg->route() << ", " << _msg->json() << "]" << std::endl;
+ gzmsg << std::endl;
+
+ try
+ {
+ std::string event = "{";
+ event += "\"event\": " + _msg->json() + ", ";
+ physics::WorldPtr world = physics::get_world();
+ if (!world)
+ {
+ gzerr << "Can't access world before web service POST" << std::endl;
+ }
+ else
+ {
+ event += "\"session\": \"" + this->session + "\", ";
+ event += "\"world\": {";
+
+ event += "\"name\": ";
+ event += "\"";
+ event += world->GetName();
+ event += "\", ";
+
+ if (!world->IsPaused())
+ {
+ event += "\"is_running\": \"true\", ";
+ }
+ else
+ {
+ event += "\"is_running\": \"false\", ";
+ }
+
+ common::Time t;
+ event += "\"clock_time\": ";
+ event += "\"";
+ event += common::Time::GetWallTimeAsISOString();
+ event += "\", ";
+
+ event += "\"real_time\": ";
+ event += "\"";
+ t = world->GetRealTime();
+ event += t.FormattedString();
+ event += "\", ";
+
+ event += "\"sim_time\": ";
+ event += "\"";
+ t = world->GetSimTime();
+ event += t.FormattedString();
+ event += "\", ";
+
+ event += "\"pause_time\": ";
+ event += "\"";
+ t = world->GetPauseTime();
+ event += t.FormattedString();
+ event += "\" ";
+
+ event += "}";
+ }
+ event += "}";
+ this->restApi.PostJsonData(_msg->route().c_str(), event.c_str());
+ }
+ catch(RestException &x)
+ {
+ gazebo::msgs::RestError msg;
+ std::string errorMsg;
+ errorMsg = "There was a problem trying to send data to the server: ";
+ errorMsg += x.what();
+ msg.set_type("Error");
+ msg.set_msg(errorMsg);
+ // alert the user via the gui plugin
+ gzerr << "ERROR in REST request: " << errorMsg << std::endl;
+ this->pub->Publish(msg);
+ }
+}
+
+//////////////////////////////////////////////////
+void RestWebPlugin::OnRestLoginRequest(ConstRestLoginPtr &_msg)
+{
+ boost::mutex::scoped_lock lock(this->requestQMutex);
+ this->msgLoginQ.push_back(_msg);
+}
+
+//////////////////////////////////////////////////
+void RestWebPlugin::OnRestLogoutRequest(ConstRestLogoutPtr &/*_msg*/)
+{
+ boost::mutex::scoped_lock lock(this->requestQMutex);
+ this->restApi.Logout();
+}
+
+//////////////////////////////////////////////////
+void RestWebPlugin::ProcessLoginRequest(ConstRestLoginPtr _msg)
+{
+ // this is executed asynchronously
+ try
+ {
+ this->restApi.Login(_msg->url().c_str(),
+ "/login",
+ _msg->username().c_str(),
+ _msg->password().c_str());
+ }
+ catch(RestException &x)
+ {
+ gazebo::msgs::RestError msg;
+ std::string errorMsg;
+ errorMsg = "There was a problem trying to login to the server: ";
+ errorMsg += x.what();
+ msg.set_type("Error");
+ msg.set_msg(errorMsg);
+ // alert the user via the gui plugin
+ gzerr << "ERROR in REST login request. : " << errorMsg << std::endl;
+ this->pub->Publish(msg);
+ }
+}
+
+//////////////////////////////////////////////////
+void RestWebPlugin::RunRequestQ()
+{
+ // be ready to send errors back to the UI
+ std::string path("/gazebo/rest/rest_error");
+ this->pub = node->Advertise<gazebo::msgs::RestError>(path);
+ // process any login or post data that ha been received
+ while (!this->stopMsgProcessing)
+ {
+ gazebo::common::Time::MSleep(50);
+ try
+ {
+ boost::shared_ptr<const gazebo::msgs::RestLogin> login;
+ // Grab the mutex and remove first message the queue
+ {
+ boost::mutex::scoped_lock lock(this->requestQMutex);
+ if (!this->msgLoginQ.empty())
+ {
+ login = this->msgLoginQ.front();
+ this->msgLoginQ.pop_front();
+ }
+ }
+ if (login)
+ {
+ this->ProcessLoginRequest(login);
+ }
+ }
+ catch(...)
+ {
+ gzerr << "Unhandled exception while processing request message"
+ << std::endl;
+ }
+ }
+}
+
+// plugin registration
+GZ_REGISTER_SYSTEM_PLUGIN(RestWebPlugin)
diff --git a/plugins/rest_web/RestWebPlugin.hh b/plugins/rest_web/RestWebPlugin.hh
new file mode 100644
index 0000000..402724a
--- /dev/null
+++ b/plugins/rest_web/RestWebPlugin.hh
@@ -0,0 +1,117 @@
+/*
+ * 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_REST_WEB_PLUGIN_HH_
+#define _GAZEBO_REST_WEB_PLUGIN_HH_
+
+#include <list>
+#include <vector>
+#include <string>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+
+#include "RestApi.hh"
+
+namespace gazebo
+{
+ /// \class RestWebPlugin RestWebPlugin.hh RestWebPlugin.hh
+ /// \brief REST web plugin
+ class GAZEBO_VISIBLE RestWebPlugin : public SystemPlugin
+ {
+ /// \brief Constructor
+ public: RestWebPlugin();
+
+ /// \brief Destructor
+ public: virtual ~RestWebPlugin();
+
+ /// \brief Plugin Load
+ /// \param[in] _argc Argument count
+ /// \param[in] _argv Argument vector
+ public: virtual void Load(int _argc, char **_argv);
+
+ /// \brief Called everytime a login message is received.
+ /// \param[in] _msg The login message
+ public: void OnRestLoginRequest(ConstRestLoginPtr &_msg);
+
+ /// \brief Called everytime a logout message is received.
+ /// \param[in] _msg The login message
+ public: void OnRestLogoutRequest(ConstRestLogoutPtr &_msg);
+
+ /// \brief Called everytime a REST POST event message is received
+ /// \param[in] _msg The post message
+ public: void OnEventRestPost(ConstRestPostPtr &_msg);
+
+ /// \brief Called everytime a SimEvent message is received
+ /// \param[in] The SimEvent message
+ public: void OnSimEvent(ConstSimEventPtr &_msg);
+
+ /// \brief Plugin initialization
+ private: virtual void Init();
+
+ /// \brief Entry point for the web requests processing thread
+ private: void RunRequestQ();
+
+ /// \brief Process a RestRequest message from the requestThread
+ /// \param[in] The message to process
+ private: void ProcessLoginRequest(ConstRestLoginPtr _msg);
+
+ /// \brief Gazebo pub/sub node
+ private: gazebo::transport::NodePtr node;
+
+ /// \brief Gazebo subscriber for login requests
+ private: gazebo::transport::SubscriberPtr subLogin;
+
+ /// \brief Gazebo subscriber for logout requests
+ private: gazebo::transport::SubscriberPtr subLogout;
+
+ /// \brief Gazebo subscriber for POST events
+ private: gazebo::transport::SubscriberPtr subEvent;
+
+ /// \brief Gazebo subscriber for sim events
+ private: gazebo::transport::SubscriberPtr subSimEvent;
+
+ /// \brief Gazebo publisher
+ private: gazebo::transport::PublisherPtr pub;
+
+ /// \brief Gazebo events
+ private: std::vector<event::ConnectionPtr> connections;
+
+ /// \brief REST calls
+ private: RestApi restApi;
+
+ /// \brief A flag to interrupt message processing
+ private: bool stopMsgProcessing;
+
+ /// \brief A list to accumulate pending requests
+ private: std::list<boost::shared_ptr<const gazebo::msgs::RestLogin>>
+ msgLoginQ;
+
+ /// \brief A thread to process requests without stopping the simulation
+ private: boost::thread *requestQThread;
+
+ /// \brief A mutex to ensure integrity of the request list
+ private: boost::mutex requestQMutex;
+
+ /// \brief A session string to keep track of exercises
+ private: std::string session;
+ };
+}
+
+#endif
+
+
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index b66ad5b..975bed1 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -30,17 +30,6 @@ target_link_libraries(gtest_main gtest)
set(GTEST_LIBRARY "${PROJECT_BINARY_DIR}/test/libgtest.a")
set(GTEST_MAIN_LIBRARY "${PROJECT_BINARY_DIR}/test/libgtest_main.a")
-# Build static ServerFixture library
-set(ServerFixtureSources
- ${PROJECT_SOURCE_DIR}/gazebo/Server.cc
- ${PROJECT_SOURCE_DIR}/gazebo/Master.cc
- ${PROJECT_SOURCE_DIR}/gazebo/gazebo.cc
- ${PROJECT_SOURCE_DIR}/test/ServerFixture.cc
-)
-add_library(server_fixture STATIC ${ServerFixtureSources})
-add_dependencies(server_fixture gazebo_msgs)
-set(SERVER_FIXTURE_LIBRARY "${PROJECT_BINARY_DIR}/test/libserver_fixture.a")
-
execute_process(COMMAND cmake -E remove_directory ${CMAKE_BINARY_DIR}/test_results)
execute_process(COMMAND cmake -E make_directory ${CMAKE_BINARY_DIR}/test_results)
include_directories(${GTEST_INCLUDE_DIRS})
@@ -57,6 +46,3 @@ set(TEST_TYPE "REGRESSION")
add_subdirectory(regression)
add_subdirectory(cmake)
add_subdirectory(pkgconfig)
-
-set(headers ServerFixture.hh)
-gz_install_includes("test" ${headers} ${PROJECT_BINARY_DIR}/test_config.h)
diff --git a/test/ServerFixture.cc b/test/ServerFixture.cc
deleted file mode 100644
index 773ec8d..0000000
--- a/test/ServerFixture.cc
+++ /dev/null
@@ -1,1361 +0,0 @@
-/*
- * Copyright (C) 2012-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 <stdio.h>
-#include <string>
-
-#include "ServerFixture.hh"
-
-using namespace gazebo;
-
-/////////////////////////////////////////////////
-std::string custom_exec(std::string _cmd)
-{
- _cmd += " 2>/dev/null";
- FILE* pipe = popen(_cmd.c_str(), "r");
-
- if (!pipe)
- return "ERROR";
-
- char buffer[128];
- std::string result = "";
-
- while (!feof(pipe))
- {
- if (fgets(buffer, 128, pipe) != NULL)
- result += buffer;
- }
-
- pclose(pipe);
- return result;
-}
-
-/////////////////////////////////////////////////
-ServerFixture::ServerFixture()
-{
- this->server = NULL;
- this->serverRunning = false;
- this->paused = false;
- this->percentRealTime = 0;
- this->gotImage = 0;
- this->imgData = NULL;
- this->serverThread = NULL;
-
- gzLogInit("test-", "test.log");
- gazebo::common::Console::SetQuiet(false);
- common::SystemPaths::Instance()->AddGazeboPaths(
- TEST_INTEGRATION_PATH);
-
- // Add local search paths
- boost::filesystem::path path;
-
- path = PROJECT_SOURCE_PATH;
- gazebo::common::SystemPaths::Instance()->AddGazeboPaths(path.string());
-
- path = PROJECT_SOURCE_PATH;
- path /= "gazebo";
- gazebo::common::SystemPaths::Instance()->AddGazeboPaths(path.string());
-
- path = PROJECT_BINARY_PATH;
- path /= "plugins";
- gazebo::common::SystemPaths::Instance()->AddPluginPaths(path.string());
-
- path = PROJECT_BINARY_PATH;
- path /= "test";
- path /= "plugins";
- gazebo::common::SystemPaths::Instance()->AddPluginPaths(path.string());
-
- path = TEST_PATH;
- gazebo::common::SystemPaths::Instance()->AddGazeboPaths(path.string());
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::TearDown()
-{
- this->Unload();
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::Unload()
-{
- gzdbg << "ServerFixture::Unload" << std::endl;
- this->serverRunning = false;
- if (this->node)
- this->node->Fini();
-
- if (this->server)
- {
- this->server->Stop();
-
- if (this->serverThread)
- {
- this->serverThread->join();
- }
- }
-
- delete this->serverThread;
- this->serverThread = NULL;
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::Load(const std::string &_worldFilename)
-{
- this->Load(_worldFilename, false);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::Load(const std::string &_worldFilename, bool _paused)
-{
- this->Load(_worldFilename, _paused, "");
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::Load(const std::string &_worldFilename,
- bool _paused, const std::string &_physics)
-{
- delete this->server;
- this->server = NULL;
-
- // Create, load, and run the server in its own thread
- this->serverThread = new boost::thread(
- boost::bind(&ServerFixture::RunServer, this, _worldFilename,
- _paused, _physics));
-
- // Wait for the server to come up
- // Use a 60 second timeout.
- int waitCount = 0, maxWaitCount = 6000;
- while ((!this->server || !this->server->GetInitialized()) &&
- ++waitCount < maxWaitCount)
- common::Time::MSleep(100);
- gzdbg << "ServerFixture load in "
- << static_cast<double>(waitCount)/10.0
- << " seconds, timeout after "
- << static_cast<double>(maxWaitCount)/10.0
- << " seconds\n";
-
- if (waitCount >= maxWaitCount)
- this->launchTimeoutFailure(
- "while waiting for Load() function", waitCount);
-
- this->node = transport::NodePtr(new transport::Node());
- ASSERT_NO_THROW(this->node->Init());
- this->poseSub = this->node->Subscribe("~/pose/local/info",
- &ServerFixture::OnPose, this, true);
- this->statsSub = this->node->Subscribe("~/world_stats",
- &ServerFixture::OnStats, this);
-
- this->factoryPub =
- this->node->Advertise<msgs::Factory>("~/factory");
-
- this->requestPub =
- this->node->Advertise<msgs::Request>("~/request");
-
- // Wait for the world to reach the correct pause state.
- // This might not work properly with multiple worlds.
- // Use a 30 second timeout.
- waitCount = 0;
- maxWaitCount = 3000;
- while ((!physics::get_world() ||
- physics::get_world()->IsPaused() != _paused) &&
- ++waitCount < maxWaitCount)
- common::Time::MSleep(100);
- ASSERT_LT(waitCount, maxWaitCount);
-
- this->factoryPub->WaitForConnection();
- this->requestPub->WaitForConnection();
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::RunServer(const std::string &_worldFilename)
-{
- this->RunServer(_worldFilename, false, "");
-}
-
-/////////////////////////////////////////////////
-rendering::ScenePtr ServerFixture::GetScene(
- const std::string &_sceneName)
-{
- // Wait for the scene to get loaded.
- int i = 0;
- int timeoutDS = 20;
- while (rendering::get_scene(_sceneName) == NULL && i < timeoutDS)
- {
- common::Time::MSleep(100);
- ++i;
- }
-
- if (i >= timeoutDS)
- {
- gzerr << "Unable to load the rendering scene.\n"
- << "Test will fail";
- this->launchTimeoutFailure(
- "while waiting to load rendering scene", i);
- }
-
- return rendering::get_scene(_sceneName);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::RunServer(const std::string &_worldFilename, bool _paused,
- const std::string &_physics)
-{
- ASSERT_NO_THROW(this->server = new Server());
- this->server->PreLoad();
- if (_physics.length())
- ASSERT_NO_THROW(this->server->LoadFile(_worldFilename,
- _physics));
- else
- ASSERT_NO_THROW(this->server->LoadFile(_worldFilename));
-
- if (!rendering::get_scene(
- gazebo::physics::get_world()->GetName()))
- {
- ASSERT_NO_THROW(rendering::create_scene(
- gazebo::physics::get_world()->GetName(), false, true));
- }
-
- ASSERT_NO_THROW(this->SetPause(_paused));
-
- ASSERT_NO_THROW(this->server->Run());
-
- ASSERT_NO_THROW(this->server->Fini());
-
- ASSERT_NO_THROW(delete this->server);
- this->server = NULL;
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::OnStats(ConstWorldStatisticsPtr &_msg)
-{
- this->simTime = msgs::Convert(_msg->sim_time());
- this->realTime = msgs::Convert(_msg->real_time());
- this->pauseTime = msgs::Convert(_msg->pause_time());
- this->paused = _msg->paused();
-
- if (this->realTime == 0)
- this->percentRealTime = 0;
- else
- this->percentRealTime =
- (this->simTime / this->realTime).Double();
-
- this->serverRunning = true;
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SetPause(bool _pause)
-{
- physics::pause_worlds(_pause);
-}
-
-/////////////////////////////////////////////////
-double ServerFixture::GetPercentRealTime() const
-{
- while (!this->serverRunning)
- common::Time::MSleep(100);
-
- return this->percentRealTime;
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::OnPose(ConstPosesStampedPtr &_msg)
-{
- boost::mutex::scoped_lock lock(this->receiveMutex);
- for (int i = 0; i < _msg->pose_size(); ++i)
- {
- this->poses[_msg->pose(i).name()] =
- msgs::Convert(_msg->pose(i));
- }
-}
-
-/////////////////////////////////////////////////
-math::Pose ServerFixture::GetEntityPose(const std::string &_name)
-{
- boost::mutex::scoped_lock lock(this->receiveMutex);
-
- std::map<std::string, math::Pose>::iterator iter;
- iter = this->poses.find(_name);
- EXPECT_TRUE(iter != this->poses.end());
- return iter->second;
-}
-
-/////////////////////////////////////////////////
-bool ServerFixture::HasEntity(const std::string &_name)
-{
- boost::mutex::scoped_lock lock(this->receiveMutex);
- std::map<std::string, math::Pose>::iterator iter;
- iter = this->poses.find(_name);
- return iter != this->poses.end();
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::PrintImage(const std::string &_name, unsigned char **_image,
- unsigned int _width, unsigned int _height, unsigned int _depth)
-{
- unsigned int count = _height * _width * _depth;
- printf("\n");
- printf("static unsigned char __%s[] = {", _name.c_str());
- unsigned int i;
- for (i = 0; i < count-1; i++)
- {
- if (i % 10 == 0)
- printf("\n");
- else
- printf(" ");
- printf("%d,", (*_image)[i]);
- }
- printf(" %d};\n", (*_image)[i]);
- printf("static unsigned char *%s = __%s;\n", _name.c_str(),
- _name.c_str());
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::PrintScan(const std::string &_name, double *_scan,
- unsigned int _sampleCount)
-{
- printf("static double __%s[] = {\n", _name.c_str());
- for (unsigned int i = 0; i < _sampleCount-1; ++i)
- {
- if ((i+1) % 5 == 0)
- printf("%13.10f,\n", math::precision(_scan[i], 10));
- else
- printf("%13.10f, ", math::precision(_scan[i], 10));
- }
- printf("%13.10f};\n",
- math::precision(_scan[_sampleCount-1], 10));
- printf("static double *%s = __%s;\n", _name.c_str(),
- _name.c_str());
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::FloatCompare(float *_scanA, float *_scanB,
- unsigned int _sampleCount, float &_diffMax,
- float &_diffSum, float &_diffAvg)
-{
- _diffMax = 0;
- _diffSum = 0;
- _diffAvg = 0;
- for (unsigned int i = 0; i < _sampleCount; ++i)
- {
- double diff = fabs(math::precision(_scanA[i], 10) -
- math::precision(_scanB[i], 10));
- _diffSum += diff;
- if (diff > _diffMax)
- {
- _diffMax = diff;
- }
- }
- _diffAvg = _diffSum / _sampleCount;
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::DoubleCompare(double *_scanA, double *_scanB,
- unsigned int _sampleCount, double &_diffMax,
- double &_diffSum, double &_diffAvg)
-{
- _diffMax = 0;
- _diffSum = 0;
- _diffAvg = 0;
- for (unsigned int i = 0; i < _sampleCount; ++i)
- {
- double diff = fabs(math::precision(_scanA[i], 10) -
- math::precision(_scanB[i], 10));
- _diffSum += diff;
- if (diff > _diffMax)
- {
- _diffMax = diff;
- }
- }
- _diffAvg = _diffSum / _sampleCount;
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::ImageCompare(unsigned char *_imageA,
- unsigned char *_imageB,
- unsigned int _width, unsigned int _height, unsigned int _depth,
- unsigned int &_diffMax, unsigned int &_diffSum,
- double &_diffAvg)
-{
- _diffMax = 0;
- _diffSum = 0;
-
- for (unsigned int y = 0; y < _height; ++y)
- {
- for (unsigned int x = 0; x < _width*_depth; ++x)
- {
- unsigned int a = _imageA[(y*_width*_depth)+x];
- unsigned int b = _imageB[(y*_width*_depth)+x];
-
- unsigned int diff = (unsigned int)(abs(a - b));
-
- if (diff > _diffMax)
- _diffMax = diff;
-
- _diffSum += diff;
- }
- }
- _diffAvg = _diffSum / (_height*_width*_depth);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::OnNewFrame(const unsigned char *_image,
- unsigned int _width, unsigned int _height,
- unsigned int _depth,
- const std::string &/*_format*/)
-{
- memcpy(*this->imgData, _image, _width * _height * _depth);
- this->gotImage+= 1;
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::GetFrame(const std::string &_cameraName,
- unsigned char **_imgData, unsigned int &_width,
- unsigned int &_height)
-{
- sensors::SensorPtr sensor = sensors::get_sensor(_cameraName);
- EXPECT_TRUE(sensor != NULL);
- sensors::CameraSensorPtr camSensor =
- boost::dynamic_pointer_cast<sensors::CameraSensor>(sensor);
-
- _width = camSensor->GetImageWidth();
- _height = camSensor->GetImageHeight();
-
- if (*_imgData)
- {
- delete *_imgData;
- *_imgData = NULL;
- }
- (*_imgData) = new unsigned char[_width *_height*3];
- this->imgData = _imgData;
-
- this->gotImage = 0;
- event::ConnectionPtr c =
- camSensor->GetCamera()->ConnectNewImageFrame(
- boost::bind(&ServerFixture::OnNewFrame,
- this, _1, _2, _3, _4, _5));
-
- while (this->gotImage < 20)
- common::Time::MSleep(100);
-
- camSensor->GetCamera()->DisconnectNewImageFrame(c);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnCamera(const std::string &_modelName,
- const std::string &_cameraName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- unsigned int _width, unsigned int _height, double _rate,
- const std::string &_noiseType, double _noiseMean, double _noiseStdDev,
- bool _distortion, double _distortionK1, double _distortionK2,
- double _distortionK3, double _distortionP1, double _distortionP2,
- double _cx, double _cy)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _modelName << "'>"
- << "<static>true</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << " <sensor name ='" << _cameraName
- << "' type ='camera'>"
- << " <always_on>1</always_on>"
- << " <update_rate>" << _rate << "</update_rate>"
- << " <visualize>true</visualize>"
- << " <camera>"
- << " <horizontal_fov>0.78539816339744828</horizontal_fov>"
- << " <image>"
- << " <width>" << _width << "</width>"
- << " <height>" << _height << "</height>"
- << " <format>R8G8B8</format>"
- << " </image>"
- << " <clip>"
- << " <near>0.1</near><far>100</far>"
- << " </clip>";
- // << " <save enabled ='true' path ='/tmp/camera/'/>"
-
- if (_noiseType.size() > 0)
- newModelStr << " <noise>"
- << " <type>" << _noiseType << "</type>"
- << " <mean>" << _noiseMean << "</mean>"
- << " <stddev>" << _noiseStdDev << "</stddev>"
- << " </noise>";
-
- if (_distortion)
- newModelStr << " <distortion>"
- << " <k1>" << _distortionK1 << "</k1>"
- << " <k2>" << _distortionK2 << "</k2>"
- << " <k3>" << _distortionK3 << "</k3>"
- << " <p1>" << _distortionP1 << "</p1>"
- << " <p2>" << _distortionP2 << "</p2>"
- << " <center>" << _cx << " " << _cy << "</center>"
- << " </distortion>";
-
- newModelStr << " </camera>"
- << " </sensor>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- WaitUntilEntitySpawn(_modelName, 100, 50);
- WaitUntilSensorSpawn(_cameraName, 100, 100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnRaySensor(const std::string &_modelName,
- const std::string &_raySensorName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- double _hMinAngle, double _hMaxAngle,
- double _vMinAngle, double _vMaxAngle,
- double _minRange, double _maxRange,
- double _rangeResolution, unsigned int _samples,
- unsigned int _vSamples, double _hResolution,
- double _vResolution,
- const std::string &_noiseType, double _noiseMean,
- double _noiseStdDev)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _modelName << "'>"
- << "<static>true</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << "<collision name='parent_collision'>"
- << " <pose>0 0 0.0205 0 0 0</pose>"
- << " <geometry>"
- << " <cylinder>"
- << " <radius>0.021</radius>"
- << " <length>0.029</length>"
- << " </cylinder>"
- << " </geometry>"
- << "</collision>"
- << " <sensor name ='" << _raySensorName << "' type ='ray'>"
- << " <ray>"
- << " <scan>"
- << " <horizontal>"
- << " <samples>" << _samples << "</samples>"
- << " <resolution>" << _hResolution << "</resolution>"
- << " <min_angle>" << _hMinAngle << "</min_angle>"
- << " <max_angle>" << _hMaxAngle << "</max_angle>"
- << " </horizontal>"
- << " <vertical>"
- << " <samples>" << _vSamples << "</samples>"
- << " <resolution>" << _vResolution << "</resolution>"
- << " <min_angle>" << _vMinAngle << "</min_angle>"
- << " <max_angle>" << _vMaxAngle << "</max_angle>"
- << " </vertical>"
- << " </scan>"
- << " <range>"
- << " <min>" << _minRange << "</min>"
- << " <max>" << _maxRange << "</max>"
- << " <resolution>" << _rangeResolution <<"</resolution>"
- << " </range>";
-
- if (_noiseType.size() > 0)
- newModelStr << " <noise>"
- << " <type>" << _noiseType << "</type>"
- << " <mean>" << _noiseMean << "</mean>"
- << " <stddev>" << _noiseStdDev << "</stddev>"
- << " </noise>";
-
- newModelStr << " </ray>"
- << " <visualize>true</visualize>"
- << " </sensor>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- WaitUntilEntitySpawn(_modelName, 100, 100);
- WaitUntilSensorSpawn(_raySensorName, 100, 100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnGpuRaySensor(const std::string &_modelName,
- const std::string &_raySensorName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- double _hMinAngle, double _hMaxAngle,
- double _minRange, double _maxRange,
- double _rangeResolution, unsigned int _samples,
- const std::string &_noiseType, double _noiseMean,
- double _noiseStdDev)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _modelName << "'>"
- << "<static>true</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << "<collision name='parent_collision'>"
- << " <pose>0 0 0.0205 0 0 0</pose>"
- << " <geometry>"
- << " <cylinder>"
- << " <radius>0.021</radius>"
- << " <length>0.029</length>"
- << " </cylinder>"
- << " </geometry>"
- << "</collision>"
- << " <sensor name ='" << _raySensorName
- << "' type ='gpu_ray'>"
- << " <ray>"
- << " <scan>"
- << " <horizontal>"
- << " <samples>" << _samples << "</samples>"
- << " <resolution> 1 </resolution>"
- << " <min_angle>" << _hMinAngle << "</min_angle>"
- << " <max_angle>" << _hMaxAngle << "</max_angle>"
- << " </horizontal>"
- << " </scan>"
- << " <range>"
- << " <min>" << _minRange << "</min>"
- << " <max>" << _maxRange << "</max>"
- << " <resolution>" << _rangeResolution <<"</resolution>"
- << " </range>";
-
- if (_noiseType.size() > 0)
- newModelStr << " <noise>"
- << " <type>" << _noiseType << "</type>"
- << " <mean>" << _noiseMean << "</mean>"
- << " <stddev>" << _noiseStdDev << "</stddev>"
- << " </noise>";
-
- newModelStr << " </ray>"
- << " </sensor>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- WaitUntilEntitySpawn(_modelName, 100, 100);
- WaitUntilSensorSpawn(_raySensorName, 100, 100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnImuSensor(const std::string &_modelName,
- const std::string &_imuSensorName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- const std::string &_noiseType,
- double _rateNoiseMean, double _rateNoiseStdDev,
- double _rateBiasMean, double _rateBiasStdDev,
- double _accelNoiseMean, double _accelNoiseStdDev,
- double _accelBiasMean, double _accelBiasStdDev)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _modelName << "'>" << std::endl
- << "<static>true</static>" << std::endl
- << "<pose>" << _pos << " " << _rpy << "</pose>" << std::endl
- << "<link name ='body'>" << std::endl
- << "<inertial>" << std::endl
- << "<mass>0.1</mass>" << std::endl
- << "</inertial>" << std::endl
- << "<collision name='parent_collision'>" << std::endl
- << " <pose>0 0 0.0205 0 0 0</pose>" << std::endl
- << " <geometry>" << std::endl
- << " <cylinder>" << std::endl
- << " <radius>0.021</radius>" << std::endl
- << " <length>0.029</length>" << std::endl
- << " </cylinder>" << std::endl
- << " </geometry>" << std::endl
- << "</collision>" << std::endl
- << " <sensor name ='" << _imuSensorName
- << "' type ='imu'>" << std::endl
- << " <imu>" << std::endl;
-
- if (_noiseType.size() > 0)
- {
- newModelStr << " <noise>" << std::endl
- << " <type>" << _noiseType << "</type>" << std::endl
- << " <rate>" << std::endl
- << " <mean>" << _rateNoiseMean
- << "</mean>" << std::endl
- << " <stddev>" << _rateNoiseStdDev
- << "</stddev>" << std::endl
- << " <bias_mean>" << _rateBiasMean
- << "</bias_mean>" << std::endl
- << " <bias_stddev>" << _rateBiasStdDev
- << "</bias_stddev>" << std::endl
- << " </rate>" << std::endl
- << " <accel>" << std::endl
- << " <mean>" << _accelNoiseMean << "</mean>"
- << std::endl
- << " <stddev>" << _accelNoiseStdDev << "</stddev>"
- << std::endl
- << " <bias_mean>" << _accelBiasMean
- << "</bias_mean>" << std::endl
- << " <bias_stddev>" << _accelBiasStdDev
- << "</bias_stddev>" << std::endl
- << " </accel>" << std::endl
- << " </noise>" << std::endl;
- }
-
- newModelStr << " </imu>" << std::endl
- << " </sensor>" << std::endl
- << "</link>" << std::endl
- << "</model>" << std::endl
- << "</sdf>" << std::endl;
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- WaitUntilEntitySpawn(_modelName, 100, 1000);
- WaitUntilSensorSpawn(_imuSensorName, 100, 100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnUnitContactSensor(const std::string &_name,
- const std::string &_sensorName,
- const std::string &_collisionType, const math::Vector3 &_pos,
- const math::Vector3 &_rpy, bool _static)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
- std::ostringstream shapeStr;
-
- if (_collisionType == "box")
- {
- shapeStr << " <box><size>1 1 1</size></box>";
- }
- else if (_collisionType == "cylinder")
- {
- shapeStr << "<cylinder>"
- << " <radius>.5</radius><length>1.0</length>"
- << "</cylinder>";
- }
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << " <collision name ='contact_collision'>"
- << " <geometry>"
- << shapeStr.str()
- << " </geometry>"
- << " <surface>"
- << " <contact>"
- << " <ode>"
- << " <min_depth>0.005</min_depth>"
- << " </ode>"
- << " </contact>"
- << " </surface>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << shapeStr.str()
- << " </geometry>"
- << " </visual>"
- << " <sensor name='" << _sensorName << "' type='contact'>"
- << " <contact>"
- << " <collision>contact_collision</collision>"
- << " </contact>"
- << " </sensor>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- WaitUntilEntitySpawn(_name, 100, 100);
- WaitUntilSensorSpawn(_sensorName, 100, 100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnUnitImuSensor(const std::string &_name,
- const std::string &_sensorName,
- const std::string &_collisionType,
- const std::string &_topic, const math::Vector3 &_pos,
- const math::Vector3 &_rpy, bool _static)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
- std::ostringstream shapeStr;
- if (_collisionType == "box")
- shapeStr << " <box><size>1 1 1</size></box>";
- else if (_collisionType == "cylinder")
- {
- shapeStr << "<cylinder>"
- << " <radius>.5</radius><length>1.0</length>"
- << "</cylinder>";
- }
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << " <collision name ='contact_collision'>"
- << " <geometry>"
- << shapeStr.str()
- << " </geometry>"
- << " <surface>"
- << " <contact>"
- << " <ode>"
- << " <min_depth>0.01</min_depth>"
- << " </ode>"
- << " </contact>"
- << " </surface>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << shapeStr.str()
- << " </geometry>"
- << " </visual>"
- << " <sensor name='" << _sensorName << "' type='imu'>"
- << " <imu>"
- << " <topic>" << _topic << "</topic>"
- << " </imu>"
- << " </sensor>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- WaitUntilEntitySpawn(_name, 20, 50);
- WaitUntilSensorSpawn(_sensorName, 100, 100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::launchTimeoutFailure(const char *_logMsg,
- const int _timeoutCS)
-{
- FAIL() << "ServerFixture timeout (wait more than " << _timeoutCS / 100
- << "s): " << _logMsg;
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnWirelessTransmitterSensor(const std::string &_name,
- const std::string &_sensorName,
- const math::Vector3 &_pos,
- const math::Vector3 &_rpy,
- const std::string &_essid,
- double _freq,
- double _power,
- double _gain,
- bool _visualize)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>true</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='link'>"
- << " <sensor name='" << _sensorName
- << "' type='wireless_transmitter'>"
- << " <always_on>1</always_on>"
- << " <update_rate>1</update_rate>"
- << " <visualize>" << _visualize << "</visualize>"
- << " <transceiver>"
- << " <essid>" << _essid << "</essid>"
- << " <frequency>" << _freq << "</frequency>"
- << " <power>" << _power << "</power>"
- << " <gain>" << _gain << "</gain>"
- << " </transceiver>"
- << " </sensor>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- WaitUntilEntitySpawn(_name, 100, 100);
- WaitUntilSensorSpawn(_sensorName, 100, 100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnWirelessReceiverSensor(const std::string &_name,
- const std::string &_sensorName,
- const math::Vector3 &_pos,
- const math::Vector3 &_rpy,
- double _minFreq,
- double _maxFreq,
- double _power,
- double _gain,
- double _sensitivity,
- bool _visualize)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>true</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='link'>"
- << " <sensor name='" << _sensorName
- << "' type='wireless_receiver'>"
- << " <update_rate>1</update_rate>"
- << " <visualize>" << _visualize << "</visualize>"
- << " <transceiver>"
- << " <min_frequency>" << _minFreq << "</min_frequency>"
- << " <max_frequency>" << _maxFreq << "</max_frequency>"
- << " <power>" << _power << "</power>"
- << " <gain>" << _gain << "</gain>"
- << " <sensitivity>" << _sensitivity << "</sensitivity>"
- << " </transceiver>"
- << " </sensor>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- WaitUntilEntitySpawn(_name, 100, 100);
- WaitUntilSensorSpawn(_sensorName, 100, 100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::WaitUntilEntitySpawn(const std::string &_name,
- unsigned int _sleepEach,
- int _retries)
-{
- int i = 0;
- // Wait for the entity to spawn
- while (!this->HasEntity(_name) && i < _retries)
- {
- common::Time::MSleep(_sleepEach);
- ++i;
- }
- EXPECT_LT(i, _retries);
-
- if (i >= _retries)
- FAIL() << "ServerFixture timeout: max number of retries ("
- << _retries
- << ") exceeded while awaiting the spawn of " << _name;
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::WaitUntilSensorSpawn(const std::string &_name,
- unsigned int _sleepEach,
- int _retries)
-{
- int i = 0;
- // Wait for the sensor to spawn
- while (!sensors::get_sensor(_name) && i < _retries)
- {
- common::Time::MSleep(_sleepEach);
- ++i;
- }
- EXPECT_LT(i, _retries);
-
- if (i >= _retries)
- FAIL() << "ServerFixture timeout: max number of retries ("
- << _retries
- << ") exceeded while awaiting the spawn of " << _name;
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnLight(const std::string &_name,
- const std::string &_type,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- const common::Color &_diffuse,
- const common::Color &_specular,
- const math::Vector3 &_direction,
- double _attenuationRange,
- double _attenuationConstant,
- double _attenuationLinear,
- double _attenuationQuadratic,
- double _spotInnerAngle,
- double _spotOuterAngle,
- double _spotFallOff,
- bool _castShadows)
-{
- msgs::Factory msg;
- std::ostringstream newLightStr;
-
- newLightStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<light name ='" << _name << "' type = '" << _type << "'>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<diffuse>" << _diffuse << "</diffuse>"
- << "<specular>" << _specular << "</specular>"
- << "<direction>" << _direction << "</direction>"
- << "<attenuation>"
- << " <range>" << _attenuationRange << "</range>"
- << " <constant>" << _attenuationConstant << "</constant>"
- << " <linear>" << _attenuationLinear << "</linear>"
- << " <quadratic>" << _attenuationQuadratic << "</quadratic>"
- << "</attenuation>";
-
- if (_type == "spot")
- {
- newLightStr << "<spot>"
- << " <inner_angle>" << _spotInnerAngle << "</inner_angle>"
- << " <outer_angle>" << _spotOuterAngle << "</outer_angle>"
- << " <falloff>" << _spotFallOff << "</falloff>"
- << "</spot>";
- }
-
- newLightStr << "<cast_shadows>" << _castShadows << "</cast_shadows>"
- << "</light>"
- << "</sdf>";
-
- msg.set_sdf(newLightStr.str());
- this->factoryPub->Publish(msg);
-
- physics::WorldPtr world = physics::get_world();
- msgs::Scene sceneMsg;
- int timeOutCount = 0;
- int maxTimeOut = 10;
- while (timeOutCount < maxTimeOut)
- {
- sceneMsg = world->GetSceneMsg();
- for (int i = 0; i < sceneMsg.light_size(); ++i)
- {
- if (sceneMsg.light(i).name() == _name)
- break;
- }
- timeOutCount++;
- common::Time::MSleep(100);
- }
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnCylinder(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _static)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << " <collision name ='geom'>"
- << " <geometry>"
- << " <cylinder>"
- << " <radius>.5</radius><length>1.0</length>"
- << " </cylinder>"
- << " </geometry>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << " <cylinder>"
- << " <radius>.5</radius><length>1.0</length>"
- << " </cylinder>"
- << " </geometry>"
- << " </visual>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- // Wait for the entity to spawn
- while (!this->HasEntity(_name))
- common::Time::MSleep(100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnSphere(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _wait, bool _static)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << " <collision name ='geom'>"
- << " <geometry>"
- << " <sphere><radius>.5</radius></sphere>"
- << " </geometry>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << " <sphere><radius>.5</radius></sphere>"
- << " </geometry>"
- << " </visual>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- // Wait for the entity to spawn
- while (_wait && !this->HasEntity(_name))
- common::Time::MSleep(100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnSphere(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- const math::Vector3 &_cog, double _radius,
- bool _wait, bool _static)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << " <inertial>"
- << " <pose>" << _cog << " 0 0 0</pose>"
- << " </inertial>"
- << " <collision name ='geom'>"
- << " <geometry>"
- << " <sphere><radius>" << _radius << "</radius></sphere>"
- << " </geometry>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << " <sphere><radius>" << _radius << "</radius></sphere>"
- << " </geometry>"
- << " </visual>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- // Wait for the entity to spawn
- while (_wait && !this->HasEntity(_name))
- common::Time::MSleep(100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnBox(const std::string &_name,
- const math::Vector3 &_size, const math::Vector3 &_pos,
- const math::Vector3 &_rpy, bool _static)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << " <collision name ='geom'>"
- << " <geometry>"
- << " <box><size>" << _size << "</size></box>"
- << " </geometry>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << " <box><size>" << _size << "</size></box>"
- << " </geometry>"
- << " </visual>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- // Wait for the entity to spawn
- while (!this->HasEntity(_name))
- common::Time::MSleep(100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnTrimesh(const std::string &_name,
- const std::string &_modelPath, const math::Vector3 &_scale,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _static)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << " <collision name ='geom'>"
- << " <geometry>"
- << " <mesh>"
- << " <uri>" << _modelPath << "</uri>"
- << " <scale>" << _scale << "</scale>"
- << " </mesh>"
- << " </geometry>"
- << " </collision>"
- << " <visual name ='visual'>"
- << " <geometry>"
- << " <mesh><uri>" << _modelPath << "</uri></mesh>"
- << " </geometry>"
- << " </visual>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- // Wait for the entity to spawn
- while (!this->HasEntity(_name))
- common::Time::MSleep(100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnEmptyLink(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _static)
-{
- msgs::Factory msg;
- std::ostringstream newModelStr;
-
- newModelStr << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << _name << "'>"
- << "<static>" << _static << "</static>"
- << "<pose>" << _pos << " " << _rpy << "</pose>"
- << "<link name ='body'>"
- << "</link>"
- << "</model>"
- << "</sdf>";
-
- msg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(msg);
-
- // Wait for the entity to spawn
- while (!this->HasEntity(_name))
- common::Time::MSleep(100);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnModel(const std::string &_filename)
-{
- msgs::Factory msg;
- msg.set_sdf_filename(_filename);
- this->factoryPub->Publish(msg);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::SpawnSDF(const std::string &_sdf)
-{
- msgs::Factory msg;
- msg.set_sdf(_sdf);
- this->factoryPub->Publish(msg);
-
- // The code above sends a message, but it will take some time
- // before the message is processed.
- //
- // The code below parses the sdf string to find a model name,
- // then this function will block until that model
- // has been processed and recognized by the Server Fixture.
- sdf::SDF sdfParsed;
- sdfParsed.SetFromString(_sdf);
- // Check that sdf contains a model
- if (sdfParsed.root->HasElement("model"))
- {
- // Timeout of 30 seconds (3000 * 10 ms)
- int waitCount = 0, maxWaitCount = 3000;
- sdf::ElementPtr model = sdfParsed.root->GetElement("model");
- std::string name = model->Get<std::string>("name");
- while (!this->HasEntity(name) && ++waitCount < maxWaitCount)
- common::Time::MSleep(100);
- ASSERT_LT(waitCount, maxWaitCount);
- }
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::LoadPlugin(const std::string &_filename,
- const std::string &_name)
-{
- // Get the first world...we assume it the only one running
- physics::WorldPtr world = physics::get_world();
- world->LoadPlugin(_filename, _name, sdf::ElementPtr());
-}
-
-/////////////////////////////////////////////////
-physics::ModelPtr ServerFixture::GetModel(const std::string &_name)
-{
- // Get the first world...we assume it the only one running
- physics::WorldPtr world = physics::get_world();
- return world->GetModel(_name);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::RemoveModel(const std::string &_name)
-{
- msgs::Request *msg = msgs::CreateRequest("entity_delete", _name);
- this->requestPub->Publish(*msg);
- delete msg;
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::RemovePlugin(const std::string &_name)
-{
- // Get the first world...we assume it the only one running
- physics::WorldPtr world = physics::get_world();
- world->RemovePlugin(_name);
-}
-
-/////////////////////////////////////////////////
-void ServerFixture::GetMemInfo(double &_resident, double &_share)
-{
-#ifdef __linux__
- int totalSize, residentPages, sharePages;
- totalSize = residentPages = sharePages = 0;
-
- std::ifstream buffer("/proc/self/statm");
- buffer >> totalSize >> residentPages >> sharePages;
- buffer.close();
-
- // in case x86-64 is configured to use 2MB pages
- int64_t pageSizeKb = sysconf(_SC_PAGE_SIZE) / 1024;
-
- _resident = residentPages * pageSizeKb;
- _share = sharePages * pageSizeKb;
-#elif __MACH__
- // /proc is only available on Linux
- // for OSX, use task_info to get resident and virtual memory
- struct task_basic_info t_info;
- mach_msg_type_number_t t_info_count = TASK_BASIC_INFO_COUNT;
- if (KERN_SUCCESS != task_info(mach_task_self(),
- TASK_BASIC_INFO,
- (task_info_t)&t_info,
- &t_info_count))
- {
- gzerr << "failure calling task_info\n";
- return;
- }
- _resident = static_cast<double>(t_info.resident_size/1024);
- _share = static_cast<double>(t_info.virtual_size/1024);
-#else
- gzerr << "Unsupported architecture\n";
- return;
-#endif
-}
diff --git a/test/ServerFixture.hh b/test/ServerFixture.hh
deleted file mode 100644
index 5b0c1ab..0000000
--- a/test/ServerFixture.hh
+++ /dev/null
@@ -1,571 +0,0 @@
-/*
- * Copyright (C) 2012-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_SERVER_FIXTURE_HH_
-#define _GAZEBO_SERVER_FIXTURE_HH_
-
-#pragma GCC diagnostic ignored "-Wswitch-default"
-#pragma GCC diagnostic ignored "-Wfloat-equal"
-#pragma GCC diagnostic ignored "-Wshadow"
-
-// The following is needed to enable the GetMemInfo function for OSX
-#ifdef __MACH__
-# include <mach/mach.h>
-#endif // __MACH__
-
-#include <sdf/sdf.hh>
-
-#include <gtest/gtest.h>
-#include <boost/thread.hpp>
-#include <boost/filesystem.hpp>
-
-#include <map>
-#include <string>
-
-#include "gazebo/transport/transport.hh"
-
-#include "gazebo/common/CommonIface.hh"
-#include "gazebo/common/SystemPaths.hh"
-#include "gazebo/common/Console.hh"
-#include "gazebo/physics/World.hh"
-#include "gazebo/physics/PhysicsTypes.hh"
-#include "gazebo/physics/PhysicsIface.hh"
-#include "gazebo/sensors/sensors.hh"
-#include "gazebo/rendering/rendering.hh"
-#include "gazebo/msgs/msgs.hh"
-
-#include "gazebo/gazebo_config.h"
-#include "gazebo/Server.hh"
-
-#include "test_config.h"
-
-using namespace gazebo;
-
-std::string custom_exec(std::string _cmd);
-
-class ServerFixture : public testing::Test
-{
- /// \brief Constructor
- protected: ServerFixture();
-
- /// \brief Tear down the test fixture. This gets called by gtest.
- protected: virtual void TearDown();
-
- /// \brief Unload the test fixture.
- protected: virtual void Unload();
-
- /// \brief Load a world based on a filename.
- /// \param[in] _worldFilename Name of the world to load.
- protected: virtual void Load(const std::string &_worldFilename);
-
- /// \brief Load a world based on a filename and set simulation
- /// paused/un-paused.
- /// \param[in] _worldFilename Name of the world to load.
- /// \param[in] _paused True to start the world paused.
- protected: virtual void Load(const std::string &_worldFilename, bool _paused);
-
- /// \brief Load a world based on a filename and set simulation
- /// paused/un-paused, and specify physics engine.
- /// \param[in] _worldFilename Name of the world to load.
- /// \param[in] _paused True to start the world paused.
- /// \param[in] _physics Name of the physics engine.
- protected: virtual void Load(const std::string &_worldFilename,
- bool _paused, const std::string &_physics);
-
- /// \brief Run the server.
- /// \param[in] _worldFilename Name of the world to run in simulation.
- protected: void RunServer(const std::string &_worldFilename);
-
- /// \brief Get a pointer to the rendering scene.
- /// \param[in] _sceneName Name of the scene to get.
- protected: rendering::ScenePtr GetScene(
- const std::string &_sceneName = "default");
-
- /// \brief Run the server, start paused/unpaused, and specify the physics
- /// engine.
- /// \param[in] _worldFilename Name of the world to load.
- /// \param[in] _paused True to start the world paused.
- /// \param[in] _physics Name of the physics engine.
- protected: void RunServer(const std::string &_worldFilename, bool _paused,
- const std::string &_physics);
-
- /// \brief Function that received world stastics messages.
- /// \param[in] _msg World statistics message.
- protected: void OnStats(ConstWorldStatisticsPtr &_msg);
-
- /// \brief Set a running simulation paused/unpaused.
- protected: void SetPause(bool _pause);
-
- /// \brief Get the real-time factor.
- /// \return The percent real time simulation is running.
- protected: double GetPercentRealTime() const;
-
- /// \brief Function that received poses messages from a running
- /// simulation.
- /// \param[in] _msg Pose message.
- protected: void OnPose(ConstPosesStampedPtr &_msg);
-
- /// \brief Get the pose of an entity.
- /// \param[in] _name Name of the entity.
- /// \return Pose of the named entity.
- protected: math::Pose GetEntityPose(const std::string &_name);
-
- /// \brief Return true if the named entity exists.
- /// \param[in] _name Name of the entity to check for.
- /// \return True if the entity exists.
- protected: bool HasEntity(const std::string &_name);
-
- /// \brief Print image data to screen. This is used to generate test data.
- /// \param[in] _name Name to associate with the printed data.
- /// \param[in] _image The raw image data.
- /// \param[in] _width Width of the image.
- /// \param[in] _height Height of the image.
- /// \param[in] _depth Pixel depth.
- protected: void PrintImage(const std::string &_name, unsigned char **_image,
- unsigned int _width, unsigned int _height, unsigned int _depth);
-
- /// \brief Print laser scan to screen. This is used to generate test data.
- /// \param[in] _name Name to associate with the printed data.
- /// \param[in] _scan The laser scan data.
- /// \param[in] _sampleCount Number of samples in the scan data.
- protected: void PrintScan(const std::string &_name, double *_scan,
- unsigned int _sampleCount);
-
- /// \brief Function to compare two float arrays (for example two laser
- /// scans).
- /// \param[in] _scanA First float array.
- /// \param[in] _scanB Second float array.
- /// \param[in] _sampleCount Number of samples in each float array.
- /// \param[out] _diffMax Maximum difference between the two arrays.
- /// \param[out] _diffSum Sum of the differences between the two arrays.
- /// \param[out] _diffAvg Average difference between the two arrays.
- protected: void FloatCompare(float *_scanA, float *_scanB,
- unsigned int _sampleCount, float &_diffMax,
- float &_diffSum, float &_diffAvg);
-
- /// \brief Function to compare two double arrays (for example two laser
- /// scans).
- /// \param[in] _scanA First double array.
- /// \param[in] _scanB Second double array.
- /// \param[in] _sampleCount Number of samples in each double array.
- /// \param[out] _diffMax Maximum difference between the two arrays.
- /// \param[out] _diffSum Sum of the differences between the two arrays.
- /// \param[out] _diffAvg Average difference between the two arrays.
- protected: void DoubleCompare(double *_scanA, double *_scanB,
- unsigned int _sampleCount, double &_diffMax,
- double &_diffSum, double &_diffAvg);
-
- /// \brief Function to compare two images.
- /// \param[in] _imageA First image to compare.
- /// \param[in] _imageB Second image to compare.
- /// \param[in] _width Width of both images.
- /// \param[in] _height Height of both images.
- /// \param[in] _depth Depth of both images.
- /// \param[out] _diffMax Maximum difference between the two arrays.
- /// \param[out] _diffSum Sum of the differences between the two arrays.
- /// \param[out] _diffAvg Average difference between the two arrays.
- protected: void ImageCompare(unsigned char *_imageA,
- unsigned char *_imageB,
- unsigned int _width, unsigned int _height, unsigned int _depth,
- unsigned int &_diffMax, unsigned int &_diffSum,
- double &_diffAvg);
-
- /// \brief Function that receives new image frames.
- /// \param[in] _image Image data.
- /// \param[in] _width Width of the image frame.
- /// \param[in] _height Height of the image frame.
- /// \param[in] _depth Depth of the image frame.
- /// \param[in] _format Pixel format.
- private: void OnNewFrame(const unsigned char *_image,
- unsigned int _width, unsigned int _height,
- unsigned int _depth,
- const std::string &/*_format*/);
-
- /// \brief Get an image frame from a camera.
- /// \param[in] _cameraName Name of the camera to get a frame from.
- /// \param[out] _imgData Array that receives the image data.
- /// \param[in] _width Width of the image frame.
- /// \param[in] _height Height of the image frame.
- protected: void GetFrame(const std::string &_cameraName,
- unsigned char **_imgData, unsigned int &_width,
- unsigned int &_height);
-
- /// \brief Spawn a camera.
- /// \param[in] _modelName Name of the model.
- /// \param[in] _cameraName Name of the camera.
- /// \param[in] _pos Camera position.
- /// \param[in] _rpy Camera roll, pitch, yaw.
- /// \param[in] _width Output image width.
- /// \param[in] _height Output image height.
- /// \param[in] _rate Output Hz.
- /// \param[in] _noiseType Type of noise to apply.
- /// \param[in] _noiseMean Mean noise value.
- /// \param[in] _noiseStdDev Standard deviation of the noise.
- /// \param[in] _distortionK1 Distortion coefficient k1.
- /// \param[in] _distortionK2 Distortion coefficient k2.
- /// \param[in] _distortionK3 Distortion coefficient k3.
- /// \param[in] _distortionP1 Distortion coefficient P1.
- /// \param[in] _distortionP2 Distortion coefficient p2.
- /// \param[in] _cx Normalized optical center x, used for distortion.
- /// \param[in] _cy Normalized optical center y, used for distortion.
- protected: void SpawnCamera(const std::string &_modelName,
- const std::string &_cameraName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- unsigned int _width = 320, unsigned int _height = 240,
- double _rate = 25,
- const std::string &_noiseType = "",
- double _noiseMean = 0.0, double _noiseStdDev = 0.0,
- bool _distortion = false, double _distortionK1 = 0.0,
- double _distortionK2 = 0.0, double _distortionK3 = 0.0,
- double _distortionP1 = 0.0, double _distortionP2 = 0.0,
- double _cx = 0.5, double _cy = 0.5);
-
- /// \brief Spawn a laser.
- /// \param[in] _modelName Name of the model.
- /// \param[in] _raySensorName Name of the laser.
- /// \param[in] _pos Camera position.
- /// \param[in] _rpy Camera roll, pitch, yaw.
- /// \param[in] _hMinAngle Horizontal min angle
- /// \param[in] _hMaxAngle Horizontal max angle
- /// \param[in] _minRange Min range
- /// \param[in] _maxRange Max range
- /// \param[in] _rangeResolution Resolution of the scan
- /// \param[in] _samples Number of samples.
- /// \param[in] _rate Output Hz.
- /// \param[in] _noiseType Type of noise to apply.
- /// \param[in] _noiseMean Mean noise value.
- /// \param[in] _noiseStdDev Standard deviation of the noise.
- protected: void SpawnRaySensor(const std::string &_modelName,
- const std::string &_raySensorName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
- double _vMinAngle = -1.0, double _vMaxAngle = 1.0,
- double _minRange = 0.08, double _maxRange = 10,
- double _rangeResolution = 0.01, unsigned int _samples = 640,
- unsigned int _vSamples = 1, double _hResolution = 1.0,
- double _vResolution = 1.0,
- const std::string &_noiseType = "", double _noiseMean = 0.0,
- double _noiseStdDev = 0.0);
-
- /// \brief Spawn a gpu laser.
- /// \param[in] _modelName Name of the model.
- /// \param[in] _raySensorName Name of the laser.
- /// \param[in] _pos Camera position.
- /// \param[in] _rpy Camera roll, pitch, yaw.
- /// \param[in] _hMinAngle Horizontal min angle
- /// \param[in] _hMaxAngle Horizontal max angle
- /// \param[in] _minRange Min range
- /// \param[in] _maxRange Max range
- /// \param[in] _rangeResolution Resolution of the scan
- /// \param[in] _samples Number of samples.
- /// \param[in] _rate Output Hz.
- /// \param[in] _noiseType Type of noise to apply.
- /// \param[in] _noiseMean Mean noise value.
- /// \param[in] _noiseStdDev Standard deviation of the noise.
- protected: void SpawnGpuRaySensor(const std::string &_modelName,
- const std::string &_raySensorName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- double _hMinAngle = -2.0, double _hMaxAngle = 2.0,
- double _minRange = 0.08, double _maxRange = 10,
- double _rangeResolution = 0.01, unsigned int _samples = 640,
- const std::string &_noiseType = "", double _noiseMean = 0.0,
- double _noiseStdDev = 0.0);
-
- /// \brief Spawn an imu sensor laser.
- /// \param[in] _modelName Name of the model.
- /// \param[in] _imuSensorName Name of the imu sensor.
- /// \param[in] _pos Camera position.
- /// \param[in] _rpy Camera roll, pitch, yaw.
- /// \param[in] _noiseType Type of noise to apply.
- /// \param[in] _noiseMean Mean noise value.
- /// \param[in] _noiseStdDev Standard deviation of the noise.
- /// \param[in] _accelNoiseMean Acceleration based noise mean.
- /// \param[in] _accelNoiseStdDev Acceleration based noise standard
- /// deviation.
- /// \param[in] _accelBiasMean Acceleration mean bias
- /// \param[in] _accelBiasStdDev Acceleration standard deviation bias
- protected: void SpawnImuSensor(const std::string &_modelName,
- const std::string &_imuSensorName,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- const std::string &_noiseType = "",
- double _rateNoiseMean = 0.0, double _rateNoiseStdDev = 0.0,
- double _rateBiasMean = 0.0, double _rateBiasStdDev = 0.0,
- double _accelNoiseMean = 0.0, double _accelNoiseStdDev = 0.0,
- double _accelBiasMean = 0.0, double _accelBiasStdDev = 0.0);
-
- /// \brief Spawn a contact sensor with the specified collision geometry
- /// \param[in] _name Model name
- /// \param[in] _sensorName Sensor name
- /// \param[in] _collisionType Type of collision, box or cylinder
- /// \param[in] _pos World position
- /// \param[in] _rpy World rotation in Euler angles
- /// \param[in] _static True to make the model static
- protected: void SpawnUnitContactSensor(const std::string &_name,
- const std::string &_sensorName,
- const std::string &_collisionType, const math::Vector3 &_pos,
- const math::Vector3 &_rpy, bool _static = false);
-
- /// \brief Spawn an IMU sensor on a link
- /// \param[in] _name Model name
- /// \param[in] _sensorName Sensor name
- /// \param[in] _collisionType Type of collision, box or cylinder
- /// \param[in] _topic Topic to publish IMU data to
- /// \param[in] _pos World position
- /// \param[in] _rpy World rotation in Euler angles
- /// \param[in] _static True to make the model static
- protected: void SpawnUnitImuSensor(const std::string &_name,
- const std::string &_sensorName,
- const std::string &_collisionType,
- const std::string &_topic, const math::Vector3 &_pos,
- const math::Vector3 &_rpy, bool _static = false);
-
- /// \brief generate a gtest failure from a timeout error and display a
- /// log message about the problem.
- /// \param[in] log_msg: error msg related to the timeout
- /// \param[in] timeoutCS: failing period (in centiseconds)
- private: void launchTimeoutFailure(const char *_logMsg, const int _timeoutCS);
-
- /// \brief Spawn an Wireless transmitter sensor on a link
- /// \param[in] _name Model name
- /// \param[in] _sensorName Sensor name
- /// \param[in] _pos World position
- /// \param[in] _rpy World rotation in Euler angles
- /// \param[in] _essid Service set identifier (network name)
- /// \param[in] _freq Frequency of transmission (MHz)
- /// \param[in] _power Transmission power (dBm)
- /// \param[in] _gain Antenna gain (dBi)
- /// \param[in] _visualize Enable sensor visualization
- protected: void SpawnWirelessTransmitterSensor(const std::string &_name,
- const std::string &_sensorName,
- const math::Vector3 &_pos,
- const math::Vector3 &_rpy,
- const std::string &_essid,
- double _freq,
- double _power,
- double _gain,
- bool _visualize = true);
-
- /// \brief Spawn an Wireless receiver sensor on a link
- /// \param[in] _name Model name
- /// \param[in] _sensorName Sensor name
- /// \param[in] _pos World position
- /// \param[in] _rpy World rotation in Euler angles
- /// \param[in] _minFreq Minimum frequency to be filtered (MHz)
- /// \param[in] _maxFreq Maximum frequency to be filtered (MHz)
- /// \param[in] _power Transmission power (dBm)
- /// \param[in] _gain Antenna gain (dBi)
- /// \param[in] _sensitivity Receiver sensitibity (dBm)
- /// \param[in] _visualize Enable sensor visualization
- protected: void SpawnWirelessReceiverSensor(const std::string &_name,
- const std::string &_sensorName,
- const math::Vector3 &_pos,
- const math::Vector3 &_rpy,
- double _minFreq,
- double _maxFreq,
- double _power,
- double _gain,
- double _sensitivity,
- bool _visualize = true);
-
- /// \brief Wait for a number of ms. and attempts until the entity is spawned
- /// \param[in] _name Model name
- /// \param[in] _sleepEach Number of milliseconds to sleep in each iteration
- /// \param[in] _retries Number of iterations until give up
- protected: void WaitUntilEntitySpawn(const std::string &_name,
- unsigned int _sleepEach,
- int _retries);
-
- /// \brief Wait for a number of ms. and attempts until the sensor is spawned
- /// \param[in] _name Sensor name
- /// \param[in] _sleepEach Number of milliseconds to sleep in each iteration
- /// \param[in] _retries Number of iterations until give up
- protected: void WaitUntilSensorSpawn(const std::string &_name,
- unsigned int _sleepEach,
- int _retries);
-
- /// \brief Spawn a light.
- /// \param[in] _name Name for the light.
- /// \param[in] _size Type of light - "spot", "directional", or "point".
- /// \param[in] _pos Position for the light.
- /// \param[in] _rpy Roll, pitch, yaw for the light.
- /// \param[in] _diffuse Diffuse color of the light.
- /// \param[in] _specular Specular color of the light.
- /// \param[in] _direction Direction of the light ("spot" and "directional").
- /// \param[in] _attenuationRange Range of attenuation.
- /// \param[in] _attenuationConstant Constant component of attenuation
- /// \param[in] _attenuationLinear Linear component of attenuation
- /// \param[in] _attenuationQuadratic Quadratic component of attenuation
- /// \param[in] _spotInnerAngle Inner angle ("spot" only).
- /// \param[in] _spotOuterAngle Outer angle ("spot" only).
- /// \param[in] _spotFallOff Fall off ("spot" only).
- /// \param[in] _castShadows True to cast shadows.
- protected: void SpawnLight(const std::string &_name, const std::string &_type,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- const common::Color &_diffuse = common::Color::White,
- const common::Color &_specular = common::Color::White,
- const math::Vector3 &_direction = -math::Vector3::UnitZ,
- double _attenuationRange = 20,
- double _attenuationConstant = 0.5,
- double _attenuationLinear = 0.01,
- double _attenuationQuadratic = 0.001,
- double _spotInnerAngle = 0,
- double _spotOuterAngle = 0,
- double _spotFallOff = 0,
- bool _castShadows = true);
-
- /// \brief Spawn a cylinder
- /// \param[in] _name Name for the model.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _static True to make the model static.
- protected: void SpawnCylinder(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _static = false);
-
- /// \brief Spawn a sphere
- /// \param[in] _name Name for the model.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _static True to make the model static.
- /// \param[in] _wait True to wait for the sphere to spawn before
- /// returning.
- protected: void SpawnSphere(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _wait = true, bool _static = false);
-
- /// \brief Spawn a sphere
- /// \param[in] _name Name for the model.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _cog Center of gravity.
- /// \param[in] _radius Sphere radius.
- /// \param[in] _static True to make the model static.
- /// \param[in] _wait True to wait for the sphere to spawn before
- /// returning.
- protected: void SpawnSphere(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- const math::Vector3 &_cog, double _radius,
- bool _wait = true, bool _static = false);
-
- /// \brief Spawn a box.
- /// \param[in] _name Name for the model.
- /// \param[in] _size Size of the box.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _static True to make the model static.
- protected: void SpawnBox(const std::string &_name,
- const math::Vector3 &_size, const math::Vector3 &_pos,
- const math::Vector3 &_rpy, bool _static = false);
-
- /// \brief Spawn a triangle mesh.
- /// \param[in] _name Name for the model.
- /// \param[in] _modelPath Path to the mesh file.
- /// \param[in] _scale Scaling factor.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _static True to make the model static.
- protected: void SpawnTrimesh(const std::string &_name,
- const std::string &_modelPath, const math::Vector3 &_scale,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _static = false);
-
- /// \brief Spawn an empty link.
- /// \param[in] _name Name for the model.
- /// \param[in] _pos Position for the model.
- /// \param[in] _rpy Roll, pitch, yaw for the model.
- /// \param[in] _static True to make the model static.
- protected: void SpawnEmptyLink(const std::string &_name,
- const math::Vector3 &_pos, const math::Vector3 &_rpy,
- bool _static = false);
-
- /// \brief Spawn a model from file.
- /// \param[in] _filename File to load a model from.
- protected: void SpawnModel(const std::string &_filename);
-
- /// \brief Send a factory message based on an SDF string.
- /// \param[in] _sdf SDF string to publish.
- protected: void SpawnSDF(const std::string &_sdf);
-
- /// \brief Load a plugin.
- /// \param[in] _filename Plugin filename to load.
- /// \param[in] _name Name to associate with with the plugin.
- protected: void LoadPlugin(const std::string &_filename,
- const std::string &_name);
-
- /// \brief Get a pointer to a model.
- /// \param[in] _name Name of the model to get.
- /// \return Pointer to the model, or NULL if the model was not found.
- protected: physics::ModelPtr GetModel(const std::string &_name);
-
- /// \brief Remove a model by name.
- /// \param[in] _name Name of the model to remove.
- protected: void RemoveModel(const std::string &_name);
-
- /// \brief Remove a plugin.
- /// \param[in] _name Name of the plugin to remove.
- protected: void RemovePlugin(const std::string &_name);
-
- /// \brief Get the current memory information.
- /// \param[out] _resident Resident memory.
- /// \param[out] _share Shared memory.
- protected: void GetMemInfo(double &_resident, double &_share);
-
- /// \brief Pointer the Gazebo server.
- protected: Server *server;
-
- /// \brief Pointer the thread the runs the server.
- protected: boost::thread *serverThread;
-
- /// \brief Pointer to a node for communication.
- protected: transport::NodePtr node;
-
- /// \brief Pose subscription.
- protected: transport::SubscriberPtr poseSub;
-
- /// \brief World statistics subscription.
- protected: transport::SubscriberPtr statsSub;
-
- /// \brief Factory publisher.
- protected: transport::PublisherPtr factoryPub;
-
- /// \brief Request publisher.
- protected: transport::PublisherPtr requestPub;
-
- /// \brief Map of received poses.
- protected: std::map<std::string, math::Pose> poses;
-
- /// \brief Mutex to protect data structures that store messages.
- protected: boost::mutex receiveMutex;
-
- /// \brief Image data
- private: unsigned char **imgData;
-
- /// \brief Increments when images are received.
- private: int gotImage;
-
- /// \brief Current simulation time, real time, and pause time.
- protected: common::Time simTime, realTime, pauseTime;
-
- /// \brief Current percent realtime.
- private: double percentRealTime;
-
- /// \brief True if paused.
- private: bool paused;
-
- /// \brief True if server is running.
- private: bool serverRunning;
-};
-#endif // define _GAZEBO_SERVER_FIXTURE_HH_
diff --git a/test/cmake/plugin/CMakeLists.txt b/test/cmake/plugin/CMakeLists.txt
index 7a70409..e62fd7a 100644
--- a/test/cmake/plugin/CMakeLists.txt
+++ b/test/cmake/plugin/CMakeLists.txt
@@ -19,5 +19,7 @@ endif()
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+
add_library(hello_world SHARED ../../testfiles/hello_world.cc)
target_link_libraries(hello_world ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
diff --git a/test/data/svg/arc_circle.svg b/test/data/svg/arc_circle.svg
new file mode 100644
index 0000000..9269847
--- /dev/null
+++ b/test/data/svg/arc_circle.svg
@@ -0,0 +1,39 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.094488189"
+ height="1052.36220472"
+ id="svg3336"
+ version="1.1"
+ inkscape:version="0.48.4 r9939"
+ sodipodi:docname="ghost_edge.svg">
+ <metadata
+ id="metadata3503">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title />
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <desc
+ id="desc3338">C:\Users\bhuynh\Documents\SRI_ChassisBot.DXF - scale = 76.363636, origin = (-1.375000, -3.000000), auto = True</desc>
+ <g
+ inkscape:label="0"
+ inkscape:groupmode="layer"
+ id="g3355">
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 184.332498,-554.205440 A 15.220115,15.220115 -0.000000 1 0 153.892269,-554.205440 15.220115,15.220115 -0.000000 1 0 184.332498,-554.205440 z"
+ id="path3385" />
+ </g>
+</svg>
diff --git a/test/data/svg/arc_test.svg b/test/data/svg/arc_test.svg
new file mode 100644
index 0000000..e6cd4d3
--- /dev/null
+++ b/test/data/svg/arc_test.svg
@@ -0,0 +1,120 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.094488189"
+ height="1052.36220472"
+ id="svg3336"
+ version="1.1"
+ inkscape:version="0.48.4 r9939"
+ sodipodi:docname="svg_import_test.svg">
+ <metadata
+ id="metadata3503">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title></dc:title>
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <sodipodi:namedview
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1"
+ objecttolerance="10"
+ gridtolerance="10"
+ guidetolerance="10"
+ inkscape:pageopacity="0"
+ inkscape:pageshadow="2"
+ inkscape:window-width="1313"
+ inkscape:window-height="983"
+ id="namedview3501"
+ showgrid="false"
+ inkscape:zoom="0.44613918"
+ inkscape:cx="212.36708"
+ inkscape:cy="805.29074"
+ inkscape:window-x="1247"
+ inkscape:window-y="24"
+ inkscape:window-maximized="0"
+ inkscape:current-layer="g3355" />
+ <desc
+ id="desc3338">C:\Users\bhuynh\Documents\SRI_ChassisBot.DXF - scale = 76.363636, origin = (-1.375000, -3.000000), auto = True</desc>
+ <defs
+ id="defs3340">
+ <marker
+ refY="0.0"
+ refX="0.0"
+ style="overflow:visible"
+ id="DistanceX"
+ orient="auto">
+ <path
+ style="stroke:#000000; stroke-width:0.5"
+ d="M 3,-3 L -3,3 M 0,-5 L 0,5"
+ id="path3343" />
+ </marker>
+ <pattern
+ height="8"
+ width="8"
+ patternUnits="userSpaceOnUse"
+ y="0"
+ x="0"
+ id="Hatch">
+ <path
+ linecap="square"
+ stroke="#000000"
+ stroke-width="0.25"
+ d="M8 4 l-4,4"
+ id="path3346" />
+ <path
+ linecap="square"
+ stroke="#000000"
+ stroke-width="0.25"
+ d="M6 2 l-4,4"
+ id="path3348" />
+ <path
+ linecap="square"
+ stroke="#000000"
+ stroke-width="0.25"
+ d="M4 0 l-4,4"
+ id="path3350" />
+ </pattern>
+ <symbol
+ id="*Model_Space" />
+ <symbol
+ id="*Paper_Space" />
+ <symbol
+ id="*Paper_Space0" />
+ </defs>
+ <g
+ inkscape:label="0"
+ inkscape:groupmode="layer"
+ id="g3355">
+
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 397.075877,-0.826772 A 25.028633,25.028633 -0.000000 0 0 347.018611,-0.826772"
+ id="path3471" />
+
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 397.075877, -0.826772 A 25.028633,25.028633 -0.000000 0 0 347.018611, -0.8"
+ id="path3471" />
+
+<!-- correct
+
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 397,-435 A 25.028633,25.028633 -0.000000 0 0 347,-435.8"
+ id="path1" />
+-->
+
+ </g>
+</svg>
diff --git a/test/data/svg/capsule.svg b/test/data/svg/capsule.svg
new file mode 100644
index 0000000..11db3c8
--- /dev/null
+++ b/test/data/svg/capsule.svg
@@ -0,0 +1,124 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.094488189"
+ height="1052.36220472"
+ id="svg3336"
+ version="1.1"
+ inkscape:version="0.48.4 r9939"
+ sodipodi:docname="svg_import_test.svg">
+ <metadata
+ id="metadata3503">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title></dc:title>
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <sodipodi:namedview
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1"
+ objecttolerance="10"
+ gridtolerance="10"
+ guidetolerance="10"
+ inkscape:pageopacity="0"
+ inkscape:pageshadow="2"
+ inkscape:window-width="2495"
+ inkscape:window-height="1576"
+ id="namedview3501"
+ showgrid="false"
+ inkscape:zoom="0.44613918"
+ inkscape:cx="212.36708"
+ inkscape:cy="805.29074"
+ inkscape:window-x="2625"
+ inkscape:window-y="24"
+ inkscape:window-maximized="1"
+ inkscape:current-layer="g3355" />
+ <desc
+ id="desc3338">C:\Users\bhuynh\Documents\SRI_ChassisBot.DXF - scale = 76.363636, origin = (-1.375000, -3.000000), auto = True</desc>
+ <defs
+ id="defs3340">
+ <marker
+ refY="0.0"
+ refX="0.0"
+ style="overflow:visible"
+ id="DistanceX"
+ orient="auto">
+ <path
+ style="stroke:#000000; stroke-width:0.5"
+ d="M 3,-3 L -3,3 M 0,-5 L 0,5"
+ id="path3343" />
+ </marker>
+ <pattern
+ height="8"
+ width="8"
+ patternUnits="userSpaceOnUse"
+ y="0"
+ x="0"
+ id="Hatch">
+ <path
+ linecap="square"
+ stroke="#000000"
+ stroke-width="0.25"
+ d="M8 4 l-4,4"
+ id="path3346" />
+ <path
+ linecap="square"
+ stroke="#000000"
+ stroke-width="0.25"
+ d="M6 2 l-4,4"
+ id="path3348" />
+ <path
+ linecap="square"
+ stroke="#000000"
+ stroke-width="0.25"
+ d="M4 0 l-4,4"
+ id="path3350" />
+ </pattern>
+ <symbol
+ id="*Model_Space" />
+ <symbol
+ id="*Paper_Space" />
+ <symbol
+ id="*Paper_Space0" />
+ </defs>
+ <g
+ inkscape:label="0"
+ inkscape:groupmode="layer"
+ id="g3355">
+
+ <!-- vertical line left -->
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 347.018611,-435.826772 347.018611,105.332856"
+ id="path3389" />
+
+ <!-- vertical line right -->
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 397.075877,105.332856 397.075877,-435.826772"
+ id="path3391" />
+ <!-- bottom arc -->
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 347.018611,105.332856 A 25.028633,25.028633 -0.000000 0 0 397.075877,105.332856"
+ id="path3469" />
+ <!-- top arc -->
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 397.075877,-435.826772 A 25.028633,25.028633 -0.000000 0 0 347.018611,-435.826772"
+ id="path3471" />
+
+ </g>
+</svg>
diff --git a/test/data/svg/chassis.svg b/test/data/svg/chassis.svg
new file mode 100644
index 0000000..e4106ac
--- /dev/null
+++ b/test/data/svg/chassis.svg
@@ -0,0 +1,396 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.094488189"
+ height="1052.36220472"
+ id="svg3336"
+ version="1.1"
+ inkscape:version="0.48.4 r9939"
+ sodipodi:docname="svg_import_test.svg">
+ <metadata
+ id="metadata3503">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title></dc:title>
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <sodipodi:namedview
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1"
+ objecttolerance="10"
+ gridtolerance="10"
+ guidetolerance="10"
+ inkscape:pageopacity="0"
+ inkscape:pageshadow="2"
+ inkscape:window-width="2495"
+ inkscape:window-height="1576"
+ id="namedview3501"
+ showgrid="false"
+ inkscape:zoom="0.44613918"
+ inkscape:cx="212.36708"
+ inkscape:cy="805.29074"
+ inkscape:window-x="2625"
+ inkscape:window-y="24"
+ inkscape:window-maximized="1"
+ inkscape:current-layer="g3355" />
+ <desc
+ id="desc3338">C:\Users\bhuynh\Documents\SRI_ChassisBot.DXF - scale = 76.363636, origin = (-1.375000, -3.000000), auto = True</desc>
+ <defs
+ id="defs3340">
+ <marker
+ refY="0.0"
+ refX="0.0"
+ style="overflow:visible"
+ id="DistanceX"
+ orient="auto">
+ <path
+ style="stroke:#000000; stroke-width:0.5"
+ d="M 3,-3 L -3,3 M 0,-5 L 0,5"
+ id="path3343" />
+ </marker>
+ <pattern
+ height="8"
+ width="8"
+ patternUnits="userSpaceOnUse"
+ y="0"
+ x="0"
+ id="Hatch">
+ <path
+ linecap="square"
+ stroke="#000000"
+ stroke-width="0.25"
+ d="M8 4 l-4,4"
+ id="path3346" />
+ <path
+ linecap="square"
+ stroke="#000000"
+ stroke-width="0.25"
+ d="M6 2 l-4,4"
+ id="path3348" />
+ <path
+ linecap="square"
+ stroke="#000000"
+ stroke-width="0.25"
+ d="M4 0 l-4,4"
+ id="path3350" />
+ </pattern>
+ <symbol
+ id="*Model_Space" />
+ <symbol
+ id="*Paper_Space" />
+ <symbol
+ id="*Paper_Space0" />
+ </defs>
+ <g
+ inkscape:label="0"
+ inkscape:groupmode="layer"
+ id="g3355">
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 184.332498,1035.450966 A 15.220115,15.220115 -0.000000 1 0 153.892269,1035.450966 15.220115,15.220115 -0.000000 1 0 184.332498,1035.450966 z"
+ id="path3357" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 590.202219,-554.205440 A 15.220115,15.220115 -0.000000 1 0 559.761990,-554.205440 15.220115,15.220115 -0.000000 1 0 590.202219,-554.205440 z"
+ id="path3359" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 149.138480,440.894436 A 16.778079,16.778079 -0.000000 1 0 115.582322,440.894436 16.778079,16.778079 -0.000000 1 0 149.138480,440.894436 z"
+ id="path3361" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 628.512166,40.351090 A 16.778079,16.778079 -0.000000 1 0 594.956008,40.351090 16.778079,16.778079 -0.000000 1 0 628.512166,40.351090 z"
+ id="path3363" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 742.403364,-368.181818 A 15.220115,15.220115 -0.000000 1 0 711.963135,-368.181818 15.220115,15.220115 -0.000000 1 0 742.403364,-368.181818 z"
+ id="path3365" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 742.403364,443.557623 A 15.220115,15.220115 -0.000000 1 0 711.963135,443.557623 15.220115,15.220115 -0.000000 1 0 742.403364,443.557623 z"
+ id="path3367" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 32.131353,849.427344 A 15.220115,15.220115 -0.000000 1 0 1.691124,849.427344 15.220115,15.220115 -0.000000 1 0 32.131353,849.427344 z"
+ id="path3369" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 32.131353,37.687903 A 15.220115,15.220115 -0.000000 1 0 1.691124,37.687903 15.220115,15.220115 -0.000000 1 0 32.131353,37.687903 z"
+ id="path3371" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 32.131353,-368.181818 A 15.220115,15.220115 -0.000000 1 0 1.691124,-368.181818 15.220115,15.220115 -0.000000 1 0 32.131353,-368.181818 z"
+ id="path3373" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 32.131353,443.557623 A 15.220115,15.220115 -0.000000 1 0 1.691124,443.557623 15.220115,15.220115 -0.000000 1 0 32.131353,443.557623 z"
+ id="path3375" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 742.403364,849.427344 A 15.220115,15.220115 -0.000000 1 0 711.963135,849.427344 15.220115,15.220115 -0.000000 1 0 742.403364,849.427344 z"
+ id="path3377" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 742.403364,37.687903 A 15.220115,15.220115 -0.000000 1 0 711.963135,37.687903 15.220115,15.220115 -0.000000 1 0 742.403364,37.687903 z"
+ id="path3379" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 149.138480,40.351090 A 16.778079,16.778079 -0.000000 1 0 115.582322,40.351090 16.778079,16.778079 -0.000000 1 0 149.138480,40.351090 z"
+ id="path3381" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 628.512166,440.894436 A 16.778079,16.778079 -0.000000 1 0 594.956008,440.894436 16.778079,16.778079 -0.000000 1 0 628.512166,440.894436 z"
+ id="path3383" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 184.332498,-554.205440 A 15.220115,15.220115 -0.000000 1 0 153.892269,-554.205440 15.220115,15.220115 -0.000000 1 0 184.332498,-554.205440 z"
+ id="path3385" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 590.202219,1035.450966 A 15.220115,15.220115 -0.000000 1 0 559.761990,1035.450966 15.220115,15.220115 -0.000000 1 0 590.202219,1035.450966 z"
+ id="path3387" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 347.018611,-435.826772 347.018611,105.332856"
+ id="path3389" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 397.075877,105.332856 397.075877,-435.826772"
+ id="path3391" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 665.379708,820.550423 496.272177,451.588537"
+ id="path3393" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 450.766877,472.445132 619.874408,841.407018"
+ id="path3395" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 78.714781,-339.304897 247.822312,29.656989"
+ id="path3397" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 293.327611,8.800394 124.220080,-360.161492"
+ id="path3399" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 507.337151,1018.539728 507.337151,1052.362205"
+ id="path3401" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 236.757337,1018.539728 507.337151,1018.539728"
+ id="path3403" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 236.757337,1052.362205 236.757337,1018.539728"
+ id="path3405" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 0.000000,1052.362205 236.757337,1052.362205"
+ id="path3407" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 0.000000,781.782391 0.000000,1052.362205"
+ id="path3409" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 33.822477,781.782391 0.000000,781.782391"
+ id="path3411" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 33.822477,511.202577 33.822477,781.782391"
+ id="path3413" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 0.000000,511.202577 33.822477,511.202577"
+ id="path3415" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 0.000000,-29.957051 0.000000,511.202577"
+ id="path3417" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 33.822477,-29.957051 0.000000,-29.957051"
+ id="path3419" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 33.822477,-300.536865 33.822477,-29.957051"
+ id="path3421" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 0.000000,-300.536865 33.822477,-300.536865"
+ id="path3423" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 0.000000,-571.116679 0.000000,-300.536865"
+ id="path3425" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 236.757337,-571.116679 0.000000,-571.116679"
+ id="path3427" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 236.757337,-537.294202 236.757337,-571.116679"
+ id="path3429" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 507.337151,-537.294202 236.757337,-537.294202"
+ id="path3431" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 507.337151,-571.116679 507.337151,-537.294202"
+ id="path3433" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 744.094488,-571.116679 507.337151,-571.116679"
+ id="path3435" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 744.094488,-300.536865 744.094488,-571.116679"
+ id="path3437" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 710.272011,-300.536865 744.094488,-300.536865"
+ id="path3439" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 710.272011,-29.957051 710.272011,-300.536865"
+ id="path3441" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 744.094488,-29.957051 710.272011,-29.957051"
+ id="path3443" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 744.094488,511.202577 744.094488,-29.957051"
+ id="path3445" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 710.272011,511.202577 744.094488,511.202577"
+ id="path3447" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 710.272011,781.782391 710.272011,511.202577"
+ id="path3449" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 744.094488,781.782391 710.272011,781.782391"
+ id="path3451" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 744.094488,1052.362205 744.094488,781.782391"
+ id="path3453" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 507.337151,1052.362205 744.094488,1052.362205"
+ id="path3455" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 619.874408,-360.161492 450.766877,8.800394"
+ id="path3457" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 496.272177,29.656989 665.379708,-339.304897"
+ id="path3459" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 124.220080,841.407018 293.327611,472.445132"
+ id="path3461" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 247.822312,451.588537 78.714781,820.550423"
+ id="path3463" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 347.018611,375.912670 347.018611,917.072298"
+ id="path3465" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 397.075877,917.072298 397.075877,375.912670"
+ id="path3467" />
+
+<!--
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 347.018611,105.332856 A 25.028633,25.028633 -0.000000 0 0 397.075877,105.332856"
+ id="path3469" />
+
+-->
+
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 347.018611,105.332856 A 25.028633,25.028633 -0.000000 0 0 397.075877,105.3"
+ id="path3469" />
+
+
+
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 397.075877,-435.826772 A 25.028633,25.028633 -0.000000 0 0 347.018611,-435.8"
+ id="path3471" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 496.272177,451.588537 A 25.028633,25.028633 -0.000000 0 0 450.766877,472.445133"
+ id="path3473" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 619.874408,841.407018 A 25.028633,25.028633 -0.000000 0 0 665.379708,820.550423"
+ id="path3475" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 247.822312,29.656989 A 25.028633,25.028633 -0.000000 0 0 293.327611,8.800394"
+ id="path3477" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 124.220080,-360.161492 A 25.028633,25.028633 -0.000000 0 0 78.714781,-339.304897"
+ id="path3479" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 450.766877,8.800394 A 25.028633,25.028633 -0.000000 0 0 496.272177,29.656989"
+ id="path3481" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 665.379708,-339.304897 A 25.028633,25.028633 -0.000000 1 0 619.874408,-360.161492"
+ id="path3483" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 293.327611,472.445133 A 25.028633,25.028633 -0.000000 1 0 247.822312,451.588537"
+ id="path3485" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 78.714781,820.550423 A 25.028633,25.028633 -0.000000 0 0 124.220080,841.407018"
+ id="path3487" />
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 347.018611,917.072298 A 25.028633,25.028633 -0.000000 0 0 397.075877,917.072298"
+ id="path3489" />
+
+
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 397.075877,375.912670 A 25.028633,25.028633 -0.000000 0 0 347.018611,375.9"
+ id="path3491" />
+<!--
+ <path
+ style="stroke:#000000;stroke-width:0.5;fill:none;stroke-linecap: round"
+ d="M 397.075877,375.912670 A 25.028633,25.028633 -0.000000 0 0 347.018611,375.912670"
+ id="path3491" />
+-->
+
+
+
+ </g>
+</svg>
diff --git a/test/data/svg/loader.svg b/test/data/svg/loader.svg
new file mode 100644
index 0000000..86a2ae4
--- /dev/null
+++ b/test/data/svg/loader.svg
@@ -0,0 +1,75 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.09448819"
+ height="1052.3622047"
+ id="svg2"
+ version="1.1"
+ inkscape:version="0.48.4 r9939"
+ sodipodi:docname="loader.svg">
+ <defs
+ id="defs4" />
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.5966942"
+ inkscape:cx="280"
+ inkscape:cy="541.42857"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="false"
+ inkscape:window-width="1053"
+ inkscape:window-height="699"
+ inkscape:window-x="65"
+ inkscape:window-y="356"
+ inkscape:window-maximized="0" />
+ <metadata
+ id="metadata7">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title />
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1">
+ <g
+ transform="scale(1.1026416,0.90691302)"
+ style="font-size:346.61199951px;font-style:normal;font-weight:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:none;stroke:#000000;stroke-opacity:1;font-family:Sans"
+ id="theId">
+ <path
+ d="m 227.46714,109.66962 -46.37289,125.7484 92.91503,0 -46.54214,-125.7484 m -19.29383,-33.679582 38.75691,0 96.29992,252.681502 -35.54127,0 -23.01721,-64.82051 -113.9013,0 -23.01721,64.82051 -36.049,0 96.46916,-252.681502"
+ style=""
+ id="letterA" />
+ </g>
+ <path
+ style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+ d="m 322.85714,500.93361 c 87.70196,94.71561 -118.37707,52.81899 -140.26915,23.99586 -21.34058,34.06478 -39.11208,-31.6907 -49.82919,-47.95031 -0.67344,-0.67344 -20.34623,35.74589 -21.01967,35.07246 -0.67343,-0.67344 10.06785,-44.7717 9.39441,-45.44513 -24.551826,-24.31231 -34.308841,-68.64285 24.58075,-71.38716 0.67343,-0.67344 178.49379,92.66562 178.49379,91.71324 z"
+ id="path2984"
+ inkscape:connector-curvature="0"
+ sodipodi:nodetypes="cccccccc" />
+ <path
+ style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+ d="m 132.14804,419.80525 46.34575,3.75776 -5.01035,35.07247 -46.34576,-8.76812 z"
+ id="path2988"
+ inkscape:connector-curvature="0" />
+ </g>
+</svg>
diff --git a/test/examples/CMakeLists.txt b/test/examples/CMakeLists.txt
index 276d88f..bfd4b56 100644
--- a/test/examples/CMakeLists.txt
+++ b/test/examples/CMakeLists.txt
@@ -1,3 +1,5 @@
-set (tests example_plugins.cc)
+set (tests
+ examples_build.cc
+)
add_definitions("-DCMAKE_SOURCE_DIR=\"${CMAKE_SOURCE_DIR}\"")
gz_build_tests(${tests})
diff --git a/test/examples/example_plugins.cc b/test/examples/example_plugins.cc
deleted file mode 100644
index 57f6dd8..0000000
--- a/test/examples/example_plugins.cc
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * 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 <gtest/gtest.h>
-#include <stdio.h>
-#include <string>
-#include <boost/filesystem.hpp>
-
-#include "gazebo/common/Console.hh"
-
-///////////////////////////////////////////////////////////////////
-// Create a temporary build folder
-boost::filesystem::path createTempBuildFolder(const std::string &_prefix)
-{
- boost::filesystem::path path = boost::filesystem::temp_directory_path();
- path /= boost::filesystem::unique_path(_prefix + "-%%%%-%%%%-%%%%-%%%%");
- boost::filesystem::create_directories(path);
- gzdbg << "mkdir " << path.string() << std::endl;
- return path;
-}
-
-///////////////////////////////////////////////////////////////////
-// Get path to source folder with specified suffix
-boost::filesystem::path getSourcePath(const std::string &_suffix)
-{
- boost::filesystem::path path = CMAKE_SOURCE_DIR;
- path /= std::string("examples");
- path /= std::string("plugins");
- path /= _suffix;
- gzdbg << "source " << path.string() << std::endl;
- return path;
-}
-
-class ExamplePlugins : public ::testing::TestWithParam<const char*>
-{
- /// \brief Build plugin in subfolder _name in a temporary build folder
- /// \param[in] _name Subfolder to build.
- public: void Build(const std::string &_name);
-};
-
-///////////////////////////////////////////////////////////////////
-// Build plugin in subfolder _name in a temporary build folder
-void ExamplePlugins::Build(const std::string &_name)
-{
- // get a unique temporary build folder name
- boost::filesystem::path build = createTempBuildFolder(_name);
-
- // construct path of source folder
- boost::filesystem::path source = getSourcePath(_name);
-
- char cmd[1024];
-
- // cd build && cmake source
- snprintf(cmd, sizeof(cmd), "cd %s && cmake %s && make",
- build.c_str(), source.c_str());
- ASSERT_EQ(system(cmd), 0);
-
- // remove temporary folder
- gzdbg << "removing " << build.string() << std::endl;
- boost::filesystem::remove_all(build);
-}
-
-TEST_P(ExamplePlugins, Build)
-{
- Build(GetParam());
-}
-
-INSTANTIATE_TEST_CASE_P(ExamplePlugins, ExamplePlugins, ::testing::Values(
- "animate_joints"
- , "animate_pose"
- , "factory"
- , "hello_world"
- , "model_push"
- , "parameters"
- , "projector"
- , "system_gui_plugin"
- , "world_edit"
- , "gui_overlay_plugin_spawn"
- , "gui_overlay_plugin_time"
-));
-
-int main(int argc, char **argv)
-{
- ::testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/examples/examples_build.cc b/test/examples/examples_build.cc
new file mode 100644
index 0000000..5574a2e
--- /dev/null
+++ b/test/examples/examples_build.cc
@@ -0,0 +1,130 @@
+/*
+ * 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 <gtest/gtest.h>
+#include <stdio.h>
+#include <string>
+#include <boost/filesystem.hpp>
+
+#include "gazebo/common/Console.hh"
+
+///////////////////////////////////////////////////////////////////
+// Create a temporary build folder
+boost::filesystem::path createTempBuildFolder(const std::string &_prefix)
+{
+ boost::filesystem::path path = boost::filesystem::temp_directory_path();
+ path /= boost::filesystem::unique_path(_prefix + "-%%%%-%%%%-%%%%-%%%%");
+ boost::filesystem::create_directories(path);
+ gzdbg << "mkdir " << path.string() << std::endl;
+ return path;
+}
+
+///////////////////////////////////////////////////////////////////
+// Get path to source folder with specified suffix
+boost::filesystem::path getSourcePath(const std::string &_folder,
+ const std::string &_suffix)
+{
+ boost::filesystem::path path = CMAKE_SOURCE_DIR;
+ path /= std::string("examples");
+ path /= _folder;
+ path /= _suffix;
+ gzdbg << "source " << path.string() << std::endl;
+ return path;
+}
+
+///////////////////////////////////////////////////////////////////
+class ExamplesBuild : public ::testing::TestWithParam<const char*>
+{
+ /// \brief Build code in subfolder _type/_name in a temporary build folder
+ /// \param[in] _type Type of example to build (plugins, stand_alone).
+ /// \param[in] _name Subfolder to build.
+ public: void Build(const std::string &_type, const std::string &_name);
+};
+
+// Fixture for building example plugins
+class ExamplesBuild_Plugins: public ExamplesBuild {};
+
+// Fixture for building example stand_alone applications
+class ExamplesBuild_Standalone: public ExamplesBuild {};
+
+///////////////////////////////////////////////////////////////////
+// Build code in subfolder _type/_name in a temporary build folder
+void ExamplesBuild::Build(const std::string &_type, const std::string &_name)
+{
+ // get a unique temporary build folder name
+ boost::filesystem::path build = createTempBuildFolder(_name);
+
+ // construct path of source folder
+ boost::filesystem::path source = getSourcePath(_type, _name);
+
+ char cmd[1024];
+
+ // cd build && cmake source
+ snprintf(cmd, sizeof(cmd), "cd %s && cmake %s && make",
+ build.c_str(), source.c_str());
+ ASSERT_EQ(system(cmd), 0);
+
+ // remove temporary folder
+ gzdbg << "removing " << build.string() << std::endl;
+ boost::filesystem::remove_all(build);
+}
+
+///////////////////////////////////////////////////////////////////
+TEST_P(ExamplesBuild_Plugins, Plugins)
+{
+ Build("plugins", GetParam());
+}
+
+///////////////////////////////////////////////////////////////////
+INSTANTIATE_TEST_CASE_P(Plugins, ExamplesBuild_Plugins, ::testing::Values(
+ "animate_joints"
+ , "animate_pose"
+ , "factory"
+ , "gui_overlay_plugin_spawn"
+ , "gui_overlay_plugin_time"
+ , "hello_world"
+ , "model_push"
+ , "model_move"
+ , "parameters"
+ , "projector"
+ , "system_gui_plugin"
+ , "world_edit"
+));
+
+///////////////////////////////////////////////////////////////////
+TEST_P(ExamplesBuild_Standalone, Standalone)
+{
+ Build("stand_alone", GetParam());
+}
+
+///////////////////////////////////////////////////////////////////
+INSTANTIATE_TEST_CASE_P(Standalone, ExamplesBuild_Standalone, ::testing::Values(
+ "actuator"
+ // , "animated_box"
+ // , "arrange"
+ , "clone_simulation"
+ , "custom_main"
+ , "custom_main_pkgconfig"
+ // , "listener"
+ // , "publisher"
+ , "test_fixture"
+));
+
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 573f429..53353d7 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -36,7 +36,6 @@ set(tests
concave_mesh.cc
contact_sensor.cc
dem.cc
- factory.cc
file_handling.cc
gripper.cc
gz_joint.cc
@@ -48,6 +47,7 @@ set(tests
joint_controller.cc
joint_force_torque.cc
joint_gearbox.cc
+ joint_get_force_torque.cc
joint_revolute.cc
joint_screw.cc
joint_set_position_test.cc
@@ -66,20 +66,24 @@ set(tests
physics_inertia_ratio.cc
physics_link.cc
physics_msgs.cc
+ physics_presets.cc
physics_thread_safe.cc
pioneer2dx.cc
plugin.cc
- polyline.cc
+ rest_web.cc
road.cc
sensor.cc
server_fixture.cc
sim_events.cc
speed.cc
+ speed_thread_pr2.cc
stress_spawn_models.cc
#state_log.cc
surface_properties.cc
+ swarm.cc
transceiver.cc
transport.cc
+ transporter.cc
world.cc
world_clone.cc
world_entity_below_point.cc
@@ -87,6 +91,13 @@ set(tests
worlds_installed.cc
)
+if (HAVE_GTS)
+ set(tests
+ ${tests}
+ polyline.cc
+ )
+endif()
+
if (MANPAGES_SUPPORT)
set (tests ${tests}
manpages.cc)
@@ -96,8 +107,9 @@ gz_build_tests(${tests})
if (ENABLE_TESTS_COMPILATION)
# Increase timeout, to account for model download time.
- set_tests_properties(${TEST_TYPE}_factory PROPERTIES TIMEOUT 500)
+ set_tests_properties(${TEST_TYPE}_joint_revolute PROPERTIES TIMEOUT 500)
set_tests_properties(${TEST_TYPE}_physics PROPERTIES TIMEOUT 500)
+ set_tests_properties(${TEST_TYPE}_worlds_installed PROPERTIES TIMEOUT 700)
# Add plugin dependency
add_dependencies(${TEST_TYPE}_joint_test SpringTestPlugin)
@@ -112,6 +124,7 @@ gz_build_display_tests(${display_tests})
set(dri_tests
camera.cc
camera_sensor.cc
+ factory.cc
gpu_laser.cc
gz_camera.cc
heightmap.cc
@@ -134,9 +147,7 @@ gz_build_qt_tests(${qt_tests})
if (ENABLE_TESTS_COMPILATION AND VALID_DRI_DISPLAY)
# Increase timeout, to account for model download time.
+ set_tests_properties(${TEST_TYPE}_factory PROPERTIES TIMEOUT 500)
set_tests_properties(${TEST_TYPE}_pr2 PROPERTIES TIMEOUT 500)
-
- # Increase timeout, to account for model download time.
set_tests_properties(${TEST_TYPE}_pioneer2dx PROPERTIES TIMEOUT 500)
endif()
-
diff --git a/test/integration/aero_plugin.cc b/test/integration/aero_plugin.cc
index 222f456..0a95fa7 100644
--- a/test/integration/aero_plugin.cc
+++ b/test/integration/aero_plugin.cc
@@ -18,8 +18,8 @@
#include <gtest/gtest.h>
#include "gazebo/physics/physics.hh"
#include "gazebo/physics/Joint.hh"
-#include "test/ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#define TOL 1e-6
#define TOL_CONT 2.0
diff --git a/test/integration/bandwidth.cc b/test/integration/bandwidth.cc
index f497c4a..82b6cb7 100644
--- a/test/integration/bandwidth.cc
+++ b/test/integration/bandwidth.cc
@@ -17,8 +17,8 @@
#include <unistd.h>
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
diff --git a/test/integration/camera.cc b/test/integration/camera.cc
index 00f4338..acb763e 100644
--- a/test/integration/camera.cc
+++ b/test/integration/camera.cc
@@ -15,7 +15,7 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class CameraTest : public ServerFixture
diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc
index 3efe34e..6c1b1b3 100644
--- a/test/integration/camera_sensor.cc
+++ b/test/integration/camera_sensor.cc
@@ -15,6 +15,8 @@
*
*/
+#include <ignition/math/Rand.hh>
+
#include "gazebo/physics/physics.hh"
#include "gazebo/sensors/sensors.hh"
#include "gazebo/common/common.hh"
@@ -22,7 +24,7 @@
#include "gazebo/rendering/Camera.hh"
#include "gazebo/sensors/CameraSensor.hh"
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "scans_cmp.h"
using namespace gazebo;
@@ -422,7 +424,7 @@ int main(int argc, char **argv)
{
// Set a specific seed to avoid occasional test failures due to
// statistically unlikely, but possible results.
- math::Rand::SetSeed(42);
+ ignition::math::Rand::Seed(42);
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
diff --git a/test/integration/concave_mesh.cc b/test/integration/concave_mesh.cc
index d8b62ac..bd12cd8 100644
--- a/test/integration/concave_mesh.cc
+++ b/test/integration/concave_mesh.cc
@@ -15,10 +15,10 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/sensors/sensors.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
@@ -46,7 +46,7 @@ void ConcaveMeshTest::SubmeshNoCollisionTest(const std::string &_physicsEngine)
// No ray should interect a collision.
for (int i = 0; i < raySensor->GetRangeCount(); ++i)
- EXPECT_NEAR(raySensor->GetRange(i), raySensor->GetRangeMax(), 1e-2);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(i), GZ_DBL_INF);
}
/////////////////////////////////////////////////
@@ -63,26 +63,26 @@ void ConcaveMeshTest::SubmeshCollisionTest(const std::string &_physicsEngine)
sensors::get_sensor("default::hokuyo::link::laser"));
EXPECT_TRUE(raySensor != NULL);
- EXPECT_NEAR(raySensor->GetRange(0), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(1), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(2), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(3), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(4), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(5), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(6), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(7), 10, 1e-2);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(0), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(1), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(2), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(3), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(4), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(5), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(6), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(7), GZ_DBL_INF);
EXPECT_NEAR(raySensor->GetRange(8), 1.92439, 1e-2);
EXPECT_NEAR(raySensor->GetRange(9), 1.86443, 1e-2);
EXPECT_NEAR(raySensor->GetRange(10), 1.86443, 1e-2);
EXPECT_NEAR(raySensor->GetRange(11), 1.92439, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(12), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(13), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(14), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(15), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(16), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(17), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(18), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(19), 10, 1e-2);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(12), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(13), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(14), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(15), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(16), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(17), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(18), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(19), GZ_DBL_INF);
}
/////////////////////////////////////////////////
@@ -99,10 +99,10 @@ void ConcaveMeshTest::RayTest(const std::string &_physicsEngine)
sensors::get_sensor("default::hokuyo::link::laser"));
EXPECT_TRUE(raySensor != NULL);
- EXPECT_NEAR(raySensor->GetRange(0), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(1), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(2), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(3), 10, 1e-2);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(0), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(1), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(2), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(3), GZ_DBL_INF);
EXPECT_NEAR(raySensor->GetRange(4), 0.972282, 1e-2);
EXPECT_NEAR(raySensor->GetRange(5), 0.967148, 1e-2);
EXPECT_NEAR(raySensor->GetRange(6), 0.962889, 1e-2);
@@ -112,13 +112,13 @@ void ConcaveMeshTest::RayTest(const std::string &_physicsEngine)
EXPECT_NEAR(raySensor->GetRange(10), 0.948761, 1e-2);
EXPECT_NEAR(raySensor->GetRange(11), 0.847463, 1e-2);
EXPECT_NEAR(raySensor->GetRange(12), 0.847665, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(13), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(14), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(15), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(16), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(17), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(18), 10, 1e-2);
- EXPECT_NEAR(raySensor->GetRange(19), 10, 1e-2);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(13), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(14), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(15), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(16), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(17), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(18), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(19), GZ_DBL_INF);
}
/////////////////////////////////////////////////
diff --git a/test/integration/contact_sensor.cc b/test/integration/contact_sensor.cc
index 2e3ec59..4f68264 100644
--- a/test/integration/contact_sensor.cc
+++ b/test/integration/contact_sensor.cc
@@ -15,12 +15,12 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/sensors/sensors.hh"
#include "gazebo/common/common.hh"
#include "scans_cmp.h"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#define TOL 1e-4
@@ -28,19 +28,146 @@ using namespace gazebo;
class ContactSensor : public ServerFixture,
public testing::WithParamInterface<const char*>
{
- public: void EmptyWorld(const std::string &_physicsEngine);
+ /// \brief Test moving a model while in contact.
+ /// \param[in] _physicsEngine Physics engine to use.
+ public: void MoveTool(const std::string &_physicsEngine);
+
+ /// \brief Test multiple contact sensors on a single link.
+ /// \param[in] _physicsEngine Physics engine to use.
+ public: void MultipleSensors(const std::string &_physicsEngine);
public: void StackTest(const std::string &_physicsEngine);
public: void TorqueTest(const std::string &_physicsEngine);
+
+ /// \brief Callback for sensor subscribers in MultipleSensors test.
+ private: void Callback(const ConstContactsPtr &_msg);
};
-void ContactSensor::EmptyWorld(const std::string &_physicsEngine)
+unsigned int g_messageCount = 0;
+
+////////////////////////////////////////////////////////////////////////
+void ContactSensor::Callback(const ConstContactsPtr &/*_msg*/)
{
- Load("worlds/empty.world", false, _physicsEngine);
+ g_messageCount++;
+}
+
+////////////////////////////////////////////////////////////////////////
+// Test moving a model while in contact
+// addresses a failure in pull request #1610 for simbody
+////////////////////////////////////////////////////////////////////////
+void ContactSensor::MoveTool(const std::string &_physicsEngine)
+{
+ Load("worlds/contact_sensors_multiple.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world();
+ ASSERT_TRUE(world != NULL);
+
+ const std::string modelName("sphere");
+ const math::Vector3 pos(0, 0, 1.8);
+ const math::Vector3 v30;
+ const double radius = 0.5;
+ SpawnSphere(modelName, pos, v30, v30, radius);
+
+ // advertise on "~/model/modify"
+ // so that we can move the sphere
+ transport::PublisherPtr modelPub =
+ this->node->Advertise<msgs::Model>("~/model/modify");
+
+ // Step forward to allow the sphere to fall
+ world->Step(200);
+
+ // Try moving the model
+ auto model = world->GetModel(modelName);
+ ASSERT_TRUE(model != NULL);
+
+ auto pose = model->GetWorldPose();
+ pose.pos.x += 0.2;
+ pose.pos.y += 0.2;
+
+ msgs::Model msg;
+ msg.set_name(modelName);
+ msg.set_id(model->GetId());
+ msgs::Set(msg.mutable_pose(), pose.Ign());
+ modelPub->Publish(msg);
+
+ while (pose != model->GetWorldPose())
+ {
+ world->Step(1);
+ common::Time::MSleep(1);
+ }
+
+ world->Step(10);
+
+ // it just needs to exit successfully in order to pass.
}
-TEST_P(ContactSensor, EmptyWorld)
+TEST_P(ContactSensor, MoveTool)
{
- EmptyWorld(GetParam());
+ MoveTool(GetParam());
+}
+
+////////////////////////////////////////////////////////////////////////
+// Test having multiple contact sensors under a single link.
+// https://bitbucket.org/osrf/gazebo/issue/960
+////////////////////////////////////////////////////////////////////////
+void ContactSensor::MultipleSensors(const std::string &_physicsEngine)
+{
+ Load("worlds/contact_sensors_multiple.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world();
+ ASSERT_TRUE(world != NULL);
+
+ const std::string contactSensorName1("box_contact");
+ const std::string contactSensorName2("box_contact2");
+
+ {
+ sensors::SensorPtr sensor1 = sensors::get_sensor(contactSensorName1);
+ sensors::ContactSensorPtr contactSensor1 =
+ boost::dynamic_pointer_cast<sensors::ContactSensor>(sensor1);
+ ASSERT_TRUE(contactSensor1 != NULL);
+ }
+
+ {
+ sensors::SensorPtr sensor2 = sensors::get_sensor(contactSensorName2);
+ sensors::ContactSensorPtr contactSensor2 =
+ boost::dynamic_pointer_cast<sensors::ContactSensor>(sensor2);
+ ASSERT_TRUE(contactSensor2 != NULL);
+ }
+
+ // There should be 5 topics advertising Contacts messages
+ std::list<std::string> topicsExpected;
+ std::string prefix = "/gazebo/default/";
+ topicsExpected.push_back(prefix+"physics/contacts");
+ topicsExpected.push_back(prefix+"sensor_box/link/box_contact/contacts");
+ topicsExpected.push_back(prefix+"sensor_box/link/box_contact");
+ topicsExpected.push_back(prefix+"sensor_box/link/box_contact2/contacts");
+ topicsExpected.push_back(prefix+"sensor_box/link/box_contact2");
+ topicsExpected.sort();
+
+ // Sleep to ensure transport topics are all advertised
+ common::Time::MSleep(100);
+ std::list<std::string> topics =
+ transport::getAdvertisedTopics("gazebo.msgs.Contacts");
+ topics.sort();
+ EXPECT_FALSE(topics.empty());
+ EXPECT_EQ(topics.size(), topicsExpected.size());
+ EXPECT_EQ(topics, topicsExpected);
+
+ // We should expect them all to publish.
+ for (auto const &topic : topics)
+ {
+ gzdbg << "Listening to " << topic << std::endl;
+ g_messageCount = 0;
+ transport::SubscriberPtr sub = this->node->Subscribe(topic,
+ &ContactSensor::Callback, this);
+
+ const unsigned int steps = 50;
+ world->Step(steps);
+ common::Time::MSleep(steps);
+ EXPECT_GT(g_messageCount, steps / 2);
+ }
+}
+
+TEST_P(ContactSensor, MultipleSensors)
+{
+ MultipleSensors(GetParam());
}
////////////////////////////////////////////////////////////////////////
@@ -53,12 +180,6 @@ TEST_P(ContactSensor, EmptyWorld)
////////////////////////////////////////////////////////////////////////
void ContactSensor::StackTest(const std::string &_physicsEngine)
{
- if (_physicsEngine == "simbody")
- {
- gzerr << "Aborting test for Simbody, see issue #865.\n";
- return;
- }
-
if (_physicsEngine == "dart")
{
gzerr << "Aborting test for DART, see issue #1173.\n";
@@ -153,6 +274,10 @@ void ContactSensor::StackTest(const std::string &_physicsEngine)
world->Step(1);
contacts01 = contactSensor01->GetContacts();
contacts02 = contactSensor02->GetContacts();
+ // gzdbg << "steps[" << steps
+ // << "] contacts01[" << contacts01.contact_size()
+ // << "] contacts02[" << contacts02.contact_size()
+ // << "] to be > 0\n";
}
EXPECT_GT(steps, 0);
@@ -297,12 +422,6 @@ TEST_P(ContactSensor, StackTest)
////////////////////////////////////////////////////////////////////////
void ContactSensor::TorqueTest(const std::string &_physicsEngine)
{
- if (_physicsEngine == "simbody")
- {
- gzerr << "Aborting test for Simbody, see issue #865.\n";
- return;
- }
-
// Load an empty world
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
@@ -351,8 +470,12 @@ void ContactSensor::TorqueTest(const std::string &_physicsEngine)
msgs::Contacts contacts;
- physics->SetContactMaxCorrectingVel(0);
- physics->SetParam("iters", 100);
+ if (_physicsEngine == "_ode" || _physicsEngine == "bullet")
+ {
+ EXPECT_TRUE(physics->SetParam("iters", 100));
+ if (_physicsEngine == "ode")
+ EXPECT_TRUE(physics->SetParam("contact_max_correcting_vel", 0));
+ }
world->Step(1);
diff --git a/test/integration/dem.cc b/test/integration/dem.cc
index d9f40b1..d4126e5 100644
--- a/test/integration/dem.cc
+++ b/test/integration/dem.cc
@@ -15,10 +15,10 @@
*
*/
+#include <ignition/math/Angle.hh>
#include "gazebo/common/Dem.hh"
-#include "gazebo/math/Angle.hh"
#include "gazebo/sensors/GpsSensor.hh"
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#define DOUBLE_TOL 1e-3
@@ -32,7 +32,7 @@ class Dem_TEST : public ServerFixture
/// \brief Test the integration between GPS and a DEM terrain.
TEST_F(Dem_TEST, GPS)
{
- math::Angle latitude, longitude;
+ ignition::math::Angle latitude, longitude;
double elevation;
common::Dem dem;
boost::filesystem::path path = "file://media/dem/volcano.tif";
@@ -59,8 +59,8 @@ TEST_F(Dem_TEST, GPS)
dem.GetGeoReferenceOrigin(latitude, longitude);
elevation = dem.GetElevation(0.0, 0.0);
- EXPECT_NEAR(sensor->GetLatitude().Degree(), latitude.Degree(), DOUBLE_TOL);
- EXPECT_NEAR(sensor->GetLongitude().Degree(), longitude.Degree(), DOUBLE_TOL);
+ EXPECT_NEAR(sensor->Latitude().Degree(), latitude.Degree(), DOUBLE_TOL);
+ EXPECT_NEAR(sensor->Longitude().Degree(), longitude.Degree(), DOUBLE_TOL);
EXPECT_NEAR(sensor->GetAltitude(), elevation, 1);
}
#endif
diff --git a/test/integration/factory.cc b/test/integration/factory.cc
index 02bc637..b1ee194 100644
--- a/test/integration/factory.cc
+++ b/test/integration/factory.cc
@@ -23,9 +23,9 @@
#include "gazebo/rendering/Camera.hh"
#include "gazebo/sensors/SensorsIface.hh"
#include "gazebo/sensors/CameraSensor.hh"
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "images_cmp.h"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
class FactoryTest : public ServerFixture,
@@ -45,7 +45,7 @@ class FactoryTest : public ServerFixture,
// (see issue #651)
void FactoryTest::BoxSdf(const std::string &_physicsEngine)
{
- math::Pose setPose, testPose;
+ ignition::math::Pose3d setPose;
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
@@ -56,9 +56,10 @@ void FactoryTest::BoxSdf(const std::string &_physicsEngine)
{
std::ostringstream name;
name << "test_box_" << i;
- setPose.Set(math::Vector3(0, 0, i+0.5), math::Quaternion(0, 0, 0));
- SpawnBox(name.str(), math::Vector3(1, 1, 1), setPose.pos,
- setPose.rot.GetAsEuler());
+ setPose.Set(ignition::math::Vector3d(0, 0, i+0.5),
+ ignition::math::Quaterniond(0, 0, 0));
+ SpawnBox(name.str(), ignition::math::Vector3d(1, 1, 1), setPose.Pos(),
+ setPose.Rot().Euler());
}
// This loop must be separate from the previous loop to cause
@@ -86,20 +87,21 @@ TEST_P(FactoryTest, BoxSdf)
/////////////////////////////////////////////////
void FactoryTest::Box(const std::string &_physicsEngine)
{
- math::Pose setPose, testPose;
+ ignition::math::Pose3d setPose, testPose;
Load("worlds/empty.world", true, _physicsEngine);
for (unsigned int i = 0; i < 100; i++)
{
std::ostringstream name;
name << "test_box_" << i;
- setPose.Set(math::Vector3(0, 0, i+0.5), math::Quaternion(0, 0, 0));
- SpawnBox(name.str(), math::Vector3(1, 1, 1), setPose.pos,
- setPose.rot.GetAsEuler());
- testPose = GetEntityPose(name.str());
- EXPECT_TRUE(math::equal(testPose.pos.x, setPose.pos.x, 0.1));
- EXPECT_TRUE(math::equal(testPose.pos.y, setPose.pos.y, 0.1));
- EXPECT_TRUE(math::equal(testPose.pos.z, setPose.pos.z, 0.1));
+ setPose.Set(ignition::math::Vector3d(0, 0, i+0.5),
+ ignition::math::Quaterniond(0, 0, 0));
+ SpawnBox(name.str(), math::Vector3(1, 1, 1), setPose.Pos(),
+ setPose.Rot().Euler());
+ testPose = GetEntityPose(name.str()).Ign();
+ EXPECT_TRUE(math::equal(testPose.Pos().X(), setPose.Pos().X(), 0.1));
+ EXPECT_TRUE(math::equal(testPose.Pos().Y(), setPose.Pos().Y(), 0.1));
+ EXPECT_TRUE(math::equal(testPose.Pos().Z(), setPose.Pos().Z(), 0.1));
}
}
@@ -112,19 +114,20 @@ TEST_P(FactoryTest, Box)
/////////////////////////////////////////////////
void FactoryTest::Sphere(const std::string &_physicsEngine)
{
- math::Pose setPose, testPose;
+ ignition::math::Pose3d setPose, testPose;
Load("worlds/empty.world", true, _physicsEngine);
for (unsigned int i = 0; i < 100; i++)
{
std::ostringstream name;
name << "test_sphere_" << i;
- setPose.Set(math::Vector3(0, 0, i+0.5), math::Quaternion(0, 0, 0));
- SpawnSphere(name.str(), setPose.pos, setPose.rot.GetAsEuler());
- testPose = GetEntityPose(name.str());
- EXPECT_TRUE(math::equal(testPose.pos.x, setPose.pos.x, 0.1));
- EXPECT_TRUE(math::equal(testPose.pos.y, setPose.pos.y, 0.1));
- EXPECT_TRUE(math::equal(testPose.pos.z, setPose.pos.z, 0.1));
+ setPose.Set(ignition::math::Vector3d(0, 0, i+0.5),
+ ignition::math::Quaterniond(0, 0, 0));
+ SpawnSphere(name.str(), setPose.Pos(), setPose.Rot().Euler());
+ testPose = GetEntityPose(name.str()).Ign();
+ EXPECT_TRUE(math::equal(testPose.Pos().X(), setPose.Pos().X(), 0.1));
+ EXPECT_TRUE(math::equal(testPose.Pos().Y(), setPose.Pos().Y(), 0.1));
+ EXPECT_TRUE(math::equal(testPose.Pos().Z(), setPose.Pos().Z(), 0.1));
}
}
@@ -137,19 +140,21 @@ TEST_P(FactoryTest, Sphere)
/////////////////////////////////////////////////
void FactoryTest::Cylinder(const std::string &_physicsEngine)
{
- math::Pose setPose, testPose;
+ ignition::math::Pose3d setPose, testPose;
Load("worlds/empty.world", true, _physicsEngine);
for (unsigned int i = 0; i < 100; i++)
{
std::ostringstream name;
name << "test_cylinder_" << i;
- setPose.Set(math::Vector3(0, 0, i+0.5), math::Quaternion(0, 0, 0));
- SpawnCylinder(name.str(), setPose.pos, setPose.rot.GetAsEuler());
- testPose = GetEntityPose(name.str());
- EXPECT_TRUE(math::equal(testPose.pos.x, setPose.pos.x, 0.1));
- EXPECT_TRUE(math::equal(testPose.pos.y, setPose.pos.y, 0.1));
- EXPECT_TRUE(math::equal(testPose.pos.z, setPose.pos.z, 0.1));
+ setPose.Set(
+ ignition::math::Vector3d(0, 0, i+0.5),
+ ignition::math::Quaterniond(0, 0, 0));
+ SpawnCylinder(name.str(), setPose.Pos(), setPose.Rot().Euler());
+ testPose = GetEntityPose(name.str()).Ign();
+ EXPECT_TRUE(math::equal(testPose.Pos().X(), setPose.Pos().X(), 0.1));
+ EXPECT_TRUE(math::equal(testPose.Pos().Y(), setPose.Pos().Y(), 0.1));
+ EXPECT_TRUE(math::equal(testPose.Pos().Z(), setPose.Pos().Z(), 0.1));
}
}
@@ -162,14 +167,15 @@ TEST_P(FactoryTest, Cylinder)
/////////////////////////////////////////////////
void FactoryTest::Clone(const std::string &_physicsEngine)
{
- math::Pose setPose, testPose;
+ ignition::math::Pose3d testPose;
Load("worlds/pr2.world", true, _physicsEngine);
// clone the pr2
std::string name = "pr2";
msgs::Factory msg;
- math::Pose clonePose;
- clonePose.Set(math::Vector3(2, 3, 0.5), math::Quaternion(0, 0, 0));
+ ignition::math::Pose3d clonePose;
+ clonePose.Set(ignition::math::Vector3d(2, 3, 0.5),
+ ignition::math::Quaterniond(0, 0, 0));
msgs::Set(msg.mutable_pose(), clonePose);
msg.set_clone_model_name(name);
this->factoryPub->Publish(msg);
@@ -179,10 +185,10 @@ void FactoryTest::Clone(const std::string &_physicsEngine)
this->WaitUntilEntitySpawn(cloneName, 100, 100);
EXPECT_TRUE(this->HasEntity(cloneName));
- testPose = GetEntityPose(cloneName);
- EXPECT_TRUE(math::equal(testPose.pos.x, clonePose.pos.x, 0.1));
- EXPECT_TRUE(math::equal(testPose.pos.y, clonePose.pos.y, 0.1));
- EXPECT_TRUE(math::equal(testPose.pos.z, clonePose.pos.z, 0.1));
+ testPose = GetEntityPose(cloneName).Ign();
+ EXPECT_TRUE(math::equal(testPose.Pos().X(), clonePose.Pos().X(), 0.1));
+ EXPECT_TRUE(math::equal(testPose.Pos().Y(), clonePose.Pos().Y(), 0.1));
+ EXPECT_TRUE(math::equal(testPose.Pos().Z(), clonePose.Pos().Z(), 0.1));
// Verify properties of the pr2 clone with the original model.
physics::WorldPtr world = physics::get_world("default");
diff --git a/test/integration/file_handling.cc b/test/integration/file_handling.cc
index c6335cf..56e6cfe 100644
--- a/test/integration/file_handling.cc
+++ b/test/integration/file_handling.cc
@@ -16,7 +16,7 @@
*/
#include <boost/filesystem.hpp>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/common/common.hh"
#include "gazebo/common/SystemPaths.hh"
@@ -59,12 +59,18 @@ TEST_F(FileHandling, Save)
// Wait until the file exists
i = 0;
- while (i < 10 && (file = fopen(filename.str().c_str(), "r")) == NULL)
+ while (i < 10)
{
+ file = fopen(filename.str().c_str(), "r");
+ if (file != NULL)
+ {
+ fclose(file);
+ break;
+ }
+
i++;
common::Time::MSleep(100);
}
- fclose(file);
EXPECT_LT(i, 10);
diff --git a/test/integration/gpu_laser.cc b/test/integration/gpu_laser.cc
index 9255e54..a1a938e 100644
--- a/test/integration/gpu_laser.cc
+++ b/test/integration/gpu_laser.cc
@@ -15,7 +15,7 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/sensors/sensors.hh"
#define LASER_TOL 1e-5
@@ -147,9 +147,7 @@ TEST_F(GPURaySensorTest, LaserUnitBox)
EXPECT_NEAR(raySensor->GetRange(mid), expectedRangeAtMidPoint, LASER_TOL);
EXPECT_NEAR(raySensor->GetRange(0), expectedRangeAtMidPoint, LASER_TOL);
- // WARNING: for readings of no return, gazebo returns max range rather
- // than +inf. issue #124
- EXPECT_NEAR(raySensor->GetRange(samples-1), maxRange, LASER_TOL);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(samples-1), GZ_DBL_INF);
// Verify ray sensor 2 range readings
// listen to new laser frames
@@ -173,8 +171,8 @@ TEST_F(GPURaySensorTest, LaserUnitBox)
// Only box01 should be visible to ray sensor 2
EXPECT_NEAR(raySensor2->GetRange(mid), expectedRangeAtMidPoint, LASER_TOL);
- EXPECT_NEAR(raySensor2->GetRange(0), maxRange, LASER_TOL);
- EXPECT_NEAR(raySensor->GetRange(samples-1), maxRange, LASER_TOL);
+ EXPECT_DOUBLE_EQ(raySensor2->GetRange(0), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(samples-1), GZ_DBL_INF);
// Move all boxes out of range
world->GetModel(box01)->SetWorldPose(
@@ -195,10 +193,10 @@ TEST_F(GPURaySensorTest, LaserUnitBox)
EXPECT_LT(i, 300);
for (int i = 0; i < raySensor->GetRayCount(); ++i)
- EXPECT_NEAR(raySensor->GetRange(i), maxRange, LASER_TOL);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(i), GZ_DBL_INF);
for (int i = 0; i < raySensor->GetRayCount(); ++i)
- EXPECT_NEAR(raySensor2->GetRange(i), maxRange, LASER_TOL);
+ EXPECT_DOUBLE_EQ(raySensor2->GetRange(i), GZ_DBL_INF);
raySensor->DisconnectNewLaserFrame(c);
raySensor2->DisconnectNewLaserFrame(c2);
@@ -341,7 +339,7 @@ TEST_F(GPURaySensorTest, Heightmap)
EXPECT_NEAR(raySensor->GetRangeMax(), maxRange, LASER_TOL);
for (int i = 0; i < raySensor->GetRayCount(); ++i)
- EXPECT_NEAR(raySensor->GetRange(i), maxRange, LASER_TOL);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(i), GZ_DBL_INF);
// Move laser model very close to terrain, it should now returns range values
// that are less than half the max range
diff --git a/test/integration/gripper.cc b/test/integration/gripper.cc
index a309d28..4b08224 100644
--- a/test/integration/gripper.cc
+++ b/test/integration/gripper.cc
@@ -15,13 +15,14 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/physics/Gripper.hh"
#include "gazebo/physics/World.hh"
#include "gazebo/physics/Model.hh"
#include "gazebo/physics/Joint.hh"
+using namespace gazebo;
class GripperTest : public ServerFixture
{
};
diff --git a/test/integration/gz_camera.cc b/test/integration/gz_camera.cc
index 68a7ecb..c82d107 100644
--- a/test/integration/gz_camera.cc
+++ b/test/integration/gz_camera.cc
@@ -16,7 +16,7 @@
*/
#include "gazebo/sensors/SensorsIface.hh"
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class GzCamera : public ServerFixture
diff --git a/test/integration/gz_joint.cc b/test/integration/gz_joint.cc
index b2109c4..615b917 100644
--- a/test/integration/gz_joint.cc
+++ b/test/integration/gz_joint.cc
@@ -16,7 +16,7 @@
*/
#include <sstream>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class GzJoint : public ServerFixture
diff --git a/test/integration/gz_log.cc b/test/integration/gz_log.cc
index 6e3030e..8fc7289 100644
--- a/test/integration/gz_log.cc
+++ b/test/integration/gz_log.cc
@@ -16,7 +16,7 @@
*/
#include "gazebo/util/LogRecord.hh"
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class GzLog : public ServerFixture
diff --git a/test/integration/gz_model.cc b/test/integration/gz_model.cc
index 455b061..a880baf 100644
--- a/test/integration/gz_model.cc
+++ b/test/integration/gz_model.cc
@@ -16,7 +16,7 @@
*/
#include <boost/filesystem.hpp>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class GzModel : public ServerFixture
diff --git a/test/integration/gz_physics.cc b/test/integration/gz_physics.cc
index a755dff..b4473f6 100644
--- a/test/integration/gz_physics.cc
+++ b/test/integration/gz_physics.cc
@@ -15,10 +15,13 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
-class GzPhysics : public ServerFixture
+class GzPhysics : public ServerFixture,
+ public testing::WithParamInterface<const char*>
+
{
};
@@ -99,6 +102,32 @@ TEST_F(GzPhysics, Iters)
}
/////////////////////////////////////////////////
+// \brief Test setting the physics profile for each physics engine.
+TEST_P(GzPhysics, Profile)
+{
+ Load("test/worlds/presets.world", false, GetParam());
+
+ // Get a pointer to the world
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+ ASSERT_TRUE(world->GetPhysicsEngine() != NULL);
+
+ // Check initial profile and step size
+ EXPECT_EQ(world->GetPresetManager()->CurrentProfile(), "preset_1");
+ EXPECT_FLOAT_EQ(world->GetPhysicsEngine()->GetMaxStepSize(), 0.01);
+
+ // Change profile
+ custom_exec("gz physics -o preset_2");
+ EXPECT_EQ(world->GetPresetManager()->CurrentProfile(), "preset_2");
+ EXPECT_FLOAT_EQ(world->GetPhysicsEngine()->GetMaxStepSize(), 0.02);
+
+ // Change profile and override step size
+ custom_exec("gz physics -o preset_1 -s 0.001");
+ EXPECT_EQ(world->GetPresetManager()->CurrentProfile(), "preset_1");
+ EXPECT_FLOAT_EQ(world->GetPhysicsEngine()->GetMaxStepSize(), 0.001);
+}
+
+/////////////////////////////////////////////////
// \brief Test setting the update rate.
TEST_F(GzPhysics, UpdateRate)
{
@@ -118,6 +147,8 @@ TEST_F(GzPhysics, UpdateRate)
EXPECT_NEAR(world->GetPhysicsEngine()->GetRealTimeUpdateRate(), 0.5, 1e-3);
}
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, GzPhysics, PHYSICS_ENGINE_VALUES);
+
/////////////////////////////////////////////////
int main(int argc, char **argv)
{
diff --git a/test/integration/gz_world.cc b/test/integration/gz_world.cc
index 4a1dedc..e32c3ab 100644
--- a/test/integration/gz_world.cc
+++ b/test/integration/gz_world.cc
@@ -15,7 +15,7 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class GzWorld : public ServerFixture
diff --git a/test/integration/heightmap.cc b/test/integration/heightmap.cc
index b120381..bcc051c 100644
--- a/test/integration/heightmap.cc
+++ b/test/integration/heightmap.cc
@@ -16,14 +16,15 @@
*/
#include <string.h>
+#include <ignition/math/Vector3.hh>
#include "gazebo/common/SystemPaths.hh"
#include "gazebo/rendering/RenderingIface.hh"
#include "gazebo/rendering/Scene.hh"
#include "heights_cmp.h"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#include "images_cmp.h"
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
@@ -33,6 +34,7 @@ class HeightmapTest : public ServerFixture,
public: void PhysicsLoad(const std::string &_physicsEngine);
public: void WhiteAlpha(const std::string &_physicsEngine);
public: void WhiteNoAlpha(const std::string &_physicsEngine);
+ public: void Volume(const std::string &_physicsEngine);
public: void NotSquareImage();
public: void InvalidSizeImage();
// public: void Heights(const std::string &_physicsEngine);
@@ -70,8 +72,8 @@ void HeightmapTest::PhysicsLoad(const std::string &_physicsEngine)
EXPECT_TRUE(shape != NULL);
EXPECT_TRUE(shape->HasType(physics::Base::HEIGHTMAP_SHAPE));
- EXPECT_TRUE(shape->GetPos() == math::Vector3(0, 0, 0));
- EXPECT_TRUE(shape->GetSize() == math::Vector3(129, 129, 10));
+ EXPECT_TRUE(shape->GetPos() == ignition::math::Vector3d(0, 0, 0));
+ EXPECT_TRUE(shape->GetSize() == ignition::math::Vector3d(129, 129, 10));
common::Image trueImage("media/materials/textures/heightmap_bowl.png");
common::Image testImage = shape->GetImage();
@@ -194,6 +196,41 @@ void HeightmapTest::InvalidSizeImage()
delete this->server;
}
+/////////////////////////////////////////////////
+void HeightmapTest::Volume(const std::string &_physicsEngine)
+{
+ if (_physicsEngine == "simbody")
+ {
+ // SimbodyHeightmapShape unimplemented. ComputeVolume actually returns 0 as
+ // an error code, which is the correct answer, but we'll skip it for now.
+ gzerr << "Aborting test for "
+ << _physicsEngine
+ << std::endl;
+ return;
+ }
+ if (_physicsEngine == "dart")
+ {
+ gzerr << "Aborting test for "
+ << _physicsEngine
+ << ", see issue #909" << std::endl;
+ return;
+ }
+
+ Load("worlds/heightmap_test.world", true, _physicsEngine);
+
+ physics::ModelPtr model = GetModel("heightmap");
+ EXPECT_TRUE(model != NULL);
+
+ physics::CollisionPtr collision =
+ model->GetLink("link")->GetCollision("collision");
+
+ physics::HeightmapShapePtr shape =
+ boost::dynamic_pointer_cast<physics::HeightmapShape>(
+ collision->GetShape());
+
+ EXPECT_DOUBLE_EQ(shape->ComputeVolume(), 0);
+}
+
/*
void HeightmapTest::Heights(const std::string &_physicsEngine)
{
@@ -238,8 +275,8 @@ void HeightmapTest::Heights(const std::string &_physicsEngine)
EXPECT_TRUE(shape);
EXPECT_TRUE(shape->HasType(physics::Base::HEIGHTMAP_SHAPE));
- EXPECT_TRUE(shape->GetPos() == math::Vector3(0, 0, 0));
- EXPECT_TRUE(shape->GetSize() == math::Vector3(129, 129, 10));
+ EXPECT_TRUE(shape->GetPos() == ignition::math::Vector3d(0, 0, 0));
+ EXPECT_TRUE(shape->GetSize() == ignition::math::Vector3d(129, 129, 10));
std::vector<float> physicsTest;
std::vector<float> renderTest;
@@ -350,6 +387,12 @@ TEST_P(HeightmapTest, WhiteNoAlpha)
}
/////////////////////////////////////////////////
+TEST_P(HeightmapTest, Volume)
+{
+ Volume(GetParam());
+}
+
+/////////////////////////////////////////////////
//
// Disabled: segfaults ocassionally
// See https://bitbucket.org/osrf/gazebo/issue/521 for details
diff --git a/test/integration/helper_physics_generator.hh b/test/integration/helper_physics_generator.hh
deleted file mode 100644
index 479aea1..0000000
--- a/test/integration/helper_physics_generator.hh
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * Copyright (C) 2012-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 _HELPER_PHYSICS_GENERATOR_HH_
-#define _HELPER_PHYSICS_GENERATOR_HH_
-
-#include "gazebo/gazebo_config.h"
-
-#define BULLET_SUPPORT
-
-#ifdef HAVE_BULLET
-# undef BULLET_SUPPORT
-# define BULLET_SUPPORT , "bullet"
-#endif
-
-#define SIMBODY_SUPPORT
-#define DART_SUPPORT
-
-#ifdef HAVE_SIMBODY
-# undef SIMBODY_SUPPORT
-# define SIMBODY_SUPPORT , "simbody"
-#endif
-#ifdef HAVE_DART
-# undef DART_SUPPORT
-# define DART_SUPPORT , "dart"
-#endif
-
-/// \brief Helper macro to instantiate gtest for different physics engines
-#define PHYSICS_ENGINE_VALUES ::testing::Values("ode" \
- BULLET_SUPPORT \
- SIMBODY_SUPPORT \
- DART_SUPPORT \
- )
-
-#endif
diff --git a/test/integration/imu.cc b/test/integration/imu.cc
index d8620ae..e63169a 100644
--- a/test/integration/imu.cc
+++ b/test/integration/imu.cc
@@ -14,11 +14,12 @@
* limitations under the License.
*
*/
+#include <ignition/math/Rand.hh>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/sensors/sensors.hh"
#include "gazebo/common/common.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
// How tightly to compare for deterministic values
#define IMU_TOL 1e-5
@@ -60,9 +61,9 @@ class ImuTest : public ServerFixture,
/// \param[out] _orientation orientation of the imu at the end of sample
/// period
private: void GetImuData(sensors::ImuSensorPtr _imu, unsigned int _cnt,
- math::Vector3 &_rateMean,
- math::Vector3 &_accelMean,
- math::Quaternion &_orientation);
+ ignition::math::Vector3d &_rateMean,
+ ignition::math::Vector3d &_accelMean,
+ ignition::math::Quaterniond &_orientation);
};
void ImuTest::GetGravity(const math::Quaternion &_rot, math::Vector3 &_g)
@@ -77,14 +78,14 @@ void ImuTest::GetGravity(const math::Quaternion &_rot, math::Vector3 &_g)
void ImuTest::GetImuData(sensors::ImuSensorPtr _imu,
unsigned int _cnt,
- math::Vector3 &_rateMean,
- math::Vector3 &_accelMean,
- math::Quaternion& _orientation)
+ ignition::math::Vector3d &_rateMean,
+ ignition::math::Vector3d &_accelMean,
+ ignition::math::Quaterniond &_orientation)
{
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
// Collect a number of samples and return the average rate and accel values
- math::Vector3 rateSum, accelSum;
+ ignition::math::Vector3d rateSum, accelSum;
for (unsigned int i = 0; i < _cnt; ++i)
{
world->Step(1);
@@ -100,12 +101,12 @@ void ImuTest::GetImuData(sensors::ImuSensorPtr _imu,
EXPECT_LT(j, 100);
- rateSum += _imu->GetAngularVelocity();
- accelSum += _imu->GetLinearAcceleration();
+ rateSum += _imu->AngularVelocity();
+ accelSum += _imu->LinearAcceleration();
}
_rateMean = rateSum / _cnt;
_accelMean = accelSum / _cnt;
- _orientation = _imu->GetOrientation();
+ _orientation = _imu->Orientation();
}
void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
@@ -183,7 +184,8 @@ void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
// Link::GetWorldLinearAccel() [-0 0.055649 19.6158]
{
// get states from imu sensor
- math::Vector3 imuLinearAccel = pendulumImu->GetLinearAcceleration();
+ ignition::math::Vector3d imuLinearAccel =
+ pendulumImu->LinearAcceleration();
// get states from link
math::Vector3 relativeLinearAccel =
pendulumModel->GetRelativeLinearAccel();
@@ -193,9 +195,9 @@ void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
if (world->GetSimTime().Double() == 1.872)
{
// initial values
- EXPECT_NEAR(imuLinearAccel.x, 0, IMU_TOL);
- EXPECT_NEAR(imuLinearAccel.y, -0.041216, IMU_TOL);
- EXPECT_NEAR(imuLinearAccel.z, 29.42581726, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.X(), 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.Y(), -0.041216, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.Z(), 29.42581726, IMU_TOL);
EXPECT_NEAR(relativeLinearAccel.x, 0, IMU_TOL);
EXPECT_NEAR(relativeLinearAccel.y, -0.036397, IMU_TOL);
EXPECT_NEAR(relativeLinearAccel.z, 19.6158848, IMU_TOL);
@@ -206,7 +208,7 @@ void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
else
{
// initial values
- EXPECT_LE(imuLinearAccel.z, 29.4259);
+ EXPECT_LE(imuLinearAccel.Z(), 29.4259);
EXPECT_LE(relativeLinearAccel.z, 19.616);
EXPECT_LE(worldLinearAccel.z, 19.616);
}
@@ -229,7 +231,8 @@ void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
// Link::GetWorldLinearAccel() [0 0 0]
{
// get states from imu sensor
- math::Vector3 imuLinearAccel = ballFrictionImu->GetLinearAcceleration();
+ ignition::math::Vector3d imuLinearAccel =
+ ballFrictionImu->LinearAcceleration();
// get states from link
math::Vector3 relativeLinearAccel =
ballFrictionModel->GetRelativeLinearAccel();
@@ -239,9 +242,9 @@ void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
if (world->GetSimTime().Double() <= 1.0)
{
// freefall
- EXPECT_NEAR(imuLinearAccel.x, 0, IMU_TOL);
- EXPECT_NEAR(imuLinearAccel.y, 0, IMU_TOL);
- EXPECT_NEAR(imuLinearAccel.z, 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.X(), 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.Y(), 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.Z(), 0, IMU_TOL);
EXPECT_NEAR(relativeLinearAccel.x, g.x, IMU_TOL);
EXPECT_NEAR(relativeLinearAccel.y, g.y, IMU_TOL);
EXPECT_NEAR(relativeLinearAccel.z, g.z, IMU_TOL);
@@ -259,7 +262,7 @@ void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
else if (world->GetSimTime().Double() >= 1.85)
{
// on the ground
- double imuMag = imuLinearAccel.GetLength();
+ double imuMag = imuLinearAccel.Length();
double gMag = g.GetLength();
EXPECT_NEAR(imuMag, gMag, IMU_TOL);
EXPECT_NEAR(relativeLinearAccel.x, 0, IMU_TOL);
@@ -286,8 +289,8 @@ void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
// Link::GetWorldLinearAccel() [0 0 0]
{
// get states from imu sensor
- math::Vector3 imuLinearAccel =
- ballNoFrictionImu->GetLinearAcceleration();
+ ignition::math::Vector3d imuLinearAccel =
+ ballNoFrictionImu->LinearAcceleration();
// get states from link
math::Vector3 relativeLinearAccel =
ballNoFrictionModel->GetRelativeLinearAccel();
@@ -297,9 +300,9 @@ void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
if (world->GetSimTime().Double() <= 1.0)
{
// freefall
- EXPECT_NEAR(imuLinearAccel.x, 0, IMU_TOL);
- EXPECT_NEAR(imuLinearAccel.y, 0, IMU_TOL);
- EXPECT_NEAR(imuLinearAccel.z, 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.X(), 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.Y(), 0, IMU_TOL);
+ EXPECT_NEAR(imuLinearAccel.Z(), 0, IMU_TOL);
EXPECT_NEAR(relativeLinearAccel.x, g.x, IMU_TOL);
EXPECT_NEAR(relativeLinearAccel.y, g.y, IMU_TOL);
EXPECT_NEAR(relativeLinearAccel.z, g.z, IMU_TOL);
@@ -313,7 +316,7 @@ void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
// on the ramp
const double rampAngle = 0.5;
double gMag = g.GetLength();
- double imuMag = imuLinearAccel.GetLength();
+ double imuMag = imuLinearAccel.Length();
EXPECT_NEAR(imuMag, gMag*cos(rampAngle), IMU_TOL);
double relMag = relativeLinearAccel.GetLength();
@@ -324,7 +327,7 @@ void ImuTest::ImuSensorTestWorld(const std::string &_physicsEngine)
else if (world->GetSimTime().Double() >= 1.8)
{
// on the ground
- double imuMag = imuLinearAccel.GetLength();
+ double imuMag = imuLinearAccel.Length();
double gMag = g.GetLength();
EXPECT_NEAR(imuMag, gMag, IMU_TOL);
EXPECT_NEAR(relativeLinearAccel.x, 0, IMU_TOL);
@@ -368,26 +371,26 @@ void ImuTest::Stationary_EmptyWorld(const std::string &_physicsEngine)
ASSERT_TRUE(imu != NULL);
imu->Init();
- math::Vector3 rateMean, accelMean;
- math::Quaternion orientation;
+ ignition::math::Vector3d rateMean, accelMean;
+ ignition::math::Quaterniond orientation;
this->GetImuData(imu, 1, rateMean, accelMean, orientation);
- EXPECT_NEAR(rateMean.x, 0.0, IMU_TOL);
- EXPECT_NEAR(rateMean.y, 0.0, IMU_TOL);
- EXPECT_NEAR(rateMean.z, 0.0, IMU_TOL);
+ EXPECT_NEAR(rateMean.X(), 0.0, IMU_TOL);
+ EXPECT_NEAR(rateMean.Y(), 0.0, IMU_TOL);
+ EXPECT_NEAR(rateMean.Z(), 0.0, IMU_TOL);
math::Vector3 g;
this->GetGravity(testPose.rot, g);
- EXPECT_NEAR(accelMean.x, -g.x, IMU_TOL);
- EXPECT_NEAR(accelMean.y, -g.y, IMU_TOL);
- EXPECT_NEAR(accelMean.z, -g.z, IMU_TOL);
+ EXPECT_NEAR(accelMean.X(), -g.x, IMU_TOL);
+ EXPECT_NEAR(accelMean.Y(), -g.y, IMU_TOL);
+ EXPECT_NEAR(accelMean.Z(), -g.z, IMU_TOL);
// Orientation should be identity, since it is reported relative
// to reference pose.
- EXPECT_NEAR(orientation.x, 0, IMU_TOL);
- EXPECT_NEAR(orientation.y, 0, IMU_TOL);
- EXPECT_NEAR(orientation.z, 0, IMU_TOL);
- EXPECT_NEAR(orientation.w, 1, IMU_TOL);
+ EXPECT_NEAR(orientation.X(), 0, IMU_TOL);
+ EXPECT_NEAR(orientation.Y(), 0, IMU_TOL);
+ EXPECT_NEAR(orientation.Z(), 0, IMU_TOL);
+ EXPECT_NEAR(orientation.W(), 1, IMU_TOL);
}
TEST_P(ImuTest, EmptyWorld)
@@ -432,23 +435,23 @@ void ImuTest::Stationary_EmptyWorld_Noise(const std::string &_physicsEngine)
ASSERT_TRUE(imu != NULL);
imu->Init();
- math::Vector3 rateMean, accelMean;
- math::Quaternion orientation;
+ ignition::math::Vector3d rateMean, accelMean;
+ ignition::math::Quaterniond orientation;
this->GetImuData(imu, 1000, rateMean, accelMean, orientation);
double d1, d2;
// Have to account for the fact that the bias might be sampled as positive
// or negative
- d1 = fabs(rateMean.x - (rateNoiseMean + rateBiasMean));
- d2 = fabs(rateMean.x - (rateNoiseMean - rateBiasMean));
+ d1 = fabs(rateMean.X() - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.X() - (rateNoiseMean - rateBiasMean));
EXPECT_NEAR(0.0, std::min(d1, d2),
3*rateNoiseStddev + 3*rateBiasStddev);
- d1 = fabs(rateMean.y - (rateNoiseMean + rateBiasMean));
- d2 = fabs(rateMean.y - (rateNoiseMean - rateBiasMean));
+ d1 = fabs(rateMean.Y() - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.Y() - (rateNoiseMean - rateBiasMean));
EXPECT_NEAR(0.0, std::min(d1, d2),
3*rateNoiseStddev + 3*rateBiasStddev);
- d1 = fabs(rateMean.z - (rateNoiseMean + rateBiasMean));
- d2 = fabs(rateMean.z - (rateNoiseMean - rateBiasMean));
+ d1 = fabs(rateMean.Z() - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.Z() - (rateNoiseMean - rateBiasMean));
EXPECT_NEAR(0.0, std::min(d1, d2),
3*rateNoiseStddev + 3*rateBiasStddev);
@@ -456,25 +459,25 @@ void ImuTest::Stationary_EmptyWorld_Noise(const std::string &_physicsEngine)
this->GetGravity(testPose.rot, g);
// Have to account for the fact that the bias might be sampled as positive
// or negative
- d1 = fabs(accelMean.x - (accelNoiseMean + accelBiasMean) + g.x);
- d2 = fabs(accelMean.x - (accelNoiseMean - accelBiasMean) + g.x);
+ d1 = fabs(accelMean.X() - (accelNoiseMean + accelBiasMean) + g.x);
+ d2 = fabs(accelMean.X() - (accelNoiseMean - accelBiasMean) + g.x);
EXPECT_NEAR(0.0, std::min(d1, d2),
3*accelNoiseStddev + 3*accelBiasStddev);
- d1 = fabs(accelMean.y - (accelNoiseMean + accelBiasMean) + g.y);
- d2 = fabs(accelMean.y - (accelNoiseMean - accelBiasMean) + g.y);
+ d1 = fabs(accelMean.Y() - (accelNoiseMean + accelBiasMean) + g.y);
+ d2 = fabs(accelMean.Y() - (accelNoiseMean - accelBiasMean) + g.y);
EXPECT_NEAR(0.0, std::min(d1, d2),
3*accelNoiseStddev + 3*accelBiasStddev);
- d1 = fabs(accelMean.z - (accelNoiseMean + accelBiasMean) + g.z);
- d2 = fabs(accelMean.z - (accelNoiseMean - accelBiasMean) + g.z);
+ d1 = fabs(accelMean.Z() - (accelNoiseMean + accelBiasMean) + g.z);
+ d2 = fabs(accelMean.Z() - (accelNoiseMean - accelBiasMean) + g.z);
EXPECT_NEAR(0.0, std::min(d1, d2),
3*accelNoiseStddev + 3*accelBiasStddev);
// Orientation should be identity, since it is reported relative
// to reference pose.
- EXPECT_NEAR(orientation.x, 0, IMU_TOL);
- EXPECT_NEAR(orientation.y, 0, IMU_TOL);
- EXPECT_NEAR(orientation.z, 0, IMU_TOL);
- EXPECT_NEAR(orientation.w, 1, IMU_TOL);
+ EXPECT_NEAR(orientation.X(), 0, IMU_TOL);
+ EXPECT_NEAR(orientation.Y(), 0, IMU_TOL);
+ EXPECT_NEAR(orientation.Z(), 0, IMU_TOL);
+ EXPECT_NEAR(orientation.W(), 1, IMU_TOL);
}
TEST_P(ImuTest, EmptyWorldNoise)
@@ -519,23 +522,23 @@ void ImuTest::Stationary_EmptyWorld_Bias(const std::string &_physicsEngine)
ASSERT_TRUE(imu != NULL);
imu->Init();
- math::Vector3 rateMean, accelMean;
- math::Quaternion orientation;
+ ignition::math::Vector3d rateMean, accelMean;
+ ignition::math::Quaterniond orientation;
this->GetImuData(imu, 1000, rateMean, accelMean, orientation);
double d1, d2;
// Have to account for the fact that the bias might be sampled as positive
// or negative
- d1 = fabs(rateMean.x - (rateNoiseMean + rateBiasMean));
- d2 = fabs(rateMean.x - (rateNoiseMean - rateBiasMean));
+ d1 = fabs(rateMean.X() - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.X() - (rateNoiseMean - rateBiasMean));
EXPECT_NEAR(0.0, std::min(d1, d2),
3*rateNoiseStddev + 3*rateBiasStddev);
- d1 = fabs(rateMean.y - (rateNoiseMean + rateBiasMean));
- d2 = fabs(rateMean.y - (rateNoiseMean - rateBiasMean));
+ d1 = fabs(rateMean.Y() - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.Y() - (rateNoiseMean - rateBiasMean));
EXPECT_NEAR(0.0, std::min(d1, d2),
3*rateNoiseStddev + 3*rateBiasStddev);
- d1 = fabs(rateMean.z - (rateNoiseMean + rateBiasMean));
- d2 = fabs(rateMean.z - (rateNoiseMean - rateBiasMean));
+ d1 = fabs(rateMean.Z() - (rateNoiseMean + rateBiasMean));
+ d2 = fabs(rateMean.Z() - (rateNoiseMean - rateBiasMean));
EXPECT_NEAR(0.0, std::min(d1, d2),
3*rateNoiseStddev + 3*rateBiasStddev);
@@ -543,25 +546,25 @@ void ImuTest::Stationary_EmptyWorld_Bias(const std::string &_physicsEngine)
this->GetGravity(testPose.rot, g);
// Have to account for the fact that the bias might be sampled as positive
// or negative
- d1 = fabs(accelMean.x - (accelNoiseMean + accelBiasMean) + g.x);
- d2 = fabs(accelMean.x - (accelNoiseMean - accelBiasMean) + g.x);
+ d1 = fabs(accelMean.X() - (accelNoiseMean + accelBiasMean) + g.x);
+ d2 = fabs(accelMean.X() - (accelNoiseMean - accelBiasMean) + g.x);
EXPECT_NEAR(0.0, std::min(d1, d2),
3*accelNoiseStddev + 3*accelBiasStddev);
- d1 = fabs(accelMean.y - (accelNoiseMean + accelBiasMean) + g.y);
- d2 = fabs(accelMean.y - (accelNoiseMean - accelBiasMean) + g.y);
+ d1 = fabs(accelMean.Y() - (accelNoiseMean + accelBiasMean) + g.y);
+ d2 = fabs(accelMean.Y() - (accelNoiseMean - accelBiasMean) + g.y);
EXPECT_NEAR(0.0, std::min(d1, d2),
3*accelNoiseStddev + 3*accelBiasStddev);
- d1 = fabs(accelMean.z - (accelNoiseMean + accelBiasMean) + g.z);
- d2 = fabs(accelMean.z - (accelNoiseMean - accelBiasMean) + g.z);
+ d1 = fabs(accelMean.Z() - (accelNoiseMean + accelBiasMean) + g.z);
+ d2 = fabs(accelMean.Z() - (accelNoiseMean - accelBiasMean) + g.z);
EXPECT_NEAR(0.0, std::min(d1, d2),
3*accelNoiseStddev + 3*accelBiasStddev);
// Orientation should be identity, since it is reported relative
// to reference pose.
- EXPECT_NEAR(orientation.x, 0, IMU_TOL);
- EXPECT_NEAR(orientation.y, 0, IMU_TOL);
- EXPECT_NEAR(orientation.z, 0, IMU_TOL);
- EXPECT_NEAR(orientation.w, 1, IMU_TOL);
+ EXPECT_NEAR(orientation.X(), 0, IMU_TOL);
+ EXPECT_NEAR(orientation.Y(), 0, IMU_TOL);
+ EXPECT_NEAR(orientation.Z(), 0, IMU_TOL);
+ EXPECT_NEAR(orientation.W(), 1, IMU_TOL);
}
TEST_P(ImuTest, EmptyWorldBias)
@@ -575,7 +578,7 @@ int main(int argc, char **argv)
{
// Set a specific seed to avoid occasional test failures due to
// statistically unlikely, but possible results.
- math::Rand::SetSeed(42);
+ ignition::math::Rand::Seed(42);
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
diff --git a/test/integration/insert_model.cc b/test/integration/insert_model.cc
index a3713bf..bf0a10d 100644
--- a/test/integration/insert_model.cc
+++ b/test/integration/insert_model.cc
@@ -32,14 +32,8 @@ void InsertModelTest::ReadPermissions()
this->Load("worlds/empty.world", false, false, true);
- gazebo::gui::MainWindow mainWindow;
-
- mainWindow.Load();
- mainWindow.Init();
-
- // wait a bit for the event to fire (?)
gazebo::gui::InsertModelWidget *insertModelWidget =
- mainWindow.findChild<gazebo::gui::InsertModelWidget *>("insertModel");
+ new gazebo::gui::InsertModelWidget();
// Create files in a temporary directory and set permissions accordingly
boost::filesystem::path testDir = boost::filesystem::temp_directory_path() /
@@ -132,8 +126,6 @@ void InsertModelTest::ReadPermissions()
boost::filesystem::permissions(modelConfig, boost::filesystem::all_all);
}
- mainWindow.close();
-
// Delete all test files
boost::filesystem::remove_all(testDir);
}
diff --git a/test/integration/joint_controller.cc b/test/integration/joint_controller.cc
index 6646504..4d5b544 100644
--- a/test/integration/joint_controller.cc
+++ b/test/integration/joint_controller.cc
@@ -21,7 +21,7 @@
#include "gazebo/physics/Model.hh"
#include "gazebo/physics/JointController.hh"
#include "gazebo/common/PID.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "test_config.h"
using namespace gazebo;
diff --git a/test/integration/joint_force_torque.cc b/test/integration/joint_force_torque.cc
index 14ce289..603833b 100644
--- a/test/integration/joint_force_torque.cc
+++ b/test/integration/joint_force_torque.cc
@@ -18,8 +18,8 @@
#include <gtest/gtest.h>
#include "gazebo/physics/physics.hh"
#include "gazebo/physics/Joint.hh"
-#include "test/ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#define TOL 1e-6
#define TOL_CONT 2.0
diff --git a/test/integration/joint_gearbox.cc b/test/integration/joint_gearbox.cc
index 1c00d85..4818dd8 100644
--- a/test/integration/joint_gearbox.cc
+++ b/test/integration/joint_gearbox.cc
@@ -17,7 +17,7 @@
#include <gtest/gtest.h>
#include "gazebo/physics/physics.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#define TOL 1e-6
using namespace gazebo;
diff --git a/test/integration/joint_get_force_torque.cc b/test/integration/joint_get_force_torque.cc
new file mode 100644
index 0000000..50d641a
--- /dev/null
+++ b/test/integration/joint_get_force_torque.cc
@@ -0,0 +1,372 @@
+/*
+ * 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.h>
+
+#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 JointGetForceTorqueTest : public ServerFixture,
+ public testing::WithParamInterface<const char *>
+{
+ /// \brief Constructor
+ protected: JointGetForceTorqueTest() : ServerFixture()
+ {
+ }
+
+ /// \brief Class to hold parameters for spawning joints.
+ public: class SpawnGetFTBoxOptions
+ {
+ /// \brief Constructor.
+ public: SpawnGetFTBoxOptions() : mass(10.0),
+ size(1.0, 1.0, 1.0),
+ parentIsWorld(false)
+ {
+ }
+
+ /// \brief Destructor.
+ public: ~SpawnGetFTBoxOptions()
+ {
+ }
+
+ /// \brief Mass of box to spawn (inertia computed automatically).
+ public: double mass;
+
+ /// \brief Size of box to spawn.
+ public: ignition::math::Vector3d size;
+
+ /// \brief Model pose.
+ public: ignition::math::Pose3d modelPose;
+
+ /// \brief Link pose.
+ public: ignition::math::Pose3d linkPose;
+
+ /// \brief Inertial pose.
+ public: ignition::math::Pose3d inertialPose;
+
+ /// \brief Joint type
+ public: std::string jointType;
+
+ /// \brief Joint axis
+ public: ignition::math::Vector3d jointAxis;
+
+ /// \brief If true the parent of the joint is the world
+ /// otherwise the parent of the tested joint is a dummy link
+ /// that is itself connect to the world
+ public: bool parentIsWorld;
+ };
+
+ /// \brief Spawn a box rigidly attached to the world
+ /// \param[in] _opt Options for box attached to the world.
+ public: physics::ModelPtr SpawnBox(const SpawnGetFTBoxOptions &_opt)
+ {
+ msgs::Model msg;
+ std::string modelName = this->GetUniqueString("box_model");
+ msg.set_name(modelName);
+ msgs::Set(msg.mutable_pose(), _opt.modelPose);
+
+ double dx = _opt.size.X();
+ double dy = _opt.size.Y();
+ double dz = _opt.size.Z();
+ double ixx = _opt.mass/12.0 * (dy*dy + dz*dz);
+ double iyy = _opt.mass/12.0 * (dz*dz + dx*dx);
+ double izz = _opt.mass/12.0 * (dx*dx + dy*dy);
+
+
+ if ( !_opt.parentIsWorld )
+ {
+ msg.add_link();
+ int linkCount = msg.link_size();
+ auto link = msg.mutable_link(linkCount-1);
+ link->set_name("dummy_link");
+ }
+
+ msg.add_link();
+ int linkCount = msg.link_size();
+ auto link = msg.mutable_link(linkCount-1);
+ link->set_name("link");
+ msgs::Set(link->mutable_pose(), _opt.linkPose);
+ msgs::Inertial inertial;
+ inertial.set_mass(_opt.mass);
+ inertial.set_ixx(ixx);
+ inertial.set_iyy(iyy);
+ inertial.set_izz(izz);
+ inertial.set_ixy(0.0);
+ inertial.set_ixz(0.0);
+ inertial.set_iyz(0.0);
+ msgs::Set(inertial.mutable_pose(), _opt.inertialPose);
+
+ *(link->mutable_inertial()) = inertial;
+
+ // Depending on the parentIsWorld option, we test
+ // two different cases: a joint that is connecting
+ // a link to the world (parentIsWorld true) and
+ // a joint connecting two different links (and the
+ // parent one is itself connected to the world for
+ // convenience)
+ if ( !_opt.parentIsWorld )
+ {
+ msg.add_joint();
+ int jointCount = msg.joint_size();
+ auto joint = msg.mutable_joint(jointCount-1);
+ joint->set_name("dummy_joint");
+ joint->set_parent("world");
+ joint->set_child("dummy_link");
+ joint->set_type(msgs::ConvertJointType("fixed"));
+ }
+
+ msg.add_joint();
+ int jointCount = msg.joint_size();
+ auto joint = msg.mutable_joint(jointCount-1);
+ joint->set_name("joint");
+ joint->set_type(msgs::ConvertJointType(_opt.jointType));
+ if ( _opt.parentIsWorld )
+ {
+ joint->set_parent("world");
+ }
+ else
+ {
+ joint->set_parent("dummy_link");
+ }
+ joint->set_child("link");
+
+ if ( _opt.jointType != "fixed" )
+ {
+ auto axis = joint->mutable_axis1();
+ msgs::Set(axis->mutable_xyz(), _opt.jointAxis);
+ axis->set_limit_lower(0.0);
+ axis->set_limit_upper(0.0);
+ }
+
+ physics::WorldPtr world = physics::get_world("default");
+ physics::ModelPtr model =this->SpawnModel(msg);
+
+ physics::JointPtr pJoint = model->GetJoint("joint");
+ pJoint->SetProvideFeedback(true);
+
+ return model;
+ }
+
+ /// \brief Helper function for GetForceTorqueDemo
+ /// \param[in] _world Pointer to the world object
+ /// \param[in] _physics Pointer to the physics object.
+ /// \param[in] _physicsEngine Physics engine to use.
+ /// \param[in] _options Options for the joint to test.
+ public: void GetFTDemoHelper(physics::WorldPtr _world,
+ physics::PhysicsEnginePtr _physics,
+ const std::string &_physicsEngine,
+ const SpawnGetFTBoxOptions & opt);
+
+ /// \brief Test GetForceTorque method for different type joints
+ /// \param[in] _physicsEngine Physics engine to use.
+ public: void GetForceTorqueDemo(const std::string &_physicsEngine);
+};
+
+/////////////////////////////////////////////////
+void JointGetForceTorqueTest::GetFTDemoHelper(
+ physics::WorldPtr _world,
+ physics::PhysicsEnginePtr _physics,
+ const std::string& _physicsEngine,
+ const SpawnGetFTBoxOptions& opt)
+{
+ gzdbg << "GetFTDemoHelper for physics " << _physicsEngine
+ << "joint type " << opt.jointType
+ << " joint axis " << opt.jointAxis << std::endl;
+ ignition::math::Vector3d g = _physics->GetGravity().Ign();
+ double mass = opt.mass;
+
+ physics::ModelPtr model = SpawnBox(opt);
+ physics::LinkPtr link = model->GetLink("link");
+ physics::JointPtr joint = model->GetJoint("joint");
+
+ ASSERT_TRUE(model != NULL);
+ ASSERT_TRUE(link != NULL);
+ ASSERT_TRUE(joint != NULL);
+
+ ignition::math::Vector3d com = link->GetWorldCoGPose().pos.Ign();
+ ignition::math::Vector3d jointOrigin = joint->GetWorldPose().pos.Ign();
+
+ // do a simulation step to get a meaningful measure
+ _world->Step(1);
+
+ // ode need some additional steps
+ if (_physicsEngine == "ode")
+ {
+ _world->Step(15);
+ }
+
+ // bullet need some additional steps
+ // probably related to Gazebo issue 1196
+ if (_physicsEngine == "bullet")
+ {
+ _world->Step(99);
+ }
+
+ if (opt.parentIsWorld)
+ {
+ ASSERT_EQ(model->GetJointCount(), 1u);
+ }
+ else
+ {
+ ASSERT_EQ(model->GetJointCount(), 2u);
+ }
+
+ physics::JointWrench W = joint->GetForceTorque(0u);
+
+ const int TOL_FORCE = 1.0;
+ const int TOL_TORQUE = 2.0;
+
+ // Everthing is expressed in world frame, so
+ // the math is easy
+ ignition::math::Vector3d fWorld = mass*g;
+ ignition::math::Vector3d tauWorld = mass*(com-jointOrigin).Cross(g);
+
+ ignition::math::Pose3d parentPose;
+ ignition::math::Pose3d childPose = link->GetWorldPose().Ign();
+
+ if (!opt.parentIsWorld)
+ {
+ parentPose = link->GetParentJointsLinks()[0]->GetWorldPose().Ign();
+ }
+
+ ignition::math::Vector3d body1ForceExpected =
+ -(parentPose.Rot().Inverse() * fWorld);
+ ignition::math::Vector3d body1TorqueExpected =
+ -(parentPose.Rot().Inverse() * tauWorld);
+ ignition::math::Vector3d body2ForceExpected =
+ childPose.Rot().Inverse() * fWorld;
+ ignition::math::Vector3d body2TorqueExpected =
+ childPose.Rot().Inverse() * tauWorld;
+
+ EXPECT_NEAR(body1ForceExpected.X(), W.body1Force.x, TOL_FORCE);
+ EXPECT_NEAR(body1ForceExpected.Y(), W.body1Force.y, TOL_FORCE);
+ EXPECT_NEAR(body1ForceExpected.Z(), W.body1Force.z, TOL_FORCE);
+ EXPECT_NEAR(body1TorqueExpected.X(), W.body1Torque.x, TOL_TORQUE);
+ EXPECT_NEAR(body1TorqueExpected.Y(), W.body1Torque.y, TOL_TORQUE);
+ EXPECT_NEAR(body1TorqueExpected.Z(), W.body1Torque.z, TOL_TORQUE);
+
+ EXPECT_NEAR(body2ForceExpected.X(), W.body2Force.x, TOL_FORCE);
+ EXPECT_NEAR(body2ForceExpected.Y(), W.body2Force.y, TOL_FORCE);
+ EXPECT_NEAR(body2ForceExpected.Z(), W.body2Force.z, TOL_FORCE);
+ EXPECT_NEAR(body2TorqueExpected.X(), W.body2Torque.x, TOL_TORQUE);
+ EXPECT_NEAR(body2TorqueExpected.Y(), W.body2Torque.y, TOL_TORQUE);
+ EXPECT_NEAR(body2TorqueExpected.Z(), W.body2Torque.z, TOL_TORQUE);
+
+ // Remove model
+ _world->RemoveModel(model);
+}
+
+/////////////////////////////////////////////////
+// GetForceTorqueDemo test:
+// spawn a simple box connected to the world
+// with different types of "fixed" joints
+// (actual fixed joint, or revolute/prismatic joint
+// with zero limis) and then check the GetForceTorque
+// output against analytical solution.
+void JointGetForceTorqueTest::GetForceTorqueDemo(const std::string &_physEng)
+{
+ Load("worlds/empty.world", true, _physEng);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ EXPECT_EQ(physics->GetType(), _physEng);
+
+ // test for a fixed joint
+ SpawnGetFTBoxOptions opt;
+ opt.jointType = "fixed";
+ opt.inertialPose.Pos() = ignition::math::Vector3d(1.0, 2.0, 3.0);
+ opt.linkPose.Rot().Euler(0.1, 0.0, 0.0);
+
+ opt.parentIsWorld = true;
+ this->GetFTDemoHelper(world, physics, _physEng, opt);
+
+ opt.parentIsWorld = false;
+ this->GetFTDemoHelper(world, physics, _physEng, opt);
+
+ // test a revolute joint against all axis
+
+ if ( _physEng == "bullet" )
+ {
+ for (int i = 0; i < 3; ++i)
+ {
+ opt.jointType = "revolute";
+ switch (i)
+ {
+ case 0:
+ opt.jointAxis = ignition::math::Vector3d::UnitX;
+ break;
+ case 1:
+ opt.jointAxis = ignition::math::Vector3d::UnitY;
+ break;
+ case 2:
+ opt.jointAxis = ignition::math::Vector3d::UnitZ;
+ break;
+ }
+
+ this->GetFTDemoHelper(world, physics, _physEng, opt);
+ }
+ }
+
+ // test a prismatic joint against all axis
+
+ // bullet and simbody GetForceTorque() is
+ // broken for prismatic joints
+ // see gazebo issues 1639 and 1640
+ if ( _physEng != "bullet" && _physEng != "simbody" )
+ {
+ for (int i = 0; i < 3; ++i)
+ {
+ opt.jointType = "prismatic";
+ switch (i)
+ {
+ case 0:
+ opt.jointAxis = ignition::math::Vector3d::UnitX;
+ break;
+ case 1:
+ opt.jointAxis = ignition::math::Vector3d::UnitY;
+ break;
+ case 2:
+ opt.jointAxis = ignition::math::Vector3d::UnitZ;
+ break;
+ }
+ this->GetFTDemoHelper(world, physics, _physEng, opt);
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_P(JointGetForceTorqueTest, GetForceTorqueDemo)
+{
+ GetForceTorqueDemo(GetParam());
+}
+
+/////////////////////////////////////////////////
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, JointGetForceTorqueTest,
+ PHYSICS_ENGINE_VALUES);
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/joint_revolute.cc b/test/integration/joint_revolute.cc
index 37ebdd4..2dc7e35 100644
--- a/test/integration/joint_revolute.cc
+++ b/test/integration/joint_revolute.cc
@@ -14,11 +14,14 @@
* limitations under the License.
*
*/
+#include <ignition/math/Rand.hh>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/gazebo_config.h"
+#include "gazebo/math/SignalStats.hh"
#include "gazebo/physics/physics.hh"
#include "SimplePendulumIntegrator.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#include "test/integration/joint_test.hh"
using namespace gazebo;
@@ -39,7 +42,9 @@ class JointTestRevolute : public JointTest
/// Measure angular velocity of links, and verify proper axis orientation.
/// Then set joint limits and verify that links remain within limits.
/// \param[in] _physicsEngine Type of physics engine to use.
- public: void RevoluteJoint(const std::string &_physicsEngine);
+ /// \param[in] _solverType Type of solver to use.
+ public: void RevoluteJoint(const std::string &_physicsEngine,
+ const std::string &_solverType="quick");
/// \brief Load a simple pendulum, simulate and compare to numerical
/// results from SimplePendulumIntegrator.
@@ -97,13 +102,6 @@ void JointTestRevolute::PendulumEnergy(const std::string &_physicsEngine)
////////////////////////////////////////////////////////////
void JointTestRevolute::WrapAngle(const std::string &_physicsEngine)
{
- /// \TODO: bullet hinge angles are wrapped (#1074)
- if (_physicsEngine == "bullet")
- {
- gzerr << "Aborting test for bullet, see issues #1074.\n";
- return;
- }
-
// Load an empty world
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
@@ -124,6 +122,7 @@ void JointTestRevolute::WrapAngle(const std::string &_physicsEngine)
ASSERT_TRUE(joint != NULL);
// set velocity to 2 pi rad/s and step forward 1.5 seconds.
+ // angle should reach 3 pi rad.
double vel = 2*M_PI;
unsigned int stepSize = 50;
unsigned int stepCount = 30;
@@ -134,6 +133,7 @@ void JointTestRevolute::WrapAngle(const std::string &_physicsEngine)
joint->SetVelocity(0, vel);
+ math::SignalMaxAbsoluteValue angleErrorMax;
// expect that joint velocity is constant
// and that joint angle is unwrapped
for (unsigned int i = 0; i < stepCount; ++i)
@@ -141,16 +141,28 @@ void JointTestRevolute::WrapAngle(const std::string &_physicsEngine)
world->Step(stepSize);
EXPECT_NEAR(joint->GetVelocity(0), vel, g_tolerance);
double time = world->GetSimTime().Double();
- EXPECT_NEAR(joint->GetAngle(0).Radian(), time*vel, g_tolerance);
+ angleErrorMax.InsertData(joint->GetAngle(0).Radian() - time*vel);
+ }
+#ifndef LIBBULLET_VERSION_GT_282
+ if (_physicsEngine == "bullet")
+ {
+ gzerr << "Skipping portion of test, angle wrapping requires bullet 2.83"
+ << std::endl;
+ }
+ else
+#endif
+ {
+ EXPECT_NEAR(angleErrorMax.Value(), 0.0, g_tolerance);
}
}
}
////////////////////////////////////////////////////////////////////////
-void JointTestRevolute::RevoluteJoint(const std::string &_physicsEngine)
+void JointTestRevolute::RevoluteJoint(const std::string &_physicsEngine,
+ const std::string &_solverType)
{
- math::Rand::SetSeed(0);
+ ignition::math::Rand::Seed(0);
// Load world
Load("worlds/revolute_joint_test.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
@@ -161,6 +173,9 @@ void JointTestRevolute::RevoluteJoint(const std::string &_physicsEngine)
ASSERT_TRUE(physics != NULL);
EXPECT_EQ(physics->GetType(), _physicsEngine);
+ // Set solver type
+ physics->SetParam("solver_type", _solverType);
+
// Model names
std::vector<std::string> modelNames;
modelNames.push_back("pendulum_0deg");
@@ -431,6 +446,7 @@ void JointTestRevolute::RevoluteJoint(const std::string &_physicsEngine)
// Reset world again, disable gravity, detach upper_joint
// Then apply torque at lower_joint and verify motion
world->Reset();
+ physics->SetGravity(math::Vector3::Zero);
for (modelIter = modelNames.begin();
modelIter != modelNames.end(); ++modelIter)
{
@@ -438,23 +454,6 @@ void JointTestRevolute::RevoluteJoint(const std::string &_physicsEngine)
if (model)
{
gzdbg << "Check SetForce for model " << *modelIter << '\n';
- std::vector<std::string>::iterator linkIter;
- for (linkIter = linkNames.begin();
- linkIter != linkNames.end(); ++linkIter)
- {
- link = model->GetLink(*linkIter);
- if (link)
- {
- // Disable gravity for links.
- link->SetGravityMode(false);
- }
- else
- {
- gzerr << "Error loading link " << *linkIter
- << " of model " << *modelIter << '\n';
- EXPECT_TRUE(link != NULL);
- }
- }
joint = model->GetJoint("upper_joint");
if (joint)
@@ -679,7 +678,8 @@ void JointTestRevolute::SimplePendulum(const std::string &_physicsEngine)
// test with global contact_max_correcting_vel at 100
// here we expect much lower energy loss
world->Reset();
- physicsEngine->SetContactMaxCorrectingVel(100);
+ if (_physicsEngine == "ode")
+ EXPECT_TRUE(physicsEngine->SetParam("contact_max_correcting_vel", 100.0));
int steps = 10; // @todo: make this more general
for (int i = 0; i < steps; i ++)
@@ -734,6 +734,11 @@ TEST_P(JointTestRevolute, RevoluteJoint)
RevoluteJoint(this->physicsEngine);
}
+TEST_F(JointTestRevolute, RevoluteJointWorldStep)
+{
+ RevoluteJoint("ode", "world");
+}
+
TEST_P(JointTestRevolute, SimplePendulum)
{
SimplePendulum(this->physicsEngine);
diff --git a/test/integration/joint_screw.cc b/test/integration/joint_screw.cc
index 3f60844..fcb4e67 100644
--- a/test/integration/joint_screw.cc
+++ b/test/integration/joint_screw.cc
@@ -19,8 +19,8 @@
#include "gazebo/physics/physics.hh"
// #include "gazebo/physics/Joint.hh"
// #include "gazebo/physics/ScrewJoint.hh"
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#include "test/integration/joint_test.hh"
using namespace gazebo;
@@ -504,6 +504,16 @@ void JointTestScrew::ScrewJointLimitForce(const std::string &_physicsEngine)
physics::ModelPtr model = world->GetModel("pr2");
physics::LinkPtr link_00 = model->GetLink("torso_lift_link");
+ if (_physicsEngine == "dart")
+ {
+ gzerr << _physicsEngine
+ << " is broken for this test,"
+ << " because of the pr2 gripper's closed kinematic chain,"
+ << " see issue #1435."
+ << std::endl;
+ return;
+ }
+
// drop from some height
model->SetWorldPose(math::Pose(0, 0, 0.5, 0, 0, 0));
// +1sec: should have hit the ground
diff --git a/test/integration/joint_set_position_test.cc b/test/integration/joint_set_position_test.cc
index e3745ac..efed692 100644
--- a/test/integration/joint_set_position_test.cc
+++ b/test/integration/joint_set_position_test.cc
@@ -18,8 +18,8 @@
#include <gtest/gtest.h>
#include "gazebo/physics/physics.hh"
// #include "gazebo/physics/Joint.hh"
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#define TOL 0.001
using namespace gazebo;
diff --git a/test/integration/joint_spawn.cc b/test/integration/joint_spawn.cc
index 677e78d..1c23081 100644
--- a/test/integration/joint_spawn.cc
+++ b/test/integration/joint_spawn.cc
@@ -18,7 +18,7 @@
#include <gtest/gtest.h>
#include "gazebo/physics/physics.hh"
#include "gazebo/physics/Joint.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#include "test/integration/joint_test.hh"
#define TOL 1e-6
@@ -71,14 +71,6 @@ class JointSpawningTest_RotationalWorld : public JointSpawningTest {};
void JointSpawningTest::SpawnJointTypes(const std::string &_physicsEngine,
const std::string &_jointType)
{
- /// \TODO: simbody not complete for this test
- if (_physicsEngine == "simbody") // &&
- // _jointType != "revolute" && _jointType != "prismatic")
- {
- gzerr << "Aborting test for Simbody, see issues #859, #861.\n";
- return;
- }
-
// Load an empty world
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
@@ -145,10 +137,10 @@ void JointSpawningTest::SpawnJointTypes(const std::string &_physicsEngine,
gzerr << "Skip tests for child world link cases "
<< "since DART does not allow joint with world as child. "
<< "Please see issue #914. "
- << "(https://bitbucket.org/osrf/gazebo/issue/914)\n";
- return;
+ << "(https://bitbucket.org/osrf/gazebo/issue/914)"
+ << std::endl;
}
-
+ else
{
gzdbg << "SpawnJoint " << _jointType << " world parent" << std::endl;
physics::JointPtr joint = SpawnJoint(_jointType, true, false);
@@ -173,13 +165,6 @@ void JointSpawningTest::SpawnJointTypes(const std::string &_physicsEngine,
void JointSpawningTest::SpawnJointRotational(const std::string &_physicsEngine,
const std::string &_jointType)
{
- /// \TODO: simbody not complete for this test
- if (_physicsEngine == "simbody" && _jointType != "revolute")
- {
- gzerr << "Aborting test for Simbody, see issue #859.\n";
- return;
- }
-
// Load an empty world
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
@@ -226,13 +211,6 @@ void JointSpawningTest::SpawnJointRotationalWorld(
const std::string &_physicsEngine,
const std::string &_jointType)
{
- /// \TODO: simbody not complete for this test
- if (_physicsEngine == "simbody") // && _jointType != "revolute")
- {
- gzerr << "Aborting test for Simbody, see issues #859, #861.\n";
- return;
- }
-
// Load an empty world
Load("worlds/empty.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
@@ -246,17 +224,6 @@ void JointSpawningTest::SpawnJointRotationalWorld(
physics::JointPtr joint;
for (unsigned int i = 0; i < 2; ++i)
{
- if (_physicsEngine == "dart" && i == 0)
- {
- // DART assumes that: (i) every link has its parent joint (ii) root link
- // is the only link that doesn't have parent link.
- // Child world link breaks dart for now. Do we need to support it?
- gzerr << "Skip tests for child world link cases "
- << "since DART does not allow joint with world as child. "
- << "Please see issue #914. "
- << "(https://bitbucket.org/osrf/gazebo/issue/914)\n";
- continue;
- }
bool worldChild = (i == 0);
bool worldParent = (i == 1);
std::string child = worldChild ? "world" : "child";
@@ -264,15 +231,30 @@ void JointSpawningTest::SpawnJointRotationalWorld(
gzdbg << "SpawnJoint " << _jointType << " "
<< child << " "
<< parent << std::endl;
+
+ if ((_physicsEngine == "dart" || _physicsEngine == "simbody")
+ && worldChild)
+ {
+ // These physics engines don't support world as a child link.
+ // simbody https://bitbucket.org/osrf/gazebo/issue/861
+ // dart https://bitbucket.org/osrf/gazebo/issue/914
+ gzerr << "Skip tests for child world link cases since "
+ << _physicsEngine
+ << " does not allow joint with world as child. "
+ << "Please see bitbucket issues #861, #914."
+ << std::endl;
+ continue;
+ }
+
joint = SpawnJoint(_jointType, worldChild, worldParent);
- EXPECT_TRUE(joint != NULL);
+ ASSERT_TRUE(joint != NULL);
physics::LinkPtr link;
if (!worldChild)
link = joint->GetChild();
else if (!worldParent)
link = joint->GetParent();
- EXPECT_TRUE(link != NULL);
+ ASSERT_TRUE(link != NULL);
math::Pose initialPose = link->GetWorldPose();
world->Step(100);
@@ -291,8 +273,9 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
ASSERT_TRUE(world != NULL);
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
ASSERT_TRUE(physics != NULL);
- bool isOde = physics->GetType().compare("ode") == 0;
bool isBullet = physics->GetType().compare("bullet") == 0;
+ bool isDart = physics->GetType().compare("dart") == 0;
+ bool isSimbody = physics->GetType().compare("simbody") == 0;
double dt = physics->GetMaxStepSize();
if (_joint->HasType(physics::Base::HINGE2_JOINT) ||
@@ -308,6 +291,9 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
return;
}
+ // Reset world prior to testing SetVelocity
+ // This is needed for SimbodyUniversalJoint
+ world->Reset();
double velocityMagnitude = 1.0;
std::vector<double> velocities;
velocities.push_back(velocityMagnitude);
@@ -361,7 +347,7 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
}
// Test Coloumb friction
- if (isBullet && !_joint->HasType(physics::Base::HINGE_JOINT))
+ if (isBullet && _joint->HasType(physics::Base::UNIVERSAL_JOINT))
{
gzerr << "Skipping friction test for "
<< physics->GetType()
@@ -370,7 +356,7 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
<< " joint"
<< std::endl;
}
- else if (!isOde && !isBullet)
+ else if (isSimbody)
{
gzerr << "Skipping friction test for "
<< physics->GetType()
@@ -385,7 +371,7 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
// set friction to 1.0
const double friction = 1.0;
- _joint->SetParam("friction", _index, friction);
+ EXPECT_TRUE(_joint->SetParam("friction", _index, friction));
EXPECT_NEAR(_joint->GetParam("friction", _index), friction, g_tolerance);
for (unsigned int i = 0; i < 500; ++i)
@@ -409,6 +395,16 @@ void JointSpawningTest::CheckJointProperties(unsigned int _index,
// Expect motion
EXPECT_GT(_joint->GetVelocity(_index), 0.2 * friction);
EXPECT_GT(_joint->GetAngle(_index).Radian(), 0.05 * friction);
+
+ // DART has problem with joint friction and joint limits
+ // https://github.com/dartsim/dart/issues/317
+ // Set friction back to zero to not interfere with other tests
+ // until this issue is resolved.
+ if (isDart)
+ {
+ EXPECT_TRUE(_joint->SetParam("friction", _index, 0.0));
+ EXPECT_NEAR(_joint->GetParam("friction", _index), 0.0, g_tolerance);
+ }
}
// SetHighStop
@@ -462,6 +458,12 @@ TEST_P(JointSpawningTest_All, SpawnJointTypes)
<< std::endl;
return;
}
+ if (physicsEngine == "simbody" && jointType == "revolute2")
+ {
+ gzerr << "Skip test, revolute2 not supported in simbody, see issue #859."
+ << std::endl;
+ return;
+ }
SpawnJointTypes(this->physicsEngine, this->jointType);
}
@@ -481,6 +483,7 @@ INSTANTIATE_TEST_CASE_P(TestRuns, JointSpawningTest_All,
, "prismatic"
, "screw"
, "universal"
+ , "fixed"
, "ball"
, "revolute2"
, "gearbox")));
@@ -490,13 +493,15 @@ INSTANTIATE_TEST_CASE_P(TestRuns, JointSpawningTest_Rotational,
::testing::Combine(PHYSICS_ENGINE_VALUES,
::testing::Values("revolute"
, "universal"
+ , "fixed"
, "ball")));
// Skip prismatic, screw, and revolute2 because they allow translation
-// Skip universal because it can't be connected to world in bullet.
INSTANTIATE_TEST_CASE_P(TestRuns, JointSpawningTest_RotationalWorld,
::testing::Combine(PHYSICS_ENGINE_VALUES,
::testing::Values("revolute"
+ , "universal"
+ , "fixed"
, "ball")));
int main(int argc, char **argv)
diff --git a/test/integration/joint_test.cc b/test/integration/joint_test.cc
index 5de6d54..7cdb629 100644
--- a/test/integration/joint_test.cc
+++ b/test/integration/joint_test.cc
@@ -18,7 +18,7 @@
#include <gtest/gtest.h>
#include "gazebo/physics/physics.hh"
#include "gazebo/physics/Joint.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#include "test/integration/joint_test.hh"
#define TOL 1e-6
diff --git a/test/integration/joint_test.hh b/test/integration/joint_test.hh
index 07781b6..9343bf0 100644
--- a/test/integration/joint_test.hh
+++ b/test/integration/joint_test.hh
@@ -21,9 +21,10 @@
#include <string>
#include <sstream>
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/common/Time.hh"
+#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
using namespace gazebo;
@@ -31,9 +32,9 @@ using namespace gazebo;
typedef std::tr1::tuple<const char *, const char *> std_string2;
class JointTest : public ServerFixture,
- public ::testing::WithParamInterface<std_string2>
+ public ::testing::WithParamInterface<std_string2>
{
- protected: JointTest() : ServerFixture(), spawnCount(0)
+ protected: JointTest() : ServerFixture()
{
}
@@ -144,93 +145,76 @@ class JointTest : public ServerFixture,
/// \param[in] _opt Options for spawned model and joint.
public: physics::JointPtr SpawnJoint(const SpawnJointOptions &_opt)
{
- msgs::Factory msg;
- std::ostringstream modelStr;
- std::ostringstream modelName;
- modelName << "joint_model" << this->spawnCount++;
-
- modelStr
- << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << modelName.str() << "'>"
- << " <pose>" << _opt.modelPose << "</pose>";
+ msgs::Model msg;
+ std::string modelName = this->GetUniqueString("joint_model");
+ msg.set_name(modelName);
+ msgs::Set(msg.mutable_pose(), _opt.modelPose.Ign());
+
if (!_opt.worldParent)
{
- modelStr << " <link name='parent'>";
+ msg.add_link();
+ int linkCount = msg.link_size();
+ auto link = msg.mutable_link(linkCount-1);
+
+ link->set_name("parent");
if (!_opt.noLinkPose)
{
- modelStr << " <pose>" << _opt.parentLinkPose << "</pose>";
+ msgs::Set(link->mutable_pose(), _opt.parentLinkPose.Ign());
}
- modelStr << " </link>";
}
if (!_opt.worldChild)
{
- modelStr << " <link name='child'>";
+ msg.add_link();
+ int linkCount = msg.link_size();
+ auto link = msg.mutable_link(linkCount-1);
+
+ link->set_name("child");
if (!_opt.noLinkPose)
{
- modelStr << " <pose>" << _opt.childLinkPose << "</pose>";
+ msgs::Set(link->mutable_pose(), _opt.childLinkPose.Ign());
}
- modelStr << " </link>";
}
- modelStr
- << " <joint name='joint' type='" << _opt.type << "'>"
- << " <pose>" << _opt.jointPose << "</pose>";
+ msg.add_joint();
+ auto jointMsg = msg.mutable_joint(0);
+ jointMsg->set_name("joint");
+ jointMsg->set_type(msgs::ConvertJointType(_opt.type));
+ msgs::Set(jointMsg->mutable_pose(), _opt.jointPose.Ign());
if (_opt.worldParent)
- modelStr << " <parent>world</parent>";
+ {
+ jointMsg->set_parent("world");
+ }
else
- modelStr << " <parent>parent</parent>";
+ {
+ jointMsg->set_parent("parent");
+ }
if (_opt.worldChild)
- modelStr << " <child>world</child>";
+ {
+ jointMsg->set_child("world");
+ }
else
- modelStr << " <child>child</child>";
- modelStr
- << " <axis>"
- << " <xyz>" << _opt.axis << "</xyz>"
- << " <use_parent_model_frame>" << _opt.useParentModelFrame
- << " </use_parent_model_frame>"
- << " </axis>";
+ {
+ jointMsg->set_child("child");
+ }
+
+ {
+ auto axis = jointMsg->mutable_axis1();
+ msgs::Set(axis->mutable_xyz(), _opt.axis.Ign());
+ axis->set_use_parent_model_frame(_opt.useParentModelFrame);
+ }
// Hack: hardcode a second axis for universal joints
if (_opt.type == "universal")
{
- modelStr
- << " <axis2>"
- << " <xyz>" << math::Vector3(0, 1, 0) << "</xyz>"
- << " <use_parent_model_frame>" << _opt.useParentModelFrame
- << " </use_parent_model_frame>"
- << " </axis2>";
+ auto axis2 = jointMsg->mutable_axis2();
+ msgs::Set(axis2->mutable_xyz(),
+ ignition::math::Vector3d(0, 1, 0));
+ axis2->set_use_parent_model_frame(_opt.useParentModelFrame);
}
- modelStr
- << " </joint>"
- << "</model>";
-
- msg.set_sdf(modelStr.str());
- this->factoryPub->Publish(msg);
+ auto model = this->SpawnModel(msg);
physics::JointPtr joint;
- if (_opt.wait != common::Time::Zero)
- {
- common::Time wallStart = common::Time::GetWallTime();
- unsigned int waitCount = 0;
- while (_opt.wait > (common::Time::GetWallTime() - wallStart) &&
- !this->HasEntity(modelName.str()))
- {
- common::Time::MSleep(100);
- if (++waitCount % 10 == 0)
- {
- gzwarn << "Waiting " << waitCount / 10 << " seconds for "
- << _opt.type << " joint to spawn." << std::endl;
- }
- }
- if (this->HasEntity(modelName.str()) && waitCount >= 10)
- gzwarn << _opt.type << " joint has spawned." << std::endl;
+ if (model != NULL)
+ joint = model->GetJoint("joint");
- physics::WorldPtr world = physics::get_world("default");
- if (world != NULL)
- {
- physics::ModelPtr model = world->GetModel(modelName.str());
- if (model != NULL)
- joint = model->GetJoint("joint");
- }
- }
return joint;
}
@@ -239,8 +223,5 @@ class JointTest : public ServerFixture,
/// \brief Joint type for test.
protected: std::string jointType;
-
- /// \brief Count of spawned models, used to ensure unique model names.
- private: unsigned int spawnCount;
};
#endif
diff --git a/test/integration/joint_universal.cc b/test/integration/joint_universal.cc
index b2e27e0..83346da 100644
--- a/test/integration/joint_universal.cc
+++ b/test/integration/joint_universal.cc
@@ -17,8 +17,8 @@
#include <string>
#include "gazebo/physics/physics.hh"
-#include "test/ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
diff --git a/test/integration/laser.cc b/test/integration/laser.cc
index ca53bfb..2a8aeb1 100644
--- a/test/integration/laser.cc
+++ b/test/integration/laser.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
+#include <ignition/math/Rand.hh>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/sensors/sensors.hh"
#include "gazebo/common/common.hh"
#include "scans_cmp.h"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#define LASER_TOL 1e-5
#define DOUBLE_TOL 1e-6
@@ -74,16 +75,15 @@ void LaserTest::Stationary_EmptyWorld(const std::string &_physicsEngine)
EXPECT_EQ(640, laser->GetRayCount());
EXPECT_EQ(640, laser->GetRangeCount());
- EXPECT_NEAR(laser->GetAngleMin().Radian(), -2.27, DOUBLE_TOL);
- EXPECT_NEAR(laser->GetAngleMax().Radian(), 2.27, DOUBLE_TOL);
+ EXPECT_NEAR(laser->AngleMin().Radian(), -2.27, DOUBLE_TOL);
+ EXPECT_NEAR(laser->AngleMax().Radian(), 2.27, DOUBLE_TOL);
EXPECT_NEAR(laser->GetRangeMin(), 0, DOUBLE_TOL);
EXPECT_NEAR(laser->GetRangeMax(), 10, DOUBLE_TOL);
EXPECT_NEAR(laser->GetRangeResolution(), 0.01, DOUBLE_TOL);
-
for (int i = 0; i < laser->GetRangeCount(); ++i)
{
- EXPECT_EQ(10, laser->GetRange(i));
+ EXPECT_DOUBLE_EQ(GZ_DBL_INF, laser->GetRange(i));
}
// Spawn a box and test for proper laser scan
@@ -247,9 +247,7 @@ void LaserTest::LaserUnitBox(const std::string &_physicsEngine)
EXPECT_NEAR(raySensor->GetRange(mid), expectedRangeAtMidPoint, LASER_TOL);
EXPECT_NEAR(raySensor->GetRange(0), expectedRangeAtMidPoint, LASER_TOL);
- // WARNING: for readings of no return, gazebo returns max range rather
- // than +inf. issue #124
- EXPECT_NEAR(raySensor->GetRange(samples-1), maxRange, LASER_TOL);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(samples-1), GZ_DBL_INF);
// Move all boxes out of range
world->GetModel(box01)->SetWorldPose(
@@ -262,7 +260,7 @@ void LaserTest::LaserUnitBox(const std::string &_physicsEngine)
for (int i = 0; i < raySensor->GetRayCount(); ++i)
{
- EXPECT_NEAR(raySensor->GetRange(i), maxRange, LASER_TOL);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(i), GZ_DBL_INF);
}
}
@@ -347,11 +345,8 @@ void LaserTest::LaserVertical(const std::string &_physicsEngine)
angleStep += vAngleStep;
- // WARNING: for readings of no return, gazebo returns max range rather
- // than +inf. issue #124
- EXPECT_NEAR(raySensor->GetRange(i*samples), maxRange, LASER_TOL);
- EXPECT_NEAR(raySensor->GetRange(i*samples + samples-1),
- maxRange, LASER_TOL);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(i*samples), GZ_DBL_INF);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(i*samples + samples-1), GZ_DBL_INF);
}
// Move box out of range
@@ -364,8 +359,8 @@ void LaserTest::LaserVertical(const std::string &_physicsEngine)
{
for (int i = 0; i < raySensor->GetRayCount(); ++i)
{
- EXPECT_NEAR(raySensor->GetRange(j*raySensor->GetRayCount() + i),
- maxRange, LASER_TOL);
+ EXPECT_DOUBLE_EQ(raySensor->GetRange(j*raySensor->GetRayCount() + i),
+ GZ_DBL_INF);
}
}
}
@@ -586,20 +581,13 @@ void LaserTest::LaserUnitNoise(const std::string &_physicsEngine)
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
- // Expect at least one value to be non-max (empty world), and expect the
- // mean to be close to the max+noiseMean
- double total = 0.0;
bool foundNoise = false;
for (int i = 0; i < raySensor->GetRayCount(); ++i)
{
if (fabs(raySensor->GetRange(i) - maxRange) > LASER_TOL)
foundNoise = true;
- total += raySensor->GetRange(i);
}
EXPECT_TRUE(foundNoise);
- double mean = total / raySensor->GetRayCount();
- // The mean should be well within 3-sigma
- EXPECT_NEAR(mean, maxRange + noiseMean, 3*noiseStdDev);
}
TEST_P(LaserTest, LaserNoise)
@@ -613,7 +601,7 @@ int main(int argc, char **argv)
{
// Set a specific seed to avoid occasional test failures due to
// statistically unlikely, but possible results.
- math::Rand::SetSeed(42);
+ ignition::math::Rand::Seed(42);
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
diff --git a/test/integration/model.cc b/test/integration/model.cc
index b03e90c..b09912b 100644
--- a/test/integration/model.cc
+++ b/test/integration/model.cc
@@ -15,8 +15,9 @@
*
*/
#include <string.h>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
+using namespace gazebo;
class ModelTest : public ServerFixture
{
};
diff --git a/test/integration/model_database.cc b/test/integration/model_database.cc
index d3e119b..9ab944c 100644
--- a/test/integration/model_database.cc
+++ b/test/integration/model_database.cc
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2015 Open Source Robotics Foundation
+ * Copyright (C) 2013-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.
@@ -17,7 +17,7 @@
#include <unistd.h>
#include "gazebo/common/ModelDatabase.hh"
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
@@ -36,7 +36,6 @@ class ModelDatabaseTest : public ServerFixture
void OnModels(const std::map<std::string, std::string> & /*_models*/)
{
g_onModels++;
- g_Connection.reset();
}
void OnModels1(const std::map<std::string, std::string> & /*_models*/)
@@ -92,6 +91,10 @@ TEST_F(ModelDatabaseTest, GetModelsTwice)
EXPECT_EQ(g_onModels, 1);
EXPECT_EQ(g_onModels1, 1);
+
+ // Reset bool reference, so now only g_onModels1 should increment
+ g_Connection.reset();
+
common::ModelDatabase::Instance()->GetModels(boost::bind(&OnModels, _1));
while (g_onModels1 == 1)
@@ -129,6 +132,10 @@ TEST_F(ModelDatabaseTest, GetModelsThrice)
EXPECT_EQ(g_onModels1, 1);
EXPECT_EQ(g_onModels2, 1);
+ // Reset bool reference, so now only g_onModels1 and g_onModels2 should
+ // increment
+ g_Connection.reset();
+
common::ModelDatabase::Instance()->GetModels(
boost::bind(&OnModels, _1));
diff --git a/test/integration/multicamera_sensor.cc b/test/integration/multicamera_sensor.cc
index 6bb5616..aab872e 100644
--- a/test/integration/multicamera_sensor.cc
+++ b/test/integration/multicamera_sensor.cc
@@ -23,7 +23,7 @@
#include "gazebo/rendering/Conversions.hh"
#include "gazebo/sensors/MultiCameraSensor.hh"
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "scans_cmp.h"
using namespace gazebo;
@@ -381,27 +381,27 @@ TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest)
// model 1
// multicamera1 sensor has zero pose offset from the model
- EXPECT_EQ(model1->GetWorldPose(),
- multicamera1->GetPose() + model1->GetWorldPose());
- EXPECT_EQ(model1->GetWorldPose(),
- sensor1->GetPose() + model1->GetWorldPose());
+ EXPECT_EQ(model1->GetWorldPose().Ign(),
+ multicamera1->Pose() + model1->GetWorldPose().Ign());
+ EXPECT_EQ(model1->GetWorldPose().Ign(),
+ sensor1->Pose() + model1->GetWorldPose().Ign());
// multicamera1 sensor's camera 0 has a pose offset from the sensor
- EXPECT_NE(multicamera1->GetPose() + model1->GetWorldPose(),
- multicamera1->GetCamera(0)->GetWorldPose());
+ EXPECT_NE(multicamera1->Pose() + model1->GetWorldPose().Ign(),
+ multicamera1->GetCamera(0)->GetWorldPose().Ign());
// Get multicamera1's local pose. There is current no GetPose() in Camera,
// so grab it from it's ogre scene node
Ogre::SceneNode *cameraNode = multicamera1->GetCamera(0)->GetSceneNode();
- math::Pose cameraPose(
- rendering::Conversions::Convert(cameraNode->getPosition()),
- rendering::Conversions::Convert(cameraNode->getOrientation()));
+ ignition::math::Pose3d cameraPose(
+ rendering::Conversions::Convert(cameraNode->getPosition()).Ign(),
+ rendering::Conversions::Convert(cameraNode->getOrientation()).Ign());
// Wait for the AttachToVisual request msg to be processed so that the camera
// is attached to the parent visual.
int sleep = 0;
int maxSleep = 100;
- while (cameraPose == multicamera1->GetCamera(0)->GetWorldPose()
+ while (cameraPose == multicamera1->GetCamera(0)->GetWorldPose().Ign()
&& sleep < maxSleep)
{
common::Time::MSleep(100);
@@ -409,18 +409,21 @@ TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest)
}
// verify multicamera sensor's camera world pose
- EXPECT_EQ(cameraPose + multicamera1->GetPose() + model1->GetWorldPose(),
- multicamera1->GetCamera(0)->GetWorldPose());
- EXPECT_EQ(model1->GetWorldPose().rot * multicamera1->GetPose().rot
- * cameraPose.rot, multicamera1->GetCamera(0)->GetWorldRotation());
+ EXPECT_EQ(cameraPose + multicamera1->Pose() + model1->GetWorldPose().Ign(),
+ multicamera1->GetCamera(0)->GetWorldPose().Ign());
+ EXPECT_EQ(model1->GetWorldPose().rot.Ign() * multicamera1->Pose().Rot()
+ * cameraPose.Rot(),
+ multicamera1->GetCamera(0)->GetWorldRotation().Ign());
// multicamera1 sensor's camera 1 has zero pose offset from the sensor
- EXPECT_EQ(multicamera1->GetPose() + model1->GetWorldPose(),
- multicamera1->GetCamera(1)->GetWorldPose());
+ EXPECT_EQ(multicamera1->Pose() + model1->GetWorldPose().Ign(),
+ multicamera1->GetCamera(1)->GetWorldPose().Ign());
gzdbg << "model1 [" << model1->GetWorldPose() << "]\n"
- << "sensor1 [" << sensor1->GetPose() + model1->GetWorldPose() << "]\n"
- << "multicamera1 [" << multicamera1->GetPose() + model1->GetWorldPose()
+ << "sensor1 ["
+ << sensor1->Pose() + model1->GetWorldPose().Ign() << "]\n"
+ << "multicamera1 ["
+ << multicamera1->Pose() + model1->GetWorldPose().Ign()
<< "]\n"
<< "camera left WorldPose ["
<< multicamera1->GetCamera(0)->GetWorldPose() << "]\n"
@@ -429,41 +432,43 @@ TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest)
// model 2
// camera2 sensor has zero pose offset from the model
- EXPECT_EQ(model2->GetWorldPose(),
- camera2->GetPose() + model2->GetWorldPose());
- EXPECT_EQ(model2->GetWorldPose(),
- sensor2->GetPose() + model2->GetWorldPose());
+ EXPECT_EQ(model2->GetWorldPose().Ign(),
+ camera2->Pose() + model2->GetWorldPose().Ign());
+ EXPECT_EQ(model2->GetWorldPose().Ign(),
+ sensor2->Pose() + model2->GetWorldPose().Ign());
// camera2 sensor's camera has zero pose offset from the sensor
EXPECT_EQ(model2->GetWorldPose(), camera2->GetCamera()->GetWorldPose());
gzdbg << "model2 [" << model2->GetWorldPose() << "]\n"
- << "sensor2 [" << sensor2->GetPose() + model2->GetWorldPose() << "]\n"
- << "camera2 [" << camera2->GetPose() + model2->GetWorldPose() << "]\n"
+ << "sensor2 ["
+ << sensor2->Pose() + model2->GetWorldPose().Ign() << "]\n"
+ << "camera2 ["
+ << camera2->Pose() + model2->GetWorldPose().Ign() << "]\n"
<< "camera WorldPose [" << camera2->GetCamera()->GetWorldPose()
<< "]\n";
// model 3
// multicamera3 sensor has zero pose offset from the model
- EXPECT_EQ(model3->GetWorldPose(),
- multicamera3->GetPose() + model3->GetWorldPose());
- EXPECT_EQ(model3->GetWorldPose(),
- sensor3->GetPose() + model3->GetWorldPose());
+ EXPECT_EQ(model3->GetWorldPose().Ign(),
+ multicamera3->Pose() + model3->GetWorldPose().Ign());
+ EXPECT_EQ(model3->GetWorldPose().Ign(),
+ sensor3->Pose() + model3->GetWorldPose().Ign());
// multicamera3 sensor's camera 0 has a pose offset from the sensor
- EXPECT_NE(multicamera3->GetPose() + model3->GetWorldPose(),
- multicamera3->GetCamera(0)->GetWorldPose());
+ EXPECT_NE(multicamera3->Pose() + model3->GetWorldPose().Ign(),
+ multicamera3->GetCamera(0)->GetWorldPose().Ign());
// Get multicamera3 sensor's camera 0 local pose
cameraNode = multicamera3->GetCamera(0)->GetSceneNode();
- cameraPose = math::Pose(
- rendering::Conversions::Convert(cameraNode->getPosition()),
- rendering::Conversions::Convert(cameraNode->getOrientation()));
+ cameraPose = ignition::math::Pose3d(
+ rendering::Conversions::Convert(cameraNode->getPosition()).Ign(),
+ rendering::Conversions::Convert(cameraNode->getOrientation()).Ign());
// Wait for the AttachToVisual request msg to be processed so that the camera
// is attached to the parent visual.
sleep = 0;
maxSleep = 100;
- while (cameraPose == multicamera3->GetCamera(0)->GetWorldPose()
+ while (cameraPose == multicamera3->GetCamera(0)->GetWorldPose().Ign()
&& sleep < maxSleep)
{
common::Time::MSleep(100);
@@ -471,19 +476,23 @@ TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest)
}
// verify multicamera sensor's camera 0 world pose
- EXPECT_EQ(cameraPose + multicamera3->GetPose() + model3->GetWorldPose(),
- multicamera3->GetCamera(0)->GetWorldPose());
- EXPECT_EQ(model3->GetWorldPose().rot * multicamera3->GetPose().rot
- * cameraPose.rot, multicamera3->GetCamera(0)->GetWorldRotation());
+ EXPECT_EQ(cameraPose + multicamera3->Pose() +
+ model3->GetWorldPose().Ign(),
+ multicamera3->GetCamera(0)->GetWorldPose().Ign());
+ EXPECT_EQ(model3->GetWorldPose().rot.Ign() * multicamera3->Pose().Rot()
+ * cameraPose.Rot(),
+ multicamera3->GetCamera(0)->GetWorldRotation().Ign());
// multicamera3 sensor's camera 1 has zero pose offset from the sensor
- EXPECT_EQ(multicamera3->GetPose() + model3->GetWorldPose(),
- multicamera3->GetCamera(1)->GetWorldPose());
+ EXPECT_EQ(multicamera3->Pose() + model3->GetWorldPose().Ign(),
+ multicamera3->GetCamera(1)->GetWorldPose().Ign());
EXPECT_EQ(model3->GetWorldPose(), multicamera3->GetCamera(1)->GetWorldPose());
gzdbg << "model3 [" << model3->GetWorldPose() << "]\n"
- << "sensor3 [" << sensor3->GetPose() + model3->GetWorldPose() << "]\n"
- << "multicamera3 [" << multicamera3->GetPose() + model3->GetWorldPose()
+ << "sensor3 ["
+ << sensor3->Pose() + model3->GetWorldPose().Ign() << "]\n"
+ << "multicamera3 ["
+ << multicamera3->Pose() + model3->GetWorldPose().Ign()
<< "]\n"
<< "camera left WorldPose ["
<< multicamera3->GetCamera(0)->GetWorldPose() << "]\n"
@@ -492,24 +501,24 @@ TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest)
// model 4
// multicamera4 sensor has zero pose offset from the model
- EXPECT_EQ(model4->GetWorldPose(),
- multicamera4->GetPose() + model4->GetWorldPose());
- EXPECT_EQ(model4->GetWorldPose(),
- sensor4->GetPose() + model4->GetWorldPose());
+ EXPECT_EQ(model4->GetWorldPose().Ign(),
+ multicamera4->Pose() + model4->GetWorldPose().Ign());
+ EXPECT_EQ(model4->GetWorldPose().Ign(),
+ sensor4->Pose() + model4->GetWorldPose().Ign());
// multicamera4 sensor's camera 0 has a pose offset from the sensor
EXPECT_NE(model4->GetWorldPose(), multicamera4->GetCamera(0)->GetWorldPose());
// Get multicamera4's camera 0 local pose
cameraNode = multicamera4->GetCamera(0)->GetSceneNode();
- cameraPose = math::Pose(
- rendering::Conversions::Convert(cameraNode->getPosition()),
- rendering::Conversions::Convert(cameraNode->getOrientation()));
+ cameraPose = ignition::math::Pose3d(
+ rendering::Conversions::Convert(cameraNode->getPosition()).Ign(),
+ rendering::Conversions::Convert(cameraNode->getOrientation()).Ign());
// Wait for the AttachToVisual request msg to be processed so that the camera
// is attached to the parent visual.
sleep = 0;
maxSleep = 100;
- while (cameraPose == multicamera4->GetCamera(0)->GetWorldPose()
+ while (cameraPose == multicamera4->GetCamera(0)->GetWorldPose().Ign()
&& sleep < maxSleep)
{
common::Time::MSleep(100);
@@ -517,24 +526,25 @@ TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest)
}
// verify multicamera sensor's camera 0 world pose
- EXPECT_EQ(cameraPose + multicamera4->GetPose() + model4->GetWorldPose(),
- multicamera4->GetCamera(0)->GetWorldPose());
- EXPECT_EQ(model4->GetWorldPose().rot * multicamera4->GetPose().rot
- * cameraPose.rot, multicamera4->GetCamera(0)->GetWorldRotation());
+ EXPECT_EQ(cameraPose + multicamera4->Pose() + model4->GetWorldPose().Ign(),
+ multicamera4->GetCamera(0)->GetWorldPose().Ign());
+ EXPECT_EQ(model4->GetWorldPose().rot * multicamera4->Pose().Rot()
+ * cameraPose.Rot(),
+ multicamera4->GetCamera(0)->GetWorldRotation().Ign());
// multicamera4 sensor's camera 1 has a pose offset from the sensor
EXPECT_NE(model4->GetWorldPose(), multicamera4->GetCamera(1)->GetWorldPose());
// Get multicamera4 sensor's camera 1 local pose
cameraNode = multicamera4->GetCamera(1)->GetSceneNode();
- cameraPose = math::Pose(
- rendering::Conversions::Convert(cameraNode->getPosition()),
- rendering::Conversions::Convert(cameraNode->getOrientation()));
+ cameraPose = ignition::math::Pose3d(
+ rendering::Conversions::Convert(cameraNode->getPosition()).Ign(),
+ rendering::Conversions::Convert(cameraNode->getOrientation()).Ign());
// Wait for the AttachToVisual request msg to be processed so that the camera
// is attached to the parent visual.
sleep = 0;
maxSleep = 100;
- while (cameraPose == multicamera4->GetCamera(1)->GetWorldPose()
+ while (cameraPose == multicamera4->GetCamera(1)->GetWorldPose().Ign()
&& sleep < maxSleep)
{
common::Time::MSleep(100);
@@ -542,14 +552,16 @@ TEST_F(MultiCameraSensor, CameraRotationWorldPoseTest)
}
// verify multicamera4 sensor's camera 1 world pose
- EXPECT_EQ(cameraPose + multicamera4->GetPose() + model4->GetWorldPose(),
- multicamera4->GetCamera(1)->GetWorldPose());
- EXPECT_EQ(model4->GetWorldPose().rot * multicamera4->GetPose().rot
- * cameraPose.rot, multicamera4->GetCamera(1)->GetWorldRotation());
+ EXPECT_EQ(cameraPose + multicamera4->Pose() + model4->GetWorldPose().Ign(),
+ multicamera4->GetCamera(1)->GetWorldPose().Ign());
+ EXPECT_EQ(model4->GetWorldPose().rot.Ign() * multicamera4->Pose().Rot()
+ * cameraPose.Rot(), multicamera4->GetCamera(1)->GetWorldRotation().Ign());
gzdbg << "model4 [" << model4->GetWorldPose() << "]\n"
- << "sensor4 [" << sensor4->GetPose() + model4->GetWorldPose() << "]\n"
- << "multicamera4 [" << multicamera4->GetPose() + model4->GetWorldPose()
+ << "sensor4 ["
+ << sensor4->Pose() + model4->GetWorldPose().Ign() << "]\n"
+ << "multicamera4 ["
+ << multicamera4->Pose() + model4->GetWorldPose().Ign()
<< "]\n"
<< "camera1 WorldPose [" << multicamera4->GetCamera(0)->GetWorldPose()
<< "]\n"
diff --git a/test/integration/noise.cc b/test/integration/noise.cc
index 98b90c6..e676622 100644
--- a/test/integration/noise.cc
+++ b/test/integration/noise.cc
@@ -15,11 +15,11 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/sensors/sensors.hh"
#include "gazebo/common/common.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#define LASER_TOL 1e-5
#define DOUBLE_TOL 1e-6
@@ -75,7 +75,7 @@ void NoiseTest::NoisePlugin(const std::string &_physicsEngine)
<< " <type>custom</type>"
<< " </noise>"
<< " </ray>"
- << " <plugin name ='laser' filename='" << pluginFileName << "'>"
+ << " <plugin name ='laser' filename='" << pluginFileName << "'/>"
<< " </sensor>"
<< "</link>"
<< "</model>"
@@ -100,19 +100,19 @@ void NoiseTest::NoisePlugin(const std::string &_physicsEngine)
// Expect the range to be within (max-noise) < max < (max+noise), see
// custom noise model in RaySensorNoisePlugin.
// Noise rate value also taken directly from plugin.
- bool foundNoise = false;
double fixedNoiseRate = 0.005;
double noise = maxRange*fixedNoiseRate;
for (int i = 0; i < raySensor->GetRayCount(); ++i)
{
double range = raySensor->GetRange(i);
- if (fabs(range - maxRange) > LASER_TOL)
- foundNoise = true;
+ if (std::isinf(range))
+ {
+ continue;
+ }
EXPECT_TRUE(range >= maxRange - noise);
EXPECT_TRUE(range <= maxRange + noise);
}
- EXPECT_TRUE(foundNoise);
}
TEST_P(NoiseTest, NoisePlugin)
diff --git a/test/integration/nondefault_world.cc b/test/integration/nondefault_world.cc
index 8a5e303..73e83ab 100644
--- a/test/integration/nondefault_world.cc
+++ b/test/integration/nondefault_world.cc
@@ -15,8 +15,8 @@
*
*/
#include <string.h>
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
class NonDefaultWorld : public ServerFixture,
diff --git a/test/integration/ogre_log.cc b/test/integration/ogre_log.cc
index 38554a4..b0bd169 100644
--- a/test/integration/ogre_log.cc
+++ b/test/integration/ogre_log.cc
@@ -17,8 +17,9 @@
#include <boost/filesystem.hpp>
#include <fstream>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
+using namespace gazebo;
class OgreLog : public ServerFixture
{
};
diff --git a/test/integration/physics.cc b/test/integration/physics.cc
index d25c91b..84e1099 100644
--- a/test/integration/physics.cc
+++ b/test/integration/physics.cc
@@ -19,11 +19,13 @@
#include <string>
#include <vector>
-#include "ServerFixture.hh"
+#include <ignition/math/Rand.hh>
+
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "SimplePendulumIntegrator.hh"
#include "gazebo/msgs/msgs.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#define PHYSICS_TOL 1e-2
using namespace gazebo;
@@ -686,7 +688,7 @@ TEST_P(PhysicsTest, SpawnDropCoGOffset)
void PhysicsTest::JointDampingTest(const std::string &_physicsEngine)
{
// Random seed is set to prevent brittle failures (gazebo issue #479)
- math::Rand::SetSeed(18420503);
+ ignition::math::Rand::Seed(18420503);
Load("worlds/damp_test.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
diff --git a/test/integration/physics_basic_controller_response.cc b/test/integration/physics_basic_controller_response.cc
index b606efd..b994963 100644
--- a/test/integration/physics_basic_controller_response.cc
+++ b/test/integration/physics_basic_controller_response.cc
@@ -19,7 +19,9 @@
#include <string>
#include <vector>
-#include "ServerFixture.hh"
+#include <ignition/math/Rand.hh>
+
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/common/PID.hh"
#include "SimplePendulumIntegrator.hh"
@@ -55,7 +57,7 @@ void PhysicsTest::TrikeyWheelResponse(const std::string &_physicsEngine,
}
// Random seed is set to prevent brittle failures (gazebo issue #479)
- math::Rand::SetSeed(18420503);
+ ignition::math::Rand::Seed(18420503);
Load(_worldFileName, true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
diff --git a/test/integration/physics_collision.cc b/test/integration/physics_collision.cc
index 0b932c4..9bb81dd 100644
--- a/test/integration/physics_collision.cc
+++ b/test/integration/physics_collision.cc
@@ -14,16 +14,17 @@
* limitations under the License.
*
*/
-#include <string.h>
+#include <map>
+#include <string>
#include "gazebo/physics/physics.hh"
-#include "test/ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
-const double g_tolerance = 1e-4;
const double g_big = 1e29;
+const double g_physics_tol = 1e-2;
class PhysicsCollisionTest : public ServerFixture,
public testing::WithParamInterface<const char*>
@@ -64,6 +65,96 @@ void PhysicsCollisionTest::GetBoundingBox(const std::string &_physicsEngine)
}
/////////////////////////////////////////////////
+TEST_F(PhysicsCollisionTest, ModelSelfCollide)
+{
+ // self_collide is only implemented in ODE
+ Load("worlds/model_self_collide.world", true, "ode");
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // check the gravity vector
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+
+ math::Vector3 g = physics->GetGravity();
+ // Assume gravity vector points down z axis only.
+ EXPECT_EQ(g.x, 0);
+ EXPECT_EQ(g.y, 0);
+ EXPECT_LE(g.z, -9.8);
+
+ // get physics time step
+ double dt = physics->GetMaxStepSize();
+ EXPECT_GT(dt, 0);
+
+ // 4 models: all_collide, some_collide, no_collide, and explicit_no_collide
+ std::map<std::string, physics::ModelPtr> models;
+ models["all_collide"] = physics::ModelPtr();
+ models["some_collide"] = physics::ModelPtr();
+ models["no_collide"] = physics::ModelPtr();
+ models["explicit_no_collide"] = physics::ModelPtr();
+ for (auto &iter : models)
+ {
+ gzdbg << "Getting model " << iter.first << std::endl;
+ iter.second = world->GetModel(iter.first);
+ ASSERT_TRUE(iter.second != NULL);
+ }
+
+ // Step forward 0.2 s
+ double stepTime = 0.2;
+ unsigned int steps = floor(stepTime / dt);
+ world->Step(steps);
+
+ // Expect boxes to be falling
+ double fallVelocity = g.z * stepTime;
+ for (auto &iter : models)
+ {
+ auto links = iter.second->GetLinks();
+ for (auto &link : links)
+ {
+ ASSERT_TRUE(link != NULL);
+ gzdbg << "Check falling: " << link->GetScopedName() << std::endl;
+ EXPECT_LT(link->GetWorldLinearVel().z, fallVelocity*(1-g_physics_tol));
+ }
+ }
+
+ // Another 3000 steps should put the boxes at rest
+ world->Step(3000);
+
+ // Expect 3 boxes to be stationary
+ for (auto &iter : models)
+ {
+ auto links = iter.second->GetLinks();
+ for (auto &link : links)
+ {
+ ASSERT_TRUE(link != NULL);
+ gzdbg << "Check resting: " << link->GetScopedName() << std::endl;
+ EXPECT_NEAR(link->GetWorldLinearVel().z, 0, g_physics_tol);
+ }
+ }
+
+ gzdbg << "Check resting positions" << std::endl;
+
+ // link2 of all_collide should have the highest z-coordinate (around 3)
+ EXPECT_NEAR(models["all_collide"]->GetLink("link2")->GetWorldPose().pos.z,
+ 2.5, g_physics_tol);
+
+ // link2 of some_collide should have a middling z-coordinate (around 2)
+ EXPECT_NEAR(models["some_collide"]->GetLink("link2")->GetWorldPose().pos.z,
+ 1.5, g_physics_tol);
+
+ // link2 of no_collide should have a low z-coordinate (around 1)
+ EXPECT_NEAR(models["no_collide"]->GetLink("link2")->GetWorldPose().pos.z,
+ 0.5, g_physics_tol);
+
+ // link2 of explicit_no_collide should have the same z-coordinate as above
+ EXPECT_NEAR(models["no_collide"]->GetLink("link2")->GetWorldPose().pos.z,
+ models["explicit_no_collide"]->GetLink("link2")->GetWorldPose().pos.z,
+ g_physics_tol);
+
+ Unload();
+}
+
+/////////////////////////////////////////////////
TEST_P(PhysicsCollisionTest, GetBoundingBox)
{
GetBoundingBox(GetParam());
diff --git a/test/integration/physics_friction.cc b/test/integration/physics_friction.cc
index d2ac605..878fcc2 100644
--- a/test/integration/physics_friction.cc
+++ b/test/integration/physics_friction.cc
@@ -18,17 +18,10 @@
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
-#include "gazebo/physics/ode/ODESurfaceParams.hh"
-#include "gazebo/physics/ode/ODETypes.hh"
-
-#ifdef HAVE_BULLET
-#include "gazebo/physics/bullet/BulletSurfaceParams.hh"
-#include "gazebo/physics/bullet/BulletTypes.hh"
-#endif
-
#include "gazebo/transport/transport.hh"
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+#include "gazebo/gazebo_config.h"
using namespace gazebo;
@@ -57,29 +50,10 @@ class PhysicsFrictionTest : public ServerFixture,
physics::Collision_V::iterator iter = collisions.begin();
if (iter != collisions.end())
{
- physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
- if (physics->GetType() == "ode")
- {
- physics::ODESurfaceParamsPtr surface =
- boost::dynamic_pointer_cast<physics::ODESurfaceParams>(
- (*iter)->GetSurface());
- // Average the mu1 and mu2 values
- this->friction = (surface->frictionPyramid.GetMuPrimary()
- + surface->frictionPyramid.GetMuSecondary())
- / 2.0;
- }
-#ifdef HAVE_BULLET
- else if (physics->GetType() == "bullet")
- {
- physics::BulletSurfaceParamsPtr surface =
- boost::dynamic_pointer_cast<physics::BulletSurfaceParams>(
- (*iter)->GetSurface());
- // Average the mu1 and mu2 values
- this->friction = (surface->frictionPyramid.GetMuPrimary()
- + surface->frictionPyramid.GetMuSecondary())
- / 2.0;
- }
-#endif
+ physics::SurfaceParamsPtr surf = (*iter)->GetSurface();
+ // Use the Secondary friction value,
+ // since gravity has a non-zero component in the y direction
+ this->friction = surf->GetFrictionPyramid()->GetMuSecondary();
}
}
public: ~FrictionDemoBox() {}
@@ -187,7 +161,8 @@ class PhysicsFrictionTest : public ServerFixture,
<< " </geometry>"
<< " </visual>"
<< " </link>"
- << "</model>";
+ << "</model>"
+ << "</sdf>";
physics::WorldPtr world = physics::get_world("default");
world->InsertModelString(modelStr.str());
@@ -218,7 +193,15 @@ class PhysicsFrictionTest : public ServerFixture,
/// \brief Use the friction_demo world.
/// \param[in] _physicsEngine Physics engine to use.
- public: void FrictionDemo(const std::string &_physicsEngine);
+ public: void FrictionDemo(const std::string &_physicsEngine,
+ const std::string &_solverType="quick",
+ const std::string &_worldSolverType="ODE_DANTZIG");
+
+ /// \brief Friction test of maximum dissipation principle.
+ /// Basically test that friction force vector is aligned with
+ /// and opposes velocity vector.
+ /// \param[in] _physicsEngine Physics engine to use.
+ public: void MaximumDissipation(const std::string &_physicsEngine);
/// \brief Test friction directions for friction pyramid with boxes.
/// \param[in] _physicsEngine Physics engine to use.
@@ -229,16 +212,14 @@ class PhysicsFrictionTest : public ServerFixture,
/// \param[in] _physicsEngine Physics engine to use.
public: void DirectionNaN(const std::string &_physicsEngine);
- /// \brief Test Link::GetWorldInertia* functions.
- /// \TODO: move the SpawnBox function to ServerFixture,
- /// and then move this test to a different file.
- /// \param[in] _physicsEngine Physics engine to use.
- public: void LinkGetWorldInertia(const std::string &_physicsEngine);
-
/// \brief Count of spawned models, used to ensure unique model names.
private: unsigned int spawnCount;
};
+class WorldStepFrictionTest : public PhysicsFrictionTest
+{
+};
+
/////////////////////////////////////////////////
// FrictionDemo test:
// Uses the test_friction world, which has a bunch of boxes on the ground
@@ -246,7 +227,9 @@ class PhysicsFrictionTest : public ServerFixture,
// box has a different coefficient of friction. These friction coefficients
// are chosen to be close to the value that would prevent sliding according
// to the Coulomb model.
-void PhysicsFrictionTest::FrictionDemo(const std::string &_physicsEngine)
+void PhysicsFrictionTest::FrictionDemo(const std::string &_physicsEngine,
+ const std::string &_solverType,
+ const std::string &_worldSolverType)
{
if (_physicsEngine == "simbody")
{
@@ -255,13 +238,6 @@ void PhysicsFrictionTest::FrictionDemo(const std::string &_physicsEngine)
<< std::endl;
return;
}
- if (_physicsEngine == "dart")
- {
- gzerr << "Aborting test since there's an issue with dart's friction"
- << " parameters (#1000)"
- << std::endl;
- return;
- }
Load("worlds/friction_demo.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
@@ -278,6 +254,15 @@ void PhysicsFrictionTest::FrictionDemo(const std::string &_physicsEngine)
EXPECT_DOUBLE_EQ(g.y, -1.0);
EXPECT_DOUBLE_EQ(g.z, -1.0);
+ if (_physicsEngine == "ode")
+ {
+ // Set solver type
+ physics->SetParam("solver_type", _solverType);
+
+ // Set world step solver type
+ physics->SetParam("world_step_solver", _worldSolverType);
+ }
+
std::vector<PhysicsFrictionTest::FrictionDemoBox> boxes;
std::vector<PhysicsFrictionTest::FrictionDemoBox>::iterator box;
boxes.push_back(PhysicsFrictionTest::FrictionDemoBox(world, "box_01_model"));
@@ -300,24 +285,33 @@ void PhysicsFrictionTest::FrictionDemo(const std::string &_physicsEngine)
world->Step(500);
t = world->GetSimTime();
+ double yTolerance = g_friction_tolerance;
+ if (_solverType == "world")
+ {
+ if (_worldSolverType == "DART_PGS")
+ yTolerance *= 2;
+ else if (_worldSolverType == "ODE_DANTZIG")
+ yTolerance = 0.84;
+ }
+
for (box = boxes.begin(); box != boxes.end(); ++box)
{
math::Vector3 vel = box->model->GetWorldLinearVel();
EXPECT_NEAR(vel.x, 0, g_friction_tolerance);
- EXPECT_NEAR(vel.z, 0, g_friction_tolerance);
+ EXPECT_NEAR(vel.z, 0, yTolerance);
// Coulomb friction model
if (box->friction >= 1.0)
{
// Friction is large enough to prevent motion
- EXPECT_NEAR(vel.y, 0, g_friction_tolerance);
+ EXPECT_NEAR(vel.y, 0, yTolerance);
}
else
{
// Friction is small enough to allow motion
// Expect velocity = acceleration * time
EXPECT_NEAR(vel.y, (g.y + box->friction) * t.Double(),
- g_friction_tolerance);
+ yTolerance);
}
}
}
@@ -328,6 +322,95 @@ void PhysicsFrictionTest::FrictionDemo(const std::string &_physicsEngine)
}
/////////////////////////////////////////////////
+// MaximumDissipation test:
+// Start with empty world,
+// spawn a bunch of boxes,
+// sets box velocities to different angles,
+// expect velocity unit vectors to stay constant while in motion.
+void PhysicsFrictionTest::MaximumDissipation(const std::string &_physicsEngine)
+{
+ // Load an empty world
+ Load("worlds/empty.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);
+
+ // get the gravity vector
+ // small positive y component
+ math::Vector3 g = physics->GetGravity();
+
+ // Set friction model
+ // "cone_model", "pyramid_model", "box_model"
+ const std::string frictionModel = "cone_model";
+ physics->SetParam("friction_model", frictionModel);
+
+ // Spawn concentric semi-circles of boxes
+ int boxes = 32;
+ double dx = 0.5;
+ double dy = 0.5;
+ double dz = 0.2;
+ std::map<physics::ModelPtr, double> modelAngles;
+
+ for (int ring = 0; ring < 5; ++ring)
+ {
+ gzdbg << "Spawn ring " << ring+1 << " of boxes" << std::endl;
+ for (int i = 0; i < boxes; ++i)
+ {
+ // Set box size and anisotropic friction
+ SpawnFrictionBoxOptions opt;
+ opt.size.Set(dx, dy, dz);
+ opt.friction1 = 0.3;
+ opt.friction2 = opt.friction1;
+
+ // Compute angle for each box
+ double radius = 9.0 + ring;
+ double angle = 2*M_PI*static_cast<double>(i) / static_cast<double>(boxes);
+ opt.modelPose.pos.Set(radius*cos(angle), radius*sin(angle), dz/2);
+
+ if (ring == 0)
+ opt.direction1 = math::Vector3(-sin(angle), cos(angle), 0);
+ else if (ring < 4)
+ opt.direction1 = math::Vector3(0.0, 1.0, 0.0);
+
+ if (ring == 1)
+ opt.collisionPose.rot.SetFromEuler(0.0, 0.0, angle);
+
+ if (ring == 2)
+ opt.linkPose.rot.SetFromEuler(0.0, 0.0, angle);
+
+ if (ring == 3)
+ opt.modelPose.rot.SetFromEuler(0.0, 0.0, angle);
+
+ physics::ModelPtr model = SpawnBox(opt);
+ ASSERT_TRUE(model != NULL);
+ modelAngles[model] = angle;
+
+ // Set velocity, larger for outer rings.
+ model->SetLinearVel(radius * math::Vector3(cos(angle), sin(angle), 0));
+ }
+ }
+
+ world->Step(1500);
+
+ gzdbg << "Checking position of boxes" << std::endl;
+ std::map<physics::ModelPtr, double>::iterator iter;
+ for (iter = modelAngles.begin(); iter != modelAngles.end(); ++iter)
+ {
+ double cosAngle = cos(iter->second);
+ double sinAngle = sin(iter->second);
+ math::Vector3 pos = iter->first->GetWorldPose().pos;
+ double cosPosAngle = pos.x / pos.GetLength();
+ double sinPosAngle = pos.y / pos.GetLength();
+ EXPECT_NEAR(cosAngle, cosPosAngle, 1e-2);
+ EXPECT_NEAR(sinAngle, sinPosAngle, 1e-2);
+ }
+}
+
+/////////////////////////////////////////////////
// BoxDirectionRing:
// Spawn several boxes with different friction direction parameters.
void PhysicsFrictionTest::BoxDirectionRing(const std::string &_physicsEngine)
@@ -365,7 +448,6 @@ void PhysicsFrictionTest::BoxDirectionRing(const std::string &_physicsEngine)
EXPECT_EQ(physics->GetType(), _physicsEngine);
// set the gravity vector
- // small positive y component
math::Vector3 g(0.0, 1.0, -9.81);
physics->SetGravity(g);
@@ -497,157 +579,30 @@ void PhysicsFrictionTest::DirectionNaN(const std::string &_physicsEngine)
}
/////////////////////////////////////////////////
-// LinkGetWorldInertia:
-// Spawn boxes and verify Link::GetWorldInertia* functions
-void PhysicsFrictionTest::LinkGetWorldInertia(const std::string &_physicsEngine)
+TEST_P(PhysicsFrictionTest, FrictionDemo)
{
- // Load a blank world (no ground plane)
- Load("worlds/blank.world", true, _physicsEngine);
- physics::WorldPtr world = physics::get_world("default");
- ASSERT_TRUE(world != NULL);
-
- // Verify physics engine type
- physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
- ASSERT_TRUE(physics != NULL);
- EXPECT_EQ(physics->GetType(), _physicsEngine);
-
- // disable gravity
- physics->SetGravity(math::Vector3::Zero);
-
- // Box size
- double dx = 1.0;
- double dy = 4.0;
- double dz = 9.0;
- double mass = 10.0;
- double angle = M_PI / 3.0;
-
- const unsigned int testCases = 4;
- for (unsigned int i = 0; i < testCases; ++i)
- {
- // Set box size and position
- SpawnFrictionBoxOptions opt;
- opt.size.Set(dx, dy, dz);
- opt.mass = mass;
- opt.modelPose.pos.x = i * dz;
- opt.modelPose.pos.z = dz;
-
- // i=0: rotated model pose
- // expect inertial pose to match model pose
- if (i == 0)
- {
- opt.modelPose.rot.SetFromEuler(0.0, 0.0, angle);
- }
- // i=1: rotated link pose
- // expect inertial pose to match link pose
- else if (i == 1)
- {
- opt.linkPose.rot.SetFromEuler(0.0, 0.0, angle);
- }
- // i=2: rotated inertial pose
- // expect inertial pose to differ from link pose
- else if (i == 2)
- {
- opt.inertialPose.rot.SetFromEuler(0.0, 0.0, angle);
- }
- // i=3: offset inertial pose
- // expect inertial pose to differ from link pose
- else if (i == 3)
- {
- opt.inertialPose.pos.Set(1, 1, 1);
- }
-
- physics::ModelPtr model = SpawnBox(opt);
- ASSERT_TRUE(model != NULL);
-
- physics::LinkPtr link = model->GetLink();
- ASSERT_TRUE(link != NULL);
-
- EXPECT_EQ(model->GetWorldPose(), opt.modelPose);
- EXPECT_EQ(link->GetWorldPose(), opt.linkPose + opt.modelPose);
- EXPECT_EQ(link->GetWorldInertialPose(),
- opt.inertialPose + opt.linkPose + opt.modelPose);
-
- // i=0: rotated model pose
- // expect inertial pose to match model pose
- if (i == 0)
- {
- EXPECT_EQ(model->GetWorldPose(),
- link->GetWorldInertialPose());
- }
- // i=1: rotated link pose
- // expect inertial pose to match link pose
- else if (i == 1)
- {
- EXPECT_EQ(link->GetWorldPose(),
- link->GetWorldInertialPose());
- }
- // i=2: offset and rotated inertial pose
- // expect inertial pose to differ from link pose
- else if (i == 2)
- {
- EXPECT_EQ(link->GetWorldPose().pos,
- link->GetWorldInertialPose().pos);
- }
- // i=3: offset inertial pose
- // expect inertial pose to differ from link pose
- else if (i == 3)
- {
- EXPECT_EQ(link->GetWorldPose().pos + opt.inertialPose.pos,
- link->GetWorldInertialPose().pos);
- }
-
- // Expect rotated inertia matrix
- math::Matrix3 inertia = link->GetWorldInertiaMatrix();
- if (i == 3)
- {
- EXPECT_NEAR(inertia[0][0], 80.8333, 1e-4);
- EXPECT_NEAR(inertia[1][1], 68.3333, 1e-4);
- EXPECT_NEAR(inertia[2][2], 14.1667, 1e-4);
- for (int row = 0; row < 3; ++row)
- for (int col = 0; col < 3; ++col)
- if (row != col)
- EXPECT_NEAR(inertia[row][col], 0.0, g_friction_tolerance);
- }
- else
- {
- EXPECT_NEAR(inertia[0][0], 71.4583, 1e-4);
- EXPECT_NEAR(inertia[1][1], 77.7083, 1e-4);
- EXPECT_NEAR(inertia[2][2], 14.1667, 1e-4);
- EXPECT_NEAR(inertia[0][1], 5.4126, 1e-4);
- EXPECT_NEAR(inertia[1][0], 5.4126, 1e-4);
- EXPECT_NEAR(inertia[0][2], 0, g_friction_tolerance);
- EXPECT_NEAR(inertia[2][0], 0, g_friction_tolerance);
- EXPECT_NEAR(inertia[1][2], 0, g_friction_tolerance);
- EXPECT_NEAR(inertia[2][1], 0, g_friction_tolerance);
- }
+ FrictionDemo(GetParam());
+}
- // For 0-2, apply torque and expect equivalent response
- if (i <= 2)
- {
- for (int step = 0; step < 50; ++step)
- {
- link->SetTorque(math::Vector3(100, 0, 0));
- world->Step(1);
- }
- if (_physicsEngine.compare("dart") == 0)
- {
- gzerr << "Dart fails this portion of the test (#1090)" << std::endl;
- }
- else
- {
- math::Vector3 vel = link->GetWorldAngularVel();
- EXPECT_NEAR(vel.x, 0.0703, g_friction_tolerance);
- EXPECT_NEAR(vel.y, -0.0049, g_friction_tolerance);
- EXPECT_NEAR(vel.z, 0.0000, g_friction_tolerance);
- }
- }
- }
+/////////////////////////////////////////////////
+TEST_P(WorldStepFrictionTest, FrictionDemoWorldStep)
+{
+ FrictionDemo("ode", "world", GetParam());
}
/////////////////////////////////////////////////
-TEST_P(PhysicsFrictionTest, FrictionDemo)
+TEST_P(PhysicsFrictionTest, MaximumDissipation)
{
- FrictionDemo(GetParam());
+ if (std::string("ode").compare(GetParam()) == 0)
+ {
+ MaximumDissipation(GetParam());
+ }
+ else
+ {
+ gzerr << "Skipping test for physics engine "
+ << GetParam()
+ << std::endl;
+ }
}
/////////////////////////////////////////////////
@@ -662,15 +617,12 @@ TEST_P(PhysicsFrictionTest, DirectionNaN)
DirectionNaN(GetParam());
}
-/////////////////////////////////////////////////
-TEST_P(PhysicsFrictionTest, LinkGetWorldInertia)
-{
- LinkGetWorldInertia(GetParam());
-}
-
INSTANTIATE_TEST_CASE_P(PhysicsEngines, PhysicsFrictionTest,
PHYSICS_ENGINE_VALUES);
+INSTANTIATE_TEST_CASE_P(WorldStepSolvers, WorldStepFrictionTest,
+ WORLD_STEP_SOLVERS);
+
/////////////////////////////////////////////////
int main(int argc, char **argv)
{
diff --git a/test/integration/physics_inertia_ratio.cc b/test/integration/physics_inertia_ratio.cc
index 506ea6b..f3f09d2 100644
--- a/test/integration/physics_inertia_ratio.cc
+++ b/test/integration/physics_inertia_ratio.cc
@@ -23,13 +23,13 @@
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Vector3Stats.hh"
#include "gazebo/physics/physics.hh"
-#include "test/integration/helper_physics_generator.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
const double g_angle_y_tol = 0.21;
-const double g_angle_z_tol = 0.21;
+const double g_angle_z_tol = 0.23;
class PhysicsTest : public ServerFixture,
public testing::WithParamInterface<const char*>
@@ -81,6 +81,10 @@ void PhysicsTest::InertiaRatioPendulum(const std::string &_physicsEngine)
EXPECT_NEAR((upperAngles.Z().Map())["maxAbs"], 0.0, g_angle_z_tol);
EXPECT_NEAR((lowerAngles.Y().Map())["maxAbs"], 0.0, g_angle_y_tol);
EXPECT_NEAR((lowerAngles.Z().Map())["maxAbs"], 0.0, g_angle_z_tol);
+
+ RecordProperty("engine", _physicsEngine);
+ this->Record("lowerAngles", lowerAngles);
+ this->Record("upperAngles", upperAngles);
}
TEST_P(PhysicsTest, InertiaRatioPendulum)
diff --git a/test/integration/physics_link.cc b/test/integration/physics_link.cc
index 1b7e972..f9ee5cb 100644
--- a/test/integration/physics_link.cc
+++ b/test/integration/physics_link.cc
@@ -16,9 +16,11 @@
*/
#include <string.h>
+#include "gazebo/math/Vector3Stats.hh"
+#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
-#include "test/ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
@@ -27,16 +29,301 @@ const double g_tolerance = 1e-4;
class PhysicsLinkTest : public ServerFixture,
public testing::WithParamInterface<const char*>
{
+ /// \brief Test force adding functions.
+ /// \param[in] _physicsEngine Type of physics engine to use.
+ public: void AddForce(const std::string &_physicsEngine);
+
+ /// \brief Use AddLinkForce on the given direction and then the opposite
+ /// direction so they cancel out.
+ /// \param[in] _world World pointer.
+ /// \param[in] _link Link pointer.
+ /// \param[in] _force Force expressed in link frame.
+ /// \param[in] _offset Offset expressed in link frame, defaults to link
+ /// origin.
+ public: void AddLinkForceTwoWays(physics::WorldPtr _world,
+ physics::LinkPtr _link, math::Vector3 _force,
+ math::Vector3 _offset = math::Vector3::Zero);
+
+ /// \brief Test GetWorldAngularMomentum.
+ /// \param[in] _physicsEngine Type of physics engine to use.
+ public: void GetWorldAngularMomentum(const std::string &_physicsEngine);
+
/// \brief Test GetWorldEnergy* functions.
/// \param[in] _physicsEngine Type of physics engine to use.
public: void GetWorldEnergy(const std::string &_physicsEngine);
+ /// \brief Test Link::GetWorldInertia* functions.
+ /// \param[in] _physicsEngine Physics engine to use.
+ public: void GetWorldInertia(const std::string &_physicsEngine);
+
+ /// \brief Test wrench subscriber.
+ /// \param[in] _physicsEngine Type of physics engine to use.
+ public: void OnWrenchMsg(const std::string &_physicsEngine);
+
/// \brief Test velocity setting functions.
/// \param[in] _physicsEngine Type of physics engine to use.
public: void SetVelocity(const std::string &_physicsEngine);
};
/////////////////////////////////////////////////
+void PhysicsLinkTest::AddLinkForceTwoWays(physics::WorldPtr _world,
+ physics::LinkPtr _link, math::Vector3 _force,
+ math::Vector3 _offset)
+{
+ // Get state before adding force
+ math::Vector3 linearVelWorld0 = _link->GetWorldCoGLinearVel();
+ math::Vector3 angularVelWorld0 = _link->GetWorldAngularVel();
+ math::Pose poseWorld0 = _link->GetWorldPose();
+
+ // Add Link Force
+ if (_offset == math::Vector3::Zero)
+ _link->AddLinkForce(_force);
+ else
+ _link->AddLinkForce(_force, _offset);
+
+ double dt = _world->GetPhysicsEngine()->GetMaxStepSize();
+ _world->Step(1);
+
+ int moreThanOneStep = 2;
+
+ // Check force and torque (at CoG?) in world frame
+ math::Vector3 forceWorld = poseWorld0.rot.RotateVector(_force);
+ EXPECT_EQ(forceWorld, _link->GetWorldForce());
+
+ math::Vector3 worldOffset = poseWorld0.rot.RotateVector(
+ _offset - _link->GetInertial()->GetCoG());
+ math::Vector3 torqueWorld = worldOffset.Cross(forceWorld);
+ EXPECT_EQ(torqueWorld, _link->GetWorldTorque());
+
+ // Check acceleration in world frame
+ math::Vector3 oneStepLinearAccel =
+ forceWorld/_link->GetInertial()->GetMass();
+ EXPECT_EQ(oneStepLinearAccel, _link->GetWorldLinearAccel());
+
+ // Compute angular accel by multiplying world torque
+ // by inverse of world inertia matrix.
+ // In this case, the gyroscopic coupling terms are zero
+ // since the model is a unit box.
+ math::Vector3 oneStepAngularAccel =
+ _link->GetWorldInertiaMatrix().Inverse() * torqueWorld;
+ EXPECT_EQ(oneStepAngularAccel, _link->GetWorldAngularAccel());
+
+ // Check velocity in world frame
+ math::Vector3 oneStepLinearVel = linearVelWorld0 + dt*oneStepLinearAccel;
+ EXPECT_EQ(oneStepLinearVel, _link->GetWorldCoGLinearVel());
+
+ math::Vector3 oneStepAngularVel = angularVelWorld0 + dt*oneStepAngularAccel;
+ EXPECT_EQ(oneStepAngularVel, _link->GetWorldAngularVel());
+
+ // Step forward and check again
+ _world->Step(moreThanOneStep);
+
+ // Check that force and torque are zero
+ EXPECT_EQ(math::Vector3::Zero, _link->GetWorldForce());
+ EXPECT_EQ(math::Vector3::Zero, _link->GetWorldTorque());
+
+ // Check that acceleration is zero
+ EXPECT_EQ(math::Vector3::Zero, _link->GetWorldLinearAccel());
+ EXPECT_EQ(math::Vector3::Zero, _link->GetWorldAngularAccel());
+
+ // Check that velocity hasn't changed
+ EXPECT_EQ(oneStepLinearVel, _link->GetWorldCoGLinearVel());
+ EXPECT_EQ(oneStepAngularVel, _link->GetWorldAngularVel());
+
+ // Add opposing force in link frame and check that link is back to initial
+ // velocity
+ if (_offset == math::Vector3::Zero)
+ _link->AddLinkForce(-_force);
+ else
+ _link->AddLinkForce(-_force, _offset);
+
+ _world->Step(moreThanOneStep);
+ EXPECT_EQ(math::Vector3::Zero, _link->GetWorldForce());
+ EXPECT_EQ(math::Vector3::Zero, _link->GetWorldTorque());
+ EXPECT_EQ(linearVelWorld0, _link->GetWorldCoGLinearVel());
+ EXPECT_EQ(angularVelWorld0, _link->GetWorldAngularVel());
+ EXPECT_EQ(math::Vector3::Zero, _link->GetWorldLinearAccel());
+ EXPECT_EQ(math::Vector3::Zero, _link->GetWorldAngularAccel());
+}
+
+/////////////////////////////////////////////////
+void PhysicsLinkTest::AddForce(const std::string &_physicsEngine)
+{
+ // TODO bullet, dart and simbody currently fail this test
+ if (_physicsEngine != "ode")
+ {
+ gzerr << "Aborting AddForce test for Bullet, DART and Simbody. "
+ << "See issues #1476, #1477, and #1478."
+ << std::endl;
+ return;
+ }
+
+ Load("worlds/blank.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // check the physics engine
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ EXPECT_EQ(physics->GetType(), _physicsEngine);
+ double dt = physics->GetMaxStepSize();
+ EXPECT_GT(dt, 0);
+
+ // disable gravity
+ physics->SetGravity(math::Vector3::Zero);
+
+ // Spawn a box
+ math::Vector3 size(1, 1, 1);
+ SpawnBox("box", size, math::Vector3::Zero, math::Vector3::Zero, false);
+ physics::ModelPtr model = world->GetModel("box");
+ ASSERT_TRUE(model != NULL);
+ physics::LinkPtr link = model->GetLink();
+ ASSERT_TRUE(link != NULL);
+
+ // Check that link is at rest
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearVel());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularVel());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearAccel());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularAccel());
+
+ // Add force at link frame
+ gzdbg << "World == link == inertial frames, no offset" << std::endl;
+ EXPECT_EQ(math::Pose(), link->GetWorldPose());
+ EXPECT_EQ(math::Pose(), link->GetWorldInertialPose());
+ this->AddLinkForceTwoWays(world, link, math::Vector3(1, 20, 31));
+
+ gzdbg << "World != link == inertial frames, no offset" << std::endl;
+ model->SetLinkWorldPose(math::Pose(math::Vector3(2, 3, 4),
+ math::Vector3(0, M_PI/2.0, 1)), link);
+ EXPECT_NE(math::Pose(), link->GetWorldPose());
+ EXPECT_EQ(link->GetWorldPose(), link->GetWorldInertialPose());
+ this->AddLinkForceTwoWays(world, link, math::Vector3(-1, 10, 5));
+
+ gzdbg << "World == link == inertial frames, with offset" << std::endl;
+ model->SetLinkWorldPose(math::Pose(), link);
+ EXPECT_EQ(math::Pose(), link->GetWorldPose());
+ EXPECT_EQ(math::Pose(), link->GetWorldInertialPose());
+ this->AddLinkForceTwoWays(world, link, math::Vector3(5, 4, 3),
+ math::Vector3(-2, 1, 0));
+
+ gzdbg << "World == link != inertial frames, no offset" << std::endl;
+ model->SetLinkWorldPose(math::Pose(), link);
+ math::Pose inertialPose = math::Pose(math::Vector3(1, 5, 8),
+ math::Vector3(M_PI/3.0, M_PI*1.5, M_PI/4));
+ link->GetInertial()->SetCoG(inertialPose);
+ EXPECT_EQ(math::Pose(), link->GetWorldPose());
+ EXPECT_EQ(inertialPose, link->GetWorldInertialPose());
+ this->AddLinkForceTwoWays(world, link, math::Vector3(1, 2, 1));
+
+ gzdbg << "World != link != inertial frames, with offset" << std::endl;
+ model->SetLinkWorldPose(math::Pose(math::Vector3(5, 10, -4),
+ math::Vector3(0, M_PI/2.0, M_PI/6)), link);
+ inertialPose = math::Pose(math::Vector3(0, -5, 10),
+ math::Vector3(0, 2.0*M_PI, M_PI/3));
+ link->GetInertial()->SetCoG(inertialPose);
+ this->AddLinkForceTwoWays(world, link, math::Vector3(1, 2, 1),
+ math::Vector3(-2, 0.5, 1));
+
+ gzdbg << "World != link != inertial frames, with offset and initial vel"
+ << std::endl;
+ model->SetLinkWorldPose(math::Pose(math::Vector3(-1.5, 0.8, 3),
+ math::Vector3(-M_PI/4.5, M_PI/3.0, M_PI*1.2)), link);
+ inertialPose = math::Pose(math::Vector3(1, 0, -5.6),
+ math::Vector3(M_PI/9, 0, M_PI*3));
+ link->GetInertial()->SetCoG(inertialPose);
+ link->SetLinearVel(math::Vector3(2, -0.1, 5));
+ link->SetAngularVel(math::Vector3(-M_PI/10, 0, 0.0001));
+ this->AddLinkForceTwoWays(world, link, math::Vector3(-3, 2.5, -15),
+ math::Vector3(-6, -1, -0.2));
+}
+
+/////////////////////////////////////////////////
+// GetWorldAngularMomentum:
+// Spawn box and verify Link::GetWorldAngularMomentum functions
+// Make dimensions unequal and give angular velocity that causes
+// gyroscopic tumbling.
+void PhysicsLinkTest::GetWorldAngularMomentum(const std::string &_physicsEngine)
+{
+ // Load a blank world (no ground plane)
+ Load("worlds/blank.world", true, _physicsEngine);
+ auto world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // Verify physics engine type
+ auto physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ EXPECT_EQ(physics->GetType(), _physicsEngine);
+
+ // disable gravity
+ physics->SetGravity(math::Vector3::Zero);
+
+ physics::ModelPtr model;
+ {
+ // Box size
+ const double dx = 0.1;
+ const double dy = 0.4;
+ const double dz = 0.9;
+ const double mass = 10.0;
+
+ msgs::Model msgModel;
+ msgModel.set_name(this->GetUniqueString("model"));
+ msgs::AddBoxLink(msgModel, mass, ignition::math::Vector3d(dx, dy, dz));
+ model = this->SpawnModel(msgModel);
+ }
+ ASSERT_TRUE(model != NULL);
+
+ // inertia matrix, recompute if dimensions change
+ const double Ixx = 0.80833333;
+ const double Iyy = 0.68333333;
+ const double Izz = 0.14166667;
+ const math::Matrix3 I0(Ixx, 0.0, 0.0
+ , 0.0, Iyy, 0.0
+ , 0.0, 0.0, Izz);
+
+ // Since Ixx > Iyy > Izz,
+ // angular velocity with large y component
+ // will cause gyroscopic tumbling
+ const math::Vector3 w0(1e-3, 1.5e0, 1.5e-2);
+ model->SetAngularVel(w0);
+
+ // Get link and verify inertia and initial velocity
+ auto link = model->GetLink();
+ ASSERT_TRUE(link != NULL);
+ ASSERT_EQ(w0, link->GetWorldAngularVel());
+ ASSERT_EQ(I0, link->GetWorldInertiaMatrix());
+
+ // Compute initial angular momentum
+ const math::Vector3 H0(I0 * w0);
+ ASSERT_EQ(H0, link->GetWorldAngularMomentum());
+ const double H0mag = H0.GetLength();
+
+ math::Vector3Stats angularMomentumError;
+ const std::string stat("maxAbs");
+ EXPECT_TRUE(angularMomentumError.InsertStatistic(stat));
+ const int steps = 5000;
+ for (int i = 0; i < steps; ++i)
+ {
+ world->Step(1);
+ math::Vector3 H = link->GetWorldAngularMomentum();
+ angularMomentumError.InsertData((H - H0) / H0mag);
+ }
+ if (_physicsEngine == "dart")
+ {
+ gzdbg << "dart has higher error for this test (see #1487), "
+ << "so a larger tolerance is used."
+ << std::endl;
+ EXPECT_LT(angularMomentumError.Mag().Map()[stat], g_tolerance * 1e3);
+ }
+ else
+ {
+ EXPECT_LT(angularMomentumError.Mag().Map()[stat], g_tolerance * 10);
+ }
+
+ RecordProperty("engine", _physicsEngine);
+ this->Record("angularMomentumError", angularMomentumError);
+}
+
+/////////////////////////////////////////////////
void PhysicsLinkTest::GetWorldEnergy(const std::string &_physicsEngine)
{
Load("worlds/empty.world", true, _physicsEngine);
@@ -79,9 +366,294 @@ void PhysicsLinkTest::GetWorldEnergy(const std::string &_physicsEngine)
}
/////////////////////////////////////////////////
+// GetWorldInertia:
+// Spawn boxes and verify Link::GetWorldInertia* functions
+void PhysicsLinkTest::GetWorldInertia(const std::string &_physicsEngine)
+{
+ // Load a blank world (no ground plane)
+ Load("worlds/blank.world", true, _physicsEngine);
+ auto world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // Verify physics engine type
+ auto physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ EXPECT_EQ(physics->GetType(), _physicsEngine);
+
+ // disable gravity
+ physics->SetGravity(math::Vector3::Zero);
+
+ // Box size
+ const double dx = 1.0;
+ const double dy = 4.0;
+ const double dz = 9.0;
+ const double mass = 10.0;
+ const double angle = M_PI / 3.0;
+
+ const unsigned int testCases = 4;
+ for (unsigned int i = 0; i < testCases; ++i)
+ {
+ // Use msgs::AddBoxLink
+ msgs::Model msgModel;
+ math::Pose modelPose, linkPose, inertialPose;
+
+ msgModel.set_name(this->GetUniqueString("model"));
+ msgs::AddBoxLink(msgModel, mass, ignition::math::Vector3d(dx, dy, dz));
+ modelPose.pos.x = i * dz;
+ modelPose.pos.z = dz;
+
+ // i=0: rotated model pose
+ // expect inertial pose to match model pose
+ if (i == 0)
+ {
+ modelPose.rot.SetFromEuler(0.0, 0.0, angle);
+ }
+ // i=1: rotated link pose
+ // expect inertial pose to match link pose
+ else if (i == 1)
+ {
+ linkPose.rot.SetFromEuler(0.0, 0.0, angle);
+ }
+ // i=2: rotated inertial pose
+ // expect inertial pose to differ from link pose
+ else if (i == 2)
+ {
+ inertialPose.rot.SetFromEuler(0.0, 0.0, angle);
+ }
+ // i=3: offset inertial pose
+ // expect inertial pose to differ from link pose
+ else if (i == 3)
+ {
+ inertialPose.pos.Set(1, 1, 1);
+ }
+
+ {
+ auto msgLink = msgModel.mutable_link(0);
+ auto msgInertial = msgLink->mutable_inertial();
+
+ msgs::Set(msgModel.mutable_pose(), modelPose.Ign());
+ msgs::Set(msgLink->mutable_pose(), linkPose.Ign());
+ msgs::Set(msgInertial->mutable_pose(), inertialPose.Ign());
+ }
+
+ auto model = this->SpawnModel(msgModel);
+ ASSERT_TRUE(model != NULL);
+
+ auto link = model->GetLink();
+ ASSERT_TRUE(link != NULL);
+
+ EXPECT_EQ(model->GetWorldPose(), modelPose);
+ EXPECT_EQ(link->GetWorldPose(), linkPose + modelPose);
+ EXPECT_EQ(link->GetWorldInertialPose(),
+ inertialPose + linkPose + modelPose);
+
+ // i=0: rotated model pose
+ // expect inertial pose to match model pose
+ if (i == 0)
+ {
+ EXPECT_EQ(model->GetWorldPose(),
+ link->GetWorldInertialPose());
+ }
+ // i=1: rotated link pose
+ // expect inertial pose to match link pose
+ else if (i == 1)
+ {
+ EXPECT_EQ(link->GetWorldPose(),
+ link->GetWorldInertialPose());
+ }
+ // i=2: offset and rotated inertial pose
+ // expect inertial pose to differ from link pose
+ else if (i == 2)
+ {
+ EXPECT_EQ(link->GetWorldPose().pos,
+ link->GetWorldInertialPose().pos);
+ }
+ // i=3: offset inertial pose
+ // expect inertial pose to differ from link pose
+ else if (i == 3)
+ {
+ EXPECT_EQ(link->GetWorldPose().pos + inertialPose.pos,
+ link->GetWorldInertialPose().pos);
+ }
+
+ // Expect rotated inertia matrix
+ math::Matrix3 inertia = link->GetWorldInertiaMatrix();
+ if (i == 3)
+ {
+ EXPECT_NEAR(inertia[0][0], 80.8333, 1e-4);
+ EXPECT_NEAR(inertia[1][1], 68.3333, 1e-4);
+ EXPECT_NEAR(inertia[2][2], 14.1667, 1e-4);
+ for (int row = 0; row < 3; ++row)
+ for (int col = 0; col < 3; ++col)
+ if (row != col)
+ EXPECT_NEAR(inertia[row][col], 0.0, g_tolerance);
+ }
+ else
+ {
+ EXPECT_NEAR(inertia[0][0], 71.4583, 1e-4);
+ EXPECT_NEAR(inertia[1][1], 77.7083, 1e-4);
+ EXPECT_NEAR(inertia[2][2], 14.1667, 1e-4);
+ EXPECT_NEAR(inertia[0][1], 5.4126, 1e-4);
+ EXPECT_NEAR(inertia[1][0], 5.4126, 1e-4);
+ EXPECT_NEAR(inertia[0][2], 0, g_tolerance);
+ EXPECT_NEAR(inertia[2][0], 0, g_tolerance);
+ EXPECT_NEAR(inertia[1][2], 0, g_tolerance);
+ EXPECT_NEAR(inertia[2][1], 0, g_tolerance);
+ }
+
+ // For 0-2, apply torque and expect equivalent response
+ if (i <= 2)
+ {
+ for (int step = 0; step < 50; ++step)
+ {
+ link->SetTorque(math::Vector3(100, 0, 0));
+ world->Step(1);
+ }
+ if (_physicsEngine.compare("dart") == 0)
+ {
+ gzerr << "Dart fails this portion of the test (#1090)" << std::endl;
+ }
+ else
+ {
+ math::Vector3 vel = link->GetWorldAngularVel();
+ EXPECT_NEAR(vel.x, 0.0703, g_tolerance);
+ EXPECT_NEAR(vel.y, -0.0049, g_tolerance);
+ EXPECT_NEAR(vel.z, 0.0000, g_tolerance);
+ }
+ }
+ }
+}
+
+/////////////////////////////////////////////////
+void PhysicsLinkTest::OnWrenchMsg(const std::string &_physicsEngine)
+{
+ // TODO bullet, dart and simbody currently fail this test
+ if (_physicsEngine != "ode")
+ {
+ gzerr << "Aborting OnWrenchMsg test for Bullet, DART and Simbody. "
+ << "Because of issues #1476, #1477, and #1478."
+ << std::endl;
+ return;
+ }
+
+ Load("worlds/blank.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // check the physics engine
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ EXPECT_EQ(physics->GetType(), _physicsEngine);
+ double dt = physics->GetMaxStepSize();
+ EXPECT_GT(dt, 0);
+
+ // disable gravity
+ physics->SetGravity(math::Vector3::Zero);
+
+ // Spawn a box
+ math::Vector3 size(1, 1, 1);
+ SpawnBox("box", size, math::Vector3::Zero, math::Vector3::Zero, false);
+ physics::ModelPtr model = world->GetModel("box");
+ ASSERT_TRUE(model != NULL);
+ physics::LinkPtr link = model->GetLink();
+ ASSERT_TRUE(link != NULL);
+
+ // Check that link is at rest
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearVel());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularVel());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearAccel());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularAccel());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldForce());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldTorque());
+
+ // Publish wrench message
+ std::string topicName = "~/" + link->GetScopedName() + "/wrench";
+ boost::replace_all(topicName, "::", "/");
+ transport::PublisherPtr wrenchPub =
+ this->node->Advertise<msgs::Wrench>(topicName);
+
+ msgs::Wrench msg;
+
+ std::vector<math::Vector3> forces;
+ std::vector<math::Vector3> torques;
+ std::vector<math::Vector3> forceOffsets;
+
+ // Only force
+ forces.push_back(math::Vector3(1, 0, 0));
+ torques.push_back(math::Vector3::Zero);
+ forceOffsets.push_back(math::Vector3::Zero);
+
+ // Only force, with an offset
+ forces.push_back(math::Vector3(5.2, 0.1, 10));
+ torques.push_back(math::Vector3::Zero);
+ forceOffsets.push_back(math::Vector3(2.1, 1, -0.6));
+
+ // Only torque
+ forces.push_back(math::Vector3::Zero);
+ torques.push_back(math::Vector3(-0.2, 5, 0));
+ forceOffsets.push_back(math::Vector3::Zero);
+
+ // All fields set
+ forces.push_back(math::Vector3(5, 6, -0.9));
+ torques.push_back(math::Vector3(-0.2, 5, 0));
+ forceOffsets.push_back(math::Vector3(-1, -4, -0.8));
+
+ for (unsigned int i = 0; i < forces.size(); ++i)
+ {
+ gzdbg << "Testing force: " << forces[i].x << ", "
+ << forces[i].y << ", "
+ << forces[i].z <<
+ " torque: " << torques[i].x << ", "
+ << torques[i].y << ", "
+ << torques[i].z <<
+ " force offset: " << forceOffsets[i].x << ", "
+ << forceOffsets[i].y << ", "
+ << forceOffsets[i].z << std::endl;
+
+ // Publish message
+ msgs::Set(msg.mutable_force(), forces[i].Ign());
+ msgs::Set(msg.mutable_torque(), torques[i].Ign());
+ // Leave optional field unset if it's zero
+ if (forceOffsets[i] != math::Vector3::Zero)
+ msgs::Set(msg.mutable_force_offset(), forceOffsets[i].Ign());
+
+ wrenchPub->Publish(msg);
+
+ // Calculate expected values
+ math::Vector3 forceWorld = forces[i];
+ math::Vector3 worldOffset = forceOffsets[i] - link->GetInertial()->GetCoG();
+ math::Vector3 torqueWorld = worldOffset.Cross(forces[i]) + torques[i];
+
+ // Wait for message to be received
+ while (link->GetWorldForce() != forceWorld ||
+ link->GetWorldTorque() != torqueWorld)
+ {
+ world->Step(1);
+ common::Time::MSleep(1);
+ }
+
+ // Check force and torque (at CoG?) in world frame
+ EXPECT_EQ(link->GetWorldForce(), forceWorld);
+ EXPECT_EQ(link->GetWorldTorque(), torqueWorld);
+
+ // Reset link's physics states
+ link->ResetPhysicsStates();
+ link->SetWorldPose(math::Pose());
+ world->Step(1);
+
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearVel());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularVel());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldLinearAccel());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldAngularAccel());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldForce());
+ EXPECT_EQ(math::Vector3::Zero, link->GetWorldTorque());
+ }
+}
+
+/////////////////////////////////////////////////
void PhysicsLinkTest::SetVelocity(const std::string &_physicsEngine)
{
- Load("worlds/empty.world", true, _physicsEngine);
+ Load("worlds/blank.world", true, _physicsEngine);
physics::WorldPtr world = physics::get_world("default");
ASSERT_TRUE(world != NULL);
@@ -154,12 +726,36 @@ void PhysicsLinkTest::SetVelocity(const std::string &_physicsEngine)
}
/////////////////////////////////////////////////
+TEST_P(PhysicsLinkTest, AddForce)
+{
+ AddForce(GetParam());
+}
+
+/////////////////////////////////////////////////
+TEST_P(PhysicsLinkTest, GetWorldAngularMomentum)
+{
+ GetWorldAngularMomentum(GetParam());
+}
+
+/////////////////////////////////////////////////
TEST_P(PhysicsLinkTest, GetWorldEnergy)
{
GetWorldEnergy(GetParam());
}
/////////////////////////////////////////////////
+TEST_P(PhysicsLinkTest, GetWorldInertia)
+{
+ GetWorldInertia(GetParam());
+}
+
+/////////////////////////////////////////////////
+TEST_P(PhysicsLinkTest, OnWrenchMsg)
+{
+ OnWrenchMsg(GetParam());
+}
+
+/////////////////////////////////////////////////
TEST_P(PhysicsLinkTest, SetVelocity)
{
SetVelocity(GetParam());
diff --git a/test/integration/physics_msgs.cc b/test/integration/physics_msgs.cc
index c019af5..bf490b8 100644
--- a/test/integration/physics_msgs.cc
+++ b/test/integration/physics_msgs.cc
@@ -19,8 +19,8 @@
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#define PHYSICS_TOL 1e-2
using namespace gazebo;
@@ -33,6 +33,7 @@ class PhysicsMsgsTest : public ServerFixture,
public: void LinkProperties(const std::string &_physicsEngine);
public: void LinkPose(const std::string &_physicsEngine);
public: void SimpleShapeResize(const std::string &_physicsEngine);
+ public: void LinkVisualMsg(const std::string &_physicsEngine);
};
/////////////////////////////////////////////////
@@ -75,7 +76,7 @@ void PhysicsMsgsTest::SetGravity(const std::string &_physicsEngine)
for (std::vector<math::Vector3>::iterator iter = gravity.begin();
iter != gravity.end(); ++iter)
{
- msgs::Set(msg.mutable_gravity(), *iter);
+ msgs::Set(msg.mutable_gravity(), (*iter).Ign());
physicsPub->Publish(msg);
while (*iter != physics->GetGravity())
@@ -140,7 +141,7 @@ void PhysicsMsgsTest::MoveTool(const std::string &_physicsEngine)
for (std::vector<math::Pose>::iterator iter = poses.begin();
iter != poses.end(); ++iter)
{
- msgs::Set(msg.mutable_pose(), *iter);
+ msgs::Set(msg.mutable_pose(), (*iter).Ign());
modelPub->Publish(msg);
while (*iter != model->GetWorldPose())
@@ -355,7 +356,7 @@ void PhysicsMsgsTest::LinkPose(const std::string &_physicsEngine)
linkMsg->set_id(link->GetId());
linkMsg->set_name(link->GetScopedName());
- msgs::Set(linkMsg->mutable_pose(), *iter);
+ msgs::Set(linkMsg->mutable_pose(), (*iter).Ign());
modelPub->Publish(msg);
int sleep = 0;
@@ -416,18 +417,31 @@ void PhysicsMsgsTest::SimpleShapeResize(const std::string &_physicsEngine)
modelPos["test_box"] = math::Vector3(0, 0, z0);
modelPos["test_sphere"] = math::Vector3(4, 0, z0);
modelPos["test_cylinder"] = math::Vector3(8, 0, z0);
+ modelPos["test_non_unit_box"] = math::Vector3(16, 0, z0);
+
+ std::map<std::string, math::Vector3> modelSize;
+ modelSize["test_box"] = math::Vector3::One;
+ modelSize["test_sphere"] = math::Vector3::One;
+ modelSize["test_cylinder"] = math::Vector3::One;
+ modelSize["test_non_unit_box"] = math::Vector3(3, 8, 1.0);
SpawnBox("test_box", math::Vector3(1, 1, 1), modelPos["test_box"],
math::Vector3::Zero);
SpawnSphere("test_sphere", modelPos["test_sphere"], math::Vector3::Zero);
SpawnCylinder("test_cylinder", modelPos["test_cylinder"],
math::Vector3::Zero);
+ SpawnBox("test_non_unit_box", modelSize["test_non_unit_box"],
+ modelPos["test_non_unit_box"], math::Vector3::Zero);
// spawn another set of shapes and use messages to resize these
modelPos["test_box2"] = math::Vector3(0, 9, z0);
modelPos["test_sphere2"] = math::Vector3(4, 9, z0);
modelPos["test_cylinder2"] = math::Vector3(8, 9, z0);
+ modelSize["test_box2"] = math::Vector3::One;
+ modelSize["test_sphere2"] = math::Vector3::One;
+ modelSize["test_cylinder2"] = math::Vector3::One;
+
SpawnBox("test_box2", math::Vector3(1, 1, 1), modelPos["test_box2"],
math::Vector3::Zero);
SpawnSphere("test_sphere2", modelPos["test_sphere2"], math::Vector3::Zero);
@@ -448,10 +462,9 @@ void PhysicsMsgsTest::SimpleShapeResize(const std::string &_physicsEngine)
world->Step(100);
// Verify the initial model pose is where we set it to be.
- for (std::map<std::string, math::Vector3>::iterator iter = modelPos.begin();
- iter != modelPos.end(); ++iter)
+ for (auto const &iter : modelPos)
{
- std::string name = iter->first;
+ std::string name = iter.first;
// Make sure the model is loaded
model = world->GetModel(name);
EXPECT_TRUE(model != NULL);
@@ -467,10 +480,9 @@ void PhysicsMsgsTest::SimpleShapeResize(const std::string &_physicsEngine)
// resize model to half of it's size
double scaleFactor = 0.5;
- for (std::map<std::string, math::Vector3>::iterator iter = modelPos.begin();
- iter != modelPos.end(); ++iter)
+ for (auto const &iter : modelPos)
{
- std::string name = iter->first;
+ std::string name = iter.first;
model = world->GetModel(name);
if (*(name.rbegin()) == '2')
{
@@ -478,7 +490,8 @@ void PhysicsMsgsTest::SimpleShapeResize(const std::string &_physicsEngine)
msgs::Model msg;
msg.set_name(name);
msg.set_id(model->GetId());
- msgs::Set(msg.mutable_scale(), scaleFactor * math::Vector3::One);
+ msgs::Set(msg.mutable_scale(),
+ scaleFactor * ignition::math::Vector3d::One);
modelPub->Publish(msg);
}
else
@@ -508,10 +521,9 @@ void PhysicsMsgsTest::SimpleShapeResize(const std::string &_physicsEngine)
// This loop checks the velocity and pose of each model 0.5 seconds
// after the time of predicted ground contact. The pose is expected to be
// underneath the initial pose.
- for (std::map<std::string, math::Vector3>::iterator iter = modelPos.begin();
- iter != modelPos.end(); ++iter)
+ for (auto const &iter : modelPos)
{
- std::string name = iter->first;
+ std::string name = iter.first;
// Make sure the model is loaded
model = world->GetModel(name);
if (model != NULL)
@@ -531,8 +543,223 @@ void PhysicsMsgsTest::SimpleShapeResize(const std::string &_physicsEngine)
EXPECT_TRUE(model != NULL);
}
}
+
+ // verify geom msgs contain resized values.
+ for (auto const &iter : modelPos)
+ {
+ std::string name = iter.first;
+ model = world->GetModel(name);
+ msgs::Model modelMsg;
+ model->FillMsg(modelMsg);
+
+ EXPECT_EQ(msgs::ConvertIgn(modelMsg.scale()),
+ scaleFactor * ignition::math::Vector3d::One);
+ for (int i = 0; i < modelMsg.link_size(); ++i)
+ {
+ msgs::Link linkMsg = modelMsg.link(i);
+
+ // verify visual geom msgs
+ for (int j = 0; j < linkMsg.visual_size(); ++j)
+ {
+ msgs::Visual visualMsg = linkMsg.visual(j);
+ msgs::Geometry geomMsg = visualMsg.geometry();
+ if (geomMsg.has_box())
+ {
+ EXPECT_EQ(msgs::ConvertIgn(geomMsg.box().size()),
+ modelSize[name].Ign() * scaleFactor);
+ }
+ else if (geomMsg.has_sphere())
+ {
+ EXPECT_DOUBLE_EQ(geomMsg.sphere().radius(),
+ modelSize[name].x * 0.5 * scaleFactor);
+ }
+ else if (geomMsg.has_cylinder())
+ {
+ EXPECT_DOUBLE_EQ(geomMsg.cylinder().radius(),
+ modelSize[name].x * 0.5 * scaleFactor);
+ EXPECT_DOUBLE_EQ(geomMsg.cylinder().length(),
+ modelSize[name].z * scaleFactor);
+ }
+ }
+
+ // verify collision geom msgs
+ for (int j = 0; j < linkMsg.collision_size(); ++j)
+ {
+ msgs::Collision collisionMsg = linkMsg.collision(j);
+ msgs::Geometry geomMsg = collisionMsg.geometry();
+ if (geomMsg.has_box())
+ {
+ EXPECT_EQ(msgs::ConvertIgn(geomMsg.box().size()),
+ modelSize[name].Ign() * scaleFactor);
+ }
+ else if (geomMsg.has_sphere())
+ {
+ EXPECT_DOUBLE_EQ(geomMsg.sphere().radius(),
+ modelSize[name].x * 0.5 * scaleFactor);
+ }
+ else if (geomMsg.has_cylinder())
+ {
+ EXPECT_DOUBLE_EQ(geomMsg.cylinder().radius(),
+ modelSize[name].x * 0.5 * scaleFactor);
+ EXPECT_DOUBLE_EQ(geomMsg.cylinder().length(),
+ modelSize[name].z * scaleFactor);
+ }
+ }
+ }
+ }
+
+ // verify geom sdfs contain resized values.
+ for (auto const &iter : modelPos)
+ {
+ std::string name = iter.first;
+ model = world->GetModel(name);
+ sdf::ElementPtr modelElem = model->GetSDF();
+
+ EXPECT_TRUE(modelElem->HasElement("link"));
+ sdf::ElementPtr linkElem = modelElem->GetElement("link");
+ while (linkElem)
+ {
+ // verify visual geom sdf
+ EXPECT_TRUE(linkElem->HasElement("visual"));
+ sdf::ElementPtr visualElem = linkElem->GetElement("visual");
+
+ EXPECT_TRUE(visualElem->HasElement("geometry"));
+ sdf::ElementPtr visualGeomElem = visualElem->GetElement("geometry");
+ if (visualGeomElem->HasElement("box"))
+ {
+ sdf::ElementPtr boxElem = visualGeomElem->GetElement("box");
+ math::Vector3 size = boxElem->Get<math::Vector3>("size");
+ EXPECT_EQ(size, modelSize[name] * scaleFactor);
+ }
+ else if (visualGeomElem->HasElement("sphere"))
+ {
+ sdf::ElementPtr sphereElem = visualGeomElem->GetElement("sphere");
+ double radius = sphereElem->Get<double>("radius");
+ EXPECT_EQ(radius, modelSize[name].x * 0.5 * scaleFactor);
+ }
+ else if (visualGeomElem->HasElement("cylinder"))
+ {
+ sdf::ElementPtr cylinderElem = visualGeomElem->GetElement("cylinder");
+ double radius = cylinderElem->Get<double>("radius");
+ EXPECT_EQ(radius, modelSize[name].x * 0.5 * scaleFactor);
+ double length = cylinderElem->Get<double>("length");
+ EXPECT_EQ(length, modelSize[name].z * scaleFactor);
+ }
+
+ // verify collision geom sdf
+ EXPECT_TRUE(linkElem->HasElement("collision"));
+ sdf::ElementPtr collisionElem = linkElem->GetElement("collision");
+
+ EXPECT_TRUE(collisionElem->HasElement("geometry"));
+ sdf::ElementPtr collisionGeomElem = collisionElem->GetElement("geometry");
+ if (collisionGeomElem->HasElement("box"))
+ {
+ sdf::ElementPtr boxElem = collisionGeomElem->GetElement("box");
+ math::Vector3 size = boxElem->Get<math::Vector3>("size");
+ EXPECT_EQ(size, modelSize[name] * scaleFactor);
+ }
+ else if (collisionGeomElem->HasElement("sphere"))
+ {
+ sdf::ElementPtr sphereElem = collisionGeomElem->GetElement("sphere");
+ double radius = sphereElem->Get<double>("radius");
+ EXPECT_EQ(radius, modelSize[name].x * 0.5 * scaleFactor);
+ }
+ else if (collisionGeomElem->HasElement("cylinder"))
+ {
+ sdf::ElementPtr cylinderElem =
+ collisionGeomElem->GetElement("cylinder");
+ double radius = cylinderElem->Get<double>("radius");
+ EXPECT_EQ(radius, modelSize[name].x * 0.5 * scaleFactor);
+ double length = cylinderElem->Get<double>("length");
+ EXPECT_EQ(length, modelSize[name].z * scaleFactor);
+ }
+ linkElem = linkElem->GetNextElement("link");
+ }
+ }
}
+////////////////////////////////////////////////////////////////////////
+void PhysicsMsgsTest::LinkVisualMsg(const std::string &_physicsEngine)
+{
+ // load an empty world
+ Load("worlds/empty.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);
+
+ std::ostringstream sdfStream;
+ math::Pose pose(0, 0, 0, 0, 0, 0);
+ math::Vector3 boxSize(1, 1, 1);
+ sdfStream << "<sdf version='" << SDF_VERSION << "'>"
+ << "<model name ='box_test'>"
+ << "<allow_auto_disable>false</allow_auto_disable>"
+ << "<link name ='body'>"
+ << " <pose>" << pose << "</pose>"
+ << " <inertial>"
+ << " <mass>4.0</mass>"
+ << " <inertia>"
+ << " <ixx>0.1667</ixx> <ixy>0.0</ixy> <ixz>0.0</ixz>"
+ << " <iyy>0.1667</iyy> <iyz>0.0</iyz>"
+ << " <izz>0.1667</izz>"
+ << " </inertia>"
+ << " </inertial>"
+ << " <collision name ='geom'>"
+ << " <geometry>"
+ << " <box><size>" << boxSize << "</size></box>"
+ << " </geometry>"
+ << " </collision>"
+ << " <visual name ='visual'>"
+ << " <geometry>"
+ << " <box><size>" << boxSize << "</size></box>"
+ << " </geometry>"
+ << " </visual>"
+ << " <visual name ='visual2'>"
+ << " <geometry>"
+ << " <box><size>" << boxSize << "</size></box>"
+ << " </geometry>"
+ << " </visual>"
+ << "</link>"
+ << "</model>"
+ << "</sdf>";
+ SpawnSDF(sdfStream.str());
+
+ physics::ModelPtr model;
+ model = world->GetModel("box_test");
+ EXPECT_TRUE(model != NULL);
+ msgs::Model msg;
+ model->FillMsg(msg);
+
+ EXPECT_EQ(msg.link_size(), 1);
+ msgs::Link linkMsg = msg.link(0);
+
+ // 3 visuals to be created: 1 link body + 2 visuals
+ EXPECT_EQ(linkMsg.visual_size(), 3);
+
+ // verify link body visual
+ msgs::Visual visualMsg = linkMsg.visual(0);
+ EXPECT_EQ(visualMsg.name(), "box_test::body");
+ EXPECT_TRUE(visualMsg.has_type());
+ EXPECT_EQ(visualMsg.type(), msgs::Visual::LINK);
+ EXPECT_FALSE(visualMsg.has_geometry());
+
+ // verify remaining visual msgs
+ for (int i = 1; i < linkMsg.visual_size(); ++i)
+ {
+ msgs::Visual visualMsg = linkMsg.visual(i);
+ std::stringstream visName;
+ visName << "box_test::body::visual";
+ if (i > 1)
+ visName << i;
+ EXPECT_EQ(visualMsg.name(), visName.str());
+ EXPECT_TRUE(visualMsg.has_type());
+ EXPECT_EQ(visualMsg.type(), msgs::Visual::VISUAL);
+ msgs::Geometry geomMsg = visualMsg.geometry();
+ EXPECT_TRUE(geomMsg.has_box());
+ EXPECT_EQ(msgs::ConvertIgn(geomMsg.box().size()), boxSize.Ign());
+ }
+}
/////////////////////////////////////////////////
TEST_P(PhysicsMsgsTest, SetGravity)
@@ -564,6 +791,12 @@ TEST_P(PhysicsMsgsTest, SimpleShapeResize)
SimpleShapeResize(GetParam());
}
+/////////////////////////////////////////////////
+TEST_P(PhysicsMsgsTest, LinkVisualMsg)
+{
+ LinkVisualMsg(GetParam());
+}
+
INSTANTIATE_TEST_CASE_P(PhysicsEngines, PhysicsMsgsTest,
PHYSICS_ENGINE_VALUES);
diff --git a/test/integration/physics_presets.cc b/test/integration/physics_presets.cc
new file mode 100644
index 0000000..873fa91
--- /dev/null
+++ b/test/integration/physics_presets.cc
@@ -0,0 +1,296 @@
+/*
+ * 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 <boost/any.hpp>
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/PhysicsTypes.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+
+using namespace gazebo;
+
+class PresetManagerTest : public ServerFixture,
+ public testing::WithParamInterface<const char*>
+{
+};
+
+/////////////////////////////////////////////////
+TEST_P(PresetManagerTest, InitializeAllPhysicsEngines)
+{
+ const std::string physicsEngineName = GetParam();
+ Load("test/worlds/presets.world", false, physicsEngineName);
+ physics::WorldPtr world = physics::get_world("default");
+
+ physics::PhysicsEnginePtr physicsEngine = world->GetPhysicsEngine();
+
+ physics::PresetManagerPtr presetManager = world->GetPresetManager();
+ if (!presetManager)
+ {
+ FAIL();
+ }
+ try
+ {
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("max_step_size")), 0.01);
+ if (physicsEngineName == "ode" || physicsEngineName == "bullet")
+ {
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("min_step_size")), 0.001);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(physicsEngine->GetParam("cfm")),
+ 0.01);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(physicsEngine->GetParam("erp")),
+ 0.3);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("contact_surface_layer")), 0.002);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(physicsEngine->GetParam("sor")),
+ 1.4);
+ EXPECT_EQ(boost::any_cast<int>(physicsEngine->GetParam("iters")), 50);
+ }
+ if (physicsEngineName == "ode")
+ {
+ EXPECT_TRUE(boost::any_cast<bool>(
+ physicsEngine->GetParam("inertia_ratio_reduction")));
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("contact_max_correcting_vel")), 200);
+ }
+ if (physicsEngineName == "bullet")
+ {
+ EXPECT_FALSE(boost::any_cast<bool>(
+ physicsEngine->GetParam("split_impulse")));
+ }
+ if (physicsEngineName == "simbody")
+ {
+ EXPECT_FLOAT_EQ(
+ boost::any_cast<double>(physicsEngine->GetParam("accuracy")), 0.01);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(physicsEngine->GetParam(
+ "max_transient_velocity")), 0.001);
+ }
+ }
+ catch(const boost::bad_any_cast& e)
+ {
+ FAIL();
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(PresetManagerTest, MultipleDefaults)
+{
+ Load("test/worlds/presets.world", false, "ode");
+ physics::WorldPtr world = physics::get_world("default");
+
+ physics::PhysicsEnginePtr physicsEngine = world->GetPhysicsEngine();
+
+ physics::PresetManagerPtr presetManager = world->GetPresetManager();
+ if (!presetManager)
+ {
+ FAIL();
+ }
+ EXPECT_EQ(presetManager->CurrentProfile(), "preset_1");
+}
+
+/////////////////////////////////////////////////
+TEST_F(PresetManagerTest, NoDefault)
+{
+ Load("test/worlds/presets_nodefault.world", false, "ode");
+ physics::WorldPtr world = physics::get_world("default");
+
+ physics::PhysicsEnginePtr physicsEngine = world->GetPhysicsEngine();
+
+ physics::PresetManagerPtr presetManager = world->GetPresetManager();
+ if (!presetManager)
+ {
+ FAIL();
+ }
+ EXPECT_EQ(presetManager->CurrentProfile(), "preset_1");
+}
+
+/////////////////////////////////////////////////
+TEST_F(PresetManagerTest, SetProfileParam)
+{
+ Load("test/worlds/presets.world", false, "ode");
+ physics::WorldPtr world = physics::get_world("default");
+
+ physics::PhysicsEnginePtr physicsEngine = world->GetPhysicsEngine();
+
+ physics::PresetManagerPtr presetManager = world->GetPresetManager();
+ if (!presetManager)
+ {
+ FAIL();
+ }
+
+ EXPECT_TRUE(presetManager->SetCurrentProfileParam("max_step_size", 10.0));
+ try
+ {
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("max_step_size")), 10.0);
+
+ // preset_2 is not the current profile, so we do not expect to see a change
+ // in the physics engine when we change preset_2.
+ EXPECT_TRUE(presetManager->SetProfileParam("preset_2", "max_step_size",
+ 20));
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("max_step_size")), 10.0);
+ }
+ catch(const boost::bad_any_cast& e)
+ {
+ FAIL();
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(PresetManagerTest, SetCurrentProfile)
+{
+ Load("test/worlds/presets.world", false, "ode");
+ physics::WorldPtr world = physics::get_world("default");
+
+ physics::PhysicsEnginePtr physicsEngine = world->GetPhysicsEngine();
+
+ physics::PresetManagerPtr presetManager = world->GetPresetManager();
+
+ if (!presetManager)
+ {
+ FAIL();
+ }
+
+ std::vector<std::string> profileNames(presetManager->AllProfiles());
+ EXPECT_EQ(profileNames.size(), 3u);
+
+ presetManager->CurrentProfile("preset_2");
+
+ EXPECT_EQ(presetManager->CurrentProfile(), "preset_2");
+
+ try
+ {
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("max_step_size")), 0.02);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("min_step_size")), 0.002);
+ EXPECT_EQ(boost::any_cast<int>(physicsEngine->GetParam("iters")), 100);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(physicsEngine->GetParam("cfm")),
+ 0.02);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(physicsEngine->GetParam("erp")),
+ 0.6);
+ }
+ catch(const boost::bad_any_cast& e)
+ {
+ FAIL();
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_F(PresetManagerTest, CreateProfileFromSDF)
+{
+ Load("test/worlds/presets.world", false, "ode");
+ physics::WorldPtr world = physics::get_world("default");
+
+ physics::PhysicsEnginePtr physicsEngine = world->GetPhysicsEngine();
+
+ physics::PresetManagerPtr presetManager = world->GetPresetManager();
+ if (!presetManager)
+ {
+ FAIL();
+ }
+
+ sdf::SDF worldSDF;
+ worldSDF.SetFromString(
+ "<sdf version = \"1.5\">\
+ <world name = \"default\">\
+ <physics name = \"preset_3\" type = \"ode\">\
+ <max_step_size>0.03</max_step_size>\
+ <ode>\
+ <solver>\
+ <min_step_size>0.003</min_step_size>\
+ <iters>150</iters>\
+ <sor>1.6</sor>\
+ </solver>\
+ <constraints>\
+ <cfm>0.03</cfm>\
+ <erp>0.7</erp>\
+ </constraints>\
+ </ode>\
+ </physics>\
+ </world>\
+ </sdf>");
+ sdf::ElementPtr physicsSDF = worldSDF.Root()->GetElement("world")
+ ->GetElement("physics");
+ presetManager->CreateProfile(physicsSDF);
+ presetManager->CurrentProfile("preset_3");
+ try
+ {
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("max_step_size")), 0.03);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("min_step_size")), 0.003);
+ EXPECT_EQ(boost::any_cast<int>(physicsEngine->GetParam("iters")), 150);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(physicsEngine->GetParam("cfm")),
+ 0.03);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(physicsEngine->GetParam("erp")),
+ 0.7);
+ }
+ catch(const boost::bad_any_cast& e)
+ {
+ FAIL();
+ }
+}
+
+TEST_F(PresetManagerTest, BackwardsCompatibilityTest)
+{
+ Load("worlds/empty.world", false, "ode");
+ physics::WorldPtr world = physics::get_world("default");
+
+ physics::PhysicsEnginePtr physicsEngine = world->GetPhysicsEngine();
+
+ physics::PresetManagerPtr presetManager = world->GetPresetManager();
+ if (!presetManager)
+ {
+ FAIL();
+ }
+ try
+ {
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("max_step_size")), 0.001);
+ EXPECT_EQ(boost::any_cast<int>(physicsEngine->GetParam("iters")), 50);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(physicsEngine->GetParam("cfm")),
+ 0.0);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(physicsEngine->GetParam("erp")),
+ 0.2);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("contact_max_correcting_vel")), 100);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("contact_surface_layer")), 0.001);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(physicsEngine->GetParam("sor")),
+ 1.3);
+ EXPECT_FLOAT_EQ(boost::any_cast<double>(
+ physicsEngine->GetParam("min_step_size")), 0.0001);
+ EXPECT_FALSE(boost::any_cast<bool>(
+ physicsEngine->GetParam("inertia_ratio_reduction")));
+ }
+ catch(const boost::bad_any_cast& e)
+ {
+ FAIL();
+ }
+}
+
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, PresetManagerTest,
+ PHYSICS_ENGINE_VALUES);
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/physics_thread_safe.cc b/test/integration/physics_thread_safe.cc
index b1ad993..0a62d2d 100644
--- a/test/integration/physics_thread_safe.cc
+++ b/test/integration/physics_thread_safe.cc
@@ -19,8 +19,8 @@
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#define PHYSICS_TOL 1e-2
using namespace gazebo;
diff --git a/test/integration/pioneer2dx.cc b/test/integration/pioneer2dx.cc
index d7624ac..d42b247 100644
--- a/test/integration/pioneer2dx.cc
+++ b/test/integration/pioneer2dx.cc
@@ -14,10 +14,10 @@
* limitations under the License.
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/common/Time.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
class Pioneer2dx : public ServerFixture,
@@ -55,9 +55,9 @@ void Pioneer2dx::StraightLine(const std::string &_physicsEngine)
gazebo::msgs::Pose msg;
gazebo::msgs::Set(msg.mutable_position(),
- gazebo::math::Vector3(0.2, 0, 0));
+ ignition::math::Vector3d(0.2, 0, 0));
gazebo::msgs::Set(msg.mutable_orientation(),
- gazebo::math::Quaternion(0, 0, 0));
+ ignition::math::Quaterniond(0, 0, 0));
velPub->Publish(msg);
math::Pose startPose, endPose;
diff --git a/test/integration/plugin.cc b/test/integration/plugin.cc
index 3d2545e..f660f42 100644
--- a/test/integration/plugin.cc
+++ b/test/integration/plugin.cc
@@ -16,7 +16,7 @@
*/
#include <boost/filesystem.hpp>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class PluginTest : public ServerFixture
diff --git a/test/integration/polyline.cc b/test/integration/polyline.cc
index 4cfdd2d..a647d75 100644
--- a/test/integration/polyline.cc
+++ b/test/integration/polyline.cc
@@ -14,18 +14,66 @@
* limitations under the License.
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/msgs/msgs.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+using namespace gazebo;
class PolylineTest : public ServerFixture,
public testing::WithParamInterface<const char*>
{
+ public: void ComputeVolume(const std::string &_physicsEngine);
public: void PolylineWorld(const std::string &_physicsEngine);
};
/////////////////////////////////////////////////
+// Test polyline shape bounding box volume computation
+void PolylineTest::ComputeVolume(const std::string &_physicsEngine)
+{
+ // Load the sample world
+ Load("worlds/polyline.world", false, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ physics::ModelPtr cubeModel = world->GetModel("cube");
+ EXPECT_TRUE(cubeModel != NULL);
+
+ physics::LinkPtr cubeLink = cubeModel->GetLink("polyLine2");
+ EXPECT_TRUE(cubeLink != NULL);
+
+ physics::CollisionPtr cubeColl = cubeLink->GetCollision("collision");
+ EXPECT_TRUE(cubeColl != NULL);
+
+ physics::ShapePtr shape = cubeColl->GetShape();
+ EXPECT_TRUE(shape != NULL);
+
+ // The actual volume of the cube shape is 1*1*1.5 = 1.5
+ // We expect ComputeVolume to be accurate because it's also a box
+
+ // see issue #1506 (https://bitbucket.org/osrf/gazebo/issue/1506)
+ if (_physicsEngine == "bullet")
+ {
+ EXPECT_NEAR(shape->ComputeVolume(), 1.5, 0.09);
+ }
+ else
+ {
+ EXPECT_DOUBLE_EQ(shape->ComputeVolume(), 1.5);
+ }
+}
+
+/////////////////////////////////////////////////
+TEST_P(PolylineTest, ComputeVolume)
+{
+ if (GetParam() == std::string("simbody"))
+ gzwarn << "Polyline not supported in simbody" << std::endl;
+ else if (GetParam() == std::string("dart"))
+ gzwarn << "Bounding box not supported in DART" << std::endl;
+ else
+ ComputeVolume(GetParam());
+}
+
+/////////////////////////////////////////////////
// Test polyline instantiation and polyline collision
void PolylineTest::PolylineWorld(const std::string &_physicsEngine)
{
@@ -53,33 +101,34 @@ void PolylineTest::PolylineWorld(const std::string &_physicsEngine)
EXPECT_DOUBLE_EQ(polyShape->GetHeight(), 1.0);
- std::vector<math::Vector2d> vertices = polyShape->GetVertices();
- EXPECT_EQ(vertices[0], math::Vector2d(-0.5, -0.5));
- EXPECT_EQ(vertices[1], math::Vector2d(-0.5, 0.5));
- EXPECT_EQ(vertices[2], math::Vector2d(0.5, 0.5));
- EXPECT_EQ(vertices[3], math::Vector2d(0.0, 0.0));
- EXPECT_EQ(vertices[4], math::Vector2d(0.5, -0.5));
+ std::vector<std::vector<ignition::math::Vector2d> > vertices =
+ polyShape->Vertices();
+ EXPECT_EQ(vertices[0][0], ignition::math::Vector2d(-0.5, -0.5));
+ EXPECT_EQ(vertices[0][1], ignition::math::Vector2d(-0.5, 0.5));
+ EXPECT_EQ(vertices[0][2], ignition::math::Vector2d(0.5, 0.5));
+ EXPECT_EQ(vertices[0][3], ignition::math::Vector2d(0.0, 0.0));
+ EXPECT_EQ(vertices[0][4], ignition::math::Vector2d(0.5, -0.5));
// Check the FillMsg function
{
msgs::Geometry msg;
polyShape->FillMsg(msg);
EXPECT_EQ(msg.type(), msgs::Geometry::POLYLINE);
- EXPECT_DOUBLE_EQ(msg.polyline().height(), 1);
- EXPECT_DOUBLE_EQ(msg.polyline().point(0).x(), -0.5);
- EXPECT_DOUBLE_EQ(msg.polyline().point(0).y(), -0.5);
+ EXPECT_DOUBLE_EQ(msg.polyline(0).height(), 1);
+ EXPECT_DOUBLE_EQ(msg.polyline(0).point(0).x(), -0.5);
+ EXPECT_DOUBLE_EQ(msg.polyline(0).point(0).y(), -0.5);
- EXPECT_DOUBLE_EQ(msg.polyline().point(1).x(), -0.5);
- EXPECT_DOUBLE_EQ(msg.polyline().point(1).y(), 0.5);
+ EXPECT_DOUBLE_EQ(msg.polyline(0).point(1).x(), -0.5);
+ EXPECT_DOUBLE_EQ(msg.polyline(0).point(1).y(), 0.5);
- EXPECT_DOUBLE_EQ(msg.polyline().point(2).x(), 0.5);
- EXPECT_DOUBLE_EQ(msg.polyline().point(2).y(), 0.5);
+ EXPECT_DOUBLE_EQ(msg.polyline(0).point(2).x(), 0.5);
+ EXPECT_DOUBLE_EQ(msg.polyline(0).point(2).y(), 0.5);
- EXPECT_DOUBLE_EQ(msg.polyline().point(3).x(), 0.0);
- EXPECT_DOUBLE_EQ(msg.polyline().point(3).y(), 0.0);
+ EXPECT_DOUBLE_EQ(msg.polyline(0).point(3).x(), 0.0);
+ EXPECT_DOUBLE_EQ(msg.polyline(0).point(3).y(), 0.0);
- EXPECT_DOUBLE_EQ(msg.polyline().point(4).x(), 0.5);
- EXPECT_DOUBLE_EQ(msg.polyline().point(4).y(), -0.5);
+ EXPECT_DOUBLE_EQ(msg.polyline(0).point(4).x(), 0.5);
+ EXPECT_DOUBLE_EQ(msg.polyline(0).point(4).y(), -0.5);
}
// Spawn a sphere over the polyline shape, and check that it doesn't pass
diff --git a/test/integration/pr2.cc b/test/integration/pr2.cc
index 7c87c34..3787ab6 100644
--- a/test/integration/pr2.cc
+++ b/test/integration/pr2.cc
@@ -16,9 +16,9 @@
*/
#include <boost/filesystem.hpp>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
class PR2Test : public ServerFixture,
@@ -153,8 +153,10 @@ void PR2Test::StaticPR2(std::string _physicsEngine)
{
if (_physicsEngine == "simbody")
{
- gzerr << "Abort test since simbody does not support screw joints in PR2, "
- << "Please see issue #857.\n";
+ gzerr << "Abort test since simbody does not support static models "
+ << "with pose offsets, "
+ << "please see issue #860."
+ << std::endl;
return;
}
if (_physicsEngine == "dart")
diff --git a/test/integration/projector.cc b/test/integration/projector.cc
index 6b5edc6..5dab556 100644
--- a/test/integration/projector.cc
+++ b/test/integration/projector.cc
@@ -14,7 +14,7 @@
* limitations under the License.
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "images_cmp.h"
using namespace gazebo;
diff --git a/test/integration/rendering_sensor.cc b/test/integration/rendering_sensor.cc
index ff5d5b9..3f64e2a 100644
--- a/test/integration/rendering_sensor.cc
+++ b/test/integration/rendering_sensor.cc
@@ -15,7 +15,7 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class RenderingSensorTest : public ServerFixture
diff --git a/test/integration/rest_web.cc b/test/integration/rest_web.cc
new file mode 100644
index 0000000..72f9a43
--- /dev/null
+++ b/test/integration/rest_web.cc
@@ -0,0 +1,146 @@
+/*
+ * 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 "gazebo/test/ServerFixture.hh"
+#include "gazebo/msgs/msgs.hh"
+
+#include "gazebo/test/helper_physics_generator.hh"
+
+using namespace gazebo;
+
+// certain tests fail (with the symbody engine
+// setting this to true skips those tests
+bool SKIP_FAILING_TESTS = true;
+
+// this is the test fixture
+class RestWebTest : public ServerFixture,
+ public testing::WithParamInterface<const char*>
+{
+ public: void FirstTest(const std::string &_physicsEngine);
+};
+
+// globals to exchange data between threads
+boost::mutex g_mutex;
+unsigned int g_count;
+
+// RestLogin: string url, username, password
+// RestError: string type, msg
+// RestPost: string route, json
+
+
+// callback for SimEvent messages
+// increment a counter and keep the data around
+void ReceiveRestError(ConstRestErrorPtr &_msg)
+{
+ gzmsg << "ReceiveRestError:" << std::endl;
+ gzmsg << " \"" << _msg->msg() << "\"" << std::endl;
+ boost::mutex::scoped_lock lock(g_mutex);
+ g_count += 1;
+}
+
+// get the count in a thread safe way
+unsigned int GetErrorCount()
+{
+ boost::mutex::scoped_lock lock(g_mutex);
+ return g_count;
+}
+
+// get the last event type in thread safe way
+std::string LastRestError()
+{
+ boost::mutex::scoped_lock lock(g_mutex);
+ return "";
+}
+
+// waits for one or multiple events. if the expected number is
+// specified, then the function can return early
+unsigned int WaitForNewError(unsigned int current,
+ unsigned int max_tries = 10,
+ unsigned int ms = 100)
+{
+ gzmsg << "WaitForNewError " << current << std::endl;
+ for (unsigned int i = 0; i < max_tries; i++)
+ {
+ unsigned int count = GetErrorCount();
+ if (count > current)
+ {
+ return count;
+ }
+ common::Time::MSleep(ms);
+ }
+ gzmsg << "WaitForNewError (timeout)" << std::endl;
+ return GetErrorCount();
+}
+
+// test macro
+TEST_P(RestWebTest, FirstTest)
+{
+ FirstTest(GetParam());
+}
+
+////////////////////////////////////////////////////////////////////////
+// SimPauseRun:
+// Load test world, publish login and post messages
+////////////////////////////////////////////////////////////////////////
+void RestWebTest::FirstTest(const std::string &_physicsEngine)
+{
+ Load("test/worlds/rest_web.world",
+ false,
+ _physicsEngine,
+ {"libRestWebPlugin.so"});
+ physics::WorldPtr world = physics::get_world("default");
+
+ // setup the callback that increments a counter each time a
+ // RestError is emitted.
+ transport::NodePtr node = transport::NodePtr(new transport::Node());
+ node->Init();
+
+ transport::SubscriberPtr sub = node->Subscribe("/gazebo/rest/rest_error",
+ &ReceiveRestError);
+
+ // check that after pause, we have received a new event
+ unsigned int count_before = GetErrorCount();
+
+ // publish to the login topic
+ gazebo::transport::PublisherPtr pub;
+ pub = node->Advertise<gazebo::msgs::RestLogin>("/gazebo/rest/rest_login");
+ pub->WaitForConnection();
+
+ gazebo::msgs::RestLogin msg;
+ msg.set_url("https://localhost:3000");
+ msg.set_username("myuser");
+ msg.set_password("mypass");
+ pub->Publish(msg);
+
+ // since this test does not expect a server to be running (yet), we should
+ // receive a Rest_error via our sub
+
+ // wait for it ...
+ unsigned int count_after = WaitForNewError(count_before);
+ EXPECT_GT(count_after, count_before);
+}
+
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, RestWebTest, PHYSICS_ENGINE_VALUES);
+
+// main, where we can specify to skip certain tests
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/road.cc b/test/integration/road.cc
index e62aadc..6cd12c3 100644
--- a/test/integration/road.cc
+++ b/test/integration/road.cc
@@ -19,11 +19,11 @@
#include <string>
#include <vector>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "SimplePendulumIntegrator.hh"
#include "gazebo/msgs/msgs.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#define PHYSICS_TOL 1e-2
using namespace gazebo;
diff --git a/test/integration/scans_cmp.h b/test/integration/scans_cmp.h
index ff89484..fbbda5f 100644
--- a/test/integration/scans_cmp.h
+++ b/test/integration/scans_cmp.h
@@ -27,266 +27,268 @@
#define MAYBE_UNUSED
#endif
+double inf = GZ_DBL_INF;
+
static double __box_scan[] = {
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-1.5782253900, 1.5746053417, 1.5710809871, 1.5676513303, 1.5643154072,
-1.5610722851, 1.5579210616, 1.5548608639, 1.5518908487, 1.5490102012,
-1.5462181345, 1.5435138895, 1.5408967337, 1.5383659613, 1.5359208926,
-1.5335608733, 1.5312852740, 1.5290934905, 1.5269849423, 1.5249590733,
-1.5230153506, 1.5211532647, 1.5193723288, 1.5176720787, 1.5160520724,
-1.5145118900, 1.5130511332, 1.5116694251, 1.5103664100, 1.5091417531,
-1.5079951406, 1.5069262791, 1.5059348955, 1.5050207370, 1.5041835708,
-1.5034231841, 1.5027393839, 1.5021319965, 1.5016008682, 1.5011458645,
-1.5007668702, 1.5004637896, 1.5002365461, 1.5000850823, 1.5000093600,
-1.5000093600, 1.5000850823, 1.5002365461, 1.5004637896, 1.5007668702,
-1.5011458645, 1.5016008682, 1.5021319965, 1.5027393839, 1.5034231841,
-1.5041835708, 1.5050207370, 1.5059348955, 1.5069262791, 1.5079951406,
-1.5091417531, 1.5103664099, 1.5116694251, 1.5130511332, 1.5145118900,
-1.5160520724, 1.5176720786, 1.5193723287, 1.5211532647, 1.5230153506,
-1.5249590733, 1.5269849423, 1.5290934904, 1.5312852740, 1.5335608732,
-1.5359208926, 1.5383659613, 1.5408967337, 1.5435138894, 1.5462181345,
-1.5490102012, 1.5518908487, 1.5548608639, 1.5579210616, 1.5610722851,
-1.5643154072, 1.5676513302, 1.5710809871, 1.5746053417, 1.5782253899,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000};
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ 1.5782255003, 1.5746054518, 1.5710810969, 1.5676514398, 1.5643155166,
+ 1.5610723943, 1.5579211705, 1.5548609726, 1.5518909572, 1.5490103095,
+ 1.5462182426, 1.5435139973, 1.5408968414, 1.5383660689, 1.5359210000,
+ 1.5335609804, 1.5312853811, 1.5290935973, 1.5269850490, 1.5249591799,
+ 1.5230154571, 1.5211533710, 1.5193724350, 1.5176721847, 1.5160521784,
+ 1.5145119959, 1.5130512390, 1.5116695307, 1.5103665155, 1.5091418586,
+ 1.5079952460, 1.5069263844, 1.5059350007, 1.5050208422, 1.5041836760,
+ 1.5034232892, 1.5027394889, 1.5021321015, 1.5016009732, 1.5011459694,
+ 1.5007669751, 1.5004638945, 1.5002366510, 1.5000851872, 1.5000094648,
+ 1.5000094648, 1.5000851872, 1.5002366510, 1.5004638945, 1.5007669751,
+ 1.5011459694, 1.5016009732, 1.5021321015, 1.5027394889, 1.5034232892,
+ 1.5041836760, 1.5050208422, 1.5059350007, 1.5069263844, 1.5079952460,
+ 1.5091418586, 1.5103665155, 1.5116695307, 1.5130512390, 1.5145119959,
+ 1.5160521784, 1.5176721847, 1.5193724350, 1.5211533710, 1.5230154571,
+ 1.5249591799, 1.5269850490, 1.5290935973, 1.5312853811, 1.5335609804,
+ 1.5359210000, 1.5383660689, 1.5408968414, 1.5435139973, 1.5462182426,
+ 1.5490103095, 1.5518909572, 1.5548609726, 1.5579211705, 1.5610723943,
+ 1.5643155166, 1.5676514398, 1.5710810969, 1.5746054518, 1.5782255003,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf};
static double *box_scan MAYBE_UNUSED = __box_scan;
static double __plane_scan[] = {
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 9.6656585634, 9.0478807252,
-8.5047324589, 8.0234841835, 7.5941456600, 7.2087667472, 6.8609419394,
-6.5454526113, 6.2580042926, 5.9950307712, 5.7535460007, 5.5310307326,
-5.3253447326, 5.1346580881, 4.9573969333, 4.7922001805, 4.6378847381,
-4.4934173364, 4.3578915401, 4.2305088671, 4.1105631832, 3.9974277273,
-3.8905442644, 3.7894139705, 3.6935897344, 3.6026696258, 3.5162913303,
-3.4341273861, 3.3558810921, 3.2812829795, 3.2100877580, 3.1420716642,
-3.0770301506, 3.0147758675, 2.9551368928, 2.8979551762, 2.8430851689,
-2.7903926111, 2.7397534594, 2.6910529333, 2.6441846666, 2.5990499509,
-2.5555570588, 2.5136206380, 2.4731611670, 2.4341044661, 2.3963812565,
-2.3599267625, 2.3246803517, 2.2905852094, 2.2575880428, 2.2256388129,
-2.1946904899, 2.1646988309, 2.1356221764, 2.1074212653, 2.0800590642,
-2.0535006127, 2.0277128801, 2.0026646344, 1.9783263219, 1.9546699563,
-1.9316690161, 1.9092983502, 1.8875340914, 1.8663535750, 1.8457352644,
-1.8256586826, 1.8061043472, 1.7870537115, 1.7684891092, 1.7503937029,
-1.7327514359, 1.7155469886, 1.6987657360, 1.6823937094, 1.6664175601,
-1.6508245260, 1.6356023991, 1.6207394968, 1.6062246337, 1.5920470957,
-1.5781966157, 1.5646633508, 1.5514378607, 1.5385110875, 1.5258743370,
-1.5135192604, 1.5014378379, 1.4896223627, 1.4780654261, 1.4667599035,
-1.4556989409, 1.4448759425, 1.4342845592, 1.4239186765, 1.4137724050,
-1.4038400696, 1.3941162003, 1.3845955233, 1.3752729523, 1.3661435806,
-1.3572026733, 1.3484456602, 1.3398681287, 1.3314658175, 1.3232346102,
-1.3151705296, 1.3072697317, 1.2995285007, 1.2919432439, 1.2845104866,
-1.2772268677, 1.2700891354, 1.2630941425, 1.2562388432, 1.2495202886,
-1.2429356234, 1.2364820824, 1.2301569870, 1.2239577424, 1.2178818341,
-1.2119268257, 1.2060903552, 1.2003701333, 1.1947639402, 1.1892696236,
-1.1838850960, 1.1786083331, 1.1734373709, 1.1683703044, 1.1634052849,
-1.1585405190, 1.1537742661, 1.1491048371, 1.1445305925, 1.1400499409,
-1.1356613378, 1.1313632835, 1.1271543224, 1.1230330412, 1.1189980678,
-1.1150480699, 1.1111817542, 1.1073978648, 1.1036951824, 1.1000725232,
-1.0965287377, 1.0930627101, 1.0896733570, 1.0863596268, 1.0831204985,
-1.0799549813, 1.0768621133, 1.0738409614, 1.0708906196, 1.0680102095,
-1.0651988785, 1.0624557997, 1.0597801715, 1.0571712163, 1.0546281804,
-1.0521503335, 1.0497369679, 1.0473873979, 1.0451009599, 1.0428770110,
-1.0407149295, 1.0386141136, 1.0365739817, 1.0345939715, 1.0326735396,
-1.0308121617, 1.0290093316, 1.0272645609, 1.0255773792, 1.0239473333,
-1.0223739870, 1.0208569207, 1.0193957316, 1.0179900327, 1.0166394533,
-1.0153436380, 1.0141022473, 1.0129149566, 1.0117814565, 1.0107014523,
-1.0096746639, 1.0087008260, 1.0077796872, 1.0069110104, 1.0060945724,
-1.0053301640, 1.0046175896, 1.0039566671, 1.0033472281, 1.0027891173,
-1.0022821928, 1.0018263259, 1.0014214010, 1.0010673155, 1.0007639796,
-1.0005113168, 1.0003092630, 1.0001577673, 1.0000567915, 1.0000063099,
-1.0000063099, 1.0000567915, 1.0001577673, 1.0003092630, 1.0005113168,
-1.0007639796, 1.0010673155, 1.0014214010, 1.0018263259, 1.0022821928,
-1.0027891173, 1.0033472281, 1.0039566671, 1.0046175896, 1.0053301640,
-1.0060945724, 1.0069110104, 1.0077796872, 1.0087008260, 1.0096746639,
-1.0107014523, 1.0117814565, 1.0129149566, 1.0141022473, 1.0153436380,
-1.0166394533, 1.0179900327, 1.0193957316, 1.0208569207, 1.0223739870,
-1.0239473333, 1.0255773792, 1.0272645609, 1.0290093316, 1.0308121617,
-1.0326735396, 1.0345939715, 1.0365739817, 1.0386141136, 1.0407149295,
-1.0428770110, 1.0451009599, 1.0473873979, 1.0497369679, 1.0521503335,
-1.0546281804, 1.0571712163, 1.0597801715, 1.0624557997, 1.0651988785,
-1.0680102095, 1.0708906196, 1.0738409614, 1.0768621133, 1.0799549813,
-1.0831204985, 1.0863596268, 1.0896733570, 1.0930627101, 1.0965287377,
-1.1000725232, 1.1036951824, 1.1073978648, 1.1111817542, 1.1150480699,
-1.1189980678, 1.1230330412, 1.1271543224, 1.1313632835, 1.1356613378,
-1.1400499409, 1.1445305925, 1.1491048371, 1.1537742661, 1.1585405190,
-1.1634052849, 1.1683703044, 1.1734373709, 1.1786083331, 1.1838850960,
-1.1892696236, 1.1947639402, 1.2003701333, 1.2060903552, 1.2119268257,
-1.2178818341, 1.2239577424, 1.2301569870, 1.2364820824, 1.2429356234,
-1.2495202886, 1.2562388432, 1.2630941425, 1.2700891354, 1.2772268677,
-1.2845104866, 1.2919432439, 1.2995285007, 1.3072697317, 1.3151705296,
-1.3232346102, 1.3314658175, 1.3398681287, 1.3484456602, 1.3572026733,
-1.3661435806, 1.3752729523, 1.3845955233, 1.3941162003, 1.4038400696,
-1.4137724050, 1.4239186765, 1.4342845592, 1.4448759425, 1.4556989409,
-1.4667599035, 1.4780654261, 1.4896223627, 1.5014378379, 1.5135192604,
-1.5258743370, 1.5385110875, 1.5514378607, 1.5646633508, 1.5781966157,
-1.5920470957, 1.6062246337, 1.6207394968, 1.6356023991, 1.6508245260,
-1.6664175601, 1.6823937094, 1.6987657360, 1.7155469886, 1.7327514359,
-1.7503937029, 1.7684891092, 1.7870537115, 1.8061043472, 1.8256586826,
-1.8457352644, 1.8663535750, 1.8875340914, 1.9092983502, 1.9316690161,
-1.9546699563, 1.9783263219, 2.0026646344, 2.0277128801, 2.0535006127,
-2.0800590642, 2.1074212653, 2.1356221764, 2.1646988309, 2.1946904899,
-2.2256388129, 2.2575880428, 2.2905852094, 2.3246803517, 2.3599267625,
-2.3963812565, 2.4341044661, 2.4731611670, 2.5136206380, 2.5555570588,
-2.5990499509, 2.6441846666, 2.6910529333, 2.7397534594, 2.7903926111,
-2.8430851689, 2.8979551762, 2.9551368928, 3.0147758675, 3.0770301506,
-3.1420716642, 3.2100877580, 3.2812829795, 3.3558810921, 3.4341273861,
-3.5162913303, 3.6026696258, 3.6935897344, 3.7894139705, 3.8905442644,
-3.9974277273, 4.1105631832, 4.2305088671, 4.3578915401, 4.4934173364,
-4.6378847381, 4.7922001805, 4.9573969333, 5.1346580881, 5.3253447326,
-5.5310307326, 5.7535460007, 5.9950307712, 6.2580042926, 6.5454526113,
-6.8609419394, 7.2087667472, 7.5941456600, 8.0234841835, 8.5047324589,
-9.0478807252, 9.6656585634, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000,
-10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000, 10.0000000000};
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, 9.6656585634, 9.0478807252,
+ 8.5047324589, 8.0234841835, 7.5941456600, 7.2087667472, 6.8609419394,
+ 6.5454526113, 6.2580042926, 5.9950307712, 5.7535460007, 5.5310307326,
+ 5.3253447326, 5.1346580881, 4.9573969333, 4.7922001805, 4.6378847381,
+ 4.4934173364, 4.3578915401, 4.2305088671, 4.1105631832, 3.9974277273,
+ 3.8905442644, 3.7894139705, 3.6935897344, 3.6026696258, 3.5162913303,
+ 3.4341273861, 3.3558810921, 3.2812829795, 3.2100877580, 3.1420716642,
+ 3.0770301506, 3.0147758675, 2.9551368928, 2.8979551762, 2.8430851689,
+ 2.7903926111, 2.7397534594, 2.6910529333, 2.6441846666, 2.5990499509,
+ 2.5555570588, 2.5136206380, 2.4731611670, 2.4341044661, 2.3963812565,
+ 2.3599267625, 2.3246803517, 2.2905852094, 2.2575880428, 2.2256388129,
+ 2.1946904899, 2.1646988309, 2.1356221764, 2.1074212653, 2.0800590642,
+ 2.0535006127, 2.0277128801, 2.0026646344, 1.9783263219, 1.9546699563,
+ 1.9316690161, 1.9092983502, 1.8875340914, 1.8663535750, 1.8457352644,
+ 1.8256586826, 1.8061043472, 1.7870537115, 1.7684891092, 1.7503937029,
+ 1.7327514359, 1.7155469886, 1.6987657360, 1.6823937094, 1.6664175601,
+ 1.6508245260, 1.6356023991, 1.6207394968, 1.6062246337, 1.5920470957,
+ 1.5781966157, 1.5646633508, 1.5514378607, 1.5385110875, 1.5258743370,
+ 1.5135192604, 1.5014378379, 1.4896223627, 1.4780654261, 1.4667599035,
+ 1.4556989409, 1.4448759425, 1.4342845592, 1.4239186765, 1.4137724050,
+ 1.4038400696, 1.3941162003, 1.3845955233, 1.3752729523, 1.3661435806,
+ 1.3572026733, 1.3484456602, 1.3398681287, 1.3314658175, 1.3232346102,
+ 1.3151705296, 1.3072697317, 1.2995285007, 1.2919432439, 1.2845104866,
+ 1.2772268677, 1.2700891354, 1.2630941425, 1.2562388432, 1.2495202886,
+ 1.2429356234, 1.2364820824, 1.2301569870, 1.2239577424, 1.2178818341,
+ 1.2119268257, 1.2060903552, 1.2003701333, 1.1947639402, 1.1892696236,
+ 1.1838850960, 1.1786083331, 1.1734373709, 1.1683703044, 1.1634052849,
+ 1.1585405190, 1.1537742661, 1.1491048371, 1.1445305925, 1.1400499409,
+ 1.1356613378, 1.1313632835, 1.1271543224, 1.1230330412, 1.1189980678,
+ 1.1150480699, 1.1111817542, 1.1073978648, 1.1036951824, 1.1000725232,
+ 1.0965287377, 1.0930627101, 1.0896733570, 1.0863596268, 1.0831204985,
+ 1.0799549813, 1.0768621133, 1.0738409614, 1.0708906196, 1.0680102095,
+ 1.0651988785, 1.0624557997, 1.0597801715, 1.0571712163, 1.0546281804,
+ 1.0521503335, 1.0497369679, 1.0473873979, 1.0451009599, 1.0428770110,
+ 1.0407149295, 1.0386141136, 1.0365739817, 1.0345939715, 1.0326735396,
+ 1.0308121617, 1.0290093316, 1.0272645609, 1.0255773792, 1.0239473333,
+ 1.0223739870, 1.0208569207, 1.0193957316, 1.0179900327, 1.0166394533,
+ 1.0153436380, 1.0141022473, 1.0129149566, 1.0117814565, 1.0107014523,
+ 1.0096746639, 1.0087008260, 1.0077796872, 1.0069110104, 1.0060945724,
+ 1.0053301640, 1.0046175896, 1.0039566671, 1.0033472281, 1.0027891173,
+ 1.0022821928, 1.0018263259, 1.0014214010, 1.0010673155, 1.0007639796,
+ 1.0005113168, 1.0003092630, 1.0001577673, 1.0000567915, 1.0000063099,
+ 1.0000063099, 1.0000567915, 1.0001577673, 1.0003092630, 1.0005113168,
+ 1.0007639796, 1.0010673155, 1.0014214010, 1.0018263259, 1.0022821928,
+ 1.0027891173, 1.0033472281, 1.0039566671, 1.0046175896, 1.0053301640,
+ 1.0060945724, 1.0069110104, 1.0077796872, 1.0087008260, 1.0096746639,
+ 1.0107014523, 1.0117814565, 1.0129149566, 1.0141022473, 1.0153436380,
+ 1.0166394533, 1.0179900327, 1.0193957316, 1.0208569207, 1.0223739870,
+ 1.0239473333, 1.0255773792, 1.0272645609, 1.0290093316, 1.0308121617,
+ 1.0326735396, 1.0345939715, 1.0365739817, 1.0386141136, 1.0407149295,
+ 1.0428770110, 1.0451009599, 1.0473873979, 1.0497369679, 1.0521503335,
+ 1.0546281804, 1.0571712163, 1.0597801715, 1.0624557997, 1.0651988785,
+ 1.0680102095, 1.0708906196, 1.0738409614, 1.0768621133, 1.0799549813,
+ 1.0831204985, 1.0863596268, 1.0896733570, 1.0930627101, 1.0965287377,
+ 1.1000725232, 1.1036951824, 1.1073978648, 1.1111817542, 1.1150480699,
+ 1.1189980678, 1.1230330412, 1.1271543224, 1.1313632835, 1.1356613378,
+ 1.1400499409, 1.1445305925, 1.1491048371, 1.1537742661, 1.1585405190,
+ 1.1634052849, 1.1683703044, 1.1734373709, 1.1786083331, 1.1838850960,
+ 1.1892696236, 1.1947639402, 1.2003701333, 1.2060903552, 1.2119268257,
+ 1.2178818341, 1.2239577424, 1.2301569870, 1.2364820824, 1.2429356234,
+ 1.2495202886, 1.2562388432, 1.2630941425, 1.2700891354, 1.2772268677,
+ 1.2845104866, 1.2919432439, 1.2995285007, 1.3072697317, 1.3151705296,
+ 1.3232346102, 1.3314658175, 1.3398681287, 1.3484456602, 1.3572026733,
+ 1.3661435806, 1.3752729523, 1.3845955233, 1.3941162003, 1.4038400696,
+ 1.4137724050, 1.4239186765, 1.4342845592, 1.4448759425, 1.4556989409,
+ 1.4667599035, 1.4780654261, 1.4896223627, 1.5014378379, 1.5135192604,
+ 1.5258743370, 1.5385110875, 1.5514378607, 1.5646633508, 1.5781966157,
+ 1.5920470957, 1.6062246337, 1.6207394968, 1.6356023991, 1.6508245260,
+ 1.6664175601, 1.6823937094, 1.6987657360, 1.7155469886, 1.7327514359,
+ 1.7503937029, 1.7684891092, 1.7870537115, 1.8061043472, 1.8256586826,
+ 1.8457352644, 1.8663535750, 1.8875340914, 1.9092983502, 1.9316690161,
+ 1.9546699563, 1.9783263219, 2.0026646344, 2.0277128801, 2.0535006127,
+ 2.0800590642, 2.1074212653, 2.1356221764, 2.1646988309, 2.1946904899,
+ 2.2256388129, 2.2575880428, 2.2905852094, 2.3246803517, 2.3599267625,
+ 2.3963812565, 2.4341044661, 2.4731611670, 2.5136206380, 2.5555570588,
+ 2.5990499509, 2.6441846666, 2.6910529333, 2.7397534594, 2.7903926111,
+ 2.8430851689, 2.8979551762, 2.9551368928, 3.0147758675, 3.0770301506,
+ 3.1420716642, 3.2100877580, 3.2812829795, 3.3558810921, 3.4341273861,
+ 3.5162913303, 3.6026696258, 3.6935897344, 3.7894139705, 3.8905442644,
+ 3.9974277273, 4.1105631832, 4.2305088671, 4.3578915401, 4.4934173364,
+ 4.6378847381, 4.7922001805, 4.9573969333, 5.1346580881, 5.3253447326,
+ 5.5310307326, 5.7535460007, 5.9950307712, 6.2580042926, 6.5454526113,
+ 6.8609419394, 7.2087667472, 7.5941456600, 8.0234841835, 8.5047324589,
+ 9.0478807252, 9.6656585634, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf,
+ inf, inf, inf, inf, inf};
static double *plane_scan MAYBE_UNUSED = __plane_scan;
#endif
diff --git a/test/integration/sdf.cc b/test/integration/sdf.cc
index a0a2d6d..2b699b3 100644
--- a/test/integration/sdf.cc
+++ b/test/integration/sdf.cc
@@ -14,7 +14,7 @@
* limitations under the License.
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class SDFTest : public ServerFixture
diff --git a/test/integration/sensor.cc b/test/integration/sensor.cc
index 78c335c..eefc286 100644
--- a/test/integration/sensor.cc
+++ b/test/integration/sensor.cc
@@ -15,8 +15,9 @@
*
*/
#include <string.h>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
+using namespace gazebo;
class SensorTest : public ServerFixture
{
};
diff --git a/test/integration/server_fixture.cc b/test/integration/server_fixture.cc
index 479222b..801b26f 100644
--- a/test/integration/server_fixture.cc
+++ b/test/integration/server_fixture.cc
@@ -15,8 +15,8 @@
*
*/
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
class ServerFixtureTest : public ServerFixture,
diff --git a/test/integration/sim_events.cc b/test/integration/sim_events.cc
index 1cf9b21..589dc16 100644
--- a/test/integration/sim_events.cc
+++ b/test/integration/sim_events.cc
@@ -14,15 +14,12 @@
* limitations under the License.
*
*/
-
-
#include <limits>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/msgs/msgs.hh"
-// #include "gazebo/physics/physics.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
@@ -37,6 +34,7 @@ class SimEventsTest : public ServerFixture,
public: void SimPauseRun(const std::string &_physicsEngine);
public: void SpawnAndDeleteModel(const std::string &_physicsEngine);
public: void ModelInAndOutOfRegion(const std::string &_physicsEngine);
+ public: void OccupiedEventSource(const std::string &_physicsEngine);
};
// globals to exchange data between threads
@@ -96,12 +94,6 @@ unsigned int WaitForNewEvent(unsigned int current,
return GetEventCount();
}
-// test macro
-TEST_P(SimEventsTest, ModelInAnOutOfRegion)
-{
- ModelInAndOutOfRegion(GetParam());
-}
-
////////////////////////////////////////////////////////////////////////
// SimPauseRun:
// Load test world, pause, run and verify that events are generated.
@@ -130,12 +122,6 @@ void SimEventsTest::SimPauseRun(const std::string &_physicsEngine)
EXPECT_GT(count_after, count_before);
}
-// test macro
-TEST_P(SimEventsTest, SimPauseRun)
-{
- SimPauseRun(GetParam());
-}
-
////////////////////////////////////////////////////////////////////////
// SpawnAndDeleteModel
// Load test world, add/delete models and verify that events are
@@ -169,11 +155,6 @@ void SimEventsTest::SpawnAndDeleteModel(const std::string &_physicsEngine)
EXPECT_EQ(GetEventType(), "existence");
}
-// test macro
-TEST_P(SimEventsTest, SpawnAndDeleteModel)
-{
- SpawnAndDeleteModel(GetParam());
-}
////////////////////////////////////////////////////////////////////////
// ModelInAndOutOfRegion:
@@ -208,9 +189,67 @@ void SimEventsTest::ModelInAndOutOfRegion(const std::string &_physicsEngine)
EXPECT_GT(countAfter2, countBefore2);
}
-// magic macro
+////////////////////////////////////////////////////////////////////////
+// OccupiedEventSource:
+// Load test world, move model and verify that events are generated by
+// checking the position of the elevator.
+////////////////////////////////////////////////////////////////////////
+void SimEventsTest::OccupiedEventSource(const std::string &_physicsEngine)
+{
+ // simbody stepTo() failure
+ if (SKIP_FAILING_TESTS && _physicsEngine != "ode") return;
+
+ this->Load("worlds/elevator.world", false, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+
+ // Get the elevator model
+ physics::ModelPtr elevatorModel = world->GetModel("elevator");
+
+ gzdbg << "Elevator Pose1["
+ << elevatorModel->GetWorldPose().pos << "]\n";
+
+ // Make sure the elevator is on the ground level
+ EXPECT_LT(elevatorModel->GetWorldPose().pos.z, 0.08);
+ EXPECT_GT(elevatorModel->GetWorldPose().pos.z, 0.07);
+
+ // Spawn a box on the second floor, which should call the elevator up.
+ this->SpawnBox("_my_test_box_", math::Vector3(0.5, 0.5, 0.5),
+ math::Vector3(2, 0, 3.65), math::Vector3(0, 0, 0));
+
+ // Wait for elevator to move. 10 seconds is more than long enough.
+ common::Time::Sleep(10);
+
+ gzdbg << "Elevator Pose2["
+ << elevatorModel->GetWorldPose().pos << "]\n";
+
+ // Make sure the elevator has moved up to the second floor.
+ EXPECT_LT(elevatorModel->GetWorldPose().pos.z, 3.08);
+ EXPECT_GT(elevatorModel->GetWorldPose().pos.z, 3.05);
+}
+
+// Run all test cases
INSTANTIATE_TEST_CASE_P(PhysicsEngines, SimEventsTest, PHYSICS_ENGINE_VALUES);
+TEST_P(SimEventsTest, SimPauseRun)
+{
+ SimPauseRun(GetParam());
+}
+
+TEST_P(SimEventsTest, SpawnAndDeleteModel)
+{
+ SpawnAndDeleteModel(GetParam());
+}
+
+TEST_P(SimEventsTest, ModelInAnOutOfRegion)
+{
+ ModelInAndOutOfRegion(GetParam());
+}
+
+TEST_P(SimEventsTest, OccupiedEventSource)
+{
+ OccupiedEventSource(GetParam());
+}
+
// main, where we can specify to skip certain tests
int main(int argc, char **argv)
{
@@ -219,7 +258,6 @@ int main(int argc, char **argv)
std::string skipStr = argv[1];
if (skipStr == "no_skip")
{
- std::cout << "Not skipping failing tests" << std::endl;
SKIP_FAILING_TESTS = false;
}
}
diff --git a/test/integration/speed.cc b/test/integration/speed.cc
index 692694a..1f500d0 100644
--- a/test/integration/speed.cc
+++ b/test/integration/speed.cc
@@ -14,17 +14,31 @@
* limitations under the License.
*
*/
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
class SpeedTest : public ServerFixture,
public testing::WithParamInterface<const char*>
{
+ /// \brief Spawn 500 spheres into empty.world and
+ /// verify that real-time factor is fast enough.
+ /// \param[in] _physicsEngine Physics engine to use.
public: void BallTest(const std::string &_physicsEngine);
+
+ /// \brief Spawn 500 spheres into shapes.world and
+ /// verify that real-time factor is fast enough.
+ /// \param[in] _physicsEngine Physics engine to use.
public: void ShapesWorld(const std::string &_physicsEngine);
+
+ /// \brief Unthrottle real-time update rate and call World::Step
+ /// in an empty world.
+ /// Verify that it goes at least 2 times faster than real-time.
+ /// \param[in] _physicsEngine Physics engine to use.
+ public: void UnthrottledStep(const std::string &_physicsEngine);
};
+//////////////////////////////////////////////////
void SpeedTest::BallTest(const std::string &_physicsEngine)
{
Load("worlds/empty.world", false, _physicsEngine);
@@ -65,6 +79,7 @@ TEST_P(SpeedTest, BallTest)
BallTest(GetParam());
}
+//////////////////////////////////////////////////
void SpeedTest::ShapesWorld(const std::string &_physicsEngine)
{
Load("worlds/shapes.world", false, _physicsEngine);
@@ -103,6 +118,40 @@ TEST_P(SpeedTest, ShapesWorld)
ShapesWorld(GetParam());
}
+//////////////////////////////////////////////////
+void SpeedTest::UnthrottledStep(const std::string &_physicsEngine)
+{
+ Load("worlds/empty.world", true, _physicsEngine);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // Unthrottle physics updates
+ physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+ ASSERT_TRUE(physics != NULL);
+ physics->SetRealTimeUpdateRate(0.0);
+ double dt = physics->GetMaxStepSize();
+
+ const unsigned int steps = 2000;
+ common::Time startTime = common::Time::GetWallTime();
+ for (unsigned int i = 0; i < steps; ++i)
+ {
+ world->Step(1);
+ }
+ common::Time runTime = common::Time::GetWallTime() - startTime;
+
+ double realTimeFactor = dt * steps / runTime.Double();
+ gzdbg << "realTimeFactor " << realTimeFactor << std::endl;
+ EXPECT_GT(realTimeFactor, 2.0);
+
+ RecordProperty("engine", _physicsEngine);
+ this->Record("realTimeFactor", realTimeFactor);
+}
+
+TEST_P(SpeedTest, UnthrottledStep)
+{
+ UnthrottledStep(GetParam());
+}
+
INSTANTIATE_TEST_CASE_P(PhysicsEngines, SpeedTest, PHYSICS_ENGINE_VALUES);
int main(int argc, char **argv)
diff --git a/test/integration/speed_pr2.cc b/test/integration/speed_pr2.cc
index ed5b2c0..41352bb 100644
--- a/test/integration/speed_pr2.cc
+++ b/test/integration/speed_pr2.cc
@@ -14,8 +14,8 @@
* limitations under the License.
*
*/
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
class SpeedPR2Test : public ServerFixture,
diff --git a/test/integration/speed_thread_pr2.cc b/test/integration/speed_thread_pr2.cc
new file mode 100644
index 0000000..34deb81
--- /dev/null
+++ b/test/integration/speed_thread_pr2.cc
@@ -0,0 +1,116 @@
+/*
+ * 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 <boost/filesystem.hpp>
+#include "gazebo/util/Diagnostics.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+
+using namespace gazebo;
+class SpeedThreadPR2Test : public ServerFixture
+{
+};
+
+/////////////////////////////////////////////////
+// Get timing information from World::Step()
+// \param[in] _world Pointer to the world
+// \param[out] _avgTime Average duration of a World::Step
+// \param[out] _maxTime Max duration of a World::Step
+// \param[out] _minTime Min duration of a World::Step
+void stats(physics::WorldPtr _world, common::Time &_avgTime,
+ common::Time &_maxTime, common::Time &_minTime)
+{
+ common::Timer timer;
+ common::Time timeLap = common::Time::Zero;
+
+ _avgTime = common::Time::Zero;
+ _maxTime = common::Time::Zero;
+ _minTime.Set(GZ_INT32_MAX, 0);
+
+ int repetitions = 3;
+ int steps = 5000;
+
+ for (int i = 0; i < repetitions; ++i)
+ {
+ // Time the world for 5000 iterations
+ timer.Reset();
+ timer.Start();
+ _world->Step(steps);
+ timer.Stop();
+
+ timeLap = timer.GetElapsed();
+ _avgTime += timeLap;
+ if (timeLap >= _maxTime)
+ _maxTime = timeLap;
+ if (timeLap <= _minTime)
+ _minTime = timeLap;
+ }
+
+ _avgTime = _avgTime.Double() / (repetitions * steps);
+}
+
+/////////////////////////////////////////////////
+// Test ODE threaded position correction.
+TEST_F(SpeedThreadPR2Test, PR2SplitImpulseWorld)
+{
+ Load("worlds/pr2_no_sensors_test.world", true);
+
+ // Get a pointer to the world
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // Get a pointer to the physics engine
+ physics::PhysicsEnginePtr physicsEngine = world->GetPhysicsEngine();
+ ASSERT_TRUE(physicsEngine != NULL);
+
+ // Unleash the physics engine to maximum speed.
+ physicsEngine->SetRealTimeUpdateRate(0);
+
+ // Let things settle
+ world->Step(5000);
+
+ // Make sure threading is off
+ physicsEngine->SetParam("thread_position_correction", false);
+
+ // Collect base-line statistics (no threading)
+ common::Time baseAvgTime, baseMaxTime, baseMinTime;
+ stats(world, baseAvgTime, baseMaxTime, baseMinTime);
+
+ std::cout << "Base Time\n";
+ std::cout << "\t Avg[" << baseAvgTime << "]\n"
+ << "\t Max[" << baseMaxTime << "]\n"
+ << "\t Min[" << baseMinTime << "]\n";
+
+
+ // Turn on threading
+ physicsEngine->SetParam("thread_position_correction", true);
+
+ // Collect threaded statistics
+ common::Time threadAvgTime, threadMaxTime, threadMinTime;
+ stats(world, threadAvgTime, threadMaxTime, threadMinTime);
+
+ std::cout << "Thread Time\n";
+ std::cout << "\t Avg[" << threadAvgTime << "]\n"
+ << "\t Max[" << threadMaxTime << "]\n"
+ << "\t Min[" << threadMinTime << "]\n";
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/state_log.cc b/test/integration/state_log.cc
index fdfa32b..4740ffc 100644
--- a/test/integration/state_log.cc
+++ b/test/integration/state_log.cc
@@ -20,7 +20,7 @@
#include "gazebo/common/SystemPaths.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/transport/transport.hh"
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class StateLogTest : public ServerFixture
diff --git a/test/integration/stress_spawn_models.cc b/test/integration/stress_spawn_models.cc
index 7cde18b..151df8e 100644
--- a/test/integration/stress_spawn_models.cc
+++ b/test/integration/stress_spawn_models.cc
@@ -17,7 +17,7 @@
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/sensors/sensors.hh"
#include "gazebo/common/common.hh"
diff --git a/test/integration/surface_properties.cc b/test/integration/surface_properties.cc
index 3d6bc92..e56888a 100644
--- a/test/integration/surface_properties.cc
+++ b/test/integration/surface_properties.cc
@@ -15,10 +15,10 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/msgs/msgs.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
const double g_physics_tol = 1e-2;
@@ -28,6 +28,7 @@ class SurfaceTest : public ServerFixture,
public testing::WithParamInterface<const char*>
{
public: void CollideWithoutContact(const std::string &_physicsEngine);
+ public: void CollideBitmask(const std::string &_physicsEngine);
};
////////////////////////////////////////////////////////////////////////
@@ -163,16 +164,104 @@ void SurfaceTest::CollideWithoutContact(const std::string &_physicsEngine)
}
}
+////////////////////////////////////////////////////////////////////////
+// CollideBitmask:
+// Load the collide_bitmask test world. It drops three boxes onto
+// a ground plane. Each model has the following bitmask
+// - ground_plane: 0xff
+// - box1: 0x01
+// - box2: 0x02
+// - box3: 0x03
+// This set of bitmasks will make box1 collide with the ground plane,
+// box2 to pass through box1 and collide with the ground plane, and
+// box3 will collide with both box1 and box2.
+////////////////////////////////////////////////////////////////////////
+void SurfaceTest::CollideBitmask(const std::string &_physicsEngine)
+{
+ // load an empty world
+ Load("worlds/collide_bitmask.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();
+ // Assume gravity vector points down z axis only.
+ EXPECT_EQ(g.x, 0);
+ EXPECT_EQ(g.y, 0);
+ EXPECT_LE(g.z, -9.8);
+
+ // get physics time step
+ double dt = physics->GetMaxStepSize();
+ EXPECT_GT(dt, 0);
+
+ // get pointers to the falling boxes.
+ physics::ModelPtr box1, box2, box3, box4;
+ box1 = world->GetModel("box1");
+ box2 = world->GetModel("box2");
+ box3 = world->GetModel("box3");
+ box4 = world->GetModel("box4");
+ ASSERT_TRUE(box1 != NULL);
+ ASSERT_TRUE(box2 != NULL);
+ ASSERT_TRUE(box3 != NULL);
+ ASSERT_TRUE(box4 != NULL);
+
+ // Step forward 0.2 s
+ double stepTime = 0.2;
+ unsigned int steps = floor(stepTime / dt);
+ world->Step(steps);
+
+ // Expect boxes to be falling
+ double fallVelocity = g.z * stepTime;
+ EXPECT_LT(box1->GetWorldLinearVel().z, fallVelocity*(1-g_physics_tol));
+ EXPECT_LT(box2->GetWorldLinearVel().z, fallVelocity*(1-g_physics_tol));
+ EXPECT_LT(box3->GetWorldLinearVel().z, fallVelocity*(1-g_physics_tol));
+ EXPECT_LT(box4->GetWorldLinearVel().z, fallVelocity*(1-g_physics_tol));
+
+ // Another 2000 steps should put the boxes at rest
+ world->Step(2000);
+
+ // Expect 3 boxes to be stationary
+ EXPECT_NEAR(box1->GetWorldLinearVel().z, 0, 1e-3);
+ EXPECT_NEAR(box2->GetWorldLinearVel().z, 0, 1e-3);
+ EXPECT_NEAR(box3->GetWorldLinearVel().z, 0, 1e-3);
+
+ // The first and second boxes should be on the ground plane
+ EXPECT_NEAR(box1->GetWorldPose().pos.z, 0.5, 1e-3);
+ EXPECT_NEAR(box2->GetWorldPose().pos.z, 0.5, 1e-3);
+
+ // The third boxs should be ontop of the firs two boxes
+ EXPECT_NEAR(box3->GetWorldPose().pos.z, 1.5, 1e-3);
+
+ // Expect 4th box to still be falling
+ fallVelocity = g.z * world->GetSimTime().Double();
+ EXPECT_LT(box4->GetWorldLinearVel().z, fallVelocity*(1-g_physics_tol));
+
+ Unload();
+}
+
+/////////////////////////////////////////////////
+// Run the CollidWithContact test
TEST_P(SurfaceTest, CollideWithoutContact)
{
CollideWithoutContact(GetParam());
}
-// This test doesn't yet work in bullet, so we'll declare it only for ode.
+/////////////////////////////////////////////////
+// Run the CollidBitmask test
+TEST_P(SurfaceTest, CollideBitmask)
+{
+ CollideBitmask(GetParam());
+}
+
+// These tests only work with ODE.
// Issue #1038
// INSTANTIATE_TEST_CASE_P(PhysicsEngines, SurfaceTest, PHYSICS_ENGINE_VALUES);
INSTANTIATE_TEST_CASE_P(TestODE, SurfaceTest, ::testing::Values("ode"));
+/////////////////////////////////////////////////
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
diff --git a/test/integration/swarm.cc b/test/integration/swarm.cc
new file mode 100644
index 0000000..fab9cb5
--- /dev/null
+++ b/test/integration/swarm.cc
@@ -0,0 +1,68 @@
+/*
+ * 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/test/ServerFixture.hh"
+#include "gazebo/physics/physics.hh"
+#include "gazebo/test/helper_physics_generator.hh"
+
+using namespace gazebo;
+
+class SwarmTest : public ServerFixture,
+ public testing::WithParamInterface<const char*>
+{
+ public: void FlockingWorld(const std::string &_physicsEngine);
+};
+
+////////////////////////////////////////////////////////////////////////
+// FlockingWorld:
+// Load a world with 1001 robots
+//////////////////////////////////////////////////////////////////////////
+void SwarmTest::FlockingWorld(const std::string &_physicsEngine)
+{
+ // Load an empty world
+ Load("worlds/flocking.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);
+
+ // simulate 1 step
+ world->Step(1);
+ double t = world->GetSimTime().Double();
+ // verify that time moves forward
+ EXPECT_GT(t, 0);
+
+ EXPECT_EQ(world->GetModels().size(), 1003u);
+}
+
+TEST_P(SwarmTest, FlockingWorld)
+{
+ FlockingWorld(GetParam());
+}
+
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, SwarmTest, PHYSICS_ENGINE_VALUES);
+
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/transceiver.cc b/test/integration/transceiver.cc
index f58179b..38b71d0 100644
--- a/test/integration/transceiver.cc
+++ b/test/integration/transceiver.cc
@@ -14,13 +14,14 @@
* limitations under the License.
*
*/
+#include <ignition/math/Rand.hh>
#include <boost/foreach.hpp>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/sensors/sensors.hh"
#include "gazebo/common/common.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
@@ -78,18 +79,19 @@ void TransceiverTest::TxRxEmptySpace(const std::string &_physicsEngine)
Load("worlds/empty.world", true, _physicsEngine);
// Generate a random number [1-10] of transmitters
- int nTransmitters = math::Rand::GetIntUniform(1, 10);
+ int nTransmitters = ignition::math::Rand::IntUniform(1, 10);
for (int i = 0; i < nTransmitters; ++i)
{
- double txFreq = math::Rand::GetDblUniform(this->MinFreq, this->MaxFreq);
+ double txFreq = ignition::math::Rand::DblUniform(this->MinFreq,
+ this->MaxFreq);
std::ostringstream convert;
convert << i;
std::string txModelName = "tx" + convert.str();
std::string txSensorName = "wirelessTransmitter" + convert.str();
std::string txEssid = "osrf" + convert.str();
- double x = math::Rand::GetDblUniform(-this->MaxPos, this->MaxPos);
- double y = math::Rand::GetDblUniform(-this->MaxPos, this->MaxPos);
+ double x = ignition::math::Rand::DblUniform(-this->MaxPos, this->MaxPos);
+ double y = ignition::math::Rand::DblUniform(-this->MaxPos, this->MaxPos);
math::Pose txPose(math::Vector3(x, y, 0.055), math::Quaternion(0, 0, 0));
SpawnWirelessTransmitterSensor(txModelName, txSensorName, txPose.pos,
@@ -178,8 +180,8 @@ void TransceiverTest::TxRxFreqOutOfBounds(const std::string &_physicsEngine)
std::string tx2ModelName = "tx2";
std::string tx2SensorName = "wirelessTransmitter2";
std::string txEssid = "osrf";
- double x = math::Rand::GetDblUniform(-this->MaxPos, this->MaxPos);
- double y = math::Rand::GetDblUniform(-this->MaxPos, this->MaxPos);
+ double x = ignition::math::Rand::DblUniform(-this->MaxPos, this->MaxPos);
+ double y = ignition::math::Rand::DblUniform(-this->MaxPos, this->MaxPos);
math::Pose txPose(math::Vector3(x, y, 0.055), math::Quaternion(0, 0, 0));
SpawnWirelessTransmitterSensor(tx1ModelName, tx1SensorName, txPose.pos,
diff --git a/test/integration/transport.cc b/test/integration/transport.cc
index 2fbc3bb..a695b7a 100644
--- a/test/integration/transport.cc
+++ b/test/integration/transport.cc
@@ -16,7 +16,7 @@
*/
#include <unistd.h>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
diff --git a/test/integration/transport_stress_nodes.cc b/test/integration/transport_stress_nodes.cc
index fb444b0..0adc4f8 100644
--- a/test/integration/transport_stress_nodes.cc
+++ b/test/integration/transport_stress_nodes.cc
@@ -16,7 +16,7 @@
*/
#include <boost/thread.hpp>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
diff --git a/test/integration/transporter.cc b/test/integration/transporter.cc
new file mode 100644
index 0000000..e4a3647
--- /dev/null
+++ b/test/integration/transporter.cc
@@ -0,0 +1,99 @@
+/*
+ * 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/test/ServerFixture.hh"
+#include "gazebo/msgs/msgs.hh"
+
+using namespace gazebo;
+
+// this is the test fixture
+class TransporterTest : public ServerFixture
+{
+};
+
+/////////////////////////////////////////////////
+TEST_F(TransporterTest, Transport)
+{
+ this->Load("worlds/transporter_test.world");
+
+ // Get the world
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // Get the box model
+ physics::ModelPtr boxModel = world->GetModel("box");
+ ASSERT_TRUE(boxModel != NULL);
+
+ // Make sure the starting location is correct.
+ EXPECT_NEAR(boxModel->GetWorldPose().pos.x, 10.0, 1e-3);
+ EXPECT_NEAR(boxModel->GetWorldPose().pos.y, 10.0, 1e-3);
+
+ // Move the box onto the auto transport pad
+ boxModel->SetWorldPose(math::Pose(100.5, 0, 0.5, 0, 0, 0));
+ common::Time::Sleep(common::Time(1, 0));
+
+ // Check that the box transported to the correct location
+ EXPECT_NEAR(boxModel->GetWorldPose().pos.x, 0.0, 1e-3);
+ EXPECT_NEAR(boxModel->GetWorldPose().pos.y, 0.0, 1e-3);
+
+ // Move the box away
+ boxModel->SetWorldPose(math::Pose(10, 10, 0.5, 0, 0, 0));
+ common::Time::Sleep(common::Time(1, 0));
+
+ // Check that the box is in the correct location
+ EXPECT_NEAR(boxModel->GetWorldPose().pos.x, 10.0, 1e-3);
+ EXPECT_NEAR(boxModel->GetWorldPose().pos.y, 10.0, 1e-3);
+
+ // Move the box to the manual transporter pad
+ boxModel->SetWorldPose(math::Pose(-100.5, 0, 0.5, 0, 0, 0));
+ common::Time::Sleep(common::Time(1, 0));
+
+ // Check that the box is in the correct location
+ EXPECT_NEAR(boxModel->GetWorldPose().pos.x, -100.5, 1e-3);
+ EXPECT_NEAR(boxModel->GetWorldPose().pos.y, 0, 1e-3);
+
+ // Trigger the transporter
+ {
+ // Create our node for communication
+ gazebo::transport::NodePtr node(new gazebo::transport::Node());
+ node->Init();
+
+ // Publish to a Gazebo topic
+ gazebo::transport::PublisherPtr pub =
+ node->Advertise<gazebo::msgs::GzString>("~/transporter");
+
+ // Wait for a subscriber to connect
+ pub->WaitForConnection();
+
+ // Convert to a pose message
+ gazebo::msgs::GzString msg;
+ msg.set_data("pad2");
+ pub->Publish(msg);
+ }
+
+ common::Time::Sleep(common::Time(1, 0));
+
+ // Check that the box transported to the correct location
+ EXPECT_NEAR(boxModel->GetWorldPose().pos.x, 0, 1e-3);
+ EXPECT_NEAR(boxModel->GetWorldPose().pos.y, 0, 1e-3);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/world.cc b/test/integration/world.cc
index dbbc429..eb274f3 100644
--- a/test/integration/world.cc
+++ b/test/integration/world.cc
@@ -14,9 +14,10 @@
* limitations under the License.
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
+using namespace gazebo;
class WorldTest : public ServerFixture
{
};
@@ -164,7 +165,9 @@ TEST_F(WorldTest, ModifyLight)
msgs::Light lightMsg;
lightMsg.set_name("test_spot_light");
msgs::Set(lightMsg.mutable_pose(),
- math::Pose(math::Vector3(3, 2, 1), math::Quaternion(0, 1, 0, 0)));
+ ignition::math::Pose3d(
+ ignition::math::Vector3d(3, 2, 1),
+ ignition::math::Quaterniond(0, 1, 0, 0)));
lightPub->Publish(lightMsg);
}
diff --git a/test/integration/world_clone.cc b/test/integration/world_clone.cc
index a3e5d23..b33ce10 100644
--- a/test/integration/world_clone.cc
+++ b/test/integration/world_clone.cc
@@ -14,11 +14,12 @@
* limitations under the License.
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
+using namespace gazebo;
class WorldClone : public ServerFixture
{
};
diff --git a/test/integration/world_entity_below_point.cc b/test/integration/world_entity_below_point.cc
index 5b058df..121be57 100644
--- a/test/integration/world_entity_below_point.cc
+++ b/test/integration/world_entity_below_point.cc
@@ -18,8 +18,8 @@
#include <gtest/gtest.h>
#include "gazebo/physics/PhysicsIface.hh"
#include "gazebo/common/Time.hh"
-#include "test/ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
class WorldTest : public ServerFixture,
diff --git a/test/integration/world_population.cc b/test/integration/world_population.cc
index c76da99..d171286 100644
--- a/test/integration/world_population.cc
+++ b/test/integration/world_population.cc
@@ -17,8 +17,8 @@
#include <string>
#include <vector>
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#include "gazebo/math/Vector3.hh"
using namespace gazebo;
diff --git a/test/integration/world_reset.cc b/test/integration/world_reset.cc
index bbc05fa..13923c1 100644
--- a/test/integration/world_reset.cc
+++ b/test/integration/world_reset.cc
@@ -17,8 +17,8 @@
#include <string.h>
#include "gazebo/physics/physics.hh"
-#include "ServerFixture.hh"
-#include "helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
diff --git a/test/integration/worlds_installed.cc b/test/integration/worlds_installed.cc
index 5cb6ce9..f5d153f 100644
--- a/test/integration/worlds_installed.cc
+++ b/test/integration/worlds_installed.cc
@@ -67,6 +67,7 @@ TEST(WorldsInstalled, checkWorlds)
if (dir_itr->path().filename().extension() == ".world")
{
std::string cmd = "gz sdf --check " + dir_itr->path().string();
+ std::cerr << cmd << std::endl;
std::string result = customExec(cmd);
bool success = boost::algorithm::find_first(result, "Check complete");
diff --git a/test/logs/invalidHeader.log b/test/logs/invalidHeader.log
new file mode 100644
index 0000000..714e3c4
--- /dev/null
+++ b/test/logs/invalidHeader.log
@@ -0,0 +1,23 @@
+<?xml version='1.0'?>
+<gazebo_log_WRONG>
+<header>
+<log_version>1.0</log_version>WRONG
+<gazebo_version>6.0.0</gazebo_version>
+<rand_seed>22889
+</header>
+<chunk encoding='zlib'>
+<![CDATA[eJztWktv4zYQvudX6JZDYUmkni60Bnootue2QI8CI9EOu7Lk6pHE+fWdIUVJlOU6u8i2TeIgtsThcEh+M5wZmkyafGs98LoRVWl9uiV2cLu5SR6rusitku35p9ucb1lXtEC2rKQQu/u2r2i68tZqjwfkETXPWhDBCskHnBlr2rS5Z3n12GxI4hhlxXKoGr5xLdci+LWCr8SRNFWdi+22Qw47tvQHBGmyYmoOPOsKVgMXtfQHuAa6YmNty8uO4RAVBWg1K3d8Q1wXulXvuiaryqZlZQtC1zByXdLVhSi57NGFjvqCrvurY3kN/WRYjfUjQY3EORlKMqC3Wbl2ADMgAAb2PFYg9o4EX77uq5wXvRp2ddWVeXooWMk1+DBe7BFxaCd9w1i/9K3w9XYy4aIQ0gRU7VAeWIBpx6s9b+vjSEIdYrdTCtDKqt6zQmk2cfqSydKIZwk9qB7Ql6WpVGcmNnFOO0+art6yzGy [...]
+</chunk>
+<chunk encoding='zlib'>
+<![CDATA[eJzsve2SK7uNLfgq59/9dWwCBAEywrdfxeE79p3pGHe7oz9ux7z9kASYkkoSBVUyfarKabdbp85WaUOZuQhgAVj4w3/8+X//8n/+8u//8c9/+9f/+T/gd+l//NMf/uM///Sff/nlv//273/98x//9U//8pf/+T/+/Jf//af/+ut/tj/753/543/+87/85Z+AfinB/vOH32//+g///ae//nW8AzMRCdMvEAIBlgR/+P3lz//w73/5018vH5Yipfa/P/z+8u//8C9/+/Nf/vqLGfG3//pff/3LH//tL//65//663/9yx//+5//8//54//603/8pZr1b3/7j/opv6u2/PIr/o4Afvk19J/0//968/KH3/e3/+Gv//yv/6999vXHhN+V9h/7oMj2awH0w7bPCeMvgbR95P/5y1//9n/983/+f/9kb7Ff0Tcm/ZGK/ig4/vQPv99+7w+/b1bd2PbXv/33X/79j+1fXL4 [...]
+</chunk>
+<chunk encoding='zlib'>
+<![CDATA[eJzsve1yLDmOJfgq+W9+ZTUJkgBoVtOvUlazXbPbttVdbf0xbfv2S4KgR4QigoGQk5WSkjU2E3nvlTSUux8HcHBw8Mf/+Kf//cv/+cu//8c//+1f/+f/8H9I/+Mf//gf//nn//zLL//9t3//6z/96V///C9/+Z//45/+8r///F9//c/6b//8L3/6z3/+l7/8o8dffMxO/vfHfzj++o///ee//lW/IgLHGAnTLyE5dM7H8pWXf//jv//lz389fhi4gBFSoD/+w+Xv//gvf/unv/z1Fz3E3/7rf/31L3/6t7/86z/911//61/+9N///J//z5/+15//4y/lWP/2t/8oP+UP5Sy//Ap/iN7/8quTP13/v/pXf/wH+eo//vWf//X/1R99/VPcH3L9n/6cEPXb+k85PqH/Qzp+4v/5y1//9n/983/+f/+o//LLzWf5JcuHh/aBoD+nfPfxfX/8h3qom6P99W///Zd//1P [...]
+</chunk>
+<chunk encoding='zlib'>
+<![CDATA[eJzsveuSNLlxpnkr/U+/PhKAHwCY9ehWaJxla1e2LVImkSPbu18A7ojMyMpEemYA7KrqkM2o+quTUBHxhJ9ed//5v//ybz/9n1/+67///W9//V//4v9A//KvP//33//8919++p+//devf/nTX//8H7/8r3/5yy//9ud//Pr3+rV//48//f3f/+OXf/XxJ09O/ufnP26f/vl//vzrr/odGBIiRuafgAL6gCn9/MfL13/+r1/+/Ov2y4IjJMzJ//zHy+d//o+//eWXX3/SQ/ztH//711/+9J+//PUv//j1H//xp//597//P3/633/+71/Ksf7zb/9dfssfyll++hH+gN7/5No/rv/3D/nw8x/bN//867//9f/V33z9S9wfcvkf1l+D+sPu40f5dei3X/h/fvn1b//Xv//9//tX+Q79hv4D2z9vP/78x+0Hf/5jPdTuaL/+7X9++a8/1U9c/sqAOfqffkD5+cD+p/K [...]
+</chunk>
+<chunk encoding='zlib'>
+<![CDATA[eJzsveuSJL1xJfgq3z/9ahLwCwA34+hVaJwltStbipTpMrJ9+wXgjqjKrCyUZwWgri6FbDT5dddF6Ig44Zdz/Pgf/v3P//Tb//nLv/37P//9b//rH+Lv+B/+8Q///h9/+o+//PZff/+3v/75j3/707/85X/9w5//8k9/+s+//kf72j//yx//45//5S//GMtvkWPo//OH3x9//Yf/+tNf/2rfQVCIKKf8GzIBlBzkD79/+fof/u0vf/rr8csgZMgxcPrD71/+/g//8vc//+Wvv9kh/v6f//uvf/njv/7lb3/+z7/+57/88b/++T/+nz/+7z/9+1/qsf717/9ef8vv6ll++wG/oxh/+xH0T48+/vD7/v1/+Os//+3/tV/++veE30n9n2S/icaPhff/A8vxO//PX/769//rn//j//tH+5bfXn+U/hHtx/j4PfWnj5/7w+/bsW4O99e//9df/u2P7S9e/qnAAeoR4+8 [...]
+</chunk>
+</gazebo_log>
diff --git a/test/logs/state.log b/test/logs/state.log
new file mode 100644
index 0000000..bb6a674
--- /dev/null
+++ b/test/logs/state.log
@@ -0,0 +1,23 @@
+<?xml version='1.0'?>
+<gazebo_log>
+<header>
+<log_version>1.0</log_version>
+<gazebo_version>6.0.0</gazebo_version>
+<rand_seed>27838</rand_seed>
+</header>
+<chunk encoding='zlib'>
+<![CDATA[eJztWktzqzYU3udXsPOiEwOOuXE6CjNddNJ125kuGRmErYZXJeHY/vU9khBIGN+kndzbJvbCNueh1/nOCzDiWe7tCOO0rrzHWTiPZvENeqlZkXkVLsnjLCM5bgsBbM9DBd1sRSfgbTXzxKGROpSRVMAUuFB6oJliLhK+xVn9wuMQ+Q6tVZqakzjwAi+UX7fwhXzF0+KM5nkrNeYrz3xgIsPWSrwhaVtgBloLz3xAq+drNSwEqVost6g5wGO42pA4DAJYVl8bSVpXXOBKwKQPsHNDGXFBK6JWDGChjjCyv1qcMVgnlWIpHxh6J/7JVlBvvfg2mEdwghCMIVceBNL2vjK+uizrjBQdDBtWt1WWNAWuiDE+7FeuKO0grLVhr8/dKHk5sw5cFFS5gJb2dK8CShtSl0Sww8CSGMplbQ7wqpqVuNDIIr+jXBVOj8r0AD1YX1H2rP5oWuSfLo54y3KcugNzRlMb5I4N5nI [...]
+</chunk>
+<chunk encoding='zlib'>
+<![CDATA[eJzsveuSLLtt7/kq+5s/LYs3AGSEjl9FoTOSZxQjWw7bOo55+yEBMKuqu4qFqiS1e/Wm7FCtS3eLKzP/idsPwO//60//+sv/+fN//tdf/vbv/+uf/D/DP/3L7//rv//433/+5X/+9p9//dMf/v2P//bn//VPf/rzv/7x73/97/Z3f/m3P/z3X/7tz/8S8i8JyPF/fv+7449//z9//Otf5Zc+hZK9I8BfEqbkE7r4+99d/v73//nnP/71+GEQCCHFRL//3eXPf/9vf/vTn//6ix7ib3//33/98x/+48///qe///Xv//aH//nLf/8/f/jff/yvP9dj/cff/qv+D/5zPcsvP4J8uMcfv/8df/3v//qXf/9/9Ydf/xz3z6X9R3/S8W2jX8DxM//Pn//6t//rL//9//2L/s0vVx8e+SMU/jbvSv/2+t3H9/3+d+1YN4f769/+58//+Yf2B5d/akil5HZED4TxF//P3od [...]
+</chunk>
+<chunk encoding='zlib'>
+<![CDATA[eJzsvduSLLuNpvkq+66vlkSABAmaqetVZOqReqZsVKWyOnTZvP2QBOgRnhnBgIeTe+fK7ZJVxTpkprjc/XOcfgB/+o+//u9f/s/f/v0//vkf//o//wf8gf7HP/3pP/7zL//5t1/++x///ve//vlf//Ivf/uf/+Ovf/vff/mvv/9n/bt//pc//+c//8vf/gnzL4HYtf/86Y/bH//pv//y97/LLyFgZnCJ0i8hJhdcxPinP97+/k///re//H37YeQdsvMx/emPtz//07/8469/+/sveoh//Nf/+vvf/vxvf/vXv/7X3//rX/783//8n//Pn//XX/7jb+VY//aP/yj/g38oZ/nlB8qHu//Y/eZPf2xf/qe///O//r/6s+9/jPtDLv+J+oNc1G/bvr//uO0PAm4/8v/87e//+L/++T//v3/SL/1l/wn6ie0nAOsPKt++feOf/liPtTvc3//x33/79z/XP7j9SzHkmOs [...]
+</chunk>
+<chunk encoding='zlib'>
+<![CDATA[eJzsve2SJbttLfgq559/tUTwC0CErl9FoTs6nlHMseSwpeuYtx+SALNqf3FjV5LuqhIlh3d3V3WJnZkr8bHWAv7wX3/+t1/+z6//+V9/+dtf/9e/wO/Sv/zrH/7r73/6+6+//Pff/vO3P//xr3/691//17/8+dd/+9M/fvt7/dpf/v2Pf//Lv//6r8H9EhO79p8//P744z/8959++01+CdEzgcNEv8RMmBFC/sPv377+h//89U+/HT8sBe9c+Tb8w+/f/vwP//63P//62y96iL/943//9usf/+PXv/75H7/949//+N9/+fv/88f//af/+rUc6z/+9l/lf/B35Sy//PDy4R5//OH37fv/8Ntf/vr/6g9//3Pc77j8J+tPcln/2vH3HVz/SXz7mf/n19/+9n/95e//37/Kd/Tv1M8gn6w/IRw/qfz942/+4ff1YBfH++1v//3rf/6x/sHbP9Yn5+mXH+UXDtn9Ar+ [...]
+</chunk>
+<chunk encoding='zlib'>
+<![CDATA[eJztvduSJDmOpvkqeddXUUUAJEiK5PSrlNRsZe+WbNZB6jAt+/ZLEqCaqR1oMFdS0iJaa0bGMsI9fOiq+ilOP4Cf//mn//rp//zyj3/++W9//V//Ab8L//GfP//zX3/81y8//fff/vHrn/7w1z/+5Zf/9R9/+uW//vjvX/9Vv/bnv/zhX3/+yy//SfCTZyf/+/n321///N9//PVX+U/wmBO4GPJPPoJzGZl+/v3l6z//45c//rr9sEAefcrAP//+8vc//+Vvf/rl15/0EH/79//+9Zc//P2Xv/7p37/++y9/+O8//+v/+cP//uM/fynH+vvf/ln+P/xdOctP37B9OPnDo4+ff9++/edf//zX/1d/9vWPcb/L5X+sP8ix/Kjtn9//h7/8yP/zy69/+7/+/K//7z/lO366/gD99sunfvXn32//7Off10Ptjvbr3/77l3/8of7F5fdEn7kdMAaK/BP8znvkWH9uQEp [...]
+</chunk>
+</gazebo_log>
\ No newline at end of file
diff --git a/test/logs/state2.log b/test/logs/state2.log
new file mode 100644
index 0000000..966b912
--- /dev/null
+++ b/test/logs/state2.log
@@ -0,0 +1,20 @@
+<?xml version='1.0'?>
+<gazebo_log>
+<header>
+<log_version>1.0</log_version>
+<gazebo_version>6.0.6</gazebo_version>
+<rand_seed>27184</rand_seed>
+</header>
+<chunk encoding='zlib'>
+<![CDATA[eJztWktzqzYU3udXsPOiE4OdcON0FGa66KTrtjNdMjIIWw2vSiKx/et79AIJ45u0k3vbxF7Y5jz0Ot95AUY8L4Jnwjht6uBhtpjHs+QKvTSszIMaV+RhlpMCd6UAdhCgkm62wgh4V88CsW+lDmUkEzAFLpUeaGaYi5Rvcd688GSBQo/WKm3DSRIFUbCQX9fwhULF0+KcFkUnNearwH5gIsvWSrwlWVdiBlrLwH5Aq+drNSwEqTsst6g5wGO43pBkEUWwrL62kqypucC1gEnvYeeWsuKS1kStGMFChrCyvzqcM1gnk2IpHxh6J+HRVlBvveQ6msdwggUYQ648CKTtQ2V8dVk1OSkNDBvWdHWetiWuiTU+7FeuKO0gnLVhr09mlLycOQcuS6pcQEt7ulcBpQ1pKiLYfmBJDOWyLgd4dcMqXGpkUWgoX4XTgzI9QA/WV5Q7aziaFoXHiyPesQJn/sCC0cwF2bDBXD5 [...]
+</chunk>
+<chunk encoding='zlib'>
+<![CDATA[eJzsvduSbLmNpvkqeVdXKREkAIJm2fUqMvUoa0Y2KqlMUnXZvP2QBOgRHtudDvdFKiPClvoQmfuU3L7iWwDx/wB++cef/uOn//Pr3//x57/99X/9G/yO/u3ff/nHP//4z19/+p+//f0vf/rDX//4n7/+r3/706//8cf//ss/28/9+T//8M8//+ev/x7TTznY/375/eWHf/mfP/7lL/qPgClFgZjKT8yMMZQYf/n928//8vdf//iXyx8mTBGFsPzy+7cf/+XP//z173/8Zz3bP+qvye2/9O5HfvnPv/3p17/8ZEf823//77/8+of/+vWvf/rvv/z3f/7hf/78z//nD//7j//4tR76v/72j/rf+F096U8/25fw/svVv/zy+/7Lf/nLn//6/9qf/f6Pgd+V+j+2Pyiw/bYA4x/sj7v8gfj2R/6fX//yt//rz//8//7dfqn9Sv2FRew3sP5w1C9Qf/fl9/3y+3aqq7P [...]
+</chunk>
+<chunk encoding='zlib'>
+<![CDATA[eJzsvW+TJLmN5vlV+t2+aokAAZA0652vItOetHey04zGZjQ7dt/+SID0yH+BQKS7S5WVrrHd7OrKrGZFxOMAHv4A/Paff/rfv/yfP//Hf/7lb//2P/8H/I7/x7/89p9//+Pf//zLf//tP/76pz/82x//9c//83/86c//+4//9de/j9/7y7/+4e9/+dc//wvSLyVB0v/99vvtX//233/861/tH4FyxgpI6RcpgKUIyG+/v/3+b//x5z/+dfvD6vjTqDD99vvbv//tL3//83/88e/9bP/Zv6d/x2+/f/FvfvvXv/3pz3/9ZR7xb//1v/765z/8+5//7U//9df/+tc//Pdf/v7//OF//fE//9wP/e9/+8/+3/hdP+kvv84vSb+8/P/nv/rt9/rdv/31L//2/84/+uWfAr9r/X9l/jmpzB9bf8r2FeZvEG9/4v/581//9n/95e//37/ob6xvQPsKWX8QRL9Umv8W+k9 [...]
+</chunk>
+<chunk encoding='zlib'>
+<![CDATA[eJzsveuSLblt7/kq/c2ftsQLCJARffwqCp1Re0YxsuWwpOOYtx+SAHPdsZArSatqRzp0zureu6qalcw/ceEPwK9/+9O//fJ/fvuvv/35r//xv/7F/y79y7/++re///Hvv/3y33/9r7/86Q//8cd//+1//cuffvu3P/7jL39vf/fnf//D3//877/9a0i/kAuu/9+vv9/++Nf//uNf/sL/6CHGkH0A/wsSUsoY8NffX/7+1//67Y9/2X5YplS/gsD/+vvLn//657//9l9//Htd29/q19T/3q+/v/qTX//9r3/67S+/yBL/+o///Zff/vCfv/3Hn/7xl3/8+x/++89//3/+8L//+Lff6qL/869/q/+N39WV/vJDPlz/uP7/5Y9+/X3/6l//8uf/+H/lR1//FP+7Uv+P5Oe4LN82fsrVJ/8FlO0n/p/f/vLX/+vPf////pW/Ynzn+ML+jZG/H8L2t7/+fvu+X3/fFnW [...]
+</chunk>
+</gazebo_log>
\ No newline at end of file
diff --git a/test/performance/factory_stress.cc b/test/performance/factory_stress.cc
index 7e6e5a7..c1fb18b 100644
--- a/test/performance/factory_stress.cc
+++ b/test/performance/factory_stress.cc
@@ -14,7 +14,7 @@
* limitations under the License.
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class FactoryStressTest : public ServerFixture
diff --git a/test/performance/image_convert_stress.cc b/test/performance/image_convert_stress.cc
index fe50edb..4008165 100644
--- a/test/performance/image_convert_stress.cc
+++ b/test/performance/image_convert_stress.cc
@@ -17,7 +17,7 @@
#include <iostream>
#include <boost/shared_ptr.hpp>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/common/common.hh"
diff --git a/test/performance/sensor_stress.cc b/test/performance/sensor_stress.cc
index 081029a..8c44a4e 100644
--- a/test/performance/sensor_stress.cc
+++ b/test/performance/sensor_stress.cc
@@ -18,7 +18,7 @@
#include <gtest/gtest.h>
#include "gazebo/physics/PhysicsIface.hh"
#include "gazebo/common/Time.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
class SensorStress_TEST : public ServerFixture
diff --git a/test/performance/set_world_pose.cc b/test/performance/set_world_pose.cc
index 82dacb6..260d618 100644
--- a/test/performance/set_world_pose.cc
+++ b/test/performance/set_world_pose.cc
@@ -15,7 +15,7 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
diff --git a/test/performance/transport_stress.cc b/test/performance/transport_stress.cc
index 1522f9d..3f52278 100644
--- a/test/performance/transport_stress.cc
+++ b/test/performance/transport_stress.cc
@@ -16,7 +16,7 @@
*/
#include <boost/thread.hpp>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "RAMLibrary.hh"
using namespace gazebo;
diff --git a/test/pkgconfig/CMakeLists.txt b/test/pkgconfig/CMakeLists.txt
index 5ccf41d..99367da 100644
--- a/test/pkgconfig/CMakeLists.txt
+++ b/test/pkgconfig/CMakeLists.txt
@@ -1,3 +1,3 @@
set (tests config-pkgconfig.cc)
-add_definitions("-DSOURCE_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/plugin\"")
+add_definitions(-DSOURCE_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/plugin\" -DGAZEBO_EXPECTED_VERSION="${GAZEBO_VERSION_FULL}")
gz_build_tests(${tests})
diff --git a/test/pkgconfig/config-pkgconfig.cc b/test/pkgconfig/config-pkgconfig.cc
index b71c39f..5844f77 100644
--- a/test/pkgconfig/config-pkgconfig.cc
+++ b/test/pkgconfig/config-pkgconfig.cc
@@ -16,14 +16,33 @@
*/
#include <gtest/gtest.h>
#include <stdio.h>
+#include "test_config.h"
TEST(PkgConfig, Config)
{
char cmd[1024];
- snprintf(cmd, sizeof(cmd), "cmake %s", SOURCE_DIR);
+ // Delete previous build directory to not reuse previous CMakeCache
+ snprintf(cmd, sizeof(cmd), "rm -fr %s/test/pkgconfig/plugin",
+ PROJECT_BINARY_PATH);
ASSERT_EQ(system(cmd), 0);
- snprintf(cmd, sizeof(cmd), "make");
+
+ // Create a build directory in the project binary directory so that we
+ // don't pollute the source tree
+ snprintf(cmd, sizeof(cmd), "mkdir %s/test/pkgconfig/plugin",
+ PROJECT_BINARY_PATH);
+ ASSERT_EQ(system(cmd), 0);
+
+ // Run cmake
+ snprintf(
+ cmd, sizeof(cmd),
+ "cd %s/test/pkgconfig/plugin; cmake %s -DGAZEBO_EXPECTED_VERSION=%s",
+ PROJECT_BINARY_PATH, SOURCE_DIR, GAZEBO_EXPECTED_VERSION);
+ ASSERT_EQ(system(cmd), 0);
+
+ // Make
+ snprintf(cmd, sizeof(cmd), "cd %s/test/pkgconfig/plugin; make",
+ PROJECT_BINARY_PATH);
ASSERT_EQ(system(cmd), 0);
}
diff --git a/test/pkgconfig/plugin/CMakeLists.txt b/test/pkgconfig/plugin/CMakeLists.txt
index 60b7af8..4c47da0 100644
--- a/test/pkgconfig/plugin/CMakeLists.txt
+++ b/test/pkgconfig/plugin/CMakeLists.txt
@@ -1,15 +1,22 @@
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)
+ string (REPLACE ";" " " GAZEBO_CFLAGS "${GAZEBO_CFLAGS}")
endif()
-include_directories(${GAZEBO_INCLUDE_DIRS})
-link_directories(${GAZEBO_LIBRARY_DIRS})
+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})
+
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CFLAGS}")
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} ${IGNITION-MATH_LIBRARIES} ${Boost_LIBRARIES})
diff --git a/test/plugins/CMakeLists.txt b/test/plugins/CMakeLists.txt
index 0c30eeb..727e0c8 100644
--- a/test/plugins/CMakeLists.txt
+++ b/test/plugins/CMakeLists.txt
@@ -1,3 +1,7 @@
+if (WIN32)
+ add_definitions(-DBUILDING_DLL)
+endif()
+
include_directories(
${PROJECT_SOURCE_DIR}
${PROTOBUF_INCLUDE_DIR}
diff --git a/test/plugins/ForceTorqueModelRemovalTestPlugin.cc b/test/plugins/ForceTorqueModelRemovalTestPlugin.cc
index 56c6d88..25c3c97 100644
--- a/test/plugins/ForceTorqueModelRemovalTestPlugin.cc
+++ b/test/plugins/ForceTorqueModelRemovalTestPlugin.cc
@@ -63,24 +63,24 @@ void ForceTorqueModelRemovalTestPlugin::Load(sensors::SensorPtr _sensor,
void ForceTorqueModelRemovalTestPlugin::onUpdate(
const gazebo::common::UpdateInfo & /*_info*/)
{
- gazebo::math::Vector3 force;
- gazebo::math::Vector3 torque;
+ ignition::math::Vector3d force;
+ ignition::math::Vector3d torque;
if ( parentSensor )
{
- force = this->parentSensor->GetForce();
- torque = this->parentSensor->GetTorque();
+ force = this->parentSensor->Force();
+ torque = this->parentSensor->Torque();
int i = 0;
for (i = 0; i < 3; ++i)
{
- forcetorque_data[0+i] = force[i];
+ this->forcetorque_data[0+i] = force[i];
}
for (i = 0; i < 3; ++i)
{
- forcetorque_data[3+i] = torque[i];
+ this->forcetorque_data[3+i] = torque[i];
}
}
diff --git a/test/plugins/Issue1208Plugin.hh b/test/plugins/Issue1208Plugin.hh
index 0c28db9..3c0d42e 100644
--- a/test/plugins/Issue1208Plugin.hh
+++ b/test/plugins/Issue1208Plugin.hh
@@ -14,6 +14,12 @@
* 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 <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
diff --git a/test/plugins/rest-web/gzclient.bash b/test/plugins/rest-web/gzclient.bash
new file mode 100755
index 0000000..d3d5199
--- /dev/null
+++ b/test/plugins/rest-web/gzclient.bash
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+gzclient --verbose -g libRestUiPlugin.so menu="REST-web" title="test server" label="Login" url="https://localhost:3000"
diff --git a/test/plugins/rest-web/gzserver.bash b/test/plugins/rest-web/gzserver.bash
new file mode 100755
index 0000000..747309b
--- /dev/null
+++ b/test/plugins/rest-web/gzserver.bash
@@ -0,0 +1,4 @@
+#!/bin/bash
+
+gzserver --verbose -s libRestWebPlugin.so ../../worlds/rest_web.world
+
diff --git a/test/plugins/rest-web/key-cert.pem b/test/plugins/rest-web/key-cert.pem
new file mode 100644
index 0000000..7cdc6f2
--- /dev/null
+++ b/test/plugins/rest-web/key-cert.pem
@@ -0,0 +1,15 @@
+-----BEGIN CERTIFICATE-----
+MIICSDCCAbGgAwIBAgIJAOPuucE61HMuMA0GCSqGSIb3DQEBBQUAMD0xCzAJBgNV
+BAYTAlVTMQswCQYDVQQIDAJDQTEhMB8GA1UECgwYSW50ZXJuZXQgV2lkZ2l0cyBQ
+dHkgTHRkMB4XDTE0MDcxNjIyMzQxMFoXDTE0MDgxNTIyMzQxMFowPTELMAkGA1UE
+BhMCVVMxCzAJBgNVBAgMAkNBMSEwHwYDVQQKDBhJbnRlcm5ldCBXaWRnaXRzIFB0
+eSBMdGQwgZ8wDQYJKoZIhvcNAQEBBQADgY0AMIGJAoGBAMwG6P7ljtM2bJz6Yx+h
+CN8TLv3Q4QmhewIe20LLE93yZWvNFUSLAQDqt06Sfu4Uw0A0RUm3+Isp9YxZm94A
+DY6m05NOHimATbxbxig7ysj7mRZWzce5zsjohG4/7UFWJi/rIi5tHM8jCq0ftirr
++wUCe39adaGHW+xM5hLPOMRpAgMBAAGjUDBOMB0GA1UdDgQWBBTMTp2peczFyYeO
+w444nvrk824abjAfBgNVHSMEGDAWgBTMTp2peczFyYeOw444nvrk824abjAMBgNV
+HRMEBTADAQH/MA0GCSqGSIb3DQEBBQUAA4GBAFMSWulzLst9cBJVnqlUMaz9qGlv
+e7Jl/cE4QQ/ntTku+DU7mk3ujOUqZ8tdy1E7QHrXU5mkU2fRR6uKQjviIbQ0GXdp
+RCZ6C9HJevyTXuraEyw4L3OcJRNc4cPOFRpW+/XndX6TeT90ZzpNZsRgR7k6HQQP
+gs1ugDizeWiBgBVU
+-----END CERTIFICATE-----
diff --git a/test/plugins/rest-web/key.pem b/test/plugins/rest-web/key.pem
new file mode 100644
index 0000000..288a579
--- /dev/null
+++ b/test/plugins/rest-web/key.pem
@@ -0,0 +1,15 @@
+-----BEGIN RSA PRIVATE KEY-----
+MIICXQIBAAKBgQDMBuj+5Y7TNmyc+mMfoQjfEy790OEJoXsCHttCyxPd8mVrzRVE
+iwEA6rdOkn7uFMNANEVJt/iLKfWMWZveAA2OptOTTh4pgE28W8YoO8rI+5kWVs3H
+uc7I6IRuP+1BViYv6yIubRzPIwqtH7Yq6/sFAnt/WnWhh1vsTOYSzzjEaQIDAQAB
+AoGABb2cCLAE2MSLQc95woYRbdVoWRPrgQ3eHC0qFjMImBId8G6QzqDqok/4YGmW
+0LQc2k2eNXjLsX64vf43eIOf3frGDDS3pSjnb5UpjpF9rmyS0JYwPIcjT1s1OV8h
+HPG578pb8I4igXjTGKBrSHgBtB3zhMy+soEizL++akwi/tECQQD2Tf+1P7pJIkFC
+rQlbUgnKilg8sCrTzUn3MbaRDR+B36gKcMpjt5tvmp5xyh8ZkX2qCYE8BHGyIsc0
+tyawkU2LAkEA1A7hgzt3eA8XnRrS7lHi+Vqo3FyKqPGZzbPRLbKWDRKtCu8ucEJF
+kF/AcoQUQBAbG2lH8Gu85+w/86YIFiUcWwJBAMcgmDL4qqqlvw0gyAfEc5YD8wJd
+isJhngLzdh4e9fIR19FZDsc/kbLQ1ng4aJxLO8VUmd7j2OejplnAGM93KbECQFJN
+pEEm52t1kBitvIg6x/k3sqo0UKzZeHRaAinWUl6ojPjddAyl7BHo1+yaTrG+jobJ
+Cd7KMfthukB6hFDz/lkCQQCuDyU3OuC2J8NMzGKfnponYBX9gaaqB9wFNvP592Gw
+ln8+LZ1E9Hqc4f8kZbSRrOFhKHCFypjWp/0CiF9fP4Mb
+-----END RSA PRIVATE KEY-----
diff --git a/test/plugins/rest-web/package.json b/test/plugins/rest-web/package.json
new file mode 100644
index 0000000..e8811d5
--- /dev/null
+++ b/test/plugins/rest-web/package.json
@@ -0,0 +1,18 @@
+{
+ "name": "secure",
+ "version": "0.0.0",
+ "description": "a simple website with basic auth and https",
+ "main": "exp_server.js",
+ "scripts": {
+ "test": "echo \"Error: no test specified\" && exit 1",
+ "start": "node server.js"
+ },
+ "author": "",
+ "license": "ISC",
+ "dependencies": {
+ "body-parser": "^1.4.3",
+ "express": "^4.6.1",
+ "passport": "^0.2.0",
+ "passport-http": "^0.2.2"
+ }
+}
diff --git a/test/plugins/rest-web/readme.txt b/test/plugins/rest-web/readme.txt
new file mode 100644
index 0000000..d76fd9b
--- /dev/null
+++ b/test/plugins/rest-web/readme.txt
@@ -0,0 +1,51 @@
+
+
+This is a simple nodejs server that receives POST commands over HTTPS.
+
+The server supports username/password authentication (basic auth). The following users are hardcoded:
+ username: 'myuser', password: 'mypass'
+ username: 'joe', password: 'bloe'
+
+
+These are the routes provided by the server:
+
+ '/events/new': (HTTP method: POST) The server receives JSON messages. The user must be authenticated to
+ have access.
+ '/': (HTTP method: GET) The server serves a web page that lists all the received data on '/events/new'.
+ No authentication necessary.
+ '/login': (HTTP method: GET) If authenticated correctly, the server returns the name of the user. This is
+ used to verify user credentials.
+
+Installation:
+
+ make sure nodejs is installed (skip if you have node installed already):
+ sudo apt-get install nodejs nodejs-legacy npm
+
+ now setup the local test server:
+ cd <gazebo-src>/test/plugins/rest-web
+ npm install
+
+Running the server:
+
+ To start the web server: npm start
+ point your browser to (NOTE: use https, NOT http):
+
+ https://localhost:3000
+
+
+Send messages from Gazebo to the web server:
+
+ start the simulation server (<gazebo-src>/test/plugins/rest-web/gzserver.bash):
+
+ gzserver --verbose -s libRestWebPlugin.so ../../worlds/rest_web.world
+
+
+ start the simulation client (<gazebo-src>/test/plugins/rest-web/gzserver.bash):
+
+ gzclient --verbose -g libRestUiPlugin.so menu="REST-web" title="test server" label="Login" url="https://localhost:3000"
+
+ You should run gzserver.bash and gzclient.bash from their directory, otherwise you will get an error (Could not open file[../../worlds/rest_web.world]).
+ In gzclient, there should be a new menu titled: "REST-web". Log in with user 'myuser' and password 'mypass'. Then, start and stop the simulation, you should see events published to the web site.
+
+
+
diff --git a/test/plugins/rest-web/server.js b/test/plugins/rest-web/server.js
new file mode 100644
index 0000000..a4686ee
--- /dev/null
+++ b/test/plugins/rest-web/server.js
@@ -0,0 +1,119 @@
+var fs = require('fs');
+var https = require('https');
+var privateKey = fs.readFileSync('key.pem', 'utf8');
+var certificate = fs.readFileSync('key-cert.pem', 'utf8');
+var passport = require('passport');
+var BasicStrategy = require('passport-http').BasicStrategy;
+var bodyParser = require('body-parser');
+var credentials = {key: privateKey, cert: certificate};
+var express = require('express');
+var util = require('util');
+
+var app = express();
+
+// your express configuration here
+// var httpServer = http.createServer(app);
+var httpsServer = https.createServer(credentials, app);
+
+app.use(passport.initialize());
+// parse application/json
+app.use(bodyParser.json())
+
+var users = [
+ { id: 1, username: 'myuser', password: 'mypass', email: 'bob at example.com' }
+ , { id: 2, username: 'joe', password: 'bloe', email: 'joe at example.com' }
+];
+
+function findByUsername(username, fn) {
+ for (var i = 0, len = users.length; i < len; i++) {
+ var user = users[i];
+ if (user.username === username) {
+ return fn(null, user);
+ }
+ }
+ return fn(null, null);
+}
+
+// Use the BasicStrategy within Passport.
+// Strategies in Passport require a `verify` function, which accept
+// credentials (in this case, a username and password), and invoke a callback
+// with a user object.
+passport.use(new BasicStrategy({
+ },
+ function(username, password, done) {
+ // asynchronous verification, for effect...
+ process.nextTick(function () {
+
+ // Find the user by username. If there is no user with the given
+ // username, or the password is not correct, set the user to `false` to
+ // indicate failure. Otherwise, return the authenticated `user`.
+ findByUsername(username, function(err, user) {
+ if (err) { return done(err); }
+ if (!user) { return done(null, false); }
+ if (user.password != password) { return done(null, false); }
+ return done(null, user);
+ })
+ });
+ }
+));
+
+all_events = [];
+
+// curl -v -I http://127.0.0.1:3000/
+// curl -v -I --user bob:secret http://127.0.0.1:3000/
+app.get('/login',
+ passport.authenticate('basic', { session: false }),
+ function(req, res){
+ console.log("-----\nLogin:" + util.inspect(req.user));
+ all_events.push(req.user);
+ res.jsonp( req.user );
+ console.log('LOGIN DONE');
+ });
+
+
+app.get('/',
+ // Authenticate using HTTP Basic credentials, with session support disabled.
+ // passport.authenticate('basic', { session: false }),
+ function(req, res){
+ //res.jsonp({ username: req.user.username, email: req.user.email });
+ console.log('serving all events');
+
+ res.write('<html>');
+ // refresh the page every sec
+ res.write("<meta http-equiv=\"refresh\" content=\"2\" >");
+ res.write('<script>\n');
+ res.write('events = ' + JSON.stringify(all_events) + ';\n' );
+ res.write('</script>\n');
+
+ res.write('<script src="script.js"></script>\n');
+
+ res.write('<body onload="fillEventList()">');
+ res.write(' <h1>' + all_events.length + ' events</h1>');
+ colors = ['LightGrey', 'LightGreen'];
+ for (var i = all_events.length-1; i >=0; i--)
+ {
+ // get a nice formatted string for display
+ var s = JSON.stringify(all_events[i], null, " ");
+ res.write('<div>' + i + '</div><pre style="background:' + colors[i%2] + '">' + s + '</pre>');
+ }
+ res.write(' <div id="list"/>');
+ res.write('</body>');
+ res.end('</html>');
+ });
+
+app.post('/events/new',
+ passport.authenticate('basic', { session: false }),
+ function(req, res){
+ console.log("-----\nNEW EVENT:\n\n" + util.inspect(req.body));
+ var r = { username: req.user.username, Event: req.body };
+ all_events.push(r);
+ res.jsonp(r);
+ });
+
+app.use(express.static(__dirname));
+
+console.log('Listening on port 3000');
+//httpServer.listen(80);
+//httpsServer.listen(443);
+httpsServer.listen(3000);
+
diff --git a/test/regression/1082_pid_limits.cc b/test/regression/1082_pid_limits.cc
index 38c2232..44c86f4 100644
--- a/test/regression/1082_pid_limits.cc
+++ b/test/regression/1082_pid_limits.cc
@@ -21,7 +21,7 @@
#include "gazebo/physics/Model.hh"
#include "gazebo/physics/JointController.hh"
#include "gazebo/common/PID.hh"
-#include "test/ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "test_config.h"
using namespace gazebo;
diff --git a/test/regression/1112_force_torque_model_removal.cc b/test/regression/1112_force_torque_model_removal.cc
index f35b3a5..e6f1f66 100644
--- a/test/regression/1112_force_torque_model_removal.cc
+++ b/test/regression/1112_force_torque_model_removal.cc
@@ -15,7 +15,7 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
diff --git a/test/regression/1124_collision_pose.cc b/test/regression/1124_collision_pose.cc
index a92969a..e47c99a 100644
--- a/test/regression/1124_collision_pose.cc
+++ b/test/regression/1124_collision_pose.cc
@@ -15,7 +15,7 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
diff --git a/test/regression/1146_collision_bounding_box.cc b/test/regression/1146_collision_bounding_box.cc
index 08f6214..9072d9d 100644
--- a/test/regression/1146_collision_bounding_box.cc
+++ b/test/regression/1146_collision_bounding_box.cc
@@ -15,7 +15,7 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/math/Box.hh"
using namespace gazebo;
diff --git a/test/regression/1208_world_plugin_init.cc b/test/regression/1208_world_plugin_init.cc
index 32ed344..f905ab3 100644
--- a/test/regression/1208_world_plugin_init.cc
+++ b/test/regression/1208_world_plugin_init.cc
@@ -15,7 +15,7 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
using namespace gazebo;
@@ -36,6 +36,8 @@ TEST_F(Issue1208Test, Reset)
physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
ASSERT_TRUE(physics != NULL);
+ world->Step(1);
+
// There is a WorldPlugin attached to this world.
// It has a counter that increments with each call of Init.
// It then sets the value of that counter to the
diff --git a/test/regression/1375_world_reset.cc b/test/regression/1375_world_reset.cc
index 2d98e12..3c1d5e4 100644
--- a/test/regression/1375_world_reset.cc
+++ b/test/regression/1375_world_reset.cc
@@ -16,8 +16,8 @@
*/
#include "gazebo/common/Events.hh"
-#include "ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#include "test/integration/joint_test.hh"
using namespace gazebo;
diff --git a/test/regression/1569_hydra_crash.cc b/test/regression/1569_hydra_crash.cc
new file mode 100644
index 0000000..a3d8dc2
--- /dev/null
+++ b/test/regression/1569_hydra_crash.cc
@@ -0,0 +1,43 @@
+/*
+ * 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/test/ServerFixture.hh"
+#include "gazebo/physics/physics.hh"
+
+using namespace gazebo;
+
+class HydraCrashTest : public ServerFixture
+{
+};
+
+
+/////////////////////////////////////////////////
+// \brief Test for issue #1208
+TEST_F(HydraCrashTest, Reset)
+{
+ Load("worlds/hydra_test.world", true);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+}
+
+/////////////////////////////////////////////////
+/// Main
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/regression/346_save_lights.cc b/test/regression/346_save_lights.cc
index 0e07182..f318b84 100644
--- a/test/regression/346_save_lights.cc
+++ b/test/regression/346_save_lights.cc
@@ -17,7 +17,7 @@
#include <gtest/gtest.h>
#include <gazebo/rendering/rendering.hh>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
using namespace gazebo;
@@ -58,12 +58,12 @@ TEST_F(Issue346Test, SaveLights)
sdf::SDFPtr sdf(new sdf::SDF);
ASSERT_TRUE(sdf::init(sdf));
ASSERT_TRUE(sdf::readFile(common::find_file(filenameOut), sdf));
- ASSERT_TRUE(sdf->root != NULL);
+ ASSERT_TRUE(sdf->Root() != NULL);
// Verify there is one spot light and one point light
int hasSpotLight = 0;
int hasPointLight = 0;
- sdf::ElementPtr worldElem = sdf->root->GetElement("world");
+ sdf::ElementPtr worldElem = sdf->Root()->GetElement("world");
ASSERT_TRUE(worldElem != NULL);
sdf::ElementPtr lightElem = worldElem->GetElement("light");
while (lightElem)
diff --git a/test/regression/351_world_step.cc b/test/regression/351_world_step.cc
new file mode 100644
index 0000000..20d7519
--- /dev/null
+++ b/test/regression/351_world_step.cc
@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) 2012-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;
+
+class Issue351Test : public ServerFixture
+{
+};
+
+/////////////////////////////////////////////////
+// \brief Test for issue #351
+TEST_F(Issue351Test, WorldStep)
+{
+ Load("worlds/world_step.world", true);
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // Take 500 steps; it passes if it doesn't seg-fault
+ world->Step(500);
+}
diff --git a/test/regression/494_joint_axis_frame.cc b/test/regression/494_joint_axis_frame.cc
index 0f50568..8af1741 100644
--- a/test/regression/494_joint_axis_frame.cc
+++ b/test/regression/494_joint_axis_frame.cc
@@ -15,8 +15,8 @@
*
*/
-#include "ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#include "test/integration/joint_test.hh"
using namespace gazebo;
diff --git a/test/regression/602_unsubscribe_segfault.cc b/test/regression/602_unsubscribe_segfault.cc
new file mode 100644
index 0000000..4b8aa4c
--- /dev/null
+++ b/test/regression/602_unsubscribe_segfault.cc
@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <gtest/gtest.h>
+
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/Model.hh"
+#include "gazebo/physics/JointController.hh"
+#include "gazebo/common/PID.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "test_config.h"
+
+using namespace gazebo;
+class Issue602Test : public ServerFixture
+{
+ /// \brief Test for Unsubscribe before delete subscriber.
+ public: void UnsubscribeTest();
+
+ /// \brief Callback for sensor subscribers in MultipleSensors test.
+ /// \param[in] _msg World Statistics message.
+ public: void Callback(const ConstContactsPtr &_msg);
+};
+
+unsigned int g_messageCount = 0;
+
+////////////////////////////////////////////////////////////////////////
+void Issue602Test::Callback(const ConstContactsPtr &/*_msg*/)
+{
+ g_messageCount++;
+}
+
+/////////////////////////////////////////////////
+TEST_F(Issue602Test, Unsubscribe)
+{
+ UnsubscribeTest();
+}
+
+/////////////////////////////////////////////////
+void Issue602Test::UnsubscribeTest()
+{
+ Load("worlds/contact_sensors_multiple.world", true);
+ physics::WorldPtr world = physics::get_world();
+ ASSERT_TRUE(world != NULL);
+
+ const std::string contactSensorName1("box_contact");
+ const std::string contactSensorName2("box_contact2");
+
+ {
+ sensors::SensorPtr sensor1 = sensors::get_sensor(contactSensorName1);
+ sensors::ContactSensorPtr contactSensor1 =
+ boost::dynamic_pointer_cast<sensors::ContactSensor>(sensor1);
+ ASSERT_TRUE(contactSensor1 != NULL);
+ }
+
+ {
+ sensors::SensorPtr sensor2 = sensors::get_sensor(contactSensorName2);
+ sensors::ContactSensorPtr contactSensor2 =
+ boost::dynamic_pointer_cast<sensors::ContactSensor>(sensor2);
+ ASSERT_TRUE(contactSensor2 != NULL);
+ }
+
+ // Sleep to ensure transport topics are all advertised
+ common::Time::MSleep(100);
+ auto topics = transport::getAdvertisedTopics("gazebo.msgs.Contacts");
+ EXPECT_FALSE(topics.empty());
+ EXPECT_GE(topics.size(), 4u);
+
+ // We should expect them all to publish.
+ for (auto const &topic : topics)
+ {
+ gzdbg << "Listening to " << topic << std::endl;
+ g_messageCount = 0;
+ transport::SubscriberPtr sub = this->node->Subscribe(topic,
+ &Issue602Test::Callback, this);
+
+ const unsigned int steps = 50;
+ world->Step(steps);
+ common::Time::MSleep(steps);
+
+ sub->Unsubscribe();
+ common::Time::MSleep(steps);
+ }
+}
+
+/////////////////////////////////////////////////
+/// Main
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/regression/624_collision_world_pose.cc b/test/regression/624_collision_world_pose.cc
index 8b2049e..8f9d577 100644
--- a/test/regression/624_collision_world_pose.cc
+++ b/test/regression/624_collision_world_pose.cc
@@ -15,9 +15,9 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/physics/physics.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/helper_physics_generator.hh"
using namespace gazebo;
diff --git a/test/regression/846_typo_in_camera.cc b/test/regression/846_typo_in_camera.cc
index 8945b02..fd7a998 100644
--- a/test/regression/846_typo_in_camera.cc
+++ b/test/regression/846_typo_in_camera.cc
@@ -17,8 +17,9 @@
#include <gtest/gtest.h>
#include <gazebo/rendering/rendering.hh>
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
+using namespace gazebo;
class Issue846Test : public ServerFixture
{
};
diff --git a/test/regression/876_random_number_generator.cc b/test/regression/876_random_number_generator.cc
index 0e3c454..f6158bd 100644
--- a/test/regression/876_random_number_generator.cc
+++ b/test/regression/876_random_number_generator.cc
@@ -15,7 +15,7 @@
*
*/
-#include "ServerFixture.hh"
+#include "gazebo/test/ServerFixture.hh"
#include "gazebo/math/Rand.hh"
using namespace gazebo;
diff --git a/test/regression/940_force_torque_sensor_frame.cc b/test/regression/940_force_torque_sensor_frame.cc
index 2a96cbc..f5f468a 100644
--- a/test/regression/940_force_torque_sensor_frame.cc
+++ b/test/regression/940_force_torque_sensor_frame.cc
@@ -15,8 +15,8 @@
*
*/
-#include "ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#define TOL_GRAVITY 1e-4
#define TOL_FORCES 1.0
@@ -29,10 +29,10 @@ class Issue940Test : public ServerFixture,
public testing::WithParamInterface<const char*>
{
public: void ForceTorqueSensorFrameTest(const std::string &_physicsEngine);
- public: void ExpectForceTorqueMeasure(const std::string & sensorName,
- const Vector3 expForce,
- const Vector3 expTorque,
- sensors::SensorManager * mgr);
+ public: void ExpectForceTorqueMeasure(const std::string &_sensorName,
+ const Vector3 &_expForce,
+ const Vector3 &_expTorque,
+ sensors::SensorManager *_mgr);
};
/////////////////////////////////////////////////////////////////////
@@ -53,14 +53,6 @@ void Issue940Test::ForceTorqueSensorFrameTest(const std::string &_physicsEngine)
<< std::endl;
return;
}
- if (_physicsEngine == "dart")
- {
- gzerr << "Skipping this test for " << _physicsEngine
- << " since it consistently gives"
- << " incorrect force-torque readings."
- << std::endl;
- return;
- }
bool worldPaused = true;
Load("worlds/force_torque_frame_test.world", worldPaused, _physicsEngine);
sensors::SensorManager *mgr = sensors::SensorManager::Instance();
@@ -91,6 +83,7 @@ void Issue940Test::ForceTorqueSensorFrameTest(const std::string &_physicsEngine)
double cog_y_1 = 3.0;
double cog_y_0 = -3.0;
+ double delta_x_joint_12 = 1.0;
// For details on the expected answers, check force_torque_frame_test.world
ExpectForceTorqueMeasure("force_torque_01_parent_and_parent_to_child",
@@ -106,49 +99,52 @@ void Issue940Test::ForceTorqueSensorFrameTest(const std::string &_physicsEngine)
ExpectForceTorqueMeasure("force_torque_01_sensor_and_child_to_parent",
Vector3(0, 0, g*mAll), Vector3(g*(m0*cog_y_0+m1*cog_y_1), 0, 0), mgr);
ExpectForceTorqueMeasure("force_torque_12_parent_and_parent_to_child",
- Vector3(0, 0, -g*m1), Vector3(-g*m1*cog_y_1, 0, 0), mgr);
+ Vector3(0, 0, -g*m1),
+ Vector3(-g*m1*cog_y_1, -g*m1*delta_x_joint_12, 0), mgr);
ExpectForceTorqueMeasure("force_torque_12_parent_and_child_to_parent",
- Vector3(0, 0, g*m1), Vector3(g*m1*cog_y_1, 0, 0), mgr);
+ Vector3(0, 0, g*m1), Vector3(g*m1*cog_y_1, g*m1*delta_x_joint_12, 0), mgr);
ExpectForceTorqueMeasure("force_torque_12_child_and_parent_to_child",
- Vector3(0, -g*m1, 0), Vector3(-g*m1*cog_y_1, 0, 0), mgr);
+ Vector3(0, -g*m1, 0),
+ Vector3(-g*m1*cog_y_1, 0, g*m1*delta_x_joint_12), mgr);
ExpectForceTorqueMeasure("force_torque_12_child_and_child_to_parent",
- Vector3(0, g*m1, 0), Vector3(g*m1*cog_y_1, 0, 0), mgr);
+ Vector3(0, g*m1, 0), Vector3(g*m1*cog_y_1, 0, -g*m1*delta_x_joint_12), mgr);
ExpectForceTorqueMeasure("force_torque_12_sensor_and_parent_to_child",
- Vector3(0, -g*m1, 0), Vector3(0, 0, g*m1*cog_y_1), mgr);
+ Vector3(0, -g*m1, 0), Vector3(g*m1*delta_x_joint_12, 0, g*m1*cog_y_1), mgr);
ExpectForceTorqueMeasure("force_torque_12_sensor_and_child_to_parent",
- Vector3(0, g*m1, 0), Vector3(0, 0, -g*m1*cog_y_1), mgr);
+ Vector3(0, g*m1, 0),
+ Vector3(-g*m1*delta_x_joint_12, 0, -g*m1*cog_y_1), mgr);
}
////////////////////////////////////////////////////////////////////
// \brief Expect force and torque measures for a force torque sensor
-void Issue940Test::ExpectForceTorqueMeasure(const std::string & sensorName,
- const Vector3 expForce,
- const Vector3 expTorque,
- sensors::SensorManager * mgr)
+void Issue940Test::ExpectForceTorqueMeasure(const std::string &_sensorName,
+ const Vector3 &_expForce,
+ const Vector3 &_expTorque,
+ sensors::SensorManager *_mgr)
{
sensors::ForceTorqueSensorPtr sensor =
boost::dynamic_pointer_cast<sensors::ForceTorqueSensor>(
- mgr->GetSensor(sensorName));
+ _mgr->GetSensor(_sensorName));
// Make sure the above dynamic cast worked.
EXPECT_TRUE(sensor != NULL);
- Vector3 mesForce = sensor->GetForce();
- Vector3 mesTorque = sensor->GetTorque();
+ ignition::math::Vector3d mesForce = sensor->Force();
+ ignition::math::Vector3d mesTorque = sensor->Torque();
- gzdbg << "sensorName: " << sensorName << std::endl;
+ gzdbg << "sensorName: " << _sensorName << std::endl;
gzdbg << "mesForce : " << mesForce << std::endl;
- gzdbg << "expForce : " << expForce << std::endl;
+ gzdbg << "expForce : " << _expForce << std::endl;
gzdbg << "mesTorque : " << mesTorque << std::endl;
- gzdbg << "expTorque : " << expTorque << std::endl;
+ gzdbg << "expTorque : " << _expTorque << std::endl;
- EXPECT_NEAR(expForce.x, mesForce.x, TOL_FORCES);
- EXPECT_NEAR(expForce.y, mesForce.y, TOL_FORCES);
- EXPECT_NEAR(expForce.z, mesForce.z, TOL_FORCES);
+ EXPECT_NEAR(_expForce.x, mesForce.X(), TOL_FORCES);
+ EXPECT_NEAR(_expForce.y, mesForce.Y(), TOL_FORCES);
+ EXPECT_NEAR(_expForce.z, mesForce.Z(), TOL_FORCES);
- EXPECT_NEAR(expTorque.x, mesTorque.x, TOL_TORQUES);
- EXPECT_NEAR(expTorque.y, mesTorque.y, TOL_TORQUES);
- EXPECT_NEAR(expTorque.z, mesTorque.z, TOL_TORQUES);
+ EXPECT_NEAR(_expTorque.x, mesTorque.X(), TOL_TORQUES);
+ EXPECT_NEAR(_expTorque.y, mesTorque.Y(), TOL_TORQUES);
+ EXPECT_NEAR(_expTorque.z, mesTorque.Z(), TOL_TORQUES);
EXPECT_TRUE(sensor->IsActive());
}
diff --git a/test/regression/978_joint_anchor.cc b/test/regression/978_joint_anchor.cc
index 1515256..4f39345 100644
--- a/test/regression/978_joint_anchor.cc
+++ b/test/regression/978_joint_anchor.cc
@@ -15,8 +15,8 @@
*
*/
-#include "ServerFixture.hh"
-#include "test/integration/helper_physics_generator.hh"
+#include "gazebo/test/ServerFixture.hh"
+#include "gazebo/test/helper_physics_generator.hh"
#include "test/integration/joint_test.hh"
using namespace gazebo;
diff --git a/test/regression/CMakeLists.txt b/test/regression/CMakeLists.txt
index 6050f87..4a7b927 100644
--- a/test/regression/CMakeLists.txt
+++ b/test/regression/CMakeLists.txt
@@ -22,7 +22,9 @@ if(HAVE_DART)
endif()
set(tests
+ 351_world_step.cc
494_joint_axis_frame.cc
+ 602_unsubscribe_segfault.cc
624_collision_world_pose.cc
876_random_number_generator.cc
940_force_torque_sensor_frame.cc
@@ -33,6 +35,7 @@ set(tests
1146_collision_bounding_box.cc
1208_world_plugin_init.cc
1375_world_reset.cc
+ 1569_hydra_crash.cc
)
gz_build_tests(${tests})
diff --git a/test/worlds/collide_bitmask.world b/test/worlds/collide_bitmask.world
new file mode 100644
index 0000000..77405f9
--- /dev/null
+++ b/test/worlds/collide_bitmask.world
@@ -0,0 +1,190 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+
+ <model name="ground_plane">
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0xff</collide_bitmask>
+ </contact>
+ <friction>
+ <ode>
+ <mu>100</mu>
+ <mu2>50</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <cast_shadows>false</cast_shadows>
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box1">
+ <pose>0 0 1.0 0 0 0</pose>
+ <link name="link">
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0x01</collide_bitmask>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box2">
+ <pose>-1.5 1.5 3.0 0 0 1.57</pose>
+ <link name="link">
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0x02</collide_bitmask>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box3">
+ <pose>0 1.5 6.0 0 0 2.24</pose>
+ <link name="link">
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0x03</collide_bitmask>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box4">
+ <pose>0 -3 1.0 0 0 0</pose>
+ <link name="link">
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0x10000</collide_bitmask>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ </world>
+</sdf>
diff --git a/test/worlds/collide_without_contact.world b/test/worlds/collide_without_contact.world
index 1a2ad76..81bd996 100644
--- a/test/worlds/collide_without_contact.world
+++ b/test/worlds/collide_without_contact.world
@@ -1,3 +1,4 @@
+<?xml version='1.0'?>
<sdf version='1.4'>
<world name='default'>
<!-- include sun and ground plane -->
diff --git a/test/worlds/contact.world b/test/worlds/contact.world
index 4ec3f7e..3d57639 100644
--- a/test/worlds/contact.world
+++ b/test/worlds/contact.world
@@ -1,110 +1,172 @@
-<gazebo version="1.0">
- <world name="world_1">
+<sdf version='1.5'>
+ <world name='world_1'>
<scene>
- <ambient rgba="0.5 0.5 0.5 1.0"/>
- <background rgba="0.5 0.5 0.5 1.0">
- <sky material="Gazebo/CloudySky"/>
- </background>
- <shadows enabled="true"/>
- <fog rgba="0.0 0.0 0.5 0.5" type="linear" start="2.0" end="10.0" density="0.5"/>
+ <shadows>1</shadows>
+ <ambient>0.4 0.4 0.4 1</ambient>
+ <background>0.7 0.7 0.7 1</background>
</scene>
-
- <physics type="ode">
+ <physics type='ode'>
<max_contacts>250</max_contacts>
<ode>
- <solver type="quick" dt="0.001" iters="20" sor="1.3"/>
- <constraints cfm="0" erp="0.2" contact_max_correcting_vel="100.0" contact_surface_layer="0.0"/>
+ <solver>
+ <type>quick</type>
+ <iters>20</iters>
+ <sor>1.3</sor>
+ <use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling>
+ </solver>
+ <constraints>
+ <cfm>0</cfm>
+ <erp>0.2</erp>
+ <contact_max_correcting_vel>100</contact_max_correcting_vel>
+ <contact_surface_layer>0</contact_surface_layer>
+ </constraints>
</ode>
- <gravity xyz="0.0 0.0 -10.0"/>
+ <gravity>0 0 -10</gravity>
+ <max_step_size>0.001</max_step_size>
+ <real_time_factor>1</real_time_factor>
+ <real_time_update_rate>1000</real_time_update_rate>
+ <magnetic_field>5.5645e-06 2.28758e-05 -4.23884e-05</magnetic_field>
</physics>
-
- <model name="model_1" static="false">
-
- <origin pose="0.0 0.0 2.0 0.0 0.0 0.0"/>
-
- <link name="link_1" self_collide="true" gravity="true" kinematic="false">
- <origin pose="0.0 0.0 0.0 0.0 0.0 0.0"/>
- <inertial mass="10.0">
- <origin pose="0.0 0.0 0.0 0.0 0.0 0.0"/>
- <damping linear="0.0" angular="0.0" />
- <inertia ixx="1.0" ixy="0.0" ixz="0.0"
- iyy="1.0" iyz="0.0"
- izz="1.0"/>
+ <model name='model_1'>
+ <pose>0 0 2 0 -0 0</pose>
+ <link name='link_1'>
+ <pose>0 0 0 0 -0 0</pose>
+ <inertial>
+ <pose>0 0 0 0 -0 0</pose>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ <mass>10</mass>
</inertial>
-
- <visual cast_shadows="true" name="visual_box" laser_retro="100.0">
- <origin pose="0.0 0.0 0.0 0.0 0.0 0.0"/>
+ <visual name='visual_box'>
+ <pose>0 0 0 0 -0 0</pose>
<geometry>
- <box size="1.0 1.0 2.0"/>
+ <box>
+ <size>1 1 2</size>
+ </box>
</geometry>
- <material script="Gazebo/Green" normal_map="normal_map.png">
- <color rgba="0.03 0.5 0.5 1.0"/>
- </material>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>100</laser_retro>
</visual>
-
- <collision name="collision_box" laser_retro="100.0">
- <origin pose="0.0 0.0 0.0 0.0 0.0 0.0"/>
+ <collision name='collision_box'>
+ <pose>0 0 0 0 -0 0</pose>
<max_contacts>250</max_contacts>
<geometry>
- <box size="1.0 1.0 2.0"/>
+ <box>
+ <size>1 1 2</size>
+ </box>
</geometry>
<surface>
<friction>
- <ode mu="0.5" mu2="0.2" fdir1="1.0 0 0" slip1="0" slip2="0"/>
+ <ode>
+ <mu>0.5</mu>
+ <mu2>0.2</mu2>
+ <fdir1>1 0 0</fdir1>
+ <slip1>0</slip1>
+ <slip2>0</slip2>
+ </ode>
</friction>
- <bounce restitution_coefficient="0" threshold="1000000.0"/>
+ <bounce>
+ <restitution_coefficient>0</restitution_coefficient>
+ <threshold>1e+06</threshold>
+ </bounce>
<contact>
- <ode soft_cfm="0" soft_erp="0.2" kp="1e15" kd="1e13" max_vel="100.0" min_depth="0.0001"/>
+ <ode>
+ <soft_cfm>0</soft_cfm>
+ <soft_erp>0.2</soft_erp>
+ <kp>1e+15</kp>
+ <kd>1e+13</kd>
+ <max_vel>100</max_vel>
+ <min_depth>0.0001</min_depth>
+ </ode>
</contact>
</surface>
+ <laser_retro>100</laser_retro>
</collision>
-
- <sensor name="sensor_contact" type="contact" always_on="true" update_rate="2.0">
- <origin pose="0.0 0.0 0.0 0.0 0.0 0.0"/>
+ <sensor name='sensor_contact' type='contact'>
+ <pose>0 0 0 0 -0 0</pose>
<contact>
- <collision name="collision_box"/>
+ <collision>__default__</collision>
+ <topic>__default_topic__</topic>
</contact>
+ <always_on>1</always_on>
+ <update_rate>2</update_rate>
</sensor>
-
+ <gravity>1</gravity>
+ <self_collide>1</self_collide>
+ <kinematic>0</kinematic>
+ <velocity_decay>
+ <linear>0</linear>
+ <angular>0</angular>
+ </velocity_decay>
</link>
+ <static>0</static>
</model>
-
- <model name='gplane' static='1'>
- <origin pose='0 0 0 0 -0 0'/>
- <link name='plane' gravity='1' self_collide='0' kinematic='0'>
- <origin pose='0 0 0 0 -0 0'/>
- <collision name='plane' laser_retro='0'>
- <origin pose='0 0 0 0 -0 0'/>
+ <model name='gplane'>
+ <pose>0 0 0 0 -0 0</pose>
+ <link name='plane'>
+ <pose>0 0 0 0 -0 0</pose>
+ <collision name='plane'>
+ <pose>0 0 0 0 -0 0</pose>
<geometry>
- <plane normal='0 0 1'/>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>1 1</size>
+ </plane>
</geometry>
<surface>
<friction>
- <ode mu='50' mu2='50' fdir1='0 0 0' slip1='0' slip2='0'/>
+ <ode>
+ <mu>50</mu>
+ <mu2>50</mu2>
+ <fdir1>0 0 0</fdir1>
+ <slip1>0</slip1>
+ <slip2>0</slip2>
+ </ode>
</friction>
- <bounce restitution_coefficient='0' threshold='0'/>
+ <bounce>
+ <restitution_coefficient>0</restitution_coefficient>
+ <threshold>0</threshold>
+ </bounce>
<contact>
- <ode soft_cfm='0' soft_erp='0' kp='1000000000' kd='1' max_vel='-1' min_depth='0'/>
+ <ode>
+ <soft_cfm>0</soft_cfm>
+ <soft_erp>0</soft_erp>
+ <kp>1e+09</kp>
+ <kd>1</kd>
+ <max_vel>-1</max_vel>
+ <min_depth>0</min_depth>
+ </ode>
</contact>
</surface>
+ <laser_retro>0</laser_retro>
</collision>
- <visual name='old_gazebo_xml_visual' cast_shadows='1' laser_retro='0' transparency='0'>
- <origin pose='0 0 0 0 -0 0'/>
+ <visual name='old_gazebo_xml_visual'>
+ <pose>0 0 0 0 -0 0</pose>
<geometry>
- <plane normal='0 0 1'/>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>1 1</size>
+ </plane>
</geometry>
+ <cast_shadows>1</cast_shadows>
+ <laser_retro>0</laser_retro>
+ <transparency>0</transparency>
</visual>
+ <gravity>1</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ <velocity_decay>
+ <linear>0</linear>
+ <angular>0</angular>
+ </velocity_decay>
</link>
+ <static>1</static>
</model>
-<!--
- <plugin name="plugin_1" filename="libgazebo_ros_bumper.so">
- <data name="sensor" value="model_1::sensor_contact" />
- <data name="alwaysOn" value="true" />
- <data name="updateRate" value="100.0" />
- <data name="frameName" value="r_gripper_palm_link" />
- <data name="bumperTopicName" value="r_gripper_palm_bumper" />
- </plugin>
--->
-
</world>
-</gazebo>
+</sdf>
diff --git a/test/worlds/contact_sensors_multiple.world b/test/worlds/contact_sensors_multiple.world
new file mode 100644
index 0000000..001b500
--- /dev/null
+++ b/test/worlds/contact_sensors_multiple.world
@@ -0,0 +1,67 @@
+<sdf version='1.5'>
+ <world name='default'>
+ <!-- include sun and ground plane -->
+ <include><uri>model://sun</uri></include>
+ <include><uri>model://ground_plane</uri></include>
+
+ <model name="sensor_box">
+ <static>true</static>
+ <link name="link">
+ <pose>0 0 0.251 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>4 4 0.5</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>4 4 0.5</size>
+ </box>
+ </geometry>
+ <material><script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script></material>
+ </visual>
+ <collision name="collision2">
+ <pose>0 0 1 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>4 4 0.5</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual2">
+ <pose>0 0 1 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>4 4 0.5</size>
+ </box>
+ </geometry>
+ <material><script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script></material>
+ </visual>
+ <sensor name="box_contact" type="contact">
+ <always_on>true</always_on>
+ <update_rate>1000</update_rate>
+ <contact>
+ <collision>collision</collision>
+ </contact>
+ </sensor>
+ <sensor name="box_contact2" type="contact">
+ <always_on>true</always_on>
+ <update_rate>1000</update_rate>
+ <contact>
+ <collision>collision2</collision>
+ </contact>
+ </sensor>
+ </link>
+ </model>
+
+ </world>
+</sdf>
diff --git a/test/worlds/force_torque_frame_test.world b/test/worlds/force_torque_frame_test.world
index 2425c5e..6aebd16 100644
--- a/test/worlds/force_torque_frame_test.world
+++ b/test/worlds/force_torque_frame_test.world
@@ -166,7 +166,7 @@
<parent>link_1</parent>
<child>link_2</child>
<!-- joint_12 at the interface between the two cylinders -->
- <pose>0 -10 0 0 -0.7854 0</pose>
+ <pose>1.0 -10.0 0.0 0 -0.7854 0</pose>
<axis>
<limit>
<lower>-0.0</lower>
diff --git a/test/worlds/hydra_test.world b/test/worlds/hydra_test.world
new file mode 100644
index 0000000..b93c60b
--- /dev/null
+++ b/test/worlds/hydra_test.world
@@ -0,0 +1,21 @@
+<sdf version="1.4">
+ <world name="default">
+
+ <!-- A ground plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <!-- A global light source -->
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <!-- Load the plugin for Razer Hydra -->
+ <plugin name="hydra" filename="libHydraPlugin.so">
+ <pivot>0.04 0 0</pivot>
+ <grab>0.12 0 0</grab>
+ </plugin>
+
+</world>
+</sdf>
diff --git a/test/worlds/laser_lines_test.world b/test/worlds/laser_lines_test.world
new file mode 100644
index 0000000..86d08f0
--- /dev/null
+++ b/test/worlds/laser_lines_test.world
@@ -0,0 +1,42 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <scene>
+ <ambient>1 1 1 1</ambient>
+ <background>1 1 1 1</background>
+ <grid>false</grid>
+ <origin_visual>false</origin_visual>
+ </scene>
+ <gui>
+ <camera name="user_camera">
+ <pose>0.5 0 0.5 0 1.5707 0</pose>
+ </camera>
+ </gui>
+
+ <model name="model_1">
+ <static>true</static>
+ <link name="link_1">
+ <sensor name="laser_sensor" type="ray">
+ <ray>
+ <scan>
+ <horizontal>
+ <samples>4</samples>
+ <resolution>1</resolution>
+ <min_angle>-1.0</min_angle>
+ <max_angle>1.0</max_angle>
+ </horizontal>
+ </scan>
+ <range>
+ <min>0</min>
+ <max>1</max>
+ <resolution>1</resolution>
+ </range>
+ </ray>
+ <always_on>1</always_on>
+ <update_rate>30</update_rate>
+ <visualize>true</visualize>
+ </sensor>
+ </link>
+ </model>
+ </world>
+</sdf>
diff --git a/test/worlds/model_self_collide.world b/test/worlds/model_self_collide.world
new file mode 100644
index 0000000..34b836b
--- /dev/null
+++ b/test/worlds/model_self_collide.world
@@ -0,0 +1,378 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <model name="all_collide">
+ <self_collide>true</self_collide>
+ <link name="link">
+ <pose>0 0 1.0 0 0 0</pose>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <link name="link1">
+ <pose>0 0 3.0 0 0 0</pose>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <link name="link2">
+ <pose>0 0 6.0 0 0 0</pose>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ <!-- Demonstrate that link self_collide overrides model self_collide-->
+ <model name="some_collide">
+ <pose>0 6 0 0 0 0</pose>
+ <self_collide>true</self_collide>
+ <link name="link">
+ <self_collide>false</self_collide>
+ <pose>0 0 1.0 0 0 0</pose>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <link name="link1">
+ <pose>-1.5 1.5 3.0 0 0 1.57</pose>
+ <self_collide>false</self_collide>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <link name="link2">
+ <pose>0 1.5 6.0 0 0 2.24</pose>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ <!-- Demonstrate backwards compatibility: default is no self-collisions-->
+ <model name="no_collide">
+ <pose>0 12 0 0 0 0</pose>
+ <link name="link">
+ <pose>0 0 1.0 0 0 0</pose>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <link name="link1">
+ <pose>0 0 3.0 0 0 0</pose>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <link name="link2">
+ <pose>0 0 6.0 0 0 0</pose>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+ <!-- Demonstrate that self_collide=false produces same behavior as above-->
+ <model name="explicit_no_collide">
+ <self_collide>false</self_collide>
+ <pose>0 15 0 0 0 0</pose>
+ <link name="link">
+ <pose>0 0 1.0 0 0 0</pose>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <link name="link1">
+ <pose>0 0 3.0 0 0 0</pose>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <link name="link2">
+ <pose>0 0 6.0 0 0 0</pose>
+ <inertial>
+ <mass>1</mass>
+ <inertia>
+ <ixx>0.166667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>2.166667</iyy>
+ <iyz>0</iyz>
+ <izz>2.166667</izz>
+ </inertia>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>5 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ </world>
+</sdf>
diff --git a/test/worlds/ortho_box.world b/test/worlds/ortho_box.world
new file mode 100644
index 0000000..bcbada5
--- /dev/null
+++ b/test/worlds/ortho_box.world
@@ -0,0 +1,35 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <scene>
+ <ambient>1 1 1 1</ambient>
+ <background>0 0 0 1</background>
+ <grid>false</grid>
+ </scene>
+
+ <gui>
+ <camera name="user">
+ <pose>10 0 10 0 0.7 0</pose>
+ <projection_type>orthographic</projection_type>
+ </camera>
+ </gui>
+
+ <model name="wall1">
+ <static>true</static>
+ <pose>500 0 0 0 0 0</pose>
+ <link name="link">
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>1000 1 .2</size>
+ </box>
+ </geometry>
+ <material>
+ <ambient>1 1 1 1</ambient>
+ <diffuse>1 1 1 1</diffuse>
+ </material>
+ </visual>
+ </link>
+ </model>
+ </world>
+</sdf>
diff --git a/test/worlds/pr2_no_sensors_test.world b/test/worlds/pr2_no_sensors_test.world
new file mode 100644
index 0000000..d05e2f6
--- /dev/null
+++ b/test/worlds/pr2_no_sensors_test.world
@@ -0,0 +1,2840 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <!-- A global light source -->
+ <include>
+ <uri>model://sun</uri>
+ </include>
+ <!-- A ground plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <!-- A copy of the pr2 model sans sensors and projector -->
+ <model name="pr2">
+ <static>0</static>
+ <link name="base_footprint">
+ <self_collide>0</self_collide>
+ <inertial>
+ <mass>118.001000</mass>
+ <pose>-0.062421 0.000000 0.201365 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>8.431730</ixx>
+ <ixy>-0.009720</ixy>
+ <ixz>1.901570</ixz>
+ <iyy>8.533050</iyy>
+ <iyz>-0.007380</iyz>
+ <izz>3.787470</izz>
+ </inertia>
+ </inertial>
+ <collision name="base_footprint_geom">
+ <pose>0.000000 0.000000 0.071000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.001000 0.001000 0.001000</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="base_footprint_geom_visual">
+ <geometry>
+ <box>
+ <size>0.010000 0.010000 0.010000</size>
+ </box>
+ </geometry>
+ </visual>
+ <collision name="base_footprint_geom_base_bellow_link">
+ <pose>-0.290000 0.000000 0.851000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.050000 0.370000 0.300000</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="base_footprint_geom_base_bellow_link_visual">
+ <pose>-0.290000 0.000000 0.851000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.050000 0.370000 0.300000</size>
+ </box>
+ </geometry>
+ </visual>
+ <collision name="base_footprint_geom_base_link">
+ <pose>0.000000 0.000000 0.051000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/base_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="base_footprint_geom_base_link_visual">
+ <pose>0.000000 0.000000 0.051000 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/base.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name="bl_caster_rotation_link">
+ <self_collide>0</self_collide>
+ <pose>-0.224600 0.224600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>3.473080</mass>
+ <pose>0.000000 0.000000 0.070000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>-0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>-0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="bl_caster_rotation_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/caster_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="bl_caster_rotation_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/caster.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="bl_caster_l_wheel_link">
+ <self_collide>0</self_collide>
+ <pose>-0.224600 0.273600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.440360</mass>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>-0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>-0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="bl_caster_l_wheel_link_geom">
+ <pose>0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.074792</radius>
+ <length>0.034000</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <visual name="bl_caster_l_wheel_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="bl_caster_r_wheel_link">
+ <self_collide>0</self_collide>
+ <pose>-0.224600 0.175600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.440360</mass>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="bl_caster_r_wheel_link_geom">
+ <pose>0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.074792</radius>
+ <length>0.034000</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <visual name="bl_caster_r_wheel_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="br_caster_rotation_link">
+ <self_collide>0</self_collide>
+ <pose>-0.224600 -0.224600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>3.473080</mass>
+ <pose>0.000000 0.000000 0.070000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>-0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>-0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="br_caster_rotation_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/caster_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="br_caster_rotation_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/caster.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="br_caster_l_wheel_link">
+ <self_collide>0</self_collide>
+ <pose>-0.224600 -0.175600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.440360</mass>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>-0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>-0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="br_caster_l_wheel_link_geom">
+ <pose>0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.074792</radius>
+ <length>0.034000</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <visual name="br_caster_l_wheel_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="br_caster_r_wheel_link">
+ <self_collide>0</self_collide>
+ <pose>-0.224600 -0.273600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.440360</mass>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>-0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>-0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="br_caster_r_wheel_link_geom">
+ <pose>0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.074792</radius>
+ <length>0.034000</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <visual name="br_caster_r_wheel_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="fl_caster_rotation_link">
+ <self_collide>0</self_collide>
+ <pose>0.224600 0.224600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>3.473080</mass>
+ <pose>0.000000 0.000000 0.070000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>-0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>-0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="fl_caster_rotation_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/caster_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="fl_caster_rotation_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/caster.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="fl_caster_l_wheel_link">
+ <self_collide>0</self_collide>
+ <pose>0.224600 0.273600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.440360</mass>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>-0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>-0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="fl_caster_l_wheel_link_geom">
+ <pose>0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.074792</radius>
+ <length>0.034000</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <visual name="fl_caster_l_wheel_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="fl_caster_r_wheel_link">
+ <self_collide>0</self_collide>
+ <pose>0.224600 0.175600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.440360</mass>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>-0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>-0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="fl_caster_r_wheel_link_geom">
+ <pose>0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.074792</radius>
+ <length>0.034000</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <visual name="fl_caster_r_wheel_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="fr_caster_rotation_link">
+ <self_collide>0</self_collide>
+ <pose>0.224600 -0.224600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>3.473080</mass>
+ <pose>0.000000 0.000000 0.070000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>-0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>-0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="fr_caster_rotation_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/caster_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="fr_caster_rotation_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/caster.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="fr_caster_l_wheel_link">
+ <self_collide>0</self_collide>
+ <pose>0.224600 -0.175600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.440360</mass>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>-0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>-0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="fr_caster_l_wheel_link_geom">
+ <pose>0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.074792</radius>
+ <length>0.034000</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <visual name="fr_caster_l_wheel_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="fr_caster_r_wheel_link">
+ <self_collide>0</self_collide>
+ <pose>0.224600 -0.273600 0.079200 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.440360</mass>
+ <inertia>
+ <ixx>0.012412</ixx>
+ <ixy>-0.000712</ixy>
+ <ixz>0.000503</ixz>
+ <iyy>0.015218</iyy>
+ <iyz>-0.000004</iyz>
+ <izz>0.011764</izz>
+ </inertia>
+ </inertial>
+ <collision name="fr_caster_r_wheel_link_geom">
+ <pose>0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.074792</radius>
+ <length>0.034000</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <visual name="fr_caster_r_wheel_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="torso_lift_link">
+ <self_collide>0</self_collide>
+ <pose>-0.050000 0.000000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>36.449000</mass>
+ <pose>-0.099499 -0.000004 -0.086764 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>2.792330</ixx>
+ <ixy>0.004280</ixy>
+ <ixz>-0.160631</ixz>
+ <iyy>2.521060</iyy>
+ <iyz>0.029689</iyz>
+ <izz>0.536551</izz>
+ </inertia>
+ </inertial>
+ <collision name="torso_lift_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/torso_v0/torso_lift_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="torso_lift_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/torso_v0/torso_lift.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name="head_pan_link">
+ <self_collide>0</self_collide>
+ <pose>-0.067070 0.000000 1.172130 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>6.339000</mass>
+ <pose>0.010907 0.031693 0.090507 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.032498</ixx>
+ <ixy>0.000636</ixy>
+ <ixz>0.002585</ixz>
+ <iyy>0.046546</iyy>
+ <iyz>-0.002453</iyz>
+ <izz>0.057653</izz>
+ </inertia>
+ </inertial>
+ <collision name="head_pan_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/head_v0/head_pan_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="head_pan_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/head_v0/head_pan.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="head_tilt_link">
+ <self_collide>0</self_collide>
+ <pose>0.000930 0.000000 1.172130 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>5.319000</mass>
+ <pose>-0.005151 -0.014635 0.072819 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.126134</ixx>
+ <ixy>0.000237</ixy>
+ <ixz>0.011881</ixz>
+ <iyy>0.165102</iyy>
+ <iyz>0.001606</iyz>
+ <izz>0.166947</izz>
+ </inertia>
+ </inertial>
+ <collision name="head_tilt_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/head_v0/head_tilt_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="head_tilt_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/head_v0/head_tilt.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <collision name="head_tilt_link_geom_head_mount_kinect_ir_link">
+ <pose>-0.147067 0.012500 0.291953 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.000500</radius>
+ </sphere>
+ </geometry>
+ </collision>
+ <visual name="head_tilt_link_geom_head_mount_kinect_ir_link_visual">
+ <pose>-0.147067 0.012500 0.291953 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.000250</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script><name>Gazebo/Red</name></script>
+ </material>
+ </visual>
+ <collision name="head_tilt_link_geom_head_mount_kinect_rgb_link">
+ <pose>-0.147067 0.017500 0.291953 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.000500</radius>
+ </sphere>
+ </geometry>
+ </collision>
+ <visual name="head_tilt_link_geom_head_mount_kinect_rgb_link_visual">
+ <pose>-0.147067 0.017500 0.291953 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.000250</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script><name>Gazebo/Red</name></script>
+ </material>
+ </visual>
+ <collision name="head_tilt_link_geom_head_mount_link">
+ <pose>-0.114800 0.000000 0.155500 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.000500</radius>
+ </sphere>
+ </geometry>
+ </collision>
+ <visual name="head_tilt_link_geom_head_mount_link_visual">
+ <pose>-0.114800 0.000000 0.155500 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_back--coarse.STL</uri>
+ <scale>0.001000 0.001000 0.001000</scale>
+ </mesh>
+ </geometry>
+ </visual>
+ <collision name="head_tilt_link_geom_head_mount_prosilica_link">
+ <pose>-0.161257 0.012500 0.244421 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.000500</radius>
+ </sphere>
+ </geometry>
+ </collision>
+ <visual name="head_tilt_link_geom_head_mount_prosilica_link_visual">
+ <pose>-0.161257 0.012500 0.244421 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <sphere>
+ <radius>0.000250</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script><name>Gazebo/Red</name></script>
+ </material>
+ </visual>
+ <collision name="head_tilt_link_geom_head_plate_frame">
+ <pose>0.023200 0.000000 0.064500 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.010000 0.010000 0.010000</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="head_tilt_link_geom_head_plate_frame_visual">
+ <pose>0.023200 0.000000 0.064500 0.000000 -0.000000 0.000000</pose>
+ <geometry>
+ <box>
+ <size>0.010000 0.010000 0.010000</size>
+ </box>
+ </geometry>
+ <material>
+ <script><name>Gazebo/Grey</name></script>
+ </material>
+ </visual>
+ </link>
+ <link name="l_shoulder_pan_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>-0.050000 0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>25.799300</mass>
+ <pose>-0.001201 0.024513 -0.098231 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.866179</ixx>
+ <ixy>-0.060865</ixy>
+ <ixz>-0.121181</ixz>
+ <iyy>0.874217</iyy>
+ <iyz>-0.058866</iyz>
+ <izz>0.273538</izz>
+ </inertia>
+ </inertial>
+ <collision name="l_shoulder_pan_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/shoulder_pan.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="l_shoulder_pan_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/shoulder_pan.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="l_shoulder_lift_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.050000 0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>2.749880</mass>
+ <pose>0.021950 -0.026640 -0.031270 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.021056</ixx>
+ <ixy>0.004967</ixy>
+ <ixz>-0.001948</ixz>
+ <iyy>0.021272</iyy>
+ <iyz>0.001104</iyz>
+ <izz>0.019758</izz>
+ </inertia>
+ </inertial>
+ <collision name="l_shoulder_lift_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/shoulder_lift.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="l_shoulder_lift_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/shoulder_lift.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="l_upper_arm_roll_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.050000 0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>6.117690</mass>
+ <pose>0.210551 0.016309 -0.000561 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.025306</ixx>
+ <ixy>-0.003393</ixy>
+ <ixz>0.000608</ixz>
+ <iyy>0.084737</iyy>
+ <iyz>-0.000200</iyz>
+ <izz>0.086016</izz>
+ </inertia>
+ </inertial>
+ <collision name="l_upper_arm_roll_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/upper_arm_roll_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="l_upper_arm_roll_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/upper_arm_roll.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <collision name="l_upper_arm_roll_link_geom_l_upper_arm_link">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/upper_arm.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="l_upper_arm_roll_link_geom_l_upper_arm_link_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/upper_arm.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="l_elbow_flex_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.450000 0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>1.903270</mass>
+ <pose>0.010140 0.000320 -0.012110 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.003465</ixx>
+ <ixy>0.000041</ixy>
+ <ixz>0.000432</ixz>
+ <iyy>0.004416</iyy>
+ <iyz>-0.000040</iyz>
+ <izz>0.003592</izz>
+ </inertia>
+ </inertial>
+ <collision name="l_elbow_flex_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/elbow_flex.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="l_elbow_flex_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/elbow_flex.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="l_forearm_roll_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.450000 0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>2.689680</mass>
+ <pose>0.180727 -0.000163 -0.008583 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.014668</ixx>
+ <ixy>0.000052</ixy>
+ <ixz>0.000656</ixz>
+ <iyy>0.026279</iyy>
+ <iyz>-0.000013</iyz>
+ <izz>0.027775</izz>
+ </inertia>
+ </inertial>
+ <collision name="l_forearm_roll_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/forearm_roll_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="l_forearm_roll_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/forearm_roll.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <collision name="l_forearm_roll_link_geom_l_forearm_link">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/forearm.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="l_forearm_roll_link_geom_l_forearm_link_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/forearm.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name="l_wrist_flex_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.771000 0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.614020</mass>
+ <pose>-0.001570 0.000000 -0.000750 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000652</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000003</ixz>
+ <iyy>0.000198</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.000645</izz>
+ </inertia>
+ </inertial>
+ <collision name="l_wrist_flex_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/wrist_flex.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="l_wrist_flex_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/wrist_flex.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="l_wrist_roll_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.771000 0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.681070</mass>
+ <pose>0.056408 0.000451 -0.001014 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.011352</ixx>
+ <ixy>-0.000016</ixy>
+ <ixz>-0.000001</ixz>
+ <iyy>0.011677</iyy>
+ <iyz>-0.000001</iyz>
+ <izz>0.011866</izz>
+ </inertia>
+ </inertial>
+ <collision name="l_wrist_roll_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/wrist_roll_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="l_wrist_roll_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/wrist_roll.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <collision name="l_wrist_roll_link_geom_l_gripper_motor_accelerometer_link">
+ <geometry>
+ <box>
+ <size>0.001000 0.001000 0.001000</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="l_wrist_roll_link_geom_l_gripper_motor_accelerometer_link_visual">
+ <geometry>
+ <box>
+ <size>0.001000 0.001000 0.001000</size>
+ </box>
+ </geometry>
+ </visual>
+ <collision name="l_wrist_roll_link_geom_l_gripper_palm_link">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/gripper_palm.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="l_wrist_roll_link_geom_l_gripper_palm_link_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/gripper_palm.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="l_gripper_l_finger_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.847910 0.198000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.171260</mass>
+ <pose>0.035980 0.017300 -0.001640 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000078</ixx>
+ <ixy>0.000001</ixy>
+ <ixz>-0.000010</ixz>
+ <iyy>0.000197</iyy>
+ <iyz>-0.000003</iyz>
+ <izz>0.000181</izz>
+ </inertia>
+ </inertial>
+ <collision name="l_gripper_l_finger_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>500.000000</mu>
+ <mu2>500.000000</mu2>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <kp>1000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>100.000000</max_vel>
+ <min_depth>0.000000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="l_gripper_l_finger_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="l_gripper_l_finger_tip_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.939280 0.202950 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.044190</mass>
+ <pose>0.004230 0.002840 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000008</ixx>
+ <ixy>0.000006</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.000010</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.000015</izz>
+ </inertia>
+ </inertial>
+ <collision name="l_gripper_l_finger_tip_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger_tip.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>500.000000</mu>
+ <mu2>500.000000</mu2>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <kp>10000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>100.000000</max_vel>
+ <min_depth>0.000000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="l_gripper_l_finger_tip_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger_tip.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name="l_gripper_motor_slider_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.939280 0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.010000</mass>
+ <inertia>
+ <ixx>0.001000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.001000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.001000</izz>
+ </inertia>
+ </inertial>
+ <velocity_decay />
+ </link>
+ <link name="l_gripper_motor_screw_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.939280 0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.010000</mass>
+ <inertia>
+ <ixx>0.000100</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.000100</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.000100</izz>
+ </inertia>
+ </inertial>
+ <velocity_decay />
+ </link>
+ <link name="l_gripper_r_finger_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.847910 0.178000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.173890</mass>
+ <pose>0.035760 -0.017360 -0.000950 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000077</ixx>
+ <ixy>-0.000002</ixy>
+ <ixz>-0.000008</ixz>
+ <iyy>0.000198</iyy>
+ <iyz>0.000002</iyz>
+ <izz>0.000181</izz>
+ </inertia>
+ </inertial>
+ <collision name="l_gripper_r_finger_link_geom">
+ <pose>0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000</pose>
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>500.000000</mu>
+ <mu2>500.000000</mu2>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <kp>1000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>100.000000</max_vel>
+ <min_depth>0.000000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="l_gripper_r_finger_link_geom_visual">
+ <pose>0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000</pose>
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="l_gripper_r_finger_tip_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.939280 0.173050 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.044190</mass>
+ <pose>0.004230 -0.002840 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000008</ixx>
+ <ixy>-0.000006</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.000010</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.000015</izz>
+ </inertia>
+ </inertial>
+ <collision name="l_gripper_r_finger_tip_link_geom">
+ <pose>0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000</pose>
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger_tip.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>500.000000</mu>
+ <mu2>500.000000</mu2>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <kp>10000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>100.000000</max_vel>
+ <min_depth>0.000000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="l_gripper_r_finger_tip_link_geom_visual">
+ <pose>0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000</pose>
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger_tip.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name="laser_tilt_mount_link">
+ <self_collide>0</self_collide>
+ <pose>0.048930 0.000000 1.017680 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.592000</mass>
+ <pose>-0.001134 0.001667 -0.007067 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.001296</ixx>
+ <ixy>0.000023</ixy>
+ <ixz>0.000037</ixz>
+ <iyy>0.001086</iyy>
+ <iyz>0.000035</iyz>
+ <izz>0.000895</izz>
+ </inertia>
+ </inertial>
+ <collision name="laser_tilt_mount_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/tilting_laser_v0/tilting_hokuyo_L.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>0.000000</mu>
+ <mu2>0.000000</mu2>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <kp>1000000000000.000000</kp>
+ <kd>10000.000000</kd>
+ <max_vel>100.000000</max_vel>
+ <min_depth>0.000000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="laser_tilt_mount_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/tilting_laser_v0/tilting_hokuyo.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name="r_shoulder_pan_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>-0.050000 -0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>25.799300</mass>
+ <pose>-0.001201 0.024513 -0.098231 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.866179</ixx>
+ <ixy>-0.060865</ixy>
+ <ixz>-0.121181</ixz>
+ <iyy>0.874217</iyy>
+ <iyz>-0.058866</iyz>
+ <izz>0.273538</izz>
+ </inertia>
+ </inertial>
+ <collision name="r_shoulder_pan_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/shoulder_pan.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>0.000000</mu>
+ <mu2>0.000000</mu2>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <kp>1000000000000.000000</kp>
+ <kd>10000.000000</kd>
+ <max_vel>100.000000</max_vel>
+ <min_depth>0.000000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="r_shoulder_pan_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/shoulder_pan.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="r_shoulder_lift_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.050000 -0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>2.749880</mass>
+ <pose>0.021950 -0.026640 -0.031270 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.021056</ixx>
+ <ixy>0.004967</ixy>
+ <ixz>-0.001948</ixz>
+ <iyy>0.021272</iyy>
+ <iyz>0.001104</iyz>
+ <izz>0.019758</izz>
+ </inertia>
+ </inertial>
+ <collision name="r_shoulder_lift_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/shoulder_lift.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>0.000000</mu>
+ <mu2>0.000000</mu2>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <kp>1000000000000.000000</kp>
+ <kd>10000.000000</kd>
+ <max_vel>100.000000</max_vel>
+ <min_depth>0.000000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="r_shoulder_lift_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/shoulder_lift.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="r_upper_arm_roll_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.050000 -0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>6.117690</mass>
+ <pose>0.210482 -0.015945 -0.000197 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.025378</ixx>
+ <ixy>0.003757</ixy>
+ <ixz>-0.000709</ixz>
+ <iyy>0.084737</iyy>
+ <iyz>-0.000179</iyz>
+ <izz>0.086088</izz>
+ </inertia>
+ </inertial>
+ <collision name="r_upper_arm_roll_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/upper_arm_roll_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="r_upper_arm_roll_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/shoulder_v0/upper_arm_roll.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <collision name="r_upper_arm_roll_link_geom_r_upper_arm_link">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/upper_arm.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="r_upper_arm_roll_link_geom_r_upper_arm_link_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/upper_arm.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="r_elbow_flex_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.450000 -0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>1.903270</mass>
+ <pose>0.010140 0.000320 -0.012110 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.003465</ixx>
+ <ixy>0.000041</ixy>
+ <ixz>0.000432</ixz>
+ <iyy>0.004416</iyy>
+ <iyz>-0.000040</iyz>
+ <izz>0.003592</izz>
+ </inertia>
+ </inertial>
+ <collision name="r_elbow_flex_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/elbow_flex.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="r_elbow_flex_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/elbow_flex.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="r_forearm_roll_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.450000 -0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>2.689680</mass>
+ <pose>0.180727 -0.000163 -0.008583 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.014668</ixx>
+ <ixy>0.000052</ixy>
+ <ixz>0.000656</ixz>
+ <iyy>0.026279</iyy>
+ <iyz>-0.000013</iyz>
+ <izz>0.027775</izz>
+ </inertia>
+ </inertial>
+ <collision name="r_forearm_roll_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/forearm_roll_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="r_forearm_roll_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/upper_arm_v0/forearm_roll.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <collision name="r_forearm_roll_link_geom_r_forearm_link">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/forearm.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="r_forearm_roll_link_geom_r_forearm_link_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/forearm.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name="r_wrist_flex_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.771000 -0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.614020</mass>
+ <pose>-0.001570 0.000000 -0.000750 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000652</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000003</ixz>
+ <iyy>0.000198</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.000645</izz>
+ </inertia>
+ </inertial>
+ <collision name="r_wrist_flex_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/wrist_flex.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="r_wrist_flex_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/wrist_flex.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="r_wrist_roll_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.771000 -0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.681070</mass>
+ <pose>0.056408 0.000451 -0.001014 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.011352</ixx>
+ <ixy>-0.000016</ixy>
+ <ixz>-0.000001</ixz>
+ <iyy>0.011677</iyy>
+ <iyz>-0.000001</iyz>
+ <izz>0.011866</izz>
+ </inertia>
+ </inertial>
+ <collision name="r_wrist_roll_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/wrist_roll_L.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="r_wrist_roll_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/forearm_v0/wrist_roll.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <collision name="r_wrist_roll_link_geom_r_gripper_motor_accelerometer_link">
+ <geometry>
+ <box>
+ <size>0.001000 0.001000 0.001000</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="r_wrist_roll_link_geom_r_gripper_motor_accelerometer_link_visual">
+ <geometry>
+ <box>
+ <size>0.001000 0.001000 0.001000</size>
+ </box>
+ </geometry>
+ </visual>
+ <collision name="r_wrist_roll_link_geom_r_gripper_palm_link">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/gripper_palm.stl</uri>
+ </mesh>
+ </geometry>
+ </collision>
+ <visual name="r_wrist_roll_link_geom_r_gripper_palm_link_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/gripper_palm.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="r_gripper_l_finger_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.847910 -0.178000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.171260</mass>
+ <pose>0.035980 0.017300 -0.001640 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000078</ixx>
+ <ixy>0.000001</ixy>
+ <ixz>0.000010</ixz>
+ <iyy>0.000197</iyy>
+ <iyz>-0.000003</iyz>
+ <izz>0.000181</izz>
+ </inertia>
+ </inertial>
+ <collision name="r_gripper_l_finger_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>500.000000</mu>
+ <mu2>500.000000</mu2>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <kp>1000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>100.000000</max_vel>
+ <min_depth>0.000000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="r_gripper_l_finger_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="r_gripper_l_finger_tip_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.939280 -0.173050 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.044190</mass>
+ <pose>0.004230 0.002840 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000008</ixx>
+ <ixy>0.000006</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.000010</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.000015</izz>
+ </inertia>
+ </inertial>
+ <collision name="r_gripper_l_finger_tip_link_geom">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger_tip.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>500.000000</mu>
+ <mu2>500.000000</mu2>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <kp>10000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>100.000000</max_vel>
+ <min_depth>0.000000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="r_gripper_l_finger_tip_link_geom_visual">
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger_tip.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name="r_gripper_motor_slider_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.939280 -0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.010000</mass>
+ <inertia>
+ <ixx>0.001000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.001000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.001000</izz>
+ </inertia>
+ </inertial>
+ <velocity_decay />
+ </link>
+ <link name="r_gripper_motor_screw_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.939280 -0.188000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.010000</mass>
+ <inertia>
+ <ixx>0.000100</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.000100</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.000100</izz>
+ </inertia>
+ </inertial>
+ <velocity_decay />
+ </link>
+ <link name="r_gripper_r_finger_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.847910 -0.198000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.173890</mass>
+ <pose>0.035760 -0.017360 -0.000950 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000077</ixx>
+ <ixy>-0.000002</ixy>
+ <ixz>-0.000008</ixz>
+ <iyy>0.000198</iyy>
+ <iyz>0.000002</iyz>
+ <izz>0.000181</izz>
+ </inertia>
+ </inertial>
+ <collision name="r_gripper_r_finger_link_geom">
+ <pose>0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000</pose>
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>500.000000</mu>
+ <mu2>500.000000</mu2>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <kp>1000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>100.000000</max_vel>
+ <min_depth>0.000000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="r_gripper_r_finger_link_geom_visual">
+ <pose>0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000</pose>
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ <velocity_decay />
+ </link>
+ <link name="r_gripper_r_finger_tip_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.939280 -0.202950 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.044190</mass>
+ <pose>0.004230 -0.002840 0.000000 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000008</ixx>
+ <ixy>-0.000006</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.000010</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.000015</izz>
+ </inertia>
+ </inertial>
+ <collision name="r_gripper_r_finger_tip_link_geom">
+ <pose>0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000</pose>
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger_tip.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>500.000000</mu>
+ <mu2>500.000000</mu2>
+ <slip1>0.000000</slip1>
+ <slip2>0.000000</slip2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <kp>10000000.000000</kp>
+ <kd>1.000000</kd>
+ <max_vel>100.000000</max_vel>
+ <min_depth>0.000000</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="r_gripper_r_finger_tip_link_geom_visual">
+ <pose>0.000000 0.000000 0.000000 -3.141590 0.000000 0.000000</pose>
+ <geometry>
+ <mesh>
+ <uri>model://pr2/meshes/gripper_v0/l_finger_tip.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+ <link name="torso_lift_motor_screw_link">
+ <self_collide>0</self_collide>
+ <pose>-0.150000 0.000000 0.751000 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>1.000000</mass>
+ <inertia>
+ <ixx>0.001000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.001000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.001000</izz>
+ </inertia>
+ </inertial>
+ <velocity_decay />
+ </link>
+ <link name="r_gripper_l_parallel_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.829910 -0.157000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.171260</mass>
+ <pose>0.035980 0.017300 -0.001640 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000078</ixx>
+ <ixy>0.000001</ixy>
+ <ixz>-0.000010</ixz>
+ <iyy>0.000197</iyy>
+ <iyz>-0.000003</iyz>
+ <izz>0.000181</izz>
+ </inertia>
+ </inertial>
+ <velocity_decay />
+ </link>
+ <link name="r_gripper_r_parallel_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.829910 -0.219000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.173890</mass>
+ <pose>0.035760 -0.017360 -0.000950 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000077</ixx>
+ <ixy>-0.000002</ixy>
+ <ixz>-0.000008</ixz>
+ <iyy>0.000198</iyy>
+ <iyz>0.000002</iyz>
+ <izz>0.000181</izz>
+ </inertia>
+ </inertial>
+ <velocity_decay />
+ </link>
+ <link name="l_gripper_l_parallel_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.829910 0.219000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.171260</mass>
+ <pose>0.035980 0.017300 -0.001640 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000078</ixx>
+ <ixy>0.000001</ixy>
+ <ixz>0.000010</ixz>
+ <iyy>0.000197</iyy>
+ <iyz>-0.000003</iyz>
+ <izz>0.000181</izz>
+ </inertia>
+ </inertial>
+ <velocity_decay />
+ </link>
+ <link name="l_gripper_r_parallel_link">
+ <self_collide>0</self_collide>
+ <gravity>0</gravity>
+ <pose>0.829910 0.157000 0.790675 0.000000 -0.000000 0.000000</pose>
+ <inertial>
+ <mass>0.173890</mass>
+ <pose>0.035760 -0.017360 -0.000950 0.000000 -0.000000 0.000000</pose>
+ <inertia>
+ <ixx>0.000077</ixx>
+ <ixy>-0.000002</ixy>
+ <ixz>0.000008</ixz>
+ <iyy>0.000198</iyy>
+ <iyz>0.000002</iyz>
+ <izz>0.000181</izz>
+ </inertia>
+ </inertial>
+ <velocity_decay />
+ </link>
+ <joint name="bl_caster_rotation_joint" type="revolute">
+ <parent>base_footprint</parent>
+ <child>bl_caster_rotation_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="bl_caster_l_wheel_joint" type="revolute">
+ <parent>bl_caster_rotation_link</parent>
+ <child>bl_caster_l_wheel_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="bl_caster_r_wheel_joint" type="revolute">
+ <parent>bl_caster_rotation_link</parent>
+ <child>bl_caster_r_wheel_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="br_caster_rotation_joint" type="revolute">
+ <parent>base_footprint</parent>
+ <child>br_caster_rotation_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="br_caster_l_wheel_joint" type="revolute">
+ <parent>br_caster_rotation_link</parent>
+ <child>br_caster_l_wheel_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="br_caster_r_wheel_joint" type="revolute">
+ <parent>br_caster_rotation_link</parent>
+ <child>br_caster_r_wheel_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="fl_caster_rotation_joint" type="revolute">
+ <parent>base_footprint</parent>
+ <child>fl_caster_rotation_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="fl_caster_l_wheel_joint" type="revolute">
+ <parent>fl_caster_rotation_link</parent>
+ <child>fl_caster_l_wheel_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="fl_caster_r_wheel_joint" type="revolute">
+ <parent>fl_caster_rotation_link</parent>
+ <child>fl_caster_r_wheel_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="fr_caster_rotation_joint" type="revolute">
+ <parent>base_footprint</parent>
+ <child>fr_caster_rotation_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="fr_caster_l_wheel_joint" type="revolute">
+ <parent>fr_caster_rotation_link</parent>
+ <child>fr_caster_l_wheel_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="fr_caster_r_wheel_joint" type="revolute">
+ <parent>fr_caster_rotation_link</parent>
+ <child>fr_caster_r_wheel_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="torso_lift_joint" type="prismatic">
+ <parent>base_footprint</parent>
+ <child>torso_lift_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <limit>
+ <lower>0.000000</lower>
+ <upper>0.330000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="head_pan_joint" type="revolute">
+ <parent>torso_lift_link</parent>
+ <child>head_pan_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>2.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-3.006993</lower>
+ <upper>3.006993</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="head_tilt_joint" type="revolute">
+ <parent>head_pan_link</parent>
+ <child>head_tilt_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>10.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-0.471237</lower>
+ <upper>1.396260</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_shoulder_pan_joint" type="revolute">
+ <parent>torso_lift_link</parent>
+ <child>l_shoulder_pan_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>10.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-0.714602</lower>
+ <upper>2.285404</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_shoulder_lift_joint" type="revolute">
+ <parent>l_shoulder_pan_link</parent>
+ <child>l_shoulder_lift_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>10.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-0.523601</lower>
+ <upper>1.396300</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_upper_arm_roll_joint" type="revolute">
+ <parent>l_shoulder_lift_link</parent>
+ <child>l_upper_arm_roll_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.100000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-0.800000</lower>
+ <upper>3.900008</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_elbow_flex_joint" type="revolute">
+ <parent>l_upper_arm_roll_link</parent>
+ <child>l_elbow_flex_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>1.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-2.321305</lower>
+ <upper>0.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_forearm_roll_joint" type="revolute">
+ <parent>l_elbow_flex_link</parent>
+ <child>l_forearm_roll_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.100000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_wrist_flex_joint" type="revolute">
+ <parent>l_forearm_roll_link</parent>
+ <child>l_wrist_flex_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.100000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-2.180004</lower>
+ <upper>0.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_wrist_roll_joint" type="revolute">
+ <parent>l_wrist_flex_link</parent>
+ <child>l_wrist_roll_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.100000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_l_finger_joint" type="revolute">
+ <parent>l_wrist_roll_link</parent>
+ <child>l_gripper_l_finger_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>0.020000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>0.000000</lower>
+ <upper>0.548000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_l_finger_tip_joint" type="revolute">
+ <parent>l_gripper_l_finger_link</parent>
+ <child>l_gripper_l_finger_tip_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 -1.000000</xyz>
+ <dynamics>
+ <damping>0.001000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>0.000000</lower>
+ <upper>0.548000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_motor_slider_joint" type="prismatic">
+ <parent>l_wrist_roll_link</parent>
+ <child>l_gripper_motor_slider_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <limit>
+ <lower>-0.100000</lower>
+ <upper>0.100000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_motor_screw_joint" type="revolute">
+ <parent>l_gripper_motor_slider_link</parent>
+ <child>l_gripper_motor_screw_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.000100</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_r_finger_joint" type="revolute">
+ <parent>l_wrist_roll_link</parent>
+ <child>l_gripper_r_finger_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 -1.000000</xyz>
+ <dynamics>
+ <damping>0.020000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>0.000000</lower>
+ <upper>0.548000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_r_finger_tip_joint" type="revolute">
+ <parent>l_gripper_r_finger_link</parent>
+ <child>l_gripper_r_finger_tip_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>0.001000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>0.000000</lower>
+ <upper>0.548000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="laser_tilt_mount_joint" type="revolute">
+ <parent>torso_lift_link</parent>
+ <child>laser_tilt_mount_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.008000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-0.785400</lower>
+ <upper>1.483530</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_shoulder_pan_joint" type="revolute">
+ <parent>torso_lift_link</parent>
+ <child>r_shoulder_pan_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>10.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-2.285404</lower>
+ <upper>0.714602</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_shoulder_lift_joint" type="revolute">
+ <parent>r_shoulder_pan_link</parent>
+ <child>r_shoulder_lift_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>10.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-0.523601</lower>
+ <upper>1.396300</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_upper_arm_roll_joint" type="revolute">
+ <parent>r_shoulder_lift_link</parent>
+ <child>r_upper_arm_roll_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.100000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-3.900008</lower>
+ <upper>0.800000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_elbow_flex_joint" type="revolute">
+ <parent>r_upper_arm_roll_link</parent>
+ <child>r_elbow_flex_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>1.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-2.321305</lower>
+ <upper>0.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_forearm_roll_joint" type="revolute">
+ <parent>r_elbow_flex_link</parent>
+ <child>r_forearm_roll_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.100000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_wrist_flex_joint" type="revolute">
+ <parent>r_forearm_roll_link</parent>
+ <child>r_wrist_flex_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.100000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-2.180004</lower>
+ <upper>0.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_wrist_roll_joint" type="revolute">
+ <parent>r_wrist_flex_link</parent>
+ <child>r_wrist_roll_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.100000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_l_finger_joint" type="revolute">
+ <parent>r_wrist_roll_link</parent>
+ <child>r_gripper_l_finger_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>0.020000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>0.000000</lower>
+ <upper>0.548000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_l_finger_tip_joint" type="revolute">
+ <parent>r_gripper_l_finger_link</parent>
+ <child>r_gripper_l_finger_tip_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 -1.000000</xyz>
+ <dynamics>
+ <damping>0.001000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>0.000000</lower>
+ <upper>0.548000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_motor_slider_joint" type="prismatic">
+ <parent>r_wrist_roll_link</parent>
+ <child>r_gripper_motor_slider_link</child>
+ <axis>
+ <xyz>1.000000 0.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-0.100000</lower>
+ <upper>0.100000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_motor_screw_joint" type="revolute">
+ <parent>r_gripper_motor_slider_link</parent>
+ <child>r_gripper_motor_screw_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.000100</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_r_finger_joint" type="revolute">
+ <parent>r_wrist_roll_link</parent>
+ <child>r_gripper_r_finger_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 -1.000000</xyz>
+ <dynamics>
+ <damping>0.020000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>0.000000</lower>
+ <upper>0.548000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_r_finger_tip_joint" type="revolute">
+ <parent>r_gripper_r_finger_link</parent>
+ <child>r_gripper_r_finger_tip_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>0.001000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>0.000000</lower>
+ <upper>0.548000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="torso_lift_motor_screw_joint" type="revolute">
+ <parent>base_footprint</parent>
+ <child>torso_lift_motor_screw_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>0.000100</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="torso_lift_screw_torso_lift_joint" type="screw">
+ <parent>torso_lift_link</parent>
+ <child>torso_lift_motor_screw_link</child>
+ <thread_pitch>3141.600000</thread_pitch>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_r_screw_screw_joint" type="screw">
+ <parent>r_gripper_r_finger_tip_link</parent>
+ <child>r_gripper_motor_screw_link</child>
+ <thread_pitch>-3141.600000</thread_pitch>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_l_screw_screw_joint" type="screw">
+ <parent>r_gripper_l_finger_tip_link</parent>
+ <child>r_gripper_motor_screw_link</child>
+ <thread_pitch>3141.600000</thread_pitch>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_r_parallel_root_joint" type="revolute">
+ <pose>0.058910 -0.031000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_gripper_r_parallel_link</parent>
+ <child>r_wrist_roll_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 -1.000000</xyz>
+ <dynamics>
+ <damping>0.200000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_l_parallel_root_joint" type="revolute">
+ <pose>0.058910 0.031000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_gripper_l_parallel_link</parent>
+ <child>r_wrist_roll_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>0.200000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_r_parallel_tip_joint" type="revolute">
+ <pose>-0.018000 -0.021000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_gripper_r_parallel_link</parent>
+ <child>r_gripper_r_finger_tip_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_l_parallel_tip_joint" type="revolute">
+ <pose>-0.018000 0.021000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>r_gripper_l_parallel_link</parent>
+ <child>r_gripper_l_finger_tip_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="r_gripper_joint" type="prismatic">
+ <parent>r_gripper_r_finger_tip_link</parent>
+ <child>r_gripper_l_finger_tip_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_r_screw_screw_joint" type="screw">
+ <parent>l_gripper_r_finger_tip_link</parent>
+ <child>l_gripper_motor_screw_link</child>
+ <thread_pitch>-3141.600000</thread_pitch>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_l_screw_screw_joint" type="screw">
+ <parent>l_gripper_l_finger_tip_link</parent>
+ <child>l_gripper_motor_screw_link</child>
+ <thread_pitch>3141.600000</thread_pitch>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_r_parallel_root_joint" type="revolute">
+ <pose>0.058910 -0.031000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_gripper_r_parallel_link</parent>
+ <child>l_wrist_roll_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 -1.000000</xyz>
+ <dynamics>
+ <damping>0.200000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_l_parallel_root_joint" type="revolute">
+ <pose>0.058910 0.031000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_gripper_l_parallel_link</parent>
+ <child>l_wrist_roll_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>0.200000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_r_parallel_tip_joint" type="revolute">
+ <pose>-0.018000 -0.021000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_gripper_r_parallel_link</parent>
+ <child>l_gripper_r_finger_tip_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <dynamics>
+ <damping>0.000000</damping>
+ <friction>0.000000</friction>
+ </dynamics>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_l_parallel_tip_joint" type="revolute">
+ <pose>-0.018000 0.021000 0.000000 0.000000 -0.000000 0.000000</pose>
+ <parent>l_gripper_l_parallel_link</parent>
+ <child>l_gripper_l_finger_tip_link</child>
+ <axis>
+ <xyz>0.000000 0.000000 1.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ <joint name="l_gripper_joint" type="prismatic">
+ <parent>l_gripper_r_finger_tip_link</parent>
+ <child>l_gripper_l_finger_tip_link</child>
+ <axis>
+ <xyz>0.000000 1.000000 0.000000</xyz>
+ <limit>
+ <lower>-10000000000000000.000000</lower>
+ <upper>10000000000000000.000000</upper>
+ <effort>0.000000</effort>
+ <velocity>4.000000</velocity>
+ </limit>
+ <use_parent_model_frame>true</use_parent_model_frame>
+ </axis>
+ </joint>
+ </model>
+
+ </world>
+</sdf>
diff --git a/test/worlds/presets.world b/test/worlds/presets.world
new file mode 100644
index 0000000..109b73c
--- /dev/null
+++ b/test/worlds/presets.world
@@ -0,0 +1,75 @@
+<sdf version="1.5">
+ <world name="default">
+ <!-- A ground plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <physics name="preset_1" type="ode" default="true">
+ <max_step_size>0.01</max_step_size>
+ <ode>
+ <solver>
+ <min_step_size>0.001</min_step_size>
+ <iters>50</iters>
+ <sor>1.4</sor>
+ <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+ </solver>
+ <constraints>
+ <cfm>0.01</cfm>
+ <erp>0.3</erp>
+ <contact_max_correcting_vel>200</contact_max_correcting_vel>
+ <contact_surface_layer>0.002</contact_surface_layer>
+ </constraints>
+ </ode>
+ <simbody>
+ <min_step_size>0.001</min_step_size>
+ <accuracy>0.01</accuracy>
+ <max_transient_velocity>0.001</max_transient_velocity>
+ </simbody>
+ <bullet>
+ <solver>
+ <min_step_size>0.001</min_step_size>
+ <iters>50</iters>
+ <sor>1.4</sor>
+ </solver>
+ <constraints>
+ <cfm>0.01</cfm>
+ <erp>0.3</erp>
+ <split_impulse>false</split_impulse>
+ <contact_surface_layer>0.002</contact_surface_layer>
+ </constraints>
+ </bullet>
+ </physics>
+
+ <physics name="preset_2" type="ode">
+ <max_step_size>0.02</max_step_size>
+ <ode>
+ <solver>
+ <min_step_size>0.002</min_step_size>
+ <iters>100</iters>
+ <sor>1.5</sor>
+ </solver>
+ <constraints>
+ <cfm>0.02</cfm>
+ <erp>0.6</erp>
+ </constraints>
+ </ode>
+ </physics>
+
+ <physics name="unused" type="ode" default="true">
+ <max_step_size>0.04</max_step_size>
+ <ode>
+ <solver>
+ <min_step_size>0.004</min_step_size>
+ <iters>200</iters>
+ <sor>1.3</sor>
+ </solver>
+ <constraints>
+ <cfm>0.05</cfm>
+ <erp>0.7</erp>
+ </constraints>
+ </ode>
+ </physics>
+
+ </world>
+</sdf>
diff --git a/test/worlds/presets_nodefault.world b/test/worlds/presets_nodefault.world
new file mode 100644
index 0000000..611b238
--- /dev/null
+++ b/test/worlds/presets_nodefault.world
@@ -0,0 +1,75 @@
+<sdf version="1.5">
+ <world name="default">
+ <!-- A ground plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <physics name="preset_1" type="ode">
+ <max_step_size>0.01</max_step_size>
+ <ode>
+ <solver>
+ <min_step_size>0.001</min_step_size>
+ <iters>50</iters>
+ <sor>1.4</sor>
+ <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+ </solver>
+ <constraints>
+ <cfm>0.01</cfm>
+ <erp>0.3</erp>
+ <contact_max_correcting_vel>200</contact_max_correcting_vel>
+ <contact_surface_layer>0.002</contact_surface_layer>
+ </constraints>
+ </ode>
+ <simbody>
+ <min_step_size>0.001</min_step_size>
+ <accuracy>0.01</accuracy>
+ <max_transient_velocity>0.001</max_transient_velocity>
+ </simbody>
+ <bullet>
+ <solver>
+ <min_step_size>0.001</min_step_size>
+ <iters>50</iters>
+ <sor>1.4</sor>
+ </solver>
+ <constraints>
+ <cfm>0.01</cfm>
+ <erp>0.3</erp>
+ <split_impulse>false</split_impulse>
+ <contact_surface_layer>0.002</contact_surface_layer>
+ </constraints>
+ </bullet>
+ </physics>
+
+ <physics name="preset_2" type="ode">
+ <max_step_size>0.02</max_step_size>
+ <ode>
+ <solver>
+ <min_step_size>0.002</min_step_size>
+ <iters>100</iters>
+ <sor>1.5</sor>
+ </solver>
+ <constraints>
+ <cfm>0.02</cfm>
+ <erp>0.6</erp>
+ </constraints>
+ </ode>
+ </physics>
+
+ <physics name="unused" type="ode">
+ <max_step_size>0.04</max_step_size>
+ <ode>
+ <solver>
+ <min_step_size>0.004</min_step_size>
+ <iters>200</iters>
+ <sor>1.3</sor>
+ </solver>
+ <constraints>
+ <cfm>0.05</cfm>
+ <erp>0.7</erp>
+ </constraints>
+ </ode>
+ </physics>
+
+ </world>
+</sdf>
diff --git a/test/worlds/ray_test.world b/test/worlds/ray_test.world
index ee3d7c1..0dc5702 100644
--- a/test/worlds/ray_test.world
+++ b/test/worlds/ray_test.world
@@ -58,6 +58,9 @@
</box>
</geometry>
</collision>
+ <sensor name="box_imu_sensor" type="imu">
+ <always_on>1</always_on>
+ </sensor>
</link>
</model>
diff --git a/test/worlds/rest_web.world b/test/worlds/rest_web.world
new file mode 100644
index 0000000..2836e77
--- /dev/null
+++ b/test/worlds/rest_web.world
@@ -0,0 +1,133 @@
+<?xml version="1.0"?>
+<sdf version="1.5">
+ <world name="default">
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+
+ <!-- bowls around start region-->
+ <include>
+ <pose>-3 -3 0 0 0 0</pose>
+ <uri>model://bowl</uri>
+ </include>
+ <include>
+ <pose>3 3 0 0 0 0</pose>
+ <uri>model://bowl</uri>
+ </include>
+ <include>
+ <pose>-3 3 0 0 0 0</pose>
+ <uri>model://bowl</uri>
+ </include>
+ <include>
+ <pose>3 -3 0 0 0 0</pose>
+ <uri>model://bowl</uri>
+ </include>
+
+ <!-- bowls around end region -->
+ <include>
+ <pose>7 7 0 0 0 0</pose>
+ <uri>model://bowl</uri>
+ </include>
+ <include>
+ <pose>13 13 0 0 0 0</pose>
+ <uri>model://bowl</uri>
+ </include>
+ <include>
+ <pose>7 13 0 0 0 0</pose>
+ <uri>model://bowl</uri>
+ </include>
+ <include>
+ <pose>13 7 0 0 0 0</pose>
+ <uri>model://bowl</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 -3</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>
+ </plugin>
+
+
+
+ <!--
+ spawn a few beers in the world
+ -->
+ <include>
+ <pose>0 0 0 0 0 0</pose>
+ <name>can1</name>
+ <uri>model://beer</uri>
+ </include>
+
+ </world>
+</sdf>
diff --git a/test/worlds/revolute_joint_test.world b/test/worlds/revolute_joint_test.world
index e0640bf..56fb750 100644
--- a/test/worlds/revolute_joint_test.world
+++ b/test/worlds/revolute_joint_test.world
@@ -35,7 +35,6 @@
<uri>model://double_pendulum_with_base</uri>
<name>pendulum_225deg</name>
<pose>1.48 -1.48 0 0 0 -2.3562</pose>
- <pose>0 -2.1 0 0 0</pose>
</include>
<include>
<uri>model://double_pendulum_with_base</uri>
diff --git a/test/worlds/test_altimeter_linear_angular.world b/test/worlds/test_altimeter_linear_angular.world
new file mode 100644
index 0000000..f7091de
--- /dev/null
+++ b/test/worlds/test_altimeter_linear_angular.world
@@ -0,0 +1,85 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <model name ='model'>
+ <pose>0 0 1 0 0 0</pose>
+
+ <link name='baselink'>
+ <inertial>
+ <mass>1.0</mass>
+ <inertia>
+ <ixx>0.0016667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.0016667</iyy>
+ <iyz>0</iyz>
+ <izz>0.0016667</izz>
+ </inertia>
+ </inertial>
+ <collision name='collision'>
+ <geometry>
+ <box><size>0.1 .1 0.1</size></box>
+ </geometry>
+ </collision>"
+ <visual name='visual'>
+ <geometry>
+ <box><size>0.1 0.1 0.1</size></box>
+ </geometry>"
+ </visual>
+ </link>
+
+ <link name='link'>
+ <inertial>
+ <mass>1.0</mass>
+ <inertia>
+ <ixx>8.33416667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.00166667</iyy>
+ <iyz>0</iyz>
+ <izz>8.33416667</izz>
+ </inertia>
+ </inertial>
+ <pose>0 5 0 0 0 0</pose>
+ <collision name='contact_collision'>
+ <geometry>
+ <box><size>0.1 10 0.1</size></box>
+ </geometry>
+ </collision>"
+ <visual name='visual'>
+ <geometry>
+ <box><size>0.1 10 0.1</size></box>
+ </geometry>"
+ </visual>
+ <sensor name='altimeter' type='altimeter'>
+ <pose>0 5 0 0 0 0</pose>
+ <always_on>true</always_on>
+ <altimeter>
+ </altimeter>
+ </sensor>
+ </link>
+
+ <joint name="joint" type="revolute">
+ <pose>0 -5 0 0 0 0</pose>
+ <parent>baselink</parent>
+ <child>link</child>
+ <axis>
+ <xyz>1 0 0</xyz>
+ </axis>
+ </joint>
+
+ <joint name="joint_drop" type="prismatic">
+ <pose>0 -5 0 0 0 0</pose>
+ <parent>world</parent>
+ <child>baselink</child>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <dynamics>
+ <damping>100</damping>
+ </dynamics>
+ </axis>
+ </joint>
+
+ </model>
+ </world>
+</sdf>
diff --git a/test/worlds/test_altimeter_rotation.world b/test/worlds/test_altimeter_rotation.world
new file mode 100644
index 0000000..27dfea2
--- /dev/null
+++ b/test/worlds/test_altimeter_rotation.world
@@ -0,0 +1,46 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <model name ='model'>
+ <pose>0 0 1 0 0 0</pose>
+ <link name='link'>
+ <inertial>
+ <mass>1.0</mass>
+ <inertia>
+ <ixx>8.33416667</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.00166667</iyy>
+ <iyz>0</iyz>
+ <izz>8.33416667</izz>
+ </inertia>
+ </inertial>
+ <pose>0 5 0 0 0 0</pose>
+ <collision name='contact_collision'>
+ <geometry>
+ <box><size>0.1 10 0.1</size></box>
+ </geometry>
+ </collision>
+ <visual name='visual'>
+ <geometry>
+ <box><size>0.1 10 0.1</size></box>
+ </geometry>
+ </visual>
+ <sensor name='altimeter' type='altimeter'>
+ <pose>0 5 0 0 0 0</pose>
+ <always_on>true</always_on>
+ <altimeter>
+ </altimeter>
+ </sensor>
+ </link>
+ <joint name="joint" type="revolute">
+ <pose>0 -5 0 0 0 0</pose>
+ <parent>world</parent>
+ <child>link</child>
+ <axis>
+ <xyz>1 0 0</xyz>
+ </axis>
+ </joint>
+ </model>
+ </world>
+</sdf>
diff --git a/test/worlds/test_layers.world b/test/worlds/test_layers.world
new file mode 100644
index 0000000..a344a55
--- /dev/null
+++ b/test/worlds/test_layers.world
@@ -0,0 +1,41 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <scene>
+ <ambient>1 1 1 1</ambient>
+ <background>0 0 0 1</background>
+ <grid>false</grid>
+ </scene>
+
+ <gui>
+ <camera name="user">
+ <pose>1 0 .5 0 0 3.1415</pose>
+ </camera>
+ </gui>
+
+ <model name="box">
+ <static>true</static>
+ <pose>0 0 0.5 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <meta>
+ <layer>0</layer>
+ </meta>
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ </world>
+</sdf>
diff --git a/test/worlds/transporter_test.world b/test/worlds/transporter_test.world
new file mode 100644
index 0000000..bfbf3dd
--- /dev/null
+++ b/test/worlds/transporter_test.world
@@ -0,0 +1,86 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <scene>
+ <ambient>1 1 1 1</ambient>
+ <background>0 0 0 0</background>
+ <grid>false</grid>
+ <origin_visual>false</origin_visual>
+ </scene>
+ <gui>
+ <camera name="user_camera">
+ <pose>0 0 1.2 0 1.5707 0</pose>
+ </camera>
+ </gui>
+
+ <model name="ground_plane">
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>10 10</size>
+ </plane>
+ </geometry>
+ </collision>
+ </link>
+ </model>
+
+ <model name="box">
+ <pose>10 10 0.5 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>10 10 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>10 10 1</size>
+ </box>
+ </geometry>
+ <material>
+ <script>Gazebo/White</script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <plugin filename="libTransporterPlugin.so" name="transporter">
+ <activation_topic>~/transporter</activation_topic>
+
+ <pad name="pad1">
+ <destination>pad2</destination>
+ <activation>auto</activation>
+
+ <outgoing>
+ <min>100 -0.5 0</min>
+ <max>101 0.5 1</max>
+ </outgoing>
+
+ <incoming>
+ <pose>0 0 0.5 0 0 0</pose>
+ </incoming>
+ </pad>
+
+ <pad name="pad2">
+ <destination>pad1</destination>
+ <activation>manual</activation>
+
+ <outgoing>
+ <min>-101 -0.5 0</min>
+ <max>-100 0.5 1</max>
+ </outgoing>
+
+ <incoming>
+ <pose>0 0 0.5 0 0 0</pose>
+ </incoming>
+ </pad>
+ </plugin>
+
+ </world>
+</sdf>
diff --git a/test/worlds/usercamera_test.world b/test/worlds/usercamera_test.world
new file mode 100644
index 0000000..994c4e9
--- /dev/null
+++ b/test/worlds/usercamera_test.world
@@ -0,0 +1,40 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+ <world name="default">
+ <scene>
+ <ambient>1 1 1 1</ambient>
+ <background>0 0 0 1</background>
+ <grid>false</grid>
+ </scene>
+
+ <!-- place user camera -->
+ <gui>
+ <camera name="user_camera">
+ <pose>-100 -150 1.0 0 1.5707 0</pose>
+ </camera>
+ </gui>
+
+ <model name="box">
+ <static>true</static>
+ <pose>-100 -150 0 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>10 10 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>10 10 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ </world>
+</sdf>
+
diff --git a/test/worlds/world_step.world b/test/worlds/world_step.world
new file mode 100644
index 0000000..029df77
--- /dev/null
+++ b/test/worlds/world_step.world
@@ -0,0 +1,325 @@
+<sdf version="1.5">
+ <world name="default">
+ <physics type="ode">
+ <gravity>0.000000 0.000000 -9.810000</gravity>
+ <ode>
+ <solver>
+ <type>world</type>
+ <iters>250</iters>
+ <precon_iters>0</precon_iters>
+ <sor>1.400000</sor>
+ </solver>
+ <constraints>
+ <cfm>0.000000</cfm>
+ <erp>0.200000</erp>
+ <contact_max_correcting_vel>0.000000</contact_max_correcting_vel>
+ <contact_surface_layer>0.00000</contact_surface_layer>
+ </constraints>
+ </ode>
+ <real_time_update_rate>1000.000000</real_time_update_rate>
+ <max_step_size>0.001000</max_step_size>
+ </physics>
+ <model name="ground_plane">
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>100</mu>
+ <mu2>50</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <cast_shadows>false</cast_shadows>
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+ <model name="double_pendulum_with_base">
+ <link name="base">
+ <inertial>
+ <mass>100</mass>
+ </inertial>
+ <visual name="vis_plate_on_ground">
+ <pose>0 0 0.01 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.8</radius>
+ <length>0.02</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name="vis_pole">
+ <pose>-0.275 0 1.1 0 0 0</pose>
+ <geometry>
+ <box><size>0.2 0.2 2.2</size></box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name="col_plate_on_ground">
+ <pose>0 0 0.01 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.8</radius>
+ <length>0.02</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <collision name="col_pole">
+ <pose>-0.275 0 1.1 0 0 0</pose>
+ <geometry>
+ <box><size>0.2 0.2 2.2</size></box>
+ </geometry>
+ </collision>
+ </link>
+ <!-- upper link, length 1, IC -90 degrees -->
+ <link name="upper_link">
+ <pose>0 0 2.1 -1.5708 0 0</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0 0 0.5 0 0 0</pose>
+ </inertial>
+ <visual name="vis_upper_joint">
+ <pose>-0.05 0 0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.3</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name="vis_lower_joint">
+ <pose>0 0 1.0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.2</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name="vis_cylinder">
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.9</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name="col_upper_joint">
+ <pose>-0.05 0 0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.3</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <collision name="col_lower_joint">
+ <pose>0 0 1.0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.2</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <collision name="col_cylinder">
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.9</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ </link>
+ <!-- lower link, length 1, IC ~-120 degrees more -->
+ <link name="lower_link">
+ <pose>0.25 1.0 2.1 -2 0 0</pose>
+ <self_collide>0</self_collide>
+ <inertial>
+ <pose>0 0 0.5 0 0 0</pose>
+ </inertial>
+ <visual name="vis_lower_joint">
+ <pose>0 0 0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.08</radius>
+ <length>0.3</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <visual name="vis_cylinder">
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.9</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ <collision name="col_lower_joint">
+ <pose>0 0 0 0 1.5708 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.08</radius>
+ <length>0.3</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <collision name="col_cylinder">
+ <pose>0 0 0.5 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.1</radius>
+ <length>0.9</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ </link>
+ <!-- pin joint for upper link, at origin of upper link -->
+ <joint name="upper_joint" type="revolute">
+ <parent>base</parent>
+ <child>upper_link</child>
+ <axis>
+ <xyz>1.0 0 0</xyz>
+ </axis>
+ </joint>
+ <!-- pin joint for lower link, at origin of child link -->
+ <joint name="lower_joint" type="revolute">
+ <parent>upper_link</parent>
+ <child>lower_link</child>
+ <axis>
+ <xyz>1.0 0 0</xyz>
+ </axis>
+ </joint>
+ </model>
+ <model name="sphere">
+ <pose>0 1.5 4.5 0 0 0</pose>
+ <allow_auto_disable>false</allow_auto_disable>
+ <link name="link">
+ <inertial>
+ <inertia>
+ <ixx>1</ixx>
+ <ixy>0</ixy>
+ <ixz>0.3</ixz>
+ <iyy>1</iyy>
+ <iyz>0</iyz>
+ <izz>1</izz>
+ </inertia>
+ <mass>1.0</mass>
+ </inertial>
+ <velocity_decay>
+ <linear>0.0</linear>
+ <angular>0.0</angular>
+ </velocity_decay>
+ <collision name="collision">
+ <geometry>
+ <sphere>
+ <radius>0.5</radius>
+ </sphere>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>0.5</mu>
+ <mu2>1</mu2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <max_vel>0</max_vel>
+ <min_depth>0</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <sphere>
+ <radius>0.5</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>Gazebo/WoodPallet</script>
+ </material>
+ </visual>
+ </link>
+ </model>
+ <light type="directional" name="sun">
+ <pose>0 0 10 0 0 0</pose>
+ <diffuse>8 .8 .8 1</diffuse>
+ <specular>.1 .1 .1 1</specular>
+ <attenuation>
+ <range>10</range>
+ <linear>0.01</linear>
+ <constant>0.8</constant>
+ <quadratic>0.0</quadratic>
+ </attenuation>
+ <direction>0 .5 -.5</direction>
+ <cast_shadows>true</cast_shadows>
+ </light>
+ </world>
+</sdf>
diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt
index a39b8c4..d2af5e5 100644
--- a/tools/CMakeLists.txt
+++ b/tools/CMakeLists.txt
@@ -21,6 +21,15 @@ if (HAVE_DART)
link_directories(${DARTCore_LIBRARY_DIRS})
endif()
+if (CURL_FOUND)
+ include_directories(${CURL_INCLUDEDIR})
+ link_directories(${CURL_LIBDIR})
+ if (WIN32)
+ add_definitions(-DCURL_STATICLIB)
+ endif()
+endif()
+
+
set (test_sources
gz_log_TEST.cc
gz_TEST.cc
@@ -30,11 +39,32 @@ gz_build_tests(${test_sources})
add_executable(gz gz.cc gz_topic.cc gz_log.cc)
target_link_libraries(gz
+ libgazebo_client
gazebo_msgs gazebo_common gazebo_transport gazebo_gui gazebo_physics
- gazebo_sensors ${QT_LIBRARIES} ${Boost_LIBRARIES} pthread)
+ gazebo_physics_ode gazebo_sensors ${QT_LIBRARIES} ${Boost_LIBRARIES})
+
+if(HAVE_BULLET)
+ target_link_libraries(gz gazebo_physics_bullet)
+endif()
+if (HAVE_SIMBODY)
+ target_link_libraries(gz gazebo_physics_simbody)
+endif()
+if(HAVE_DART)
+ target_link_libraries(gz gazebo_physics_dart)
+endif()
+
+if (UNIX)
+ target_link_libraries(gz pthread)
+endif()
+
gz_install_executable(gz)
-roffman(gz 1)
+if (NOT WIN32)
+ roffman(gz 1)
+endif()
install (PROGRAMS gzprop DESTINATION ${BIN_INSTALL_DIR})
-manpage(gzprop 1)
+
+if (NOT WIN32)
+ manpage(gzprop 1)
+endif()
diff --git a/tools/bitbucket_pullrequests b/tools/bitbucket_pullrequests
index dcefc16..5171d59 100755
--- a/tools/bitbucket_pullrequests
+++ b/tools/bitbucket_pullrequests
@@ -151,11 +151,22 @@ class BitbucketPullRequests
# list of files changed by pull request
def getPullRequestFiles(id)
+ getFilesFromDiff(getPullRequestDiff(id))
+ end
+
+ # extract list of files added from diff string
+ def getFilesFromDiff(diff)
files = []
- diff = getPullRequestDiff(id)
diff.lines.map(&:chomp).each do |line|
if line.start_with? '+++ b/'
line["+++ b/"] = ""
+ # try to remove anything after a tab character
+ # this is needed by --check 0
+ begin
+ line[/\t.*$/] = ""
+ # IndexError is raised if no tab characters are found
+ rescue IndexError
+ end
files << line
end
end
@@ -174,14 +185,18 @@ class BitbucketPullRequests
return ids
end
+ # check files changed by the parent commit
+ def checkCurrent()
+ files = getFilesFromDiff(`hg log -r . --patch`)
+ changeset = `hg id`[0..11]
+ checkFiles(files, changeset)
+ end
+
# check changed files in pull request by id
def checkPullRequest(id, fork=true)
summary = getPullRequestSummary(id)
puts "checking pull request #{id}, branch #{summary.branch}"
files = getPullRequestFiles(id)
- files_list = ""
- files.each { |f| files_list += " " + f }
- hg_root = `hg root`.chomp
`hg log -r #{summary.destination} 2>&1`
if $? != 0
puts "Unknown revision #{summary.destination}, try: hg pull"
@@ -197,11 +212,18 @@ class BitbucketPullRequests
if ancestor != summary.destination
puts "Need to merge branch #{summary.branch} with #{summary.destination}"
end
+ checkFiles(files, summary.source, fork)
+ end
+
+ def checkFiles(files, changeset, fork=true)
+ files_list = ""
+ files.each { |f| files_list += " " + f }
+ hg_root = `hg root`.chomp
if fork
# this will allow real-time console output
- exec "echo #{files_list} | sh #{hg_root}/tools/code_check.sh --quick #{summary.source}"
+ exec "echo #{files_list} | sh #{hg_root}/tools/code_check.sh --quick #{changeset}"
else
- puts `echo #{files_list} | sh "#{hg_root}"/tools/code_check.sh --quick #{summary.source}`
+ puts `echo #{files_list} | sh "#{hg_root}"/tools/code_check.sh --quick #{changeset}`
end
end
end
@@ -224,7 +246,9 @@ opt_parser = OptionParser.new do |o|
"List open pull requests with fields:\n" + " "*37 +
"[id] [source] [dest] [branch]") { |o| options["list"] = o }
o.on("-c", "--check [id]", Integer,
- "") { |o| options["check_id"] = o; options["check"] = true }
+ "Run code_check on files changed by pull request [id]\n" + " "*37 +
+ "if [id] is not supplied, check all open pull requests\n" + " "*37 +
+ "if [id] is 0, check files changed by parent commit") { |o| options["check_id"] = o; options["check"] = true }
o.on("-d", "--diff [id]", Integer,
"Show diff from pull request") { |o| options["diff"] = o }
o.on("-f", "--files [id]", Integer,
@@ -264,6 +288,8 @@ elsif options["check"]
client.getOpenPullRequests().each { |id|
client.checkPullRequest(id, false)
}
+ elsif options["check_id"] == 0
+ client.checkCurrent()
else
client.checkPullRequest(options["check_id"])
end
diff --git a/tools/check_test_ran.py b/tools/check_test_ran.py
index 48f8639..0ef7db7 100755
--- a/tools/check_test_ran.py
+++ b/tools/check_test_ran.py
@@ -97,7 +97,7 @@ def check_main():
else:
pid = m.group(1)
print("killing gzserver with pid %s" % (pid))
- subprocess.call(["kill", "%s" % (pid)])
+ subprocess.call(["kill", "-9", "%s" % (pid)])
print("Checking for test results in %s"%test_file)
diff --git a/tools/code_check.sh b/tools/code_check.sh
index ded6856..4b0596a 100755
--- a/tools/code_check.sh
+++ b/tools/code_check.sh
@@ -48,7 +48,7 @@ else
CHECK_DIRS="./plugins ./gazebo ./tools ./examples ./test/integration"\
" ./test/regression ./interfaces ./test/performance"\
" ./test/examples ./test/plugins"\
-" ./test/cmake ./test/pkgconfig ./test/ServerFixture.*"
+" ./test/cmake ./test/pkgconfig"
if [ $CPPCHECK_LT_157 -eq 1 ]; then
# cppcheck is older than 1.57, so don't check header files (issue #907)
CPPCHECK_FILES=`find $CHECK_DIRS -name "*.cc"`
@@ -56,7 +56,7 @@ else
CPPCHECK_FILES=`find $CHECK_DIRS -name "*.cc" -o -name "*.hh"`
fi
CPPLINT_FILES=`\
- find $CHECK_DIRS -name "*.cc" -o -name "*.hh" -o -name "*.c" -o -name "*.h"`
+ find $CHECK_DIRS -name "*.cc" -o -name "*.hh" -o -name "*.c" -o -name "*.h" | grep -v test_fixture/gtest`
fi
SUPPRESS=/tmp/gazebo_cpp_check.suppress
@@ -64,7 +64,11 @@ echo "*:gazebo/common/STLLoader.cc:94" > $SUPPRESS
echo "*:gazebo/common/STLLoader.cc:105" >> $SUPPRESS
echo "*:gazebo/common/STLLoader.cc:126" >> $SUPPRESS
echo "*:gazebo/common/STLLoader.cc:149" >> $SUPPRESS
+# (warning) Redundant code: Found a statement that begins with string constant.
+echo "*:gazebo/common/SVGLoader.cc:687" >> $SUPPRESS
echo "*:examples/plugins/custom_messages/custom_messages.cc:22" >> $SUPPRESS
+echo "*:examples/stand_alone/test_fixture/gtest/*" >> $SUPPRESS
+
# Not defined FREEIMAGE_COLORORDER
echo "*:gazebo/common/Image.cc:1" >> $SUPPRESS
@@ -128,7 +132,7 @@ elif [ $QUICK_CHECK -eq 1 ]; then
DO_CPPCHECK=1
elif [ $CPPCHECK_LT_157 -eq 0 ]; then
DO_CPPCHECK=1
- fi
+ fi
if [ $DO_CPPCHECK -eq 1 ]; then
$CPPCHECK_BASE $CPPCHECK_CMD1A $CPPCHECK_RULES $tmp2 2>&1 \
diff --git a/tools/cpplint.py b/tools/cpplint.py
index d338a78..29c1266 100644
--- a/tools/cpplint.py
+++ b/tools/cpplint.py
@@ -1355,7 +1355,7 @@ def CheckForNonStandardConstructs(filename, clean_lines, linenum,
if Search(r'\b(const|volatile|void|char|short|int|long'
r'|float|double|signed|unsigned'
r'|schar|u?int8|u?int16|u?int32|u?int64)'
- r'\s+(auto|register|static|extern|typedef)\b',
+ r'\s+(register|static|extern|typedef)\b',
line):
error(filename, linenum, 'build/storage_class', 5,
'Storage class (static, extern, typedef, etc) should be first.')
diff --git a/tools/gz.1.roff b/tools/gz.1.roff
index 45f0b06..1cfb9a4 100644
--- a/tools/gz.1.roff
+++ b/tools/gz.1.roff
@@ -327,6 +327,10 @@ Number of iterations.
.B \-u, \-\-update\-rate\fR=\fIarg\fR
.
Target real\-time update rate.
+.TP
+.B \-o, \-\-profile\fR=\fIarg\fR
+.
+Preset physics profile.
.UNINDENT
.SS sdf
.sp
diff --git a/tools/gz.cc b/tools/gz.cc
index 1cb37f8..ba5784a 100644
--- a/tools/gz.cc
+++ b/tools/gz.cc
@@ -14,6 +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 <stdio.h>
#include <signal.h>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
@@ -137,7 +145,11 @@ bool Command::Run(int _argc, char **_argv)
{
int argvLen = strlen(_argv[i]) + 1;
this->argv[i] = new char[argvLen];
+ #ifndef _WIN32
snprintf(this->argv[i], argvLen, "%s", _argv[i]);
+ #else
+ sprintf_s(this->argv[i], argvLen, "%s", _argv[i]);
+ #endif
}
// The SDF find file callback.
@@ -289,7 +301,8 @@ PhysicsCommand::PhysicsCommand()
"eg: -g 0,0,-9.8")
("step-size,s", po::value<double>(), "Maximum step size (seconds).")
("iters,i", po::value<double>(), "Number of iterations.")
- ("update-rate,u", po::value<double>(), "Target real-time update rate.");
+ ("update-rate,u", po::value<double>(), "Target real-time update rate.")
+ ("profile,o", po::value<std::string>(), "Preset physics profile.");
}
/////////////////////////////////////////////////
@@ -351,6 +364,13 @@ bool PhysicsCommand::RunImpl()
good = true;
}
+ if (this->vm.count("profile") &&
+ this->vm["profile"].as<std::string>().size() > 0)
+ {
+ msg.set_profile_name(this->vm["profile"].as<std::string>());
+ good = true;
+ }
+
if (good)
pub->Publish(msg, true);
else
@@ -503,7 +523,7 @@ bool ModelCommand::RunImpl()
if (this->vm.count("info"))
std::cout << modelMsg.DebugString() << std::endl;
else if (this->vm.count("pose"))
- std::cout << gazebo::msgs::Convert(modelMsg.pose()) << std::endl;
+ std::cout << gazebo::msgs::ConvertIgn(modelMsg.pose()) << std::endl;
}
else
{
@@ -520,7 +540,7 @@ bool ModelCommand::RunImpl()
msgs::Model msg;
msg.set_name(modelName);
- msgs::Set(msg.mutable_pose(), pose);
+ msgs::Set(msg.mutable_pose(), pose.Ign());
pub->Publish(msg, true);
}
@@ -531,7 +551,7 @@ bool ModelCommand::RunImpl()
bool ModelCommand::ProcessSpawn(boost::shared_ptr<sdf::SDF> _sdf,
const std::string &_name, const math::Pose &_pose, transport::NodePtr _node)
{
- sdf::ElementPtr modelElem = _sdf->root->GetElement("model");
+ sdf::ElementPtr modelElem = _sdf->Root()->GetElement("model");
if (!modelElem)
{
@@ -548,7 +568,7 @@ bool ModelCommand::ProcessSpawn(boost::shared_ptr<sdf::SDF> _sdf,
msgs::Factory msg;
msg.set_sdf(_sdf->ToString());
- msgs::Set(msg.mutable_pose(), _pose);
+ msgs::Set(msg.mutable_pose(), _pose.Ign());
pub->Publish(msg, true);
return true;
@@ -929,7 +949,7 @@ bool SDFCommand::TransportRequired()
/////////////////////////////////////////////////
bool SDFCommand::RunImpl()
{
- sdf::SDF::version = SDF_VERSION;
+ sdf::SDF::Version(SDF_VERSION);
try
{
@@ -948,8 +968,8 @@ bool SDFCommand::RunImpl()
{
try
{
- sdf::SDF::version = boost::lexical_cast<std::string>(
- this->vm["version"].as<std::string>());
+ sdf::SDF::Version(boost::lexical_cast<std::string>(
+ this->vm["version"].as<std::string>()));
}
catch(...)
{
@@ -998,7 +1018,7 @@ bool SDFCommand::RunImpl()
TiXmlDocument xmlDoc;
if (xmlDoc.LoadFile(path.string()))
{
- if (sdf::Converter::Convert(&xmlDoc, sdf::SDF::version, true))
+ if (sdf::Converter::Convert(&xmlDoc, sdf::SDF::Version(), true))
{
// Create an XML printer to control formatting
TiXmlPrinter printer;
diff --git a/tools/gz_TEST.cc b/tools/gz_TEST.cc
index 25ca807..df0a82b 100644
--- a/tools/gz_TEST.cc
+++ b/tools/gz_TEST.cc
@@ -201,8 +201,6 @@ TEST_F(gzTest, Joint)
{
init();
- std::string expectedStr;
-
std::string helpOutput = custom_exec_str("gz help joint");
EXPECT_NE(helpOutput.find("gz joint"), std::string::npos);
@@ -291,7 +289,7 @@ TEST_F(gzTest, Model)
gazebo::msgs::Model msg;
msg.set_name("simple_arm");
gazebo::msgs::Set(msg.mutable_pose(),
- gazebo::math::Pose(1.1, 2.3, 4.5, 0.1, 1.2, 3.4));
+ ignition::math::Pose3d(1.1, 2.3, 4.5, 0.1, 1.2, 3.4));
EXPECT_EQ(g_msgDebugOut, msg.DebugString());
}
@@ -305,12 +303,13 @@ TEST_F(gzTest, Model)
EXPECT_TRUE(sdf::init(sdf));
EXPECT_TRUE(sdf::readFile(filename, sdf));
- sdf::ElementPtr modelElem = sdf->root->GetElement("model");
+ sdf::ElementPtr modelElem = sdf->Root()->GetElement("model");
modelElem->GetAttribute("name")->SetFromString("my_box");
gazebo::msgs::Factory msg;
msg.set_sdf(sdf->ToString());
- gazebo::msgs::Set(msg.mutable_pose(), gazebo::math::Pose(0, 0, 0, 0, 0, 0));
+ gazebo::msgs::Set(msg.mutable_pose(),
+ ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
EXPECT_EQ(g_msgDebugOut, msg.DebugString());
}
@@ -327,12 +326,13 @@ TEST_F(gzTest, Model)
EXPECT_TRUE(sdf::init(sdf));
EXPECT_TRUE(sdf::readFile(filename, sdf));
- sdf::ElementPtr modelElem = sdf->root->GetElement("model");
+ sdf::ElementPtr modelElem = sdf->Root()->GetElement("model");
modelElem->GetAttribute("name")->SetFromString("my_box");
gazebo::msgs::Factory msg;
msg.set_sdf(sdf->ToString());
- gazebo::msgs::Set(msg.mutable_pose(), gazebo::math::Pose(0, 0, 0, 0, 0, 0));
+ gazebo::msgs::Set(msg.mutable_pose(),
+ ignition::math::Pose3d(0, 0, 0, 0, 0, 0));
EXPECT_EQ(g_msgDebugOut, msg.DebugString());
}
@@ -618,11 +618,11 @@ TEST_F(gzTest, SDF)
// Regenerate each sum using:
// gz sdf -d -v <major.minor> | sha1sum'
std::map<std::string, std::string> descSums;
- descSums["1.0"] = "5235eb8464a96505c2a31fe96327d704e45c9cc4";
- descSums["1.2"] = "27973b2542d7a0f7582a615b245d81797718c89a";
- descSums["1.3"] = "30ffce1c662c17185d23f30ef3af5c110d367e10";
- descSums["1.4"] = "9a55c2992b532e0dd4f9a5d5b86cd4c6210d9902";
- descSums["1.5"] = "a64da1ba2cec921a8b8e446ef01a862dc3e38796";
+ descSums["1.0"] = "a02fbc1275100569c99d860044563f669934c0fc";
+ descSums["1.2"] = "f524458ace57d6aabbbc2303da208f65af37ef53";
+ descSums["1.3"] = "74a3aa8d31f97328175f43d03410be55631fa0e1";
+ descSums["1.4"] = "057f26137669d9d7eeb5a8c6f51e4f4077d9ddcf";
+ descSums["1.5"] = "522285759f420eba3b774e610822c357a0a683e2";
// Test each descSum
for (std::map<std::string, std::string>::iterator iter = descSums.begin();
@@ -638,10 +638,10 @@ TEST_F(gzTest, SDF)
// gz sdf -o -v <major.minor> | sha1sum'
std::map<std::string, std::string> docSums;
docSums["1.0"] = "4cf955ada785adf72503744604ffadcdf13ec0d2";
- docSums["1.2"] = "f84c1cf1b1ba04ab4859e96f6aea881134fb5a9b";
- docSums["1.3"] = "f3dd699687c8922710e4492aadedd1c038d678c1";
- docSums["1.4"] = "9627b1deb0c0437ac48c1c1cbd0e9800b0327b76";
- docSums["1.5"] = "e67df38350bed73372cb0072cfd32c8cf07f5bf9";
+ docSums["1.2"] = "27f9d91080ce8aa18eac27c9d899fde2d4b78785";
+ docSums["1.3"] = "ad80986d42eae97baf277118f52d7e8b951d8ea1";
+ docSums["1.4"] = "153ddd6ba6797c37c7fcddb2be5362c9969d97a1";
+ docSums["1.5"] = "1ccc4861895a2eb331de76a2aa92da5a98b45273";
// Test each docSum
for (std::map<std::string, std::string>::iterator iter = docSums.begin();
@@ -655,7 +655,7 @@ TEST_F(gzTest, SDF)
path = TEST_PATH;
- path /= "worlds/empty_different_name.world";
+ path /= "worlds/box_plane_low_friction_test.world";
{
// Check empty.world
@@ -672,7 +672,7 @@ TEST_F(gzTest, SDF)
std::string output =
custom_exec_str(std::string("gz sdf -p ") + path.string());
std::string shasum = gazebo::common::get_sha1<std::string>(output);
- EXPECT_EQ(shasum, "381ce9100dd002bcf7e6f9019e33d478a37ab6f1");
+ EXPECT_EQ(shasum, "ea127f9858a5e07c40ef6d949ef6113236adddfa");
}
path = PROJECT_BINARY_PATH;
diff --git a/tools/gz_log.cc b/tools/gz_log.cc
index b9834d6..03f8bb6 100644
--- a/tools/gz_log.cc
+++ b/tools/gz_log.cc
@@ -15,6 +15,12 @@
*
*/
+#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/regex.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/posix_time/posix_time_io.hpp>
@@ -47,6 +53,8 @@ std::ostringstream &FilterBase::Out(std::ostringstream &_stream,
_stream << _state.GetRealTime().Double() << " ";
else if (this->stamp == "wall")
_stream << _state.GetWallTime().Double() << " ";
+ else if (this->stamp == "iterations")
+ _stream << _state.GetIterations() << " ";
_stream.setf(flags);
}
@@ -547,7 +555,8 @@ std::string StateFilter::Filter(const std::string &_stateString)
<< "<state world_name='" << state.GetName() << "'>\n"
<< "<sim_time>" << state.GetSimTime() << "</sim_time>\n"
<< "<real_time>" << state.GetRealTime() << "</real_time>\n"
- << "<wall_time>" << state.GetWallTime() << "</wall_time>\n";
+ << "<wall_time>" << state.GetWallTime() << "</wall_time>\n"
+ << "<iterations>" << state.GetIterations() << "</iterations>\n";
}
result << this->filter.Filter(state);
@@ -908,16 +917,18 @@ std::string LogCommand::GetFileSizeStr(const std::string &_filename)
/////////////////////////////////////////////////
int LogCommand::GetChar()
{
+# ifndef _WIN32
struct termios oldt, newt;
- int ch;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
- ch = getchar();
+ int ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
-
return ch;
+# else
+ return 'q';
+# endif
}
/////////////////////////////////////////////////
diff --git a/tools/gz_log_TEST.cc b/tools/gz_log_TEST.cc
index 7e1c575..d35795f 100644
--- a/tools/gz_log_TEST.cc
+++ b/tools/gz_log_TEST.cc
@@ -85,7 +85,8 @@ TEST(gz_log, Echo)
std::string validEcho =
"<?xml version='1.0'?>\n<gazebo_log>\n<header>\n<log_version>1.0"
"</log_version>\n<gazebo_version>1.4.6</gazebo_version>\n"
- "<rand_seed>24794</rand_seed>\n</header>\n\n<chunk encoding='txt'>"
+ "<rand_seed>24794</rand_seed>\n<log_start>0 0</log_start>\n"
+ "<log_end>0 0</log_end>\n</header>\n\n<chunk encoding='txt'>"
"<![CDATA[\n<sdf version ='1.3'>\n<world name='default'>\n "
"<light name='sun' type='directional'>\n <cast_shadows>1"
"</cast_shadows>\n <pose>0.000000 0.000000 10.000000 0.000000 0.000000 "
@@ -144,7 +145,7 @@ TEST(gz_log, EchoFilter)
if (std::string("1.4").compare(SDF_VERSION) == 0)
EXPECT_EQ(shasum, "0bf1f293b164bbe820267f970c4b419acdca4b01");
else if (std::string("1.5").compare(SDF_VERSION) == 0)
- EXPECT_EQ(shasum, "55def526741fafdd92a1abd4c8a8adbd8a8d78d6");
+ EXPECT_EQ(shasum, "4c3bac95976836078ac7172d37039857e3c87e1a");
else
FAIL() << "Please add support for sdf version: " << SDF_VERSION;
@@ -156,7 +157,7 @@ TEST(gz_log, EchoFilter)
if (std::string("1.4").compare(SDF_VERSION) == 0)
EXPECT_EQ(shasum, "33db2cbd0841466a67abd7d2bbc69cf2cfae19b6");
else if (std::string("1.5").compare(SDF_VERSION) == 0)
- EXPECT_EQ(shasum, "893eaea0e47bd405d7352b0386678784785758b3");
+ EXPECT_EQ(shasum, "98841d54fb94039129ac1185f8a8e0a4cbfe3e82");
else
FAIL() << "Please add support for sdf version: " << SDF_VERSION;
@@ -168,7 +169,7 @@ TEST(gz_log, EchoFilter)
if (std::string("1.4").compare(SDF_VERSION) == 0)
EXPECT_EQ(shasum, "07113f16d44e2484f769fd1947ff5dca93f55cf4");
else if (std::string("1.5").compare(SDF_VERSION) == 0)
- EXPECT_EQ(shasum, "efd2bdd814203d10502aa519d31ebfc4d15256d6");
+ EXPECT_EQ(shasum, "173894dafa3f893e38b035e788529ccbcd0f4c5a");
else
FAIL() << "Please add support for sdf version: " << SDF_VERSION;
@@ -180,7 +181,7 @@ TEST(gz_log, EchoFilter)
if (std::string("1.4").compare(SDF_VERSION) == 0)
EXPECT_EQ(shasum, "7f34f3fac505707727a74ac8659bb8736932ab07");
else if (std::string("1.5").compare(SDF_VERSION) == 0)
- EXPECT_EQ(shasum, "faf44dc8f4a0d4e33ac08c0d6c8cd3412d26b95e");
+ EXPECT_EQ(shasum, "b2d94544b848aa5c46df129b4a38fd8995463f31");
else
FAIL() << "Please add support for sdf version: " << SDF_VERSION;
@@ -193,7 +194,7 @@ TEST(gz_log, EchoFilter)
if (std::string("1.4").compare(SDF_VERSION) == 0)
EXPECT_EQ(shasum, "d52ba4333511b7e4339db3eb71814c73473fba36");
else if (std::string("1.5").compare(SDF_VERSION) == 0)
- EXPECT_EQ(shasum, "81da84c3c601ace274ef0cb857939b8bad4be82e");
+ EXPECT_EQ(shasum, "73b5f38dbb9adc92f85166554599f05e175b950d");
else
FAIL() << "Please add support for sdf version: " << SDF_VERSION;
@@ -206,7 +207,7 @@ TEST(gz_log, EchoFilter)
if (std::string("1.4").compare(SDF_VERSION) == 0)
EXPECT_EQ(shasum, "2f689dadc66171a76f7f3400bc218485a923c324");
else if (std::string("1.5").compare(SDF_VERSION) == 0)
- EXPECT_EQ(shasum, "86b26d5d97967b65a2ef46fba38567d338033291");
+ EXPECT_EQ(shasum, "6ad8b768b7abac463a7cc038f2130647e6e1ceb6");
else
FAIL() << "Please add support for sdf version: " << SDF_VERSION;
}
@@ -285,7 +286,7 @@ TEST(gz_log, Step)
// Call gz log step and press q immediately
std::string stepq0 = custom_exec(std::string("echo 'q' | ") + stepCmd);
shasum = gazebo::common::get_sha1<std::string>(stepq0);
- EXPECT_EQ(shasum, "6d3af4f4d1214fe3a4860ab42777eb4d0f89c6b2");
+ EXPECT_EQ(shasum, "e5599c7226cb211a40f0b3ea91cf2a9c39c9b649");
// Call gz log step and press space once, then q
std::string stepq1 = custom_exec(std::string("echo ' q' | ") + stepCmd);
@@ -293,7 +294,7 @@ TEST(gz_log, Step)
if (std::string("1.4").compare(SDF_VERSION) == 0)
EXPECT_EQ(shasum, "43eacb140e00ef0525d54667bc558d63dac3d21f");
else
- EXPECT_EQ(shasum, "9e682f295f88f92217d172b9f686df2fb7a0754b");
+ EXPECT_EQ(shasum, "29fbb8b2358eab61eb17c03354b2adc5082e3a37");
// Call gz log step and press space twice, then q
std::string stepq2 = custom_exec(std::string("echo ' q' | ") + stepCmd);
@@ -301,7 +302,7 @@ TEST(gz_log, Step)
if (std::string("1.4").compare(SDF_VERSION) == 0)
EXPECT_EQ(shasum, "37e133d15d3f74cbc686bfceb26b8db46e2f6bf5");
else
- EXPECT_EQ(shasum, "f52796ad928931d158d0573f4fd000e4577efa35");
+ EXPECT_EQ(shasum, "f2ba4ad6b066aede440fcce49f5cd00753896f27");
}
/////////////////////////////////////////////////
diff --git a/tools/gz_topic.cc b/tools/gz_topic.cc
index e5be46b..0e20b01 100644
--- a/tools/gz_topic.cc
+++ b/tools/gz_topic.cc
@@ -15,11 +15,17 @@
*
*/
+#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 <gazebo/gui/qt.h>
#include <gazebo/gui/TopicSelector.hh>
#include <gazebo/gui/viewers/TopicView.hh>
#include <gazebo/gui/viewers/ViewFactory.hh>
-#include <gazebo/gazebo.hh>
+#include <gazebo/gazebo_client.hh>
#include "gz_topic.hh"
@@ -325,7 +331,7 @@ void TopicCommand::Bw(const std::string &_topic)
/////////////////////////////////////////////////
void TopicCommand::View(const std::string &_topic)
{
- if (!gazebo::setupClient())
+ if (!gazebo::client::setup())
{
printf("load error\n");
return;
@@ -372,5 +378,5 @@ void TopicCommand::View(const std::string &_topic)
delete app;
app = NULL;
- gazebo::shutdown();
+ gazebo::client::shutdown();
}
diff --git a/tools/junit_to_csv.rb b/tools/junit_to_csv.rb
new file mode 100644
index 0000000..c5bac88
--- /dev/null
+++ b/tools/junit_to_csv.rb
@@ -0,0 +1,21 @@
+#!/usr/bin/env ruby
+
+require 'rexml/document'
+
+xml = File.read(ARGV[0])
+
+doc, arrayOfHashes = REXML::Document.new(xml), []
+doc.elements.each('testsuites/testsuite/testcase') do |t|
+ arrayOfHashes << t.attributes
+end
+sortedKeys = arrayOfHashes.first.keys.sort
+sortedKeys.delete("value_param")
+puts sortedKeys.join(',')
+
+arrayOfHashes.each do |h|
+ row = []
+ sortedKeys.each do |k|
+ row << h[k]
+ end
+ puts row.join(',')
+end
diff --git a/tools/sdf2pov.cc b/tools/sdf2pov.cc
index d3b364c..49f345e 100644
--- a/tools/sdf2pov.cc
+++ b/tools/sdf2pov.cc
@@ -84,7 +84,7 @@ gazebo::math::Vector3 Convert(const gazebo::math::Vector3 &_vec)
return result;
}
-void ProcessMesh(sdf::ElementPtr _elem, const gazebo::math::Pose _pose)
+void ProcessMesh(sdf::ElementPtr _elem, const gazebo::math::Pose &_pose)
{
const gazebo::common::Mesh *mesh;
@@ -99,7 +99,7 @@ void ProcessMesh(sdf::ElementPtr _elem, const gazebo::math::Pose _pose)
const gazebo::common::SubMesh *subMesh = mesh->GetSubMesh(i);
printf("mesh2 {\n");
printf(" vertex_vectors {\n");
- printf(" %d, \n ", subMesh->GetVertexCount());
+ printf(" %u, \n ", subMesh->GetVertexCount());
for (unsigned int v = 0; v < subMesh->GetVertexCount(); v++)
{
gazebo::math::Vector3 vert = subMesh->GetVertex(v);
@@ -109,7 +109,7 @@ void ProcessMesh(sdf::ElementPtr _elem, const gazebo::math::Pose _pose)
printf(" }\n");
printf(" normal_vectors {\n");
- printf(" %d, \n ", subMesh->GetNormalCount());
+ printf(" %u, \n ", subMesh->GetNormalCount());
for (unsigned int n = 0; n < subMesh->GetNormalCount(); n++)
{
gazebo::math::Vector3 norm = subMesh->GetNormal(n);
@@ -118,7 +118,7 @@ void ProcessMesh(sdf::ElementPtr _elem, const gazebo::math::Pose _pose)
printf(" }\n");
printf(" uv_vectors {\n");
- printf(" %d, \n", subMesh->GetTexCoordCount());
+ printf(" %u, \n", subMesh->GetTexCoordCount());
for (unsigned int j = 0; j < subMesh->GetTexCoordCount(); j++)
{
printf(" <%f, %f>, \n", subMesh->GetTexCoord(j).x,
@@ -161,12 +161,12 @@ void ProcessMesh(sdf::ElementPtr _elem, const gazebo::math::Pose _pose)
{
if (mat)
{
- printf(" <%d, %d, %d>, 0\n", subMesh->GetIndex(j),
+ printf(" <%u, %u, %u>, 0\n", subMesh->GetIndex(j),
subMesh->GetIndex(j+1), subMesh->GetIndex(j+2));
}
else
{
- printf(" <%d, %d, %d>\n", subMesh->GetIndex(j),
+ printf(" <%u, %u, %u>\n", subMesh->GetIndex(j),
subMesh->GetIndex(j+1), subMesh->GetIndex(j+2));
}
}
@@ -176,7 +176,7 @@ void ProcessMesh(sdf::ElementPtr _elem, const gazebo::math::Pose _pose)
printf(" %d, \n", subMesh->GetIndexCount() / 3);
for (unsigned int j = 0; j < subMesh->GetIndexCount(); j+= 3)
{
- printf(" <%d, %d, %d>, \n", subMesh->GetIndex(j),
+ printf(" <%u, %u, %u>, \n", subMesh->GetIndex(j),
subMesh->GetIndex(j+1), subMesh->GetIndex(j+2));
}
printf(" }\n");
@@ -378,7 +378,7 @@ int main(int argc, char **argv)
printf(" right <0, 1, 0>\n");
printf("}\n");
- sdf::ElementPtr root = sdf->root;
+ sdf::ElementPtr root = sdf->Root();
gazebo::math::Pose modelPose, linkPose, visualPose;
diff --git a/win_addpath.bat b/win_addpath.bat
new file mode 100644
index 0000000..6ce1f6d
--- /dev/null
+++ b/win_addpath.bat
@@ -0,0 +1,37 @@
+ at REM Add paths to Windows. Run this script prior to running gzserver.exe
+ at REM and gzclient.ext on a Windows machine.
+ at REM Meant to be run from inside build/
+ at set build_type=Release
+ at if not "%1"=="" set build_type=%1
+ at echo Configuring for build type %build_type%
+
+ at REM Need absolute paths in order to run anything inside build/
+ at set gz_root_path=%~dp0
+ at set gz_build_path=%gz_root_path%\build
+ at pushd %~dp0\..
+ at set deps_path=%CD%
+ at popd
+ at echo - script path is %gz_root_path%
+ at echo - dependencies path is %deps_path%
+
+ at set HOME=%HOMEDRIVE%%HOMEPATH%
+
+ at set GAZEBO_MODEL_PATH=%gz_root_path%\models
+ at set GAZEBO_PLUGINS_PATH=%gz_build_path%\plugins
+ at set GAZEBO_RESOURCE_PATH=%gz_root_path%
+ at set OGRE_RESORCE_PATH=%deps_path%\ogre_src_v1-8-1-vc12-x64-release-debug\build\install\%build_type%\bin\%build_type%
+
+ at set PATH=%deps_path%\boost_1_56_0\lib64-msvc-12.0;^
+%deps_path%\FreeImage-vc12-x64-release-debug\x64\%build_type%\DLL;^
+%gz_build_path%\deps\opende;^
+%deps_path%\ogre_src_v1-8-1-vc12-x64-release-debug\build\install\%build_type%\bin\%build_type%;^
+%deps_path%\sdformat\build\install\%build_type%\lib;^
+%deps_path%\ign-math\build\install\%build_type%\lib;^
+%deps_path%\tbb43_20141023oss\bin\intel64\vc12;^
+%gz_build_path%\deps\libccd;^
+%gz_build_path%\deps\opende\OPCODE;^
+%gz_build_path%\deps\opende\ou;^
+%deps_path%\zlib-1.2.8-vc12-x64-release-debug\contrib\vstudio\vc11\x64\ZlibDll%build_type%;^
+%deps_path%\bzip2-1.0.6-vc12-x64-release-debug\%build_type%;^
+C:\Qt\4.8.6\x64\msvc2013\bin;^
+%PATH%
diff --git a/worlds/CMakeLists.txt b/worlds/CMakeLists.txt
index cd0b41a..32d93c4 100644
--- a/worlds/CMakeLists.txt
+++ b/worlds/CMakeLists.txt
@@ -1,16 +1,18 @@
set (files
actor.world
- arat.world
blank.world
camera.world
cart_demo.world
+ cessna_demo.world
contact.world
depth_camera.world
+ elevator.world
empty_1_0.world
empty_bullet.world
empty_sky.world
empty.world
everything.world
+ flocking.world
force_torque_demo.world
friction_demo.world
gripper.world
@@ -26,6 +28,8 @@ set (files
mud.world
multilink_shape.world
openal.world
+ ortho.world
+ osrf_elevator.world
pioneer2dx.world
pioneer2dx_camera.world
pioneer2dx_laser.world
@@ -49,14 +53,17 @@ set (files
robocup_3Dsim.world
rubble.world
shapes.world
+ shapes_layers.world
simple_arm.world
simple_gripper.world
single_rotor_demo.world
sonar_demo.world
sphere_atlas_demo.world
stacks.world
+ transporter.world
trigger.world
twin_rotor_demo.world
+ underwater.world
willowgarage.world
wireless_sensors.world
)
diff --git a/worlds/arat.world b/worlds/arat.world
deleted file mode 100644
index c798595..0000000
--- a/worlds/arat.world
+++ /dev/null
@@ -1,184 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
- <world name="default">
- <!-- A global light source -->
- <include>
- <uri>model://sun</uri>
- </include>
-
- <!-- A ground plane -->
- <include>
- <uri>model://ground_plane</uri>
- </include>
-
- <include>
- <uri>model://table</uri>
- </include>
-
- <include>
- <uri>model://wooden_case</uri>
- <pose>0 0.037 1.015 0 0 -1.570796</pose>
- </include>
-
- <include>
- <uri>model://cricket_ball</uri>
- <pose> 0.6 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://wood_cube_10cm</uri>
- <pose> 0.2 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://wood_cube_7_5cm</uri>
- <pose> 0.0 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://wood_cube_5cm</uri>
- <pose>-0.2 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://wood_cube_2_5cm</uri>
- <pose>-0.4 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://stone_10_2_5_1cm</uri>
- <pose>-0.6 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <name>cup_1</name>
- <uri>model://plastic_cup</uri>
- <pose>-0.8 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <name>cup_2</name>
- <uri>model://plastic_cup</uri>
- <pose>-1.0 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://wooden_peg_board</uri>
- <pose>-1.2 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://metal_peg_board</uri>
- <pose>-1.4 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://tube_2_25cm</uri>
- <pose>-1.6 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://tube_9_5mm</uri>
- <pose>-1.8 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://washer</uri>
- <pose>-2.0 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://ball_bearing</uri>
- <pose>-2.0 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://marble_1_5cm</uri>
- <pose>-2.0 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://round_tin_base</uri>
- <pose>-2.2 250 1.015 0 0 0</pose>
- </include>
-
- <include>
- <uri>model://round_tin_top</uri>
- <pose>-2.4 250 1.015 0 0 0</pose>
- </include>
-
-
- <plugin name="arrange_plugin" filename="libArrangePlugin.so">
- <model_name>cricket_ball</model_name>
- <model_name>wood_cube_10cm</model_name>
- <model_name>wood_cube_7_5cm</model_name>
- <model_name>wood_cube_5cm</model_name>
- <model_name>wood_cube_2_5cm</model_name>
- <model_name>stone_10_2_5_1cm</model_name>
-
- <model_name>cup_1</model_name>
- <model_name>cup_2</model_name>
- <model_name>wooden_peg_board</model_name>
- <model_name>metal_peg_board</model_name>
- <model_name>tube_2_25cm</model_name>
- <model_name>tube_9_5mm</model_name>
- <model_name>washer</model_name>
-
- <model_name>ball_bearing</model_name>
- <model_name>marble_1_5cm</model_name>
-
- <model_name>round_tin_top</model_name>
- <model_name>round_tin_base</model_name>
-
- <initial_arrangement>grasp_1</initial_arrangement>
- <arrangement name="grasp_1">
- <pose model="wood_cube_10cm"> 0.217 -0.265 1.04 0 0 0</pose>
- </arrangement>
- <arrangement name="grasp_2">
- <pose model="wood_cube_2_5cm"> 0.217 -0.298 1.04 0 0 0</pose>
- </arrangement>
- <arrangement name="grasp_3">
- <pose model="wood_cube_5cm"> 0.217 -0.284 1.034 0 0 0</pose>
- </arrangement>
- <arrangement name="grasp_4">
- <pose model="wood_cube_7_5cm"> 0.217 -0.268 1.034 0 0 0</pose>
- </arrangement>
- <arrangement name="grasp_5">
- <pose model="round_tin_base">0.217 -0.274 1.036 0 0 0</pose>
- <pose model="round_tin_top">0.217 0.04 1.393 0 0 0 </pose>
- <pose model="cricket_ball"> 0.217 -0.274 1.038 0 0 0</pose>
- </arrangement>
- <arrangement name="grasp_6">
- <pose model="stone_10_2_5_1cm">0.242 -0.34 1.0475 -1.571074 0 -1.14</pose>
- </arrangement>
- <arrangement name="grip_1">
- <pose model="cup_1"> 0.00 -0.20 1.034 0 0 0</pose>
- <pose model="cup_2"> 0.217 -0.20 1.034 0 0 0</pose>
- </arrangement>
- <arrangement name="grip_2">
- <pose model="wooden_peg_board">0.128 -0.08 1.034 0 0 -1.571074</pose>
- <pose model="tube_2_25cm">0.216 -0.328 1.034 0 0 0</pose>
- </arrangement>
- <arrangement name="grip_3">
- <pose model="metal_peg_board">0.128 -0.08 1.034 0 0 -1.571074</pose>
- <pose model="tube_9_5mm">0.216 -0.328 1.034 0 0 0</pose>
- </arrangement>
- <arrangement name="grip_4">
- <pose model="round_tin_top">0.244 -0.281 1.033 0 0 0</pose>
- <pose model="washer">0.244 -0.281 1.036 0 0 0</pose>
- <pose model="metal_peg_board">0.128 -0.08 1.034 0 0 -1.571074</pose>
- </arrangement>
- <arrangement name="pinch_1">
- <pose model="round_tin_base">0.217 -0.274 1.036 0 0 0</pose>
- <pose model="round_tin_top">0.217 0.04 1.393 0 0 0 </pose>
- <pose model="ball_bearing">0.217 -0.270 1.038 0 0 0</pose>
- </arrangement>
- <arrangement name="pinch_2">
- <pose model="round_tin_base">0.217 -0.274 1.036 0 0 0</pose>
- <pose model="round_tin_top">0.217 0.04 1.393 0 0 0 </pose>
- <pose model="marble_1_5cm">0.217 -0.274 1.036 0 0 0</pose>
- </arrangement>
-
- </plugin>
- </world>
-</sdf>
diff --git a/worlds/blank.world b/worlds/blank.world
index fa36358..d533e33 100644
--- a/worlds/blank.world
+++ b/worlds/blank.world
@@ -6,6 +6,7 @@
<background>1 1 1 1</background>
<shadows>false</shadows>
<grid>false</grid>
+ <origin_visual>false</origin_visual>
</scene>
</world>
</sdf>
diff --git a/worlds/cessna_demo.world b/worlds/cessna_demo.world
new file mode 100644
index 0000000..30ee638
--- /dev/null
+++ b/worlds/cessna_demo.world
@@ -0,0 +1,217 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+ <world name="default">
+ <gui>
+ <!-- A plugin for controlling the Cessna with the keyboard -->
+ <plugin name="cessna_keyboard" filename="libCessnaGUIPlugin.so"/>
+
+ <camera name="user_camera">
+ <pose>-16 0 4 0 0 0</pose>
+ </camera>
+ </gui>
+
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <model name="ground_plane">
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>5000 5000</size>
+ </plane>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>1</mu>
+ <mu2>1</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <visual name="runway">
+ <pose>700 0 0.1 0 0 0</pose>
+ <cast_shadows>false</cast_shadows>
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>1829 45</size>
+ </plane>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Runway</name>
+ </script>
+ </material>
+ </visual>
+
+ <visual name="grass">
+ <pose>0 0 -0.1 0 0 0</pose>
+ <cast_shadows>false</cast_shadows>
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>5000 5000</size>
+ </plane>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grass</name>
+ </script>
+ </material>
+ </visual>
+
+ </link>
+ </model>
+
+ <model name="cessna_c172">
+ <include>
+ <uri>model://cessna</uri>
+ </include>
+
+ <link name="blade_1_visual">
+ <pose>1.79 0 1.350 0 0 0</pose>
+ <gravity>0</gravity>
+ </link>
+
+ <link name="wing_body_debug_visuals">
+ <pose>0 0 0.495 0 0 0</pose>
+ <gravity>0</gravity>
+ </link>
+
+ <!-- Plugins for controlling the thrust and control surfaces -->
+ <plugin name="cessna_control" filename="libCessnaPlugin.so">
+ <propeller>cessna_c172::propeller_joint</propeller>
+ <propeller_max_rpm>2500</propeller_max_rpm>
+ <left_aileron>cessna_c172::left_aileron_joint</left_aileron>
+ <left_flap>cessna_c172::left_flap_joint</left_flap>
+ <right_aileron>cessna_c172::right_aileron_joint</right_aileron>
+ <right_flap>cessna_c172::right_flap_joint</right_flap>
+ <elevators>cessna_c172::elevators_joint</elevators>
+ <rudder>cessna_c172::rudder_joint</rudder>
+ <propeller_p_gain>10000</propeller_p_gain>
+ <propeller_i_gain>0</propeller_i_gain>
+ <propeller_d_gain>0</propeller_d_gain>
+ <surfaces_p_gain>2000</surfaces_p_gain>
+ <surfaces_i_gain>0</surfaces_i_gain>
+ <surfaces_d_gain>0</surfaces_d_gain>
+ </plugin>
+ <plugin name="propeller_top_blade" filename="libLiftDragPlugin.so">
+ <a0>0.4</a0>
+ <cla>4.752798721</cla>
+ <cda>0.6417112299</cda>
+ <cma>0</cma>
+ <alpha_stall>1.5</alpha_stall>
+ <cla_stall>-3.85</cla_stall>
+ <cda_stall>-0.9233984055</cda_stall>
+ <cma_stall>0</cma_stall>
+ <cp>-0.37 0 0.77</cp>
+ <area>0.1</area>
+ <air_density>1.2041</air_density>
+ <forward>0 -1 0</forward>
+ <upward>1 0 0</upward>
+ <link_name>cessna_c172::propeller</link_name>
+ </plugin>
+ <plugin name="propeller_bottom_blade" filename="libLiftDragPlugin.so">
+ <a0>0.4</a0>
+ <cla>4.752798721</cla>
+ <cda>0.6417112299</cda>
+ <cma>0</cma>
+ <alpha_stall>1.5</alpha_stall>
+ <cla_stall>-3.85</cla_stall>
+ <cda_stall>-0.9233984055</cda_stall>
+ <cma_stall>0</cma_stall>
+ <cp>-0.37 0 -0.77</cp>
+ <area>0.1</area>
+ <air_density>1.2041</air_density>
+ <forward>0 1 0</forward>
+ <upward>1 0 0</upward>
+ <link_name>cessna_c172::propeller</link_name>
+ </plugin>
+
+ <plugin name="left_wing" filename="libLiftDragPlugin.so">
+ <a0>0.05984281113</a0>
+ <cla>4.752798721</cla>
+ <cda>0.6417112299</cda>
+ <cma>-1.8</cma>
+ <alpha_stall>0.3391428111</alpha_stall>
+ <cla_stall>-3.85</cla_stall>
+ <cda_stall>-0.9233984055</cda_stall>
+ <cma_stall>0</cma_stall>
+ <cp>-1 2.205 1.5</cp>
+ <area>8.08255</area>
+ <air_density>1.2041</air_density>
+ <forward>1 0 0</forward>
+ <upward>0 0 1</upward>
+ <link_name>cessna_c172::body</link_name>
+ <control_joint_name>cessna_c172::left_aileron_joint</control_joint_name>
+ <control_joint_rad_to_cl>-2.0</control_joint_rad_to_cl>
+ </plugin>
+
+ <plugin name="right_wing" filename="libLiftDragPlugin.so">
+ <a0>0.05984281113</a0>
+ <cla>4.752798721</cla>
+ <cda>0.6417112299</cda>
+ <cma>-1.8</cma>
+ <alpha_stall>0.3391428111</alpha_stall>
+ <cla_stall>-3.85</cla_stall>
+ <cda_stall>-0.9233984055</cda_stall>
+ <cma_stall>0</cma_stall>
+ <cp>-1 -2.205 1.5</cp>
+ <area>8.08255</area>
+ <air_density>1.2041</air_density>
+ <forward>1 0 0</forward>
+ <upward>0 0 1</upward>
+ <link_name>cessna_c172::body</link_name>
+ <control_joint_name>
+ cessna_c172::right_aileron_joint
+ </control_joint_name>
+ <control_joint_rad_to_cl>-2.0</control_joint_rad_to_cl>
+ </plugin>
+
+ <plugin name="elevator" filename="libLiftDragPlugin.so">
+ <a0>-0.2</a0>
+ <cla>4.752798721</cla>
+ <cda>0.6417112299</cda>
+ <cma>-1.8</cma>
+ <alpha_stall>0.3391428111</alpha_stall>
+ <cla_stall>-3.85</cla_stall>
+ <cda_stall>-0.9233984055</cda_stall>
+ <cma_stall>0</cma_stall>
+ <cp>-5.45 0 0.55</cp>
+ <area>2.03458</area>
+ <air_density>1.2041</air_density>
+ <forward>1 0 0</forward>
+ <upward>0 0 1</upward>
+ <link_name>cessna_c172::body</link_name>
+ <control_joint_name>cessna_c172::elevators_joint</control_joint_name>
+ <control_joint_rad_to_cl>-4.0</control_joint_rad_to_cl>
+ </plugin>
+
+ <plugin name="rudder" filename="libLiftDragPlugin.so">
+ <a0>0</a0>
+ <cla>4.752798721</cla>
+ <cda>0.6417112299</cda>
+ <cma>-1.8</cma>
+ <alpha_stall>0.3391428111</alpha_stall>
+ <cla_stall>-3.85</cla_stall>
+ <cda_stall>-0.9233984055</cda_stall>
+ <cma_stall>0</cma_stall>
+ <cp>-6 0 1.55</cp>
+ <area>1.5329</area>
+ <air_density>1.2041</air_density>
+ <forward>1 0 0</forward>
+ <upward>0 1 0</upward>
+ <link_name>cessna_c172::body</link_name>
+ <control_joint_name>cessna_c172::rudder_joint</control_joint_name>
+ <control_joint_rad_to_cl>4.0</control_joint_rad_to_cl>
+ </plugin>
+ </model>
+ </world>
+</sdf>
diff --git a/worlds/elevator.world b/worlds/elevator.world
new file mode 100644
index 0000000..a9704a9
--- /dev/null
+++ b/worlds/elevator.world
@@ -0,0 +1,334 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+
+ <!-- A global light source -->
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <!-- A ground plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <!-- World plugin that publishes event messages.
+ The topics used in these messages match up with topics used by the
+ plugin in the elevator model below.
+ -->
+ <plugin filename="libSimEventsPlugin.so" name="elevator_event_plugin">
+ <!-- Region on the ground floor, in front of the elevator -->
+ <region>
+ <name>region1</name>
+ <volume>
+ <min>1.5 -1 0</min>
+ <max>2.5 1 1</max>
+ </volume>
+ </region>
+
+ <!-- Region on the first floor, in front of the elevator -->
+ <region>
+ <name>region2</name>
+ <volume>
+ <min>1.5 -1 3</min>
+ <max>2.5 1 4</max>
+ </volume>
+ </region>
+
+ <!-- Event publisher for ground floor-->
+ <event>
+ <name>region1_event</name>
+ <type>occupied</type>
+ <region>region1</region>
+ <topic>~/elevator</topic>
+ <msg_data>0</msg_data>
+ </event>
+
+ <!-- Event publisher for first floor-->
+ <event>
+ <name>region2_event</name>
+ <type>occupied</type>
+ <region>region2</region>
+ <topic>~/elevator</topic>
+ <msg_data>1</msg_data>
+ </event>
+ </plugin>
+
+ <!-- The elevator car -->
+ <model name="elevator">
+ <pose>0 0 0.075 0 0 0</pose>
+ <link name="link">
+ <inertial>
+ <mass>800</mass>
+ </inertial>
+
+ <collision name="floor_collision">
+ <geometry>
+ <box>
+ <size>2.25 2.25 0.15</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="floor_visual">
+ <geometry>
+ <box>
+ <size>2.25 2.25 0.15</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <!-- wall1_collision is the wall to the right of the door, when
+ facing the door -->
+ <collision name="wall1_collision">
+ <pose>1.0745 0.5725 1.125 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.1 1.15 2.25</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="wall1_visual">
+ <pose>1.0745 0.5725 1.125 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.1 1.15 2.25</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+
+ <!-- wall2_collision is the wall to the left of the door, when
+ facing the door -->
+ <collision name="wall2_collision">
+ <pose>1.0745 -1.0625 1.125 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.1 0.125 2.25</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="wall2_visual">
+ <pose>1.0745 -1.0625 1.125 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.1 0.125 2.25</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <link name="door">
+ <pose>1.0745 -0.5 1.125 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.08 1.0 2.25</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.08 1.0 2.25</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="door" type="prismatic">
+ <parent>link</parent>
+ <child>door</child>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>0</lower>
+ <upper>1</upper>
+ <effort>10</effort>
+ </limit>
+
+ <dynamics>
+ <damping>2</damping>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <joint name="lift" type="prismatic">
+ <parent>world</parent>
+ <child>link</child>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>0</lower>
+ <upper>10</upper>
+ <effort>100000</effort>
+ </limit>
+
+ <dynamics>
+ <damping>50</damping>
+ </dynamics>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>
+ </ode>
+ </physics>
+ </joint>
+
+ <!-- Plugin to control the elevator -->
+ <plugin filename="libElevatorPlugin.so" name="elevator_plugin">
+ <lift_joint>elevator::lift</lift_joint>
+ <door_joint>elevator::door</door_joint>
+ <floor_height>3.075</floor_height>
+
+ <!-- Time the elevator door will stay open in seconds -->
+ <door_wait_time>10</door_wait_time>
+
+ <!-- This topic is used to control the elevator -->
+ <topic>~/elevator</topic>
+ </plugin>
+ </model>
+
+ <!-- Elevator shaft -->
+ <model name="elevator_shaft">
+ <static>true</static>
+ <link name="link">
+ <collision name="shaft_wall1_collision">
+ <pose>0 1.25 3 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>2.5 0.15 6</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="shaft_wall1_visual">
+ <pose>0 1.25 3 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>2.5 0.15 6</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <collision name="shaft_wall2_collision">
+ <pose>0 -1.25 3 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>2.5 0.15 6</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="shaft_wall2_visual">
+ <pose>0 -1.25 3 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>2.5 0.15 6</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <collision name="shaft_wall3_collision">
+ <pose>-1.25 0 3 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.15 2.7 6</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="shaft_wall3_visual">
+ <pose>-1.25 0 3 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.15 2.7 6</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <collision name="floor0_collision">
+ <pose>1.19 0 0.075 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.12 2.5 0.15</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="floor0_visual">
+ <pose>1.19 0 0.075 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.12 2.5 0.15</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <collision name="floor1_collision">
+ <pose>1.19 0 3.075 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.12 2.5 0.15</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="floor1_visual">
+ <pose>1.19 0 3.075 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.12 2.5 0.15</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ <model name="floor0">
+ <pose>2.25 0 0.075 0 0 0</pose>
+ <static>true</static>
+ <link name="link">
+ <collision name="floor_collision">
+ <geometry>
+ <box>
+ <size>2.0 5.0 0.15</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="floor_visual">
+ <geometry>
+ <box>
+ <size>2.0 5.0 0.15</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ <model name="floor1">
+ <pose>2.25 0 3.075 0 0 0</pose>
+ <static>true</static>
+ <link name="link">
+ <collision name="floor_collision">
+ <geometry>
+ <box>
+ <size>2.0 5.0 0.15</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="floor_visual">
+ <geometry>
+ <box>
+ <size>2.0 5.0 0.15</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ </world>
+</sdf>
diff --git a/worlds/flocking.world b/worlds/flocking.world
new file mode 100644
index 0000000..df1476a
--- /dev/null
+++ b/worlds/flocking.world
@@ -0,0 +1,34094 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <scene>
+ <shadows>false</shadows>
+ <ambient>1 1 1 1</ambient>
+ </scene>
+
+ <physics type="ode">
+ <ode>
+ <solver>
+ <type>world</type>
+ </solver>
+ </ode>
+ <max_step_size>0.1</max_step_size>
+ <real_time_update_rate>0</real_time_update_rate>
+ </physics>
+
+
+ <model name="box_0">
+ <pose>0.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_1">
+ <pose>0.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_2">
+ <pose>0.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_3">
+ <pose>1.2000000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_4">
+ <pose>1.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_5">
+ <pose>2.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_6">
+ <pose>2.4000000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_7">
+ <pose>2.8000000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_8">
+ <pose>3.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_9">
+ <pose>3.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_10">
+ <pose>4.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_11">
+ <pose>4.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_12">
+ <pose>4.800000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_13">
+ <pose>5.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_14">
+ <pose>5.6000000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_15">
+ <pose>6.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_16">
+ <pose>6.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_17">
+ <pose>6.800000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_18">
+ <pose>7.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_19">
+ <pose>7.6000000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_20">
+ <pose>8.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_21">
+ <pose>8.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_22">
+ <pose>8.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_23">
+ <pose>9.200000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_24">
+ <pose>9.600000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_25">
+ <pose>10.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_26">
+ <pose>10.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_27">
+ <pose>10.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_28">
+ <pose>11.200000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_29">
+ <pose>11.600000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_30">
+ <pose>12.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_31">
+ <pose>12.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_32">
+ <pose>12.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_33">
+ <pose>13.200000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_34">
+ <pose>13.600000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_35">
+ <pose>14.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_36">
+ <pose>14.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_37">
+ <pose>14.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_38">
+ <pose>15.200000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_39">
+ <pose>15.600000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_40">
+ <pose>16.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_41">
+ <pose>16.400000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_42">
+ <pose>16.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_43">
+ <pose>17.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_44">
+ <pose>17.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_45">
+ <pose>18.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_46">
+ <pose>18.400000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_47">
+ <pose>18.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_48">
+ <pose>19.200000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_49">
+ <pose>19.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_50">
+ <pose>20.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_51">
+ <pose>20.400000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_52">
+ <pose>20.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_53">
+ <pose>21.200000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_54">
+ <pose>21.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_55">
+ <pose>22.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_56">
+ <pose>22.400000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_57">
+ <pose>22.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_58">
+ <pose>23.200000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_59">
+ <pose>23.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_60">
+ <pose>24.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_61">
+ <pose>24.400000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_62">
+ <pose>24.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_63">
+ <pose>25.200000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_64">
+ <pose>25.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_65">
+ <pose>26.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_66">
+ <pose>26.400000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_67">
+ <pose>26.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_68">
+ <pose>27.200000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_69">
+ <pose>27.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_70">
+ <pose>28.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_71">
+ <pose>28.400000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_72">
+ <pose>28.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_73">
+ <pose>29.200000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_74">
+ <pose>29.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_75">
+ <pose>30.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_76">
+ <pose>30.400000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_77">
+ <pose>30.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_78">
+ <pose>31.200000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_79">
+ <pose>31.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_80">
+ <pose>32.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_81">
+ <pose>32.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_82">
+ <pose>32.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_83">
+ <pose>33.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_84">
+ <pose>33.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_85">
+ <pose>34.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_86">
+ <pose>34.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_87">
+ <pose>34.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_88">
+ <pose>35.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_89">
+ <pose>35.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_90">
+ <pose>36.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_91">
+ <pose>36.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_92">
+ <pose>36.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_93">
+ <pose>37.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_94">
+ <pose>37.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_95">
+ <pose>38.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_96">
+ <pose>38.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_97">
+ <pose>38.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_98">
+ <pose>39.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_99">
+ <pose>39.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_100">
+ <pose>40.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_101">
+ <pose>40.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_102">
+ <pose>40.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_103">
+ <pose>41.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_104">
+ <pose>41.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_105">
+ <pose>42.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_106">
+ <pose>42.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_107">
+ <pose>42.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_108">
+ <pose>43.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_109">
+ <pose>43.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_110">
+ <pose>44.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_111">
+ <pose>44.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_112">
+ <pose>44.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_113">
+ <pose>45.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_114">
+ <pose>45.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_115">
+ <pose>46.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_116">
+ <pose>46.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_117">
+ <pose>46.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_118">
+ <pose>47.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_119">
+ <pose>47.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_120">
+ <pose>48.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_121">
+ <pose>48.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_122">
+ <pose>48.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_123">
+ <pose>49.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_124">
+ <pose>49.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_125">
+ <pose>50.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_126">
+ <pose>50.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_127">
+ <pose>50.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_128">
+ <pose>51.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_129">
+ <pose>51.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_130">
+ <pose>52.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_131">
+ <pose>52.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_132">
+ <pose>52.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_133">
+ <pose>53.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_134">
+ <pose>53.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_135">
+ <pose>54.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_136">
+ <pose>54.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_137">
+ <pose>54.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_138">
+ <pose>55.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_139">
+ <pose>55.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_140">
+ <pose>56.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_141">
+ <pose>56.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_142">
+ <pose>56.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_143">
+ <pose>57.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_144">
+ <pose>57.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_145">
+ <pose>58.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_146">
+ <pose>58.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_147">
+ <pose>58.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_148">
+ <pose>59.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_149">
+ <pose>59.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_150">
+ <pose>60.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_151">
+ <pose>60.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_152">
+ <pose>60.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_153">
+ <pose>61.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_154">
+ <pose>61.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_155">
+ <pose>62.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_156">
+ <pose>62.400000000000006 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_157">
+ <pose>62.800000000000004 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_158">
+ <pose>63.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_159">
+ <pose>63.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_160">
+ <pose>64.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_161">
+ <pose>64.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_162">
+ <pose>64.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_163">
+ <pose>65.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_164">
+ <pose>65.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_165">
+ <pose>66.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_166">
+ <pose>66.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_167">
+ <pose>66.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_168">
+ <pose>67.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_169">
+ <pose>67.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_170">
+ <pose>68.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_171">
+ <pose>68.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_172">
+ <pose>68.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_173">
+ <pose>69.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_174">
+ <pose>69.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_175">
+ <pose>70.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_176">
+ <pose>70.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_177">
+ <pose>70.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_178">
+ <pose>71.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_179">
+ <pose>71.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_180">
+ <pose>72.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_181">
+ <pose>72.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_182">
+ <pose>72.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_183">
+ <pose>73.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_184">
+ <pose>73.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_185">
+ <pose>74.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_186">
+ <pose>74.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_187">
+ <pose>74.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_188">
+ <pose>75.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_189">
+ <pose>75.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_190">
+ <pose>76.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_191">
+ <pose>76.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_192">
+ <pose>76.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_193">
+ <pose>77.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_194">
+ <pose>77.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_195">
+ <pose>78.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_196">
+ <pose>78.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_197">
+ <pose>78.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_198">
+ <pose>79.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_199">
+ <pose>79.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_200">
+ <pose>80.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_201">
+ <pose>80.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_202">
+ <pose>80.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_203">
+ <pose>81.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_204">
+ <pose>81.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_205">
+ <pose>82.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_206">
+ <pose>82.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_207">
+ <pose>82.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_208">
+ <pose>83.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_209">
+ <pose>83.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_210">
+ <pose>84.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_211">
+ <pose>84.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_212">
+ <pose>84.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_213">
+ <pose>85.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_214">
+ <pose>85.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_215">
+ <pose>86.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_216">
+ <pose>86.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_217">
+ <pose>86.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_218">
+ <pose>87.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_219">
+ <pose>87.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_220">
+ <pose>88.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_221">
+ <pose>88.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_222">
+ <pose>88.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_223">
+ <pose>89.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_224">
+ <pose>89.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_225">
+ <pose>90.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_226">
+ <pose>90.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_227">
+ <pose>90.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_228">
+ <pose>91.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_229">
+ <pose>91.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_230">
+ <pose>92.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_231">
+ <pose>92.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_232">
+ <pose>92.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_233">
+ <pose>93.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_234">
+ <pose>93.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_235">
+ <pose>94.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_236">
+ <pose>94.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_237">
+ <pose>94.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_238">
+ <pose>95.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_239">
+ <pose>95.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_240">
+ <pose>96.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_241">
+ <pose>96.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_242">
+ <pose>96.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_243">
+ <pose>97.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_244">
+ <pose>97.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_245">
+ <pose>98.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_246">
+ <pose>98.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_247">
+ <pose>98.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_248">
+ <pose>99.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_249">
+ <pose>99.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_250">
+ <pose>100.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_251">
+ <pose>100.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_252">
+ <pose>100.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_253">
+ <pose>101.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_254">
+ <pose>101.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_255">
+ <pose>102.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_256">
+ <pose>102.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_257">
+ <pose>102.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_258">
+ <pose>103.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_259">
+ <pose>103.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_260">
+ <pose>104.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_261">
+ <pose>104.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_262">
+ <pose>104.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_263">
+ <pose>105.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_264">
+ <pose>105.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_265">
+ <pose>106.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_266">
+ <pose>106.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_267">
+ <pose>106.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_268">
+ <pose>107.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_269">
+ <pose>107.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_270">
+ <pose>108.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_271">
+ <pose>108.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_272">
+ <pose>108.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_273">
+ <pose>109.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_274">
+ <pose>109.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_275">
+ <pose>110.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_276">
+ <pose>110.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_277">
+ <pose>110.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_278">
+ <pose>111.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_279">
+ <pose>111.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_280">
+ <pose>112.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_281">
+ <pose>112.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_282">
+ <pose>112.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_283">
+ <pose>113.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_284">
+ <pose>113.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_285">
+ <pose>114.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_286">
+ <pose>114.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_287">
+ <pose>114.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_288">
+ <pose>115.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_289">
+ <pose>115.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_290">
+ <pose>116.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_291">
+ <pose>116.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_292">
+ <pose>116.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_293">
+ <pose>117.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_294">
+ <pose>117.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_295">
+ <pose>118.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_296">
+ <pose>118.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_297">
+ <pose>118.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_298">
+ <pose>119.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_299">
+ <pose>119.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_300">
+ <pose>120.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_301">
+ <pose>120.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_302">
+ <pose>120.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_303">
+ <pose>121.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_304">
+ <pose>121.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_305">
+ <pose>122.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_306">
+ <pose>122.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_307">
+ <pose>122.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_308">
+ <pose>123.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_309">
+ <pose>123.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_310">
+ <pose>124.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_311">
+ <pose>124.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_312">
+ <pose>124.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_313">
+ <pose>125.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_314">
+ <pose>125.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_315">
+ <pose>126.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_316">
+ <pose>126.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_317">
+ <pose>126.80000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_318">
+ <pose>127.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_319">
+ <pose>127.60000000000001 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_320">
+ <pose>128.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_321">
+ <pose>128.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_322">
+ <pose>128.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_323">
+ <pose>129.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_324">
+ <pose>129.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_325">
+ <pose>130.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_326">
+ <pose>130.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_327">
+ <pose>130.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_328">
+ <pose>131.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_329">
+ <pose>131.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_330">
+ <pose>132.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_331">
+ <pose>132.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_332">
+ <pose>132.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_333">
+ <pose>133.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_334">
+ <pose>133.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_335">
+ <pose>134.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_336">
+ <pose>134.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_337">
+ <pose>134.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_338">
+ <pose>135.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_339">
+ <pose>135.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_340">
+ <pose>136.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_341">
+ <pose>136.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_342">
+ <pose>136.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_343">
+ <pose>137.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_344">
+ <pose>137.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_345">
+ <pose>138.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_346">
+ <pose>138.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_347">
+ <pose>138.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_348">
+ <pose>139.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_349">
+ <pose>139.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_350">
+ <pose>140.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_351">
+ <pose>140.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_352">
+ <pose>140.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_353">
+ <pose>141.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_354">
+ <pose>141.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_355">
+ <pose>142.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_356">
+ <pose>142.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_357">
+ <pose>142.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_358">
+ <pose>143.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_359">
+ <pose>143.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_360">
+ <pose>144.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_361">
+ <pose>144.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_362">
+ <pose>144.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_363">
+ <pose>145.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_364">
+ <pose>145.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_365">
+ <pose>146.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_366">
+ <pose>146.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_367">
+ <pose>146.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_368">
+ <pose>147.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_369">
+ <pose>147.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_370">
+ <pose>148.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_371">
+ <pose>148.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_372">
+ <pose>148.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_373">
+ <pose>149.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_374">
+ <pose>149.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_375">
+ <pose>150.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_376">
+ <pose>150.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_377">
+ <pose>150.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_378">
+ <pose>151.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_379">
+ <pose>151.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_380">
+ <pose>152.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_381">
+ <pose>152.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_382">
+ <pose>152.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_383">
+ <pose>153.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_384">
+ <pose>153.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_385">
+ <pose>154.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_386">
+ <pose>154.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_387">
+ <pose>154.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_388">
+ <pose>155.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_389">
+ <pose>155.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_390">
+ <pose>156.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_391">
+ <pose>156.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_392">
+ <pose>156.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_393">
+ <pose>157.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_394">
+ <pose>157.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_395">
+ <pose>158.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_396">
+ <pose>158.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_397">
+ <pose>158.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_398">
+ <pose>159.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_399">
+ <pose>159.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_400">
+ <pose>160.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_401">
+ <pose>160.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_402">
+ <pose>160.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_403">
+ <pose>161.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_404">
+ <pose>161.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_405">
+ <pose>162.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_406">
+ <pose>162.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_407">
+ <pose>162.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_408">
+ <pose>163.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_409">
+ <pose>163.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_410">
+ <pose>164.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_411">
+ <pose>164.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_412">
+ <pose>164.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_413">
+ <pose>165.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_414">
+ <pose>165.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_415">
+ <pose>166.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_416">
+ <pose>166.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_417">
+ <pose>166.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_418">
+ <pose>167.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_419">
+ <pose>167.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_420">
+ <pose>168.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_421">
+ <pose>168.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_422">
+ <pose>168.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_423">
+ <pose>169.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_424">
+ <pose>169.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_425">
+ <pose>170.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_426">
+ <pose>170.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_427">
+ <pose>170.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_428">
+ <pose>171.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_429">
+ <pose>171.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_430">
+ <pose>172.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_431">
+ <pose>172.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_432">
+ <pose>172.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_433">
+ <pose>173.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_434">
+ <pose>173.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_435">
+ <pose>174.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_436">
+ <pose>174.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_437">
+ <pose>174.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_438">
+ <pose>175.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_439">
+ <pose>175.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_440">
+ <pose>176.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_441">
+ <pose>176.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_442">
+ <pose>176.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_443">
+ <pose>177.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_444">
+ <pose>177.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_445">
+ <pose>178.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_446">
+ <pose>178.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_447">
+ <pose>178.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_448">
+ <pose>179.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_449">
+ <pose>179.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_450">
+ <pose>180.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_451">
+ <pose>180.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_452">
+ <pose>180.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_453">
+ <pose>181.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_454">
+ <pose>181.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_455">
+ <pose>182.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_456">
+ <pose>182.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_457">
+ <pose>182.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_458">
+ <pose>183.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_459">
+ <pose>183.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_460">
+ <pose>184.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_461">
+ <pose>184.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_462">
+ <pose>184.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_463">
+ <pose>185.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_464">
+ <pose>185.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_465">
+ <pose>186.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_466">
+ <pose>186.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_467">
+ <pose>186.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_468">
+ <pose>187.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_469">
+ <pose>187.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_470">
+ <pose>188.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_471">
+ <pose>188.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_472">
+ <pose>188.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_473">
+ <pose>189.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_474">
+ <pose>189.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_475">
+ <pose>190.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_476">
+ <pose>190.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_477">
+ <pose>190.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_478">
+ <pose>191.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_479">
+ <pose>191.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_480">
+ <pose>192.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_481">
+ <pose>192.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_482">
+ <pose>192.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_483">
+ <pose>193.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_484">
+ <pose>193.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_485">
+ <pose>194.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_486">
+ <pose>194.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_487">
+ <pose>194.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_488">
+ <pose>195.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_489">
+ <pose>195.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_490">
+ <pose>196.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_491">
+ <pose>196.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_492">
+ <pose>196.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_493">
+ <pose>197.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_494">
+ <pose>197.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_495">
+ <pose>198.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_496">
+ <pose>198.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_497">
+ <pose>198.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_498">
+ <pose>199.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_499">
+ <pose>199.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_500">
+ <pose>200.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_501">
+ <pose>200.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_502">
+ <pose>200.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_503">
+ <pose>201.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_504">
+ <pose>201.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_505">
+ <pose>202.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_506">
+ <pose>202.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_507">
+ <pose>202.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_508">
+ <pose>203.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_509">
+ <pose>203.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_510">
+ <pose>204.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_511">
+ <pose>204.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_512">
+ <pose>204.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_513">
+ <pose>205.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_514">
+ <pose>205.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_515">
+ <pose>206.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_516">
+ <pose>206.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_517">
+ <pose>206.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_518">
+ <pose>207.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_519">
+ <pose>207.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_520">
+ <pose>208.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_521">
+ <pose>208.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_522">
+ <pose>208.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_523">
+ <pose>209.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_524">
+ <pose>209.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_525">
+ <pose>210.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_526">
+ <pose>210.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_527">
+ <pose>210.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_528">
+ <pose>211.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_529">
+ <pose>211.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_530">
+ <pose>212.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_531">
+ <pose>212.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_532">
+ <pose>212.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_533">
+ <pose>213.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_534">
+ <pose>213.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_535">
+ <pose>214.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_536">
+ <pose>214.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_537">
+ <pose>214.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_538">
+ <pose>215.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_539">
+ <pose>215.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_540">
+ <pose>216.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_541">
+ <pose>216.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_542">
+ <pose>216.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_543">
+ <pose>217.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_544">
+ <pose>217.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_545">
+ <pose>218.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_546">
+ <pose>218.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_547">
+ <pose>218.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_548">
+ <pose>219.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_549">
+ <pose>219.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_550">
+ <pose>220.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_551">
+ <pose>220.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_552">
+ <pose>220.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_553">
+ <pose>221.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_554">
+ <pose>221.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_555">
+ <pose>222.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_556">
+ <pose>222.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_557">
+ <pose>222.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_558">
+ <pose>223.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_559">
+ <pose>223.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_560">
+ <pose>224.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_561">
+ <pose>224.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_562">
+ <pose>224.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_563">
+ <pose>225.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_564">
+ <pose>225.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_565">
+ <pose>226.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_566">
+ <pose>226.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_567">
+ <pose>226.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_568">
+ <pose>227.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_569">
+ <pose>227.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_570">
+ <pose>228.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_571">
+ <pose>228.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_572">
+ <pose>228.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_573">
+ <pose>229.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_574">
+ <pose>229.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_575">
+ <pose>230.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_576">
+ <pose>230.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_577">
+ <pose>230.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_578">
+ <pose>231.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_579">
+ <pose>231.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_580">
+ <pose>232.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_581">
+ <pose>232.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_582">
+ <pose>232.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_583">
+ <pose>233.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_584">
+ <pose>233.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_585">
+ <pose>234.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_586">
+ <pose>234.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_587">
+ <pose>234.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_588">
+ <pose>235.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_589">
+ <pose>235.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_590">
+ <pose>236.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_591">
+ <pose>236.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_592">
+ <pose>236.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_593">
+ <pose>237.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_594">
+ <pose>237.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_595">
+ <pose>238.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_596">
+ <pose>238.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_597">
+ <pose>238.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_598">
+ <pose>239.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_599">
+ <pose>239.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_600">
+ <pose>240.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_601">
+ <pose>240.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_602">
+ <pose>240.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_603">
+ <pose>241.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_604">
+ <pose>241.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_605">
+ <pose>242.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_606">
+ <pose>242.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_607">
+ <pose>242.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_608">
+ <pose>243.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_609">
+ <pose>243.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_610">
+ <pose>244.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_611">
+ <pose>244.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_612">
+ <pose>244.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_613">
+ <pose>245.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_614">
+ <pose>245.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_615">
+ <pose>246.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_616">
+ <pose>246.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_617">
+ <pose>246.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_618">
+ <pose>247.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_619">
+ <pose>247.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_620">
+ <pose>248.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_621">
+ <pose>248.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_622">
+ <pose>248.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_623">
+ <pose>249.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_624">
+ <pose>249.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_625">
+ <pose>250.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_626">
+ <pose>250.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_627">
+ <pose>250.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_628">
+ <pose>251.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_629">
+ <pose>251.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_630">
+ <pose>252.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_631">
+ <pose>252.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_632">
+ <pose>252.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_633">
+ <pose>253.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_634">
+ <pose>253.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_635">
+ <pose>254.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_636">
+ <pose>254.4 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_637">
+ <pose>254.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_638">
+ <pose>255.20000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_639">
+ <pose>255.60000000000002 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_640">
+ <pose>256.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_641">
+ <pose>256.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_642">
+ <pose>256.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_643">
+ <pose>257.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_644">
+ <pose>257.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_645">
+ <pose>258.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_646">
+ <pose>258.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_647">
+ <pose>258.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_648">
+ <pose>259.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_649">
+ <pose>259.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_650">
+ <pose>260.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_651">
+ <pose>260.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_652">
+ <pose>260.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_653">
+ <pose>261.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_654">
+ <pose>261.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_655">
+ <pose>262.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_656">
+ <pose>262.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_657">
+ <pose>262.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_658">
+ <pose>263.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_659">
+ <pose>263.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_660">
+ <pose>264.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_661">
+ <pose>264.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_662">
+ <pose>264.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_663">
+ <pose>265.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_664">
+ <pose>265.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_665">
+ <pose>266.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_666">
+ <pose>266.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_667">
+ <pose>266.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_668">
+ <pose>267.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_669">
+ <pose>267.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_670">
+ <pose>268.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_671">
+ <pose>268.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_672">
+ <pose>268.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_673">
+ <pose>269.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_674">
+ <pose>269.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_675">
+ <pose>270.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_676">
+ <pose>270.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_677">
+ <pose>270.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_678">
+ <pose>271.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_679">
+ <pose>271.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_680">
+ <pose>272.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_681">
+ <pose>272.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_682">
+ <pose>272.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_683">
+ <pose>273.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_684">
+ <pose>273.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_685">
+ <pose>274.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_686">
+ <pose>274.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_687">
+ <pose>274.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_688">
+ <pose>275.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_689">
+ <pose>275.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_690">
+ <pose>276.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_691">
+ <pose>276.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_692">
+ <pose>276.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_693">
+ <pose>277.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_694">
+ <pose>277.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_695">
+ <pose>278.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_696">
+ <pose>278.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_697">
+ <pose>278.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_698">
+ <pose>279.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_699">
+ <pose>279.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_700">
+ <pose>280.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_701">
+ <pose>280.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_702">
+ <pose>280.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_703">
+ <pose>281.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_704">
+ <pose>281.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_705">
+ <pose>282.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_706">
+ <pose>282.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_707">
+ <pose>282.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_708">
+ <pose>283.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_709">
+ <pose>283.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_710">
+ <pose>284.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_711">
+ <pose>284.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_712">
+ <pose>284.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_713">
+ <pose>285.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_714">
+ <pose>285.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_715">
+ <pose>286.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_716">
+ <pose>286.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_717">
+ <pose>286.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_718">
+ <pose>287.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_719">
+ <pose>287.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_720">
+ <pose>288.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_721">
+ <pose>288.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_722">
+ <pose>288.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_723">
+ <pose>289.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_724">
+ <pose>289.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_725">
+ <pose>290.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_726">
+ <pose>290.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_727">
+ <pose>290.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_728">
+ <pose>291.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_729">
+ <pose>291.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_730">
+ <pose>292.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_731">
+ <pose>292.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_732">
+ <pose>292.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_733">
+ <pose>293.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_734">
+ <pose>293.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_735">
+ <pose>294.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_736">
+ <pose>294.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_737">
+ <pose>294.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_738">
+ <pose>295.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_739">
+ <pose>295.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_740">
+ <pose>296.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_741">
+ <pose>296.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_742">
+ <pose>296.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_743">
+ <pose>297.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_744">
+ <pose>297.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_745">
+ <pose>298.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_746">
+ <pose>298.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_747">
+ <pose>298.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_748">
+ <pose>299.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_749">
+ <pose>299.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_750">
+ <pose>300.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_751">
+ <pose>300.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_752">
+ <pose>300.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_753">
+ <pose>301.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_754">
+ <pose>301.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_755">
+ <pose>302.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_756">
+ <pose>302.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_757">
+ <pose>302.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_758">
+ <pose>303.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_759">
+ <pose>303.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_760">
+ <pose>304.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_761">
+ <pose>304.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_762">
+ <pose>304.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_763">
+ <pose>305.2 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_764">
+ <pose>305.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_765">
+ <pose>306.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_766">
+ <pose>306.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_767">
+ <pose>306.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_768">
+ <pose>307.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_769">
+ <pose>307.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_770">
+ <pose>308.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_771">
+ <pose>308.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_772">
+ <pose>308.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_773">
+ <pose>309.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_774">
+ <pose>309.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_775">
+ <pose>310.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_776">
+ <pose>310.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_777">
+ <pose>310.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_778">
+ <pose>311.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_779">
+ <pose>311.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_780">
+ <pose>312.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_781">
+ <pose>312.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_782">
+ <pose>312.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_783">
+ <pose>313.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_784">
+ <pose>313.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_785">
+ <pose>314.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_786">
+ <pose>314.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_787">
+ <pose>314.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_788">
+ <pose>315.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_789">
+ <pose>315.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_790">
+ <pose>316.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_791">
+ <pose>316.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_792">
+ <pose>316.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_793">
+ <pose>317.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_794">
+ <pose>317.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_795">
+ <pose>318.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_796">
+ <pose>318.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_797">
+ <pose>318.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_798">
+ <pose>319.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_799">
+ <pose>319.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_800">
+ <pose>320.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_801">
+ <pose>320.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_802">
+ <pose>320.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_803">
+ <pose>321.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_804">
+ <pose>321.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_805">
+ <pose>322.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_806">
+ <pose>322.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_807">
+ <pose>322.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_808">
+ <pose>323.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_809">
+ <pose>323.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_810">
+ <pose>324.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_811">
+ <pose>324.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_812">
+ <pose>324.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_813">
+ <pose>325.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_814">
+ <pose>325.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_815">
+ <pose>326.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_816">
+ <pose>326.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_817">
+ <pose>326.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_818">
+ <pose>327.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_819">
+ <pose>327.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_820">
+ <pose>328.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_821">
+ <pose>328.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_822">
+ <pose>328.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_823">
+ <pose>329.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_824">
+ <pose>329.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_825">
+ <pose>330.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_826">
+ <pose>330.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_827">
+ <pose>330.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_828">
+ <pose>331.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_829">
+ <pose>331.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_830">
+ <pose>332.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_831">
+ <pose>332.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_832">
+ <pose>332.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_833">
+ <pose>333.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_834">
+ <pose>333.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_835">
+ <pose>334.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_836">
+ <pose>334.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_837">
+ <pose>334.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_838">
+ <pose>335.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_839">
+ <pose>335.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_840">
+ <pose>336.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_841">
+ <pose>336.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_842">
+ <pose>336.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_843">
+ <pose>337.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_844">
+ <pose>337.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_845">
+ <pose>338.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_846">
+ <pose>338.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_847">
+ <pose>338.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_848">
+ <pose>339.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_849">
+ <pose>339.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_850">
+ <pose>340.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_851">
+ <pose>340.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_852">
+ <pose>340.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_853">
+ <pose>341.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_854">
+ <pose>341.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_855">
+ <pose>342.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_856">
+ <pose>342.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_857">
+ <pose>342.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_858">
+ <pose>343.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_859">
+ <pose>343.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_860">
+ <pose>344.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_861">
+ <pose>344.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_862">
+ <pose>344.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_863">
+ <pose>345.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_864">
+ <pose>345.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_865">
+ <pose>346.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_866">
+ <pose>346.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_867">
+ <pose>346.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_868">
+ <pose>347.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_869">
+ <pose>347.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_870">
+ <pose>348.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_871">
+ <pose>348.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_872">
+ <pose>348.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_873">
+ <pose>349.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_874">
+ <pose>349.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_875">
+ <pose>350.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_876">
+ <pose>350.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_877">
+ <pose>350.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_878">
+ <pose>351.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_879">
+ <pose>351.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_880">
+ <pose>352.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_881">
+ <pose>352.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_882">
+ <pose>352.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_883">
+ <pose>353.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_884">
+ <pose>353.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_885">
+ <pose>354.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_886">
+ <pose>354.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_887">
+ <pose>354.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_888">
+ <pose>355.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_889">
+ <pose>355.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_890">
+ <pose>356.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_891">
+ <pose>356.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_892">
+ <pose>356.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_893">
+ <pose>357.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_894">
+ <pose>357.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_895">
+ <pose>358.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_896">
+ <pose>358.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_897">
+ <pose>358.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_898">
+ <pose>359.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_899">
+ <pose>359.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_900">
+ <pose>360.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_901">
+ <pose>360.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_902">
+ <pose>360.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_903">
+ <pose>361.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_904">
+ <pose>361.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_905">
+ <pose>362.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_906">
+ <pose>362.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_907">
+ <pose>362.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_908">
+ <pose>363.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_909">
+ <pose>363.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_910">
+ <pose>364.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_911">
+ <pose>364.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_912">
+ <pose>364.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_913">
+ <pose>365.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_914">
+ <pose>365.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_915">
+ <pose>366.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_916">
+ <pose>366.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_917">
+ <pose>366.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_918">
+ <pose>367.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_919">
+ <pose>367.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_920">
+ <pose>368.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_921">
+ <pose>368.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_922">
+ <pose>368.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_923">
+ <pose>369.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_924">
+ <pose>369.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_925">
+ <pose>370.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_926">
+ <pose>370.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_927">
+ <pose>370.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_928">
+ <pose>371.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_929">
+ <pose>371.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_930">
+ <pose>372.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_931">
+ <pose>372.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_932">
+ <pose>372.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_933">
+ <pose>373.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_934">
+ <pose>373.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_935">
+ <pose>374.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_936">
+ <pose>374.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_937">
+ <pose>374.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_938">
+ <pose>375.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_939">
+ <pose>375.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_940">
+ <pose>376.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_941">
+ <pose>376.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_942">
+ <pose>376.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_943">
+ <pose>377.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_944">
+ <pose>377.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_945">
+ <pose>378.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_946">
+ <pose>378.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_947">
+ <pose>378.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_948">
+ <pose>379.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_949">
+ <pose>379.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_950">
+ <pose>380.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_951">
+ <pose>380.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_952">
+ <pose>380.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_953">
+ <pose>381.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_954">
+ <pose>381.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_955">
+ <pose>382.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_956">
+ <pose>382.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_957">
+ <pose>382.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_958">
+ <pose>383.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_959">
+ <pose>383.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_960">
+ <pose>384.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_961">
+ <pose>384.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_962">
+ <pose>384.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_963">
+ <pose>385.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_964">
+ <pose>385.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_965">
+ <pose>386.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_966">
+ <pose>386.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_967">
+ <pose>386.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_968">
+ <pose>387.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_969">
+ <pose>387.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_970">
+ <pose>388.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_971">
+ <pose>388.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_972">
+ <pose>388.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_973">
+ <pose>389.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_974">
+ <pose>389.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_975">
+ <pose>390.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_976">
+ <pose>390.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_977">
+ <pose>390.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_978">
+ <pose>391.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_979">
+ <pose>391.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_980">
+ <pose>392.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_981">
+ <pose>392.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_982">
+ <pose>392.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_983">
+ <pose>393.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_984">
+ <pose>393.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_985">
+ <pose>394.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_986">
+ <pose>394.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_987">
+ <pose>394.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_988">
+ <pose>395.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_989">
+ <pose>395.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_990">
+ <pose>396.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_991">
+ <pose>396.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_992">
+ <pose>396.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_993">
+ <pose>397.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_994">
+ <pose>397.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_995">
+ <pose>398.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_996">
+ <pose>398.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_997">
+ <pose>398.8 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_998">
+ <pose>399.20000000000005 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_999">
+ <pose>399.6 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_1000">
+ <pose>400.0 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="box_1001">
+ <pose>400.40000000000003 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ </world>
+</sdf>
+
diff --git a/worlds/flocking.world.erb b/worlds/flocking.world.erb
new file mode 100644
index 0000000..bc87e32
--- /dev/null
+++ b/worlds/flocking.world.erb
@@ -0,0 +1,60 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <scene>
+ <shadows>false</shadows>
+ <ambient>1 1 1 1</ambient>
+ </scene>
+
+ <physics type="ode">
+ <ode>
+ <solver>
+ <type>world</type>
+ </solver>
+ </ode>
+ <max_step_size>0.1</max_step_size>
+ <real_time_update_rate>0</real_time_update_rate>
+ </physics>
+
+ <% for i in 0..1001 %>
+ <model name="box_<%=i%>">
+ <pose><%=i*0.4%> 0 0.05 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.2 .1 .1</size>
+ </box>
+ </geometry>
+ </visual>
+ <visual name="visual_front">
+ <pose>0.07 0 0.04 0 0 0</pose>
+ <geometry>
+ <sphere>
+ <radius>.04</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+ <% end %>
+ </world>
+</sdf>
+
diff --git a/worlds/imu_demo.world b/worlds/imu_demo.world
new file mode 100644
index 0000000..80deda6
--- /dev/null
+++ b/worlds/imu_demo.world
@@ -0,0 +1,156 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+
+ <model name="box_old_imu_noise">
+ <pose>0 0 5 0 0 0</pose>
+ <link name="link">
+ <inertial>
+ <inertia>
+ <ixx>1.8</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <iyy>10.8</iyy>
+ <iyz>0.0</iyz>
+ <izz>10.8</izz>
+ </inertia>
+ <mass>10.0</mass>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>3.6 0.2 0.2</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>3.6 0.2 0.2</size>
+ </box>
+ </geometry>
+ <material>
+ <ambient>0.2 0.2 0.2 1.0</ambient>
+ <diffuse>.421 0.225 0.0 1.0</diffuse>
+ </material>
+ </visual>
+ <sensor name='imu_sensor' type='imu'>
+ <always_on>1</always_on>
+ <update_rate>1000</update_rate>
+ <imu>
+ <noise>
+ <type>gaussian</type>
+ <rate>
+ <mean>0</mean>
+ <stddev>0.0002</stddev>
+ <bias_mean>7.5e-06</bias_mean>
+ <bias_stddev>8e-07</bias_stddev>
+ </rate>
+ <accel>
+ <mean>0</mean>
+ <stddev>0.017</stddev>
+ <bias_mean>0.1</bias_mean>
+ <bias_stddev>0.001</bias_stddev>
+ </accel>
+ </noise>
+ </imu>
+ </sensor>
+ </link>
+ </model>
+
+ <model name="box_new_imu_noise">
+ <pose>2 0 5 0 0 0</pose>
+ <link name="link">
+ <inertial>
+ <inertia>
+ <ixx>1.8</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <iyy>10.8</iyy>
+ <iyz>0.0</iyz>
+ <izz>10.8</izz>
+ </inertia>
+ <mass>10.0</mass>
+ </inertial>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>3.6 0.2 0.2</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>3.6 0.2 0.2</size>
+ </box>
+ </geometry>
+ <material>
+ <ambient>0.2 0.2 0.2 1.0</ambient>
+ <diffuse>.421 0.225 0.0 1.0</diffuse>
+ </material>
+ </visual>
+ <sensor name='imu_sensor' type='imu'>
+ <always_on>1</always_on>
+ <update_rate>1000</update_rate>
+ <imu>
+ <angular_velocity>
+ <x>
+ <noise type="gaussian">
+ <mean>0</mean>
+ <stddev>0.0002</stddev>
+ <bias_mean>7.5e-06</bias_mean>
+ <bias_stddev>8e-07</bias_stddev>
+ </noise>
+ </x>
+ <y>
+ <noise type="gaussian">
+ <mean>0</mean>
+ <stddev>0.0002</stddev>
+ <bias_mean>7.5e-06</bias_mean>
+ <bias_stddev>8e-07</bias_stddev>
+ </noise>
+ </y>
+ <z>
+ <noise type="gaussian">
+ <mean>0</mean>
+ <stddev>0.0002</stddev>
+ <bias_mean>7.5e-06</bias_mean>
+ <bias_stddev>8e-07</bias_stddev>
+ </noise>
+ </z>
+ </angular_velocity>
+
+ <linear_acceleration>
+ <x>
+ <noise type="gaussian">
+ <mean>0</mean>
+ <stddev>0.017</stddev>
+ <bias_mean>0.1</bias_mean>
+ <bias_stddev>0.001</bias_stddev>
+ </noise>
+ </x>
+ <y>
+ <noise type="gaussian">
+ <mean>0</mean>
+ <stddev>0.017</stddev>
+ <bias_mean>0.1</bias_mean>
+ <bias_stddev>0.001</bias_stddev>
+ </noise>
+ </y>
+ <z>
+ <noise type="gaussian">
+ <mean>0</mean>
+ <stddev>0.017</stddev>
+ <bias_mean>0.1</bias_mean>
+ <bias_stddev>0.001</bias_stddev>
+ </noise>
+ </z>
+ </linear_acceleration>
+ </imu>
+ </sensor>
+ </link>
+ </model>
+
+ </world>
+</sdf>
diff --git a/worlds/magnetometer.world b/worlds/magnetometer.world
new file mode 100644
index 0000000..708c49e
--- /dev/null
+++ b/worlds/magnetometer.world
@@ -0,0 +1,49 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+
+ <!-- Physics -->
+ <physics type="ode">
+ <magnetic_field>1 0 0</magnetic_field>
+ </physics>
+
+ <!-- A global light source -->
+ <include>
+ <uri>model://sun</uri>
+ </include>
+ <!-- A ground plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <!-- A model containing a magnetometer sensor -->
+ <model name="magnetometerModel">
+ <pose>0 0 0.25 0 0 0</pose>
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.5 0.5 0.5</size>
+ </box>
+ </geometry>
+ </collision>
+
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.5 0.5 0.5</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <sensor name="magnetometer" type="magnetometer">
+ <always_on>1</always_on>
+ <update_rate>1</update_rate>
+ <magnetometer>
+ </magnetometer>
+ </sensor>
+ </link>
+ </model>
+ </world>
+</sdf>
diff --git a/worlds/ortho.world b/worlds/ortho.world
new file mode 100644
index 0000000..abd08f8
--- /dev/null
+++ b/worlds/ortho.world
@@ -0,0 +1,19 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <gui>
+ <camera name="user">
+ <projection_type>orthographic</projection_type>
+ </camera>
+ </gui>
+
+ <!-- A global light source -->
+ <include>
+ <uri>model://sun</uri>
+ </include>
+ <!-- A ground plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+ </world>
+</sdf>
diff --git a/worlds/osrf_elevator.world b/worlds/osrf_elevator.world
new file mode 100644
index 0000000..6e1ca5f
--- /dev/null
+++ b/worlds/osrf_elevator.world
@@ -0,0 +1,284 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <!-- Disable shadows for now since they make the interiors
+ look awful -->
+ <scene>
+ <shadows>0</shadows>
+ </scene>
+
+ <!-- Position the user camera -->
+ <gui>
+ <camera name="user_camera">
+ <pose>16 -30 17 0 0.5235 2.09</pose>
+ </camera>
+ </gui>
+
+ <!-- A global light source -->
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <!-- A ground plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <!-- The OSRF building with a fictitious elevator shaft -->
+ <include>
+ <uri>model://osrf_elevator</uri>
+ </include>
+
+ <!-- World plugin that publishes event messages.
+ The topics used in these messages match up with topics used by the
+ plugin in the elevator car model below.
+ -->
+ <plugin filename="libSimEventsPlugin.so" name="elevator_event_plugin">
+ <!-- Region on the ground floor, in front of the elevator -->
+ <region>
+ <name>region1</name>
+ <volume>
+ <min>-0.223 -4.75 0.15</min>
+ <max>1.92 -3.75 1.15</max>
+ </volume>
+ </region>
+
+ <!-- Region on the first floor, in front of the elevator -->
+ <region>
+ <name>region2</name>
+ <volume>
+ <min>-0.223 -4.75 2.808107</min>
+ <max>1.92 -3.75 3.808107</max>
+ </volume>
+ </region>
+
+ <!-- Event publisher for ground floor-->
+ <event>
+ <name>region1_event</name>
+ <type>occupied</type>
+ <region>region1</region>
+ <topic>~/elevator</topic>
+ <msg_data>0</msg_data>
+ </event>
+
+ <!-- Event publisher for first floor-->
+ <event>
+ <name>region2_event</name>
+ <type>occupied</type>
+ <region>region2</region>
+ <topic>~/elevator</topic>
+ <msg_data>1</msg_data>
+ </event>
+ </plugin>
+
+ <!-- Some interior lights -->
+ <light name='light1' type='point'>
+ <pose>-2.41401 -7 2.65437 0 -0 0</pose>
+ <diffuse>0.5 0.5 0.5 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <attenuation>
+ <range>10</range>
+ <constant>0.5</constant>
+ <linear>0.3</linear>
+ <quadratic>0.1</quadratic>
+ </attenuation>
+ <cast_shadows>0</cast_shadows>
+ </light>
+ <light name='light2' type='point'>
+ <pose>3.46981 -7 2.65437 0 -0 0</pose>
+ <diffuse>0.5 0.5 0.5 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <attenuation>
+ <range>10</range>
+ <constant>0.5</constant>
+ <linear>0.3</linear>
+ <quadratic>0.1</quadratic>
+ </attenuation>
+ <cast_shadows>0</cast_shadows>
+ </light>
+ <light name='light3' type='point'>
+ <pose>0 4.56036 2.65437 0 -0 0</pose>
+ <diffuse>0.5 0.5 0.5 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <attenuation>
+ <range>10</range>
+ <constant>0.5</constant>
+ <linear>0.3</linear>
+ <quadratic>0.1</quadratic>
+ </attenuation>
+ <cast_shadows>0</cast_shadows>
+ </light>
+ <light name='light4' type='point'>
+ <pose>-5 -3.54017 2.65437 0 -0 0</pose>
+ <diffuse>0.5 0.5 0.5 1</diffuse>
+ <specular>0.1 0.1 0.1 1</specular>
+ <attenuation>
+ <range>10</range>
+ <constant>0.5</constant>
+ <linear>0.3</linear>
+ <quadratic>0.1</quadratic>
+ </attenuation>
+ <cast_shadows>0</cast_shadows>
+ </light>
+
+ <!-- The elevator car -->
+ <model name="elevator">
+ <pose>0.781821 -2.6219 0.075 0 0 -1.570796</pose>
+ <link name="link">
+ <inertial>
+ <mass>800</mass>
+ </inertial>
+
+ <collision name="floor_collision">
+ <geometry>
+ <box>
+ <size>2.25 2.25 0.15</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0x02</collide_bitmask>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="floor_visual">
+ <geometry>
+ <box>
+ <size>2.25 2.25 0.15</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <!-- wall1_collision is the wall to the right of the door, when
+ facing the door -->
+ <collision name="wall1_collision">
+ <pose>1.0745 0.5725 1.125 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.1 1.15 2.25</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0x02</collide_bitmask>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="wall1_visual">
+ <pose>1.0745 0.5725 1.125 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.1 1.15 2.25</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+
+ <!-- wall2_collision is the wall to the left of the door, when
+ facing the door -->
+ <collision name="wall2_collision">
+ <pose>1.0745 -1.0625 1.125 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.1 0.125 2.25</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0x02</collide_bitmask>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="wall2_visual">
+ <pose>1.0745 -1.0625 1.125 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.1 0.125 2.25</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <link name="door">
+ <pose>1.0745 -0.5 1.125 0 0 0</pose>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>0.08 1.0 2.25</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0x02</collide_bitmask>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>0.08 1.0 2.25</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="door" type="prismatic">
+ <parent>link</parent>
+ <child>door</child>
+ <axis>
+ <xyz>0 1 0</xyz>
+ <limit>
+ <lower>0</lower>
+ <upper>1</upper>
+ <effort>10</effort>
+ </limit>
+
+ <dynamics>
+ <damping>2</damping>
+ </dynamics>
+ </axis>
+ </joint>
+
+ <joint name="lift" type="prismatic">
+ <parent>world</parent>
+ <child>link</child>
+ <axis>
+ <xyz>0 0 1</xyz>
+ <limit>
+ <lower>0</lower>
+ <upper>10</upper>
+ <effort>100000</effort>
+ </limit>
+
+ <dynamics>
+ <damping>50</damping>
+ </dynamics>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>
+ </ode>
+ </physics>
+ </joint>
+
+ <!-- Plugin to control the elevator -->
+ <plugin filename="libElevatorPlugin.so" name="elevator_plugin">
+ <lift_joint>elevator::lift</lift_joint>
+ <door_joint>elevator::door</door_joint>
+ <floor_height>2.675</floor_height>
+
+ <!-- This topic is used to control the elevator -->
+ <topic>~/elevator</topic>
+ </plugin>
+ </model>
+ </world>
+</sdf>
diff --git a/worlds/polyline.world b/worlds/polyline.world
index b3f56eb..4ae7c11 100644
--- a/worlds/polyline.world
+++ b/worlds/polyline.world
@@ -55,7 +55,7 @@
<name>Gazebo/Red</name>
</script>
</material>
- </visual>
+ </visual>
</link>
</model>
@@ -94,7 +94,7 @@
<name>Gazebo/Blue</name>
</script>
</material>
- </visual>
+ </visual>
</link>
</model>
@@ -130,7 +130,63 @@
<name>Gazebo/Yellow</name>
</script>
</material>
- </visual>
+ </visual>
+ </link>
+ </model>
+
+ <model name="letter">
+ <pose>2 2 0 0 0 0</pose>
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <polyline>
+ <point>2.27467 1.0967</point>
+ <point>1.81094 2.35418</point>
+ <point>2.74009 2.35418</point>
+ <height>1.0</height>
+ </polyline>
+ <polyline>
+ <point>2.08173 0.7599</point>
+ <point>2.4693 0.7599</point>
+ <point>3.4323 3.28672</point>
+ <point>3.07689 3.28672</point>
+ <point>2.84672 2.63851</point>
+ <point>1.7077 2.63851</point>
+ <point>1.47753 3.28672</point>
+ <point>1.11704 3.28672</point>
+ <height>1.0</height>
+ </polyline>
+ </geometry>
+ </collision>
+
+ <visual name="visual">
+ <geometry>
+ <polyline>
+ <point>2.27467 1.0967</point>
+ <point>1.81094 2.35418</point>
+ <point>2.74009 2.35418</point>
+ <height>1.0</height>
+ </polyline>
+ <polyline>
+ <point>2.08173 0.7599</point>
+ <point>2.4693 0.7599</point>
+ <point>3.4323 3.28672</point>
+ <point>3.07689 3.28672</point>
+ <point>2.84672 2.63851</point>
+ <point>1.7077 2.63851</point>
+ <point>1.47753 3.28672</point>
+ <point>1.11704 3.28672</point>
+ <height>1.0</height>
+ </polyline>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Red</name>
+ </script>
+ </material>
+ </visual>
</link>
</model>
</world>
diff --git a/worlds/shapes_bitmask.world b/worlds/shapes_bitmask.world
new file mode 100644
index 0000000..73f62b9
--- /dev/null
+++ b/worlds/shapes_bitmask.world
@@ -0,0 +1,87 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <model name="boxA">
+ <pose>0 0 1.0 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0x01</collide_bitmask>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ <model name="boxB">
+ <pose>0 0 3.0 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0x02</collide_bitmask>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ <model name="boxC">
+ <pose>0 0 6.0 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0x03</collide_bitmask>
+ </contact>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+ </model>
+
+ </world>
+</sdf>
diff --git a/worlds/shapes_layers.world b/worlds/shapes_layers.world
new file mode 100644
index 0000000..09fcd61
--- /dev/null
+++ b/worlds/shapes_layers.world
@@ -0,0 +1,88 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+ <include>
+ <uri>model://sun</uri>
+ </include>
+ <model name="box">
+ <pose>0 0 0.5 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <meta>
+ <layer>0</layer>
+ </meta>
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ <material>
+ <script>Gazebo/WoodPallet</script>
+ </material>
+ </visual>
+ </link>
+ </model>
+ <model name="sphere">
+ <pose>0 1.5 0.5 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <sphere>
+ <radius>0.5</radius>
+ </sphere>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <meta>
+ <layer>1</layer>
+ </meta>
+ <geometry>
+ <sphere>
+ <radius>0.5</radius>
+ </sphere>
+ </geometry>
+ <material>
+ <script>Gazebo/WoodPallet</script>
+ </material>
+ </visual>
+ </link>
+ </model>
+ <model name="cylinder">
+ <pose>0 -1.5 0.5 0 1.5707 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <cylinder>
+ <radius>0.5</radius>
+ <length>1.0</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <meta>
+ <layer>2</layer>
+ </meta>
+ <geometry>
+ <cylinder>
+ <radius>0.5</radius>
+ <length>1.0</length>
+ </cylinder>
+ </geometry>
+ <material>
+ <script>Gazebo/WoodPallet</script>
+ </material>
+ </visual>
+ </link>
+ </model>
+ </world>
+</sdf>
diff --git a/worlds/timer_gui.world b/worlds/timer_gui.world
index 7d80764..d5fb531 100644
--- a/worlds/timer_gui.world
+++ b/worlds/timer_gui.world
@@ -4,9 +4,33 @@
<gui>
<plugin name="timer_gui" filename="libTimerGUIPlugin.so">
- <topic>~/timer_control</topic>
+ <topic>~/timer_control_1</topic>
<pos>10 10</pos>
- <size>150 30</size>
+ <size>1 1</size>
+ </plugin>
+
+ <plugin name="timer_start_reset_buttons" filename="libTimerGUIPlugin.so">
+ <topic>~/timer_control_2</topic>
+ <pos>210 10</pos>
+ <size>200 180</size>
+ <start_stop_button>1</start_stop_button>
+ <reset_button>1</reset_button>
+ </plugin>
+
+ <plugin name="timer_start_button" filename="libTimerGUIPlugin.so">
+ <topic>~/timer_control_3</topic>
+ <pos>-20 -20</pos>
+ <start_stop_button>1</start_stop_button>
+ <reset_button>0</reset_button>
+ </plugin>
+
+ <!-- Countdown timer -->
+ <plugin name="timer_countdown" filename="libTimerGUIPlugin.so">
+ <topic>~/timer_control_4</topic>
+ <pos>460 10</pos>
+ <start_stop_button>1</start_stop_button>
+ <reset_button>1</reset_button>
+ <countdown_time>5 5000</countdown_time>
</plugin>
</gui>
diff --git a/worlds/transporter.world b/worlds/transporter.world
new file mode 100644
index 0000000..2a2c975
--- /dev/null
+++ b/worlds/transporter.world
@@ -0,0 +1,50 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <!-- A global light source -->
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <!-- A ground plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <plugin filename="libTransporterPlugin.so" name="transporter">
+ <activation_topic>~/transporter</activation_topic>
+
+ <pad name="pad1">
+ <destination>pad2</destination>
+
+ <!-- See examples/stand_alone_transporter.cc for an example of
+ activating a manual transporter -->
+ <activation>manual</activation>
+
+ <outgoing>
+ <min>-.5 -.5 0</min>
+ <max>.5 .5 1</max>
+ </outgoing>
+
+ <incoming>
+ <pose>2 3.5 0 0 0 0</pose>
+ </incoming>
+ </pad>
+
+ <pad name="pad2">
+ <destination>pad1</destination>
+ <activation>auto</activation>
+
+ <outgoing>
+ <min>-.5 3.0 0</min>
+ <max>.5 4.0 1</max>
+ </outgoing>
+
+ <incoming>
+ <pose>2 0 0 0 0 0</pose>
+ </incoming>
+ </pad>
+ </plugin>
+
+ </world>
+</sdf>
diff --git a/worlds/underwater.world b/worlds/underwater.world
new file mode 100644
index 0000000..9f3aee1
--- /dev/null
+++ b/worlds/underwater.world
@@ -0,0 +1,179 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <gui>
+ <camera name="user_camera">
+ <pose>10 11 10 0 0 -2.356</pose>
+ </camera>
+ </gui>
+
+ <scene>
+ <ambient> 0.25 0.3 0.5 1</ambient>
+ <grid>false</grid>
+ </scene>
+
+ <light type="directional" name="sun">
+ <cast_shadows>true</cast_shadows>
+ <pose>0 0 100 0 0 0</pose>
+ <diffuse>0.8 0.8 0.8 1</diffuse>
+ <specular>0.2 0.2 0.2 1</specular>
+ <attenuation>
+ <range>1000</range>
+ <constant>0.9</constant>
+ <linear>0.01</linear>
+ <quadratic>0.001</quadratic>
+ </attenuation>
+ <direction>-0.5 0.1 -0.9</direction>
+ </light>
+
+ <model name="ground_plane">
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>200 200</size>
+ </plane>
+ </geometry>
+ <surface>
+ <contact>
+ <collide_bitmask>0xffff</collide_bitmask>
+ </contact>
+ <friction>
+ <ode>
+ <mu>100</mu>
+ <mu2>50</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <cast_shadows>false</cast_shadows>
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>200 200</size>
+ </plane>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/Grey</name>
+ </script>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+
+ <include>
+ <uri>model://submarine_sinking</uri>
+ <pose>5 0 10 1.57 0 0</pose>
+ </include>
+
+ <include>
+ <uri>model://submarine</uri>
+ <pose>0 0 10 1.57 0 0</pose>
+ </include>
+
+ <include>
+ <uri>model://submarine_buoyant</uri>
+ <pose>-5 0 10 1.57 0 0</pose>
+ </include>
+
+ <model name="ceiling_plane">
+ <static>true</static>
+ <pose>0 0 40 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>200 200 .1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <cast_shadows>false</cast_shadows>
+ <geometry>
+ <box>
+ <size>200 200 .1</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/BlueTransparent</name>
+ </script>
+ </material>
+ </visual>
+
+ <visual name="visual_sideA">
+ <pose>100.05 0 -20 0 0 0</pose>
+ <cast_shadows>false</cast_shadows>
+ <geometry>
+ <box>
+ <size>0.1 200 39.9</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/BlueTransparent</name>
+ </script>
+ </material>
+ </visual>
+
+ <visual name="visual_sideB">
+ <pose>-100.05 0 -20 0 0 0</pose>
+ <cast_shadows>false</cast_shadows>
+ <geometry>
+ <box>
+ <size>0.1 200 39.9</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/BlueTransparent</name>
+ </script>
+ </material>
+ </visual>
+
+ <visual name="visual_sideC">
+ <pose>0 100.05 -20 0 0 0</pose>
+ <cast_shadows>false</cast_shadows>
+ <geometry>
+ <box>
+ <size>200 0.1 39.9</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/BlueTransparent</name>
+ </script>
+ </material>
+ </visual>
+
+ <visual name="visual_sideD">
+ <pose>0 -100.05 -20 0 0 0</pose>
+ <cast_shadows>false</cast_shadows>
+ <geometry>
+ <box>
+ <size>200 0.1 39.9</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ <name>Gazebo/BlueTransparent</name>
+ </script>
+ </material>
+ </visual>
+
+ </link>
+ </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